PCS7: KalFunc - Kalman function (FB1926)

: 2020-06-03

: komatic

: SCL



flow



This function is a internal used for Kalman filter.
Multiplexer for calling the correct model

FB1926 code without interface



Name: FB1926
Symbolic Name: KalFunc
Symbol Comment: KalFunc
Family: System
Version: 9.0
Author: AdvLib90
Last modified: 10/10/2016
Use:
Size in work memory: 7278 bytes
Signature: compiled by SCL compiler version: SCLCOMP K05.03.00.00_11.01.00.01 release





{
Scl_ResetOptions ;
Scl_OverwriteBlocks:= 'y' ;
Scl_GenerateReferenceData := 'y' ;
Scl_S7ServerActive:= 'n' ;
Scl_CreateObjectCode:= 'y' ;
Scl_OptimizeObjectCode:= 'y' ;
Scl_MonitorArrayLimits:= 'n' ;
Scl_CreateDebugInfo := 'n' ;
Scl_SetOKFlag:= 'n' ;
Scl_SetMaximumStringLength:= '254'
}
 
FUNCTION_BLOCK FB11926 //"KalFunc"
TITLE ='Kalman-Functions'
AUTHOR : AdvLib90
FAMILY : System
NAME : KalFunc
VERSION : '9.0'
 
BEGIN
Error:=FALSE;
 
IF ((((((SampleTime=0.0) OR (NumberX=0)) OR (NumberY=0)) OR (StcX_MinusK.zeilenzahl=0)) OR (StcX_PlusK.zeilenzahl=0)) OR (StcU.zeilenzahl=0)) THEN //A7d0//
Error:=TRUE;
END_IF; //A7d0//
 
StcX_dach_minus_k.zeilenzahl:=StcX_MinusK.zeilenzahl;
 
FOR i:=1 TO StcX_MinusK.zeilenzahl BY 1 DO
 
StcX_dach_minus_k_aux.arr[i]:=StcX_MinusK_aux.arr1[i,1];
 
END_FOR;
 
StcX_dach_plus_k.zeilenzahl:=StcX_PlusK.zeilenzahl;
 
FOR i:=1 TO StcX_PlusK.zeilenzahl BY 1 DO
 
StcX_dach_plus_k_aux.arr[i]:=StcX_PlusK_aux.arr1[i,1];
 
END_FOR;
 
StcU_k.zeilenzahl:=StcU.zeilenzahl;
 
FOR i:=1 TO StcU_k.zeilenzahl BY 1 DO
 
StcU_k_aux.arr1[i]:=StcU_aux.arr1[i,1];
 
END_FOR;
 
 
IF (NOT(Error)) THEN //A7d7//
IF (ProModId=1) THEN //A7d8//

KalMod1(SampleTime := SampleTime // IN: REAL
,CalcFunc := CalcFunc // IN: INT
,NumberX := NumberX // IN: INT
,NumberY := NumberY // IN: INT
,NumberYn := NumberYn // IN: INT
,NumberZ := NumberZ // IN: INT
,StcX_MinusK := StcX_dach_minus_k // IN: STRUCT
,StcX_PlusK := StcX_dach_plus_k // IN: STRUCT
,StcU := StcU_k // IN: STRUCT
,StcX_MinK1 := StcX_min_k1 // OUT: STRUCT
,StcY_MinusK := StcY_dach_minus_k // OUT: STRUCT
,StcZ := StcZ_k // OUT: STRUCT
,StcH := StcH_k // OUT: STRUCT
,StcPHI := StcPHI_k // OUT: STRUCT
,StcKalDB := StcKalDbTemp // OUT: STRUCT
,Error := Error // OUT: BOOL
); // VOID
 
StcX_MinK1.zeilenzahl:=StcX_min_k1.zeilenzahl;
StcX_MinK1.spaltenzahl:=1;
 
 
FOR i:=1 TO StcX_MinK1.zeilenzahl BY 1 DO
 
StcX_MinK1_aux1.arr1[i,1]:=StcX_min_k1_aux.arr1[i];
 
END_FOR;

StcY_MinusK.zeilenzahl:=StcY_dach_minus_k.zeilenzahl;
StcY_MinusK.spaltenzahl:=1;

FOR i:=1 TO StcY_MinusK.zeilenzahl BY 1 DO
 
StcY_MinusK_aux1.arr1[i,1]:=StcY_dach_minus_k_aux.arr1[i];
 
END_FOR;

StcZ.zeilenzahl:=StcZ_k.zeilenzahl;
StcZ.spaltenzahl:=1;

FOR i:=1 TO StcZ.zeilenzahl BY 1 DO
 
StcZ_aux1.arr1[i,1]:=StcZ_k_aux.arr1[i];
 
END_FOR;
 
StcH:=StcH_k;
StcPHI:=StcPHI_k;
StcKalDB:=StcKalDbTemp;
 
ELSIF (ProModId=2) THEN //A7e0//

KalMod2(SampleTime := SampleTime // IN: REAL
,CalcFunc := CalcFunc // IN: INT
,NumberX := NumberX // IN: INT
,NumberY := NumberY // IN: INT
,NumberYn := NumberYn // IN: INT
,NumberZ := NumberZ // IN: INT
,StcX_MinusK := StcX_dach_minus_k // IN: STRUCT
,StcX_PlusK := StcX_dach_plus_k // IN: STRUCT
,StcU := StcU_k // IN: STRUCT
,StcX_MinK1 := StcX_min_k1 // OUT: STRUCT
,StcY_MinusK := StcY_dach_minus_k // OUT: STRUCT
,StcZ := StcZ_k // OUT: STRUCT
,StcH := StcH_k // OUT: STRUCT
,StcPHI := StcPHI_k // OUT: STRUCT
,StcKalDB := StcKalDbTemp // OUT: STRUCT
,Error := Error // OUT: BOOL
); // VOID
 
StcX_MinK1.zeilenzahl:=StcX_min_k1.zeilenzahl;
StcX_MinK1.spaltenzahl:=1;
 
 
FOR i:=1 TO StcX_MinK1.zeilenzahl BY 1 DO
 
StcX_MinK1_aux1.arr1[i,1]:=StcX_min_k1_aux.arr1[i];
 
END_FOR;

StcY_MinusK.zeilenzahl:=StcY_dach_minus_k.zeilenzahl;
StcY_MinusK.spaltenzahl:=1;

FOR i:=1 TO StcY_MinusK.zeilenzahl BY 1 DO
 
StcY_MinusK_aux1.arr1[i,1]:=StcY_dach_minus_k_aux.arr1[i];
 
END_FOR;

StcZ.zeilenzahl:=StcZ_k.zeilenzahl;
StcZ.spaltenzahl:=1;

FOR i:=1 TO StcZ.zeilenzahl BY 1 DO
 
StcZ_aux1.arr1[i,1]:=StcZ_k_aux.arr1[i];
 
END_FOR;
 
StcH:=StcH_k;
StcPHI:=StcPHI_k;
StcKalDB:=StcKalDbTemp;
 
 
ELSIF (ProModId=3) THEN //A7e7//

KalMod3(SampleTime := SampleTime // IN: REAL
,CalcFunc := CalcFunc // IN: INT
,NumberX := NumberX // IN: INT
,NumberY := NumberY // IN: INT
,NumberYn := NumberYn // IN: INT
,NumberZ := NumberZ // IN: INT
,StcX_MinusK := StcX_dach_minus_k // IN: STRUCT
,StcX_PlusK := StcX_dach_plus_k // IN: STRUCT
,StcU := StcU_k // IN: STRUCT
,StcX_MinK1 := StcX_min_k1 // OUT: STRUCT
,StcY_MinusK := StcY_dach_minus_k // OUT: STRUCT
,StcZ := StcZ_k // OUT: STRUCT
,StcH := StcH_k // OUT: STRUCT
,StcPHI := StcPHI_k // OUT: STRUCT
,StcKalDB := StcKalDbTemp // OUT: STRUCT
,Error := Error // OUT: BOOL
); // VOID
 
StcX_MinK1.zeilenzahl:=StcX_min_k1.zeilenzahl;
StcX_MinK1.spaltenzahl:=1;
 
 
FOR i:=1 TO StcX_MinK1.zeilenzahl BY 1 DO
 
StcX_MinK1_aux1.arr1[i,1]:=StcX_min_k1_aux.arr1[i];
 
END_FOR;

StcY_MinusK.zeilenzahl:=StcY_dach_minus_k.zeilenzahl;
StcY_MinusK.spaltenzahl:=1;

FOR i:=1 TO StcY_MinusK.zeilenzahl BY 1 DO
 
StcY_MinusK_aux1.arr1[i,1]:=StcY_dach_minus_k_aux.arr1[i];
 
END_FOR;

StcZ.zeilenzahl:=StcZ_k.zeilenzahl;
StcZ.spaltenzahl:=1;

FOR i:=1 TO StcZ.zeilenzahl BY 1 DO
 
StcZ_aux1.arr1[i,1]:=StcZ_k_aux.arr1[i];
 
END_FOR;
 
StcH:=StcH_k;
StcPHI:=StcPHI_k;
StcKalDB:=StcKalDbTemp;

ELSIF (ProModId=4) THEN //A7e7//

KalMod4(SampleTime := SampleTime // IN: REAL
,CalcFunc := CalcFunc // IN: INT
,NumberX := NumberX // IN: INT
,NumberY := NumberY // IN: INT
,NumberYn := NumberYn // IN: INT
,NumberZ := NumberZ // IN: INT
,StcX_MinusK := StcX_dach_minus_k // IN: STRUCT
,StcX_PlusK := StcX_dach_plus_k // IN: STRUCT
,StcU := StcU_k // IN: STRUCT
,StcX_MinK1 := StcX_min_k1 // OUT: STRUCT
,StcY_MinusK := StcY_dach_minus_k // OUT: STRUCT
,StcZ := StcZ_k // OUT: STRUCT
,StcH := StcH_k // OUT: STRUCT
,StcPHI := StcPHI_k // OUT: STRUCT
,StcKalDB := StcKalDbTemp // OUT: STRUCT
,Error := Error // OUT: BOOL
); // VOID
 
StcX_MinK1.zeilenzahl:=StcX_min_k1.zeilenzahl;
StcX_MinK1.spaltenzahl:=1;
 
 
FOR i:=1 TO StcX_MinK1.zeilenzahl BY 1 DO
 
StcX_MinK1_aux1.arr1[i,1]:=StcX_min_k1_aux.arr1[i];
 
END_FOR;

StcY_MinusK.zeilenzahl:=StcY_dach_minus_k.zeilenzahl;
StcY_MinusK.spaltenzahl:=1;

FOR i:=1 TO StcY_MinusK.zeilenzahl BY 1 DO
 
StcY_MinusK_aux1.arr1[i,1]:=StcY_dach_minus_k_aux.arr1[i];
 
END_FOR;

StcZ.zeilenzahl:=StcZ_k.zeilenzahl;
StcZ.spaltenzahl:=1;

FOR i:=1 TO StcZ.zeilenzahl BY 1 DO
 
StcZ_aux1.arr1[i,1]:=StcZ_k_aux.arr1[i];
 
END_FOR;
 
StcH:=StcH_k;
StcPHI:=StcPHI_k;
StcKalDB:=StcKalDbTemp;
 
ELSIF (ProModId=5) THEN //A7e7//

KalMod5(SampleTime := SampleTime // IN: REAL
,CalcFunc := CalcFunc // IN: INT
,NumberX := NumberX // IN: INT
,NumberY := NumberY // IN: INT
,NumberYn := NumberYn // IN: INT
,NumberZ := NumberZ // IN: INT
,StcX_MinusK := StcX_dach_minus_k // IN: STRUCT
,StcX_PlusK := StcX_dach_plus_k // IN: STRUCT
,StcU := StcU_k // IN: STRUCT
,StcX_MinK1 := StcX_min_k1 // OUT: STRUCT
,StcY_MinusK := StcY_dach_minus_k // OUT: STRUCT
,StcZ := StcZ_k // OUT: STRUCT
,StcH := StcH_k // OUT: STRUCT
,StcPHI := StcPHI_k // OUT: STRUCT
,StcKalDB := StcKalDbTemp // OUT: STRUCT
,Error := Error // OUT: BOOL
); // VOID
 
StcX_MinK1.zeilenzahl:=StcX_min_k1.zeilenzahl;
StcX_MinK1.spaltenzahl:=1;
 
 
FOR i:=1 TO StcX_MinK1.zeilenzahl BY 1 DO
 
StcX_MinK1_aux1.arr1[i,1]:=StcX_min_k1_aux.arr1[i];
 
END_FOR;

StcY_MinusK.zeilenzahl:=StcY_dach_minus_k.zeilenzahl;
StcY_MinusK.spaltenzahl:=1;

FOR i:=1 TO StcY_MinusK.zeilenzahl BY 1 DO
 
StcY_MinusK_aux1.arr1[i,1]:=StcY_dach_minus_k_aux.arr1[i];
 
END_FOR;

StcZ.zeilenzahl:=StcZ_k.zeilenzahl;
StcZ.spaltenzahl:=1;

FOR i:=1 TO StcZ.zeilenzahl BY 1 DO
 
StcZ_aux1.arr1[i,1]:=StcZ_k_aux.arr1[i];
 
END_FOR;
 
StcH:=StcH_k;
StcPHI:=StcPHI_k;
StcKalDB:=StcKalDbTemp;

ELSE //A7f5//
Error:=TRUE;
END_IF;
END_IF; //A7d7//
 
END_FUNCTION_BLOCK





flow

Block checksum equals.







Original library PCS7 APL (V9.0sp2) (zip, 4.6Mb)

Documentation (pdf, 22Mb)







: 412

.

:

(4000 max):

: