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)
Ïðîñìîòðîâ: 705
Êîììåíòàðèè ê ìàòåðèàëó
Âàø áóäåò ïåðâûì.
Äîáàâèòü êîììåíòàðèé