Âåðñèÿ äëÿ ïå÷àòè

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)







Ïðîñìîòðîâ: 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]?

Äîáàâèòü êîììåíòàðèé

Âàøå èìÿ:

Òåêñò êîììåíòàðèÿ (4000 max):

Ââåäèòå ñóììó ñ êàðòèíêè: