PCS7: KalFunc - Kalman function (FB1926)
Äàòà: 2020-06-03
Äîáàâëåíî: komatic
Òåìà: SCL
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
Block checksum equals.
Original library PCS7 APL (V9.0sp2) (zip, 4.6Mb)
Documentation (pdf, 22Mb)
Ïðîñìîòðîâ: 1384
Êîììåíòàðèè ê ìàòåðèàëó
Äîáàâëåí: RobWid Äàòà: 2021-05-20
Will there be any conversion of KalMod1, KalMod2 and KalMod5 which is used in this function block?
Äîáàâëåí: Robin W Äàòà: 2021-08-26
Hi komatic,
Do you have the interface for this FB? Possible for you to post or send to me via email [email protected]?
Äîáàâèòü êîììåíòàðèé