Subversion Repositories FlightCtrl

/*
Copyright 2008, by Michael Walter

All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.

Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/

#include "kafi.h"
#include "KalmanFc.h"
#include "main.h"
#include "mm3.h"

#include "main.h"
#include "eeprom.c"

#define MAX_GAS 250
#define MIN_GAS    15

#define sin45 sin(45.F / 180.F * PI)
#define cos45 cos(45.F / 180.F * PI)

extern void GPSupdate(void);

int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;
int AverageRoll_X = 0, AverageRoll_Y = 0, AverageRoll_Z = 0;    
int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0;    

f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F;    
f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F,  ACC_Z_Offset = 0.F;      
f32_t Phi, Theta, sinPhi,  cosPhi, tanTheta, secTheta;
f32_t sinPhi, cosPhi, sinTheta, cosTheta;
f32_t sinPhi_P,  cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P;

#ifdef USE_Extended_MM3_Measurement_Model
f32_t sinPsi_P,  cosPsi_P;
extern f32_t MM3_Hx, MM3_Hy, MM3_Hz;
#endif

int DiffAltitude = 0, DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, NominalAltitude = 0, InitialAltitude = 0;
int KompassValue = 0, SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;
int GierMischanteil;
extern int GasMischanteil;

extern char Notlandung;
extern unsigned long maxDistance;

unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
unsigned char MotorWert[5];

extern signed int GPS_Nick,  GPS_Roll;

int GasMischanteil;
char MotorenEin = 0;
char Notlandung = 0;

unsigned char SenderOkay = 0;
unsigned int I2CTimeout = 100;

struct mk_param_struct EE_Parameter;

/* ****************************************************************************
Functionname:     SendMotorData                      */ /*!
Description:          Senden der Motorwerte per I2C-Bus

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void SendMotorData(void)
{
    if(!MotorenEin)
    {
        Motor_Hinten = 0;
        Motor_Vorne = 0;
        Motor_Rechts = 0;
        Motor_Links = 0;
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
        if(MotorTest[2]) Motor_Links = MotorTest[2];
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
    }

    //Start I2C Interrupt Mode
    twi_state = 0;
    motor = 0;
    i2c_start();
}


/* ****************************************************************************
Functionname:     MotorControl                      */ /*!
Description:      

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void MotorControl(void)
{
        static unsigned int NotGasZeit;
        static unsigned char delay_neutral = 0;
        static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
        static unsigned int  modell_fliegt = 0;
       
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        // Gaswert ermitteln
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
    if(GasMischanteil < 0)
        {
                GasMischanteil = 0;
        }
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        // Emfang schlecht
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
       
        if(SenderOkay < 100)
        {
        if(!PcZugriff)
        {
                        if(BeepMuster == 0xffff)
            {
                                beeptime = 15000;
                                BeepMuster = 0x0c00;
            }
        }
        if(NotGasZeit)
                {
                        NotGasZeit--;
                }
        else
        {
                        MotorenEin = 0;
                        Notlandung = 0;
        }
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
        {
            GasMischanteil = 100; //EE_Parameter.NotGas;
            Notlandung = 1;
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
        }
        else
                {
                        MotorenEin = 0;
        }
        }
    else
        {
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                // Emfang gut
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                if(SenderOkay > 140)
                {
                        Notlandung = 0;
                        NotGasZeit = 200 * 50; //EE_Parameter.NotGasZeit * 50;
                        if(GasMischanteil > 40)
                        {
                                if(modell_fliegt < 0xffff)
                                {
                                        modell_fliegt++;
                                }
                        }
                       
                        if((GasMischanteil > 200) && MotorenEin == 0)
                        {
                                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                                // auf Nullwerte kalibrieren
                                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
                                {
                                        if(++delay_neutral > 50)  // nicht sofort
                                        {
                                                MotorenEin = 0;
                                                delay_neutral = 0;
                                                modell_fliegt = 0;
                                                if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
                                                {
                                                        if((AdWertAirPressure_Raw > 950) || (AdWertAirPressure_Raw < 750))
                                                        {      
                                                                SucheLuftruckOffset();
                                                        }
                                                }  
                                                ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                                                SetNeutral();
                                        }
                                }
                                else delay_neutral = 0;
                        }
                        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                        // Gas ist unten
                        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                        if(GasMischanteil < 35)
                        {
                                // Starten
                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
                                {
                                        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                                        // Einschalten
                                        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                                        if(++delay_einschalten > 100)
                                        {
                                                MotorenEin = 1;
                                                modell_fliegt = 1;
                                                delay_einschalten = 100;
                                        }          
                                }  
                                else
                                {
                                        delay_einschalten = 0;
                                }
                                //Auf Neutralwerte setzen
                                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                                // Auschalten
                                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
                                {
                                        if(++delay_ausschalten > 50)  // nicht sofort
                                        {
                                                MotorenEin = 0;
                                                modell_fliegt = 0;
                                                delay_ausschalten = 50;
                                        }
                                }
                                else
                                {
                                        delay_ausschalten = 0;
                                }
                        }
                }
        }
}


/* ****************************************************************************
Functionname:     SetNeutral                      */ /*!
Description:    

        @param[in]        
       
          @return           void
          @pre              -
          @post             -
          @author         Michael Walter  
**************************************************************************** */
void SetNeutral(void)
{      
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
    {    
                if((AdWertAirPressure_Raw > 950) || (AdWertAirPressure_Raw < 750))
                {
                        SucheLuftruckOffset();
                }
        }
       
        LastAltitude = CurrentAltitude;
        Delay_ms_Mess(300);
               
ANALOG_OFF;
    AdNeutralNick = AdWertNick_Raw;    
        AdNeutralRoll = AdWertRoll_Raw;
        AdNeutralGier = AdWertGier_Raw;
    NeutralAccY = AdWertAccRoll_Raw;
        NeutralAccX = AdWertAccNick_Raw;
        NeutralAccZ = AdWertAccHoch_Raw;

        Roll_X_Offset = 0.F;
        Roll_Y_Offset = 0.F;
        Roll_Z_Offset = 0.F;
    ACC_X_Offset = 0.F;
        ACC_Y_Offset = 0.F;
    ACC_Z_Offset = 0.F;        

        AccumulatedACC_X = 0;
        AccumulatedACC_X_cnt = 0;
        AccumulatedACC_Y = 0;
        AccumulatedACC_Y_cnt = 0;      
        AccumulatedACC_Z = 0;
        AccumulatedACC_Z_cnt = 0;
       
        AccumulatedRoll_X = 0;
        AccumulatedRoll_X_cnt = 0;
        AccumulatedRoll_Y = 0;
        AccumulatedRoll_Y_cnt = 0;             
        AccumulatedRoll_Z = 0;
        AccumulatedRoll_Z_cnt = 0;
       
        AveragerACC_X = 0;
        AveragerACC_Y = 0;
        AveragerACC_Z = 0;
       
        AccumulatedACC_X = 0;
        AccumulatedACC_X_cnt = 0;
        AccumulatedACC_Y = 0;
        AccumulatedACC_Y_cnt = 0;
        AccumulatedACC_Z = 0;
        AccumulatedACC_Z_cnt = 0;
 ANALOG_ON;

        SummeRoll = 0;
        SummeNick = 0;

        sollGier = status.iPsi10;
        InitialAltitude = AdWertAirPressure_Raw;
    beeptime = 50;  
}


/* ****************************************************************************
Functionname:     CalculateAverage                      */ /*!
Description:    

        @param[in]        
       
          @return           void
          @pre              -
          @post             -
          @author         Michael Walter  
**************************************************************************** */
void CalculateAverage(void)
{
        ANALOG_OFF;
        AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt;
        AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt;
        AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt;

    /* Get Pressure Differenz */
        CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;
        AccumulatedAirPressure_cnt = 0;
        AccumulatedAirPressure = 0;
       
        static char AirPressureCnt = 0;
        if (AirPressureCnt % 25 == 1)
        {
                DeltaAltitude = CurrentAltitude - LastAltitude;
                LastAltitude = CurrentAltitude;
        }
        AirPressureCnt++;

        if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10))
        {
                Roll_X_Offset = 0.9995F * Roll_X_Offset + 0.0005F *  (float) (MAX(-60, MIN(60,AverageRoll_X)));
                Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F *  (float) (MAX(-60, MIN(60,AverageRoll_Y)));
       
                if (abs(StickGier) < 15 || MotorenEin == 0)
                {      
                        Roll_Z_Offset = 0.9998F * Roll_Z_Offset + 0.0002F *  (float) ( MAX(-60, MIN(60,AverageRoll_Z)));
                }
        }      

#if 0  
        DebugOut.Analog[3] =  AdWertGier_Raw;
        DebugOut.Analog[4]  = AdWertRoll_Raw;
        DebugOut.Analog[5]  = AdWertNick_Raw;
#endif
       
        AverageRoll_X -= Roll_X_Offset;
        AverageRoll_Y -= Roll_Y_Offset;
        AverageRoll_Z -= Roll_Z_Offset;

        AccumulatedRoll_X = 0;
        AccumulatedRoll_X_cnt = 0;
        AccumulatedRoll_Y = 0;
        AccumulatedRoll_Y_cnt = 0;
        AccumulatedRoll_Z = 0;
        AccumulatedRoll_Z_cnt = 0;

#if 0  
        ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt);   
        ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt);
        ACC_Z_Offset = 0.9999F * ACC_Z_Offset + 0.0001F * ((float) AccumulatedACC_Z / (float) AccumulatedACC_Z_cnt);
#endif

        AveragerACC_X =  AccumulatedACC_X / AccumulatedACC_X_cnt;
        AveragerACC_Y =  AccumulatedACC_Y / AccumulatedACC_Y_cnt;
        AveragerACC_Z =  AccumulatedACC_Z / AccumulatedACC_Z_cnt;
       
        AccumulatedACC_X = 0;
        AccumulatedACC_X_cnt = 0;
        AccumulatedACC_Y = 0;
        AccumulatedACC_Y_cnt = 0;
        AccumulatedACC_Z = 0;
        AccumulatedACC_Z_cnt = 0;
       
        ANALOG_ON;

        if (Roll_X_Off > 60)
        {
                Roll_X_Off = 60;
        }
        if (Roll_X_Off < -60)
        {
                Roll_X_Off = -60;
        }
       
        if (Roll_Y_Off > 60)
        {
                Roll_Y_Off = 60;
        }
        if (Roll_Y_Off < -60)
        {
                Roll_Y_Off = -60;
        }

        if (Roll_Z_Off > 60)
        {
                Roll_Z_Off = 60;
        }
        if (Roll_Z_Off < -60)
        {
                Roll_Z_Off = -60;
        }
}


/* ****************************************************************************
Functionname:     PID_Regler                      */ /*!
Description:

  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
void PID_Regler(void)
{
    int StickNick45,StickRoll45;
        int DiffNick,DiffRoll, DiffGier;    
        int motorwert = 0;
    int pd_ergebnis;   
       
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        // neue Werte von der Funke
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        if(!NewPpmData--)  
        {
                StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
                StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
                StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
        } // Ende neue Funken-Werte

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        // Bei Empfangsausfall im Flug
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        if(Notlandung)
    {
                StickGier = 0;
                StickNick = 0;
                StickRoll = 0;
    }    
 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        // + Mischer und PID-Regler
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               

        /* G P S   U p d a t e */
        static char CntGPS = 0;
        if (CntGPS % 10 == 1)
        {
                //GPSupdate();
        }
        CntGPS++;

        StickRoll45 = (int) (0.707F *  (float)(StickRoll - GPS_Roll) - 0.707F * (float)(StickNick - GPS_Nick));
        StickNick45 = (int) (0.707F *  (float)(StickRoll - GPS_Roll) + 0.707F * (float)(StickNick - GPS_Nick));

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
        //  Gieren
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               

        if(GasMischanteil < 20)
        {
                sollGier = status.iPsi10;
                SummeRoll = 0;
                SummeNick = 0;
        }
       
        /* Gier-Anteil */      
        if (abs(StickGier) > 4)
        {
                sollGier = status.iPsi10 + 4 * StickGier;  
        }
        DiffGier = (sollGier - status.iPsi10);         
        GierMischanteil = (int) (-4 *  DiffGier - 4* (AdWertGier - AdNeutralGier - Roll_Z_Off)) / 10;                          
 
        #define MUL_G  0.8
    if(GierMischanteil > MUL_G * GasMischanteil)
        {
                GierMischanteil = MUL_G * GasMischanteil;
        }
    if(GierMischanteil < -MUL_G * GasMischanteil)
        {
                GierMischanteil = -MUL_G * GasMischanteil;
        }
    if(GierMischanteil > 50)
        {
                GierMischanteil = 50;
        }
    if(GierMischanteil < -50)
        {
                GierMischanteil = -50;
        }

        int scale_p = 7;
        int scale_d = 10;

        /* N i c k - A c h s e */
    DiffNick = -(status.iTheta10 + StickNick45);       
        /* R o l l - A c h s e */
        DiffRoll = -(status.iPhi10 + StickRoll45);

        SummeNick += DiffNick;
    if(SummeNick >  10000)
        {
                SummeNick =  10000;
        }
    if(SummeNick < -10000)
        {
                SummeNick = -10000;
        }      

        SummeRoll += DiffRoll;  
        if(SummeRoll >  10000)
        {
                SummeRoll =  10000;
        }      
    if(SummeRoll < -10000)
        {
                SummeRoll = -10000;
        }

        pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10) ; // + (int)(SummeNick / 2000);                         
    if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
        {
                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
    }
        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
        {
                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
        }

    /* M o t o r   V o r n */  
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
        if ((motorwert < 0))
        {
                motorwert = 0;
        }
        else if(motorwert > MAX_GAS)
        {
                motorwert = MAX_GAS;
        }
        if (motorwert < MIN_GAS)
        {
                motorwert = MIN_GAS;
        }
        Motor_Vorne = motorwert;           

    /* M o t o r   H e c k */
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
        if ((motorwert < 0))
        {
                motorwert = 0;
        }
        else if(motorwert > MAX_GAS)
        {
                motorwert = MAX_GAS;
        }
        if (motorwert < MIN_GAS)
        {
                motorwert = MIN_GAS;
        }
        Motor_Hinten = motorwert;              

    pd_ergebnis =  ((scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10) ; //+ (int)(SummeRoll / 2000);            
        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
        {
                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
        }
    if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
        {
                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
        }
       
        /* M o t o r   L i n k s */
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
    if(motorwert > MAX_GAS)
        {
                motorwert = MAX_GAS;
        }
        if (motorwert < MIN_GAS)
        {
        motorwert = MIN_GAS;
        }
        Motor_Links = motorwert;               
   
        /* M o t o r   R e c h t s */
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;    
    if(motorwert > MAX_GAS)
        {
                motorwert = MAX_GAS;
        }
        if (motorwert < MIN_GAS)
        {
                motorwert = MIN_GAS;   
        }
        Motor_Rechts = motorwert;

#if 1            
        DebugOut.Analog[0] = status.iTheta10;
        DebugOut.Analog[1] = status.iPhi10;
    DebugOut.Analog[2] = status.iPsi10 / 10;  
        //DebugOut.Analog[3] = KompassValue;
        //DebugOut.Analog[4] = (sollGier - status.iPsi10 ) / 10;
        //DebugOut.Analog[5] = AverageRoll_Z;
        //DebugOut.Analog[6] = AverageRoll_X;

        //DebugOut.Analog[9]    =  CurrentAltitude;
        //DebugOut.Analog[10] =  DiffAltitude;
        //DebugOut.Analog[8] = AverageRoll_X;          
        //DebugOut.Analog[9] = AverageRoll_Y;
        //DebugOut.Analog[10] = AverageRoll_Z;

        //DebugOut.Analog[11] = GasMischanteil;        
        //DebugOut.Analog[13] = AverageRoll_X;
        //DebugOut.Analog[14] = AverageRoll_Y;
        //DebugOut.Analog[15] = AdWertAirPressure_Raw;
#endif
}


/*****************************************************************************
  VARIABLES
*****************************************************************************/

/* Kalman filter */
kafi_t *p_kafi;
mat_Matrix_t *matP0;
mat_Matrix_t *matQ;
mat_Matrix_t *matR;
mat_Matrix_t *matXd;    /* estimated state */
mat_Matrix_t *matXs;    /* predicted state */
mat_Matrix_t *matPhi;   /* transition matrix to predict new state from current state */
mat_Matrix_t *matB;     /* control matrix to predict new state from ext. control vector U */
mat_Matrix_t *matU;     /* control vector */
mat_Matrix_t *matC;     /* Jacobi matrix */
mat_Matrix_t *matY;     /* real measurements */
mat_Matrix_t *matdY;    /* innovation */
mat_Matrix_t *matYs;    /* predicted measurements */
mat_Matrix_t *matP;     /* error covariance matrix */
mat_Matrix_t *matPDiag; /* diagonal covariance matrix */
char *fYValid;          /* measurement validity flag */


void Kafi_Init()
{
        /* create kalman filter and associated matrizes */
    p_kafi = KAFICreate();
       
    KAFIInit(p_kafi);
   
    /* get pointers to all matrices */
    KAFIGetP0(p_kafi, &matP0);
    KAFIGetQ(p_kafi, &matQ);
    KAFIGetR(p_kafi, &matR);
    KAFIGetXd(p_kafi, &matXd);
    KAFIGetXs(p_kafi, &matXs);
    KAFIGetPhi(p_kafi, &matPhi);
    KAFIGetB(p_kafi, &matB);
    KAFIGetU(p_kafi, &matU);
    KAFIGetC(p_kafi, &matC);
    KAFIGetY(p_kafi, &matY);
    KAFIGetYs(p_kafi, &matYs);
    KAFIGetPDiag(p_kafi, &matPDiag);
    KAFIGetYValid(p_kafi, &fYValid);
    KAFIGetdY(p_kafi,&matdY);
   
    trResetKalmanFilter();
}

/* ****************************************************************************
Functionname:     trResetKalmanFilter                      */ /*!
Description:

  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */

void trResetKalmanFilter()
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*--- VARIABLES ---*/
        ui32_t i, j;
       
        /* set phi to identity matrix */
        for ( j = 0; j < p_kafi->uiDimX; j++ )
        {
                for ( i = 0; i < p_kafi->uiDimX; i++ )
                {
                        matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F);
                }
        }
       
        /* set diagonal of measurement noise covariance R */
        matSetDiagonal(matR, _ax, _ax, 10.0F);
        matSetDiagonal(matR, _ay, _ay, 10.0F);
        matSetDiagonal(matR, _az, _az, 10.0F);
#ifdef USE_Extended_MM3_Measurement_Model      
        matSetDiagonal(matR, _mx, _mx, 100.0F);
        matSetDiagonal(matR, _my, _my, 100.0F);
        matSetDiagonal(matR, _mz, _mz, 100.0F);
#else
        matSetDiagonal(matR, _compass, _compass, 5.0F);
#endif
       
        /* set diagonal of system noise covariance Q */
        matSetDiagonal(matQ, _Phi, _Phi, 0.00001F);
        matSetDiagonal(matQ, _Theta, _Theta, 0.00001F);
        matSetDiagonal(matQ, _Psi, _Psi, 0.00005F);
       
        /* set diagonal init. estimation error (covariance) P0 */
        matSetDiagonal(matP0, _Phi, _Phi_, 0.00001F);
        matSetDiagonal(matP0, _Theta, _Theta, 0.00001F);
        matSetDiagonal(matP0, _Psi, _Psi, 0.00001F);
       
        matSetFull(matXd, _Phi, 0, 0.0F);
        matSetFull(matXd, _Theta, 0, 0.0F);
        matSetFull(matXd, _Psi, 0, 0.0F);
       
        matSetFull(matXs, _Phi, 0, 0.0F);
        matSetFull(matXs, _Theta, 0, 0.0F);
        matSetFull(matXs, _Psi, 0, 0.0F);      
        memset( &status, 0, sizeof(status) );
       
        KAFISetP0(p_kafi);
}




/* ****************************************************************************
Functionname:    trInnovate                      */ /*!
Description:     Update the current System State based on the current Observation

 
        @param[in]        
       
          @return           void
          @pre              -
          @post             -
          @author           Michael Walter
**************************************************************************** */
void trInnovate()
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*--- VARIABLES ---*/
       
        /*estimate the new system state (Xd), nominal case */
        KAFIInnovation(p_kafi);
        KAFIUpdatePDiag(p_kafi);
        KAFIUpdateP(p_kafi);
        return;
}



/* ****************************************************************************
Functionname:     trUpdatePhi                                          */ /*!
Description:      Setup matPhi, used to calculate the predicted state
from the current state

 
        @param[in]        
        @param[in]        fCycleTime
       
          @return           void
          @pre              -
          @post             -
          @author           Michael Walter
**************************************************************************** */
void trUpdatePhi()
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*--- VARIABLES ---*/
        matSetFull(matPhi, _Phi, _Phi, 1.0F);
        matSetFull(matPhi, _Theta, _Theta, 1.0F);
        matSetFull(matPhi, _Psi, _Psi, 1.0F);
}



#ifdef USE_Extended_MM3_Measurement_Model
/* ****************************************************************************
Functionname:     trUpdateJacobiMatrix                      */ /*!
Description:

 
        @param[in]        void
       
          @return           void
          @pre              -
          @post             -
          @author           Michael Walter
*****************************************************************************/
void trUpdateJacobiMatrix(void)
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
        /*
        Simplified Version                     
        Pre_ax =  du - sinTheta * g;                
        Pre_ay =  dv + cosTheta_sinPhi * g;    
        Pre_az =  dw + cosTheta_cosPhi * g;      
        */
       
        /*--- VARIABLES ---*/
        f32_t Phi, Theta, Psi;
        f32_t sinPsi, cosPsi;

        f32_t g = 9.81F;       
        f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */
        f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */
       
        Phi   = status.Phi;
        Theta = status.Theta;
        Psi   = status.Psi;
       
        sinPhi = sin(Phi);
        cosPhi = cos(Phi);
        sinTheta = sin(Theta);
        cosTheta = cos(Theta);
        sinPsi = sin(Psi);
        cosPsi = cos(Psi);
       
        //Pre_ax =  du - sinTheta * g;                 
        //matSetFull(matC, _ax, _Phi   , 0);
        matSetFull(matC, _ax, _Theta, -cosTheta * g);
        //matSetFull(matC, _ax, _Psi   , 0);
       
        //Pre_ay =  dv + cosTheta_sinPhi * g;    
        matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);
        matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);
        //matSetFull(matC, _ay, _Psi   , 0);
       
        //Pre_az =  dw + cosTheta_cosPhi * g;  
        matSetFull(matC, _az, _Phi   , -cosTheta * sinPhi * g);
        matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);
        //matSetFull(matC, _az, _Psi   , 0);
       
        //Pre_mx = (cos45 * (cosTheta * cosPsi) +  sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx   +  (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz;
        //matSetFull(matC, _mx, _Phi   ,  (sin45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi) )* Pre_Hx   +  ( sin45 * cosPhi * cosTheta) * Pre_Hz );
        //matSetFull(matC, _mx, _Theta,  (cos45 * (-sinTheta * cosPsi) +  sin45 * (sinPhi * cosTheta * cosPsi) )* Pre_Hx   +  (-cos45 * cosTheta - sin45 * sinPhi * sinTheta) * Pre_Hz);
        //matSetFull(matC, _mx, _Psi   ,  (-cos45 * (cosTheta * sinPsi) +  sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi) )* Pre_Hx );

        //Pre_my = (-sin45 * (cosTheta * cosPsi) +   cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx  +  (sin45 *   sinTheta + cos45 *   sinPhi * cosTheta) * Pre_Hz;
        //matSetFull(matC, _my, _Phi   ,  (cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi)) * Pre_Hx  +  ( cos45 *   cosPhi * cosTheta) * Pre_Hz );
        //matSetFull(matC, _my, _Theta, (sin45 * (sinTheta * cosPsi) +   cos45 * (sinPhi * cosTheta * cosPsi)) * Pre_Hx  +  (sin45 *   cosTheta - cos45 *   sinPhi * sinTheta) * Pre_Hz );
        //matSetFull(matC, _my, _Psi   ,  (sin45 * (cosTheta * sinPsi) +   cos45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi)) * Pre_Hx );

        //Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P)  * Pre_Hx  + cosPhi_P * cosTheta_P  * Pre_Hz;
        //matSetFull(matC, _mz, _Phi   ,  (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi)  * Pre_Hx  - sinPhi * cosTheta  * Pre_Hz);
        //matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi)  * Pre_Hx  - cosPhi * sinTheta  * Pre_Hz );
        //matSetFull(matC, _mz, _Psi   ,  (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi)  * Pre_Hx );

        /* Runtime Optimised Version */
        f32_t cos45_sinTheta_cosPsi  = cos45 * (sinTheta * cosPsi) ;
        f32_t sin45_sinPhi_sinTheta = sin45 * sinPhi * sinTheta;
        f32_t sin45_cosPhi_cosTheta = sin45 * cosPhi * cosTheta;
        f32_t cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi = cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi);
        f32_t cos45_cosTheta_sinPsi  = cos45 * (cosTheta * sinPsi) ;
        f32_t sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi = sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi);
        f32_t sin45_sinPhi_cosTheta_cosPsi = sin45 * (sinPhi * cosTheta * cosPsi);
        f32_t tmp1= (cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi)* Pre_Hx   +  (sin45_cosPhi_cosTheta) * Pre_Hz;

        //Pre_mx = (cos45 * (cosTheta * cosPsi) +  sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx   +  (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz;
        matSetFull(matC, _mx, _Phi   ,  tmp1);
        matSetFull(matC, _mx, _Theta,  (-cos45_sinTheta_cosPsi +  sin45_sinPhi_cosTheta_cosPsi)* Pre_Hx   +  (-cos45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz);
        matSetFull(matC, _mx, _Psi   ,  (-cos45_cosTheta_sinPsi+  sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi)* Pre_Hx );

        //Pre_my = (-sin45 * (cosTheta * cosPsi) +   cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx  +  (sin45 *   sinTheta + cos45 *   sinPhi * cosTheta) * Pre_Hz;
        matSetFull(matC, _my, _Phi   ,  tmp1);
        matSetFull(matC, _my, _Theta, (cos45_sinTheta_cosPsi +   sin45_sinPhi_cosTheta_cosPsi) * Pre_Hx  +  (sin45 *   cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz );
        matSetFull(matC, _my, _Psi   ,  (cos45_cosTheta_sinPsi +   sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi) * Pre_Hx );

        //Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P)  * Pre_Hx  + cosPhi_P * cosTheta_P  * Pre_Hz;
        matSetFull(matC, _mz, _Phi   ,  (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi)  * Pre_Hx  - sinPhi * cosTheta  * Pre_Hz);
        matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi)  * Pre_Hx  - cosPhi * sinTheta  * Pre_Hz );
        matSetFull(matC, _mz, _Psi   ,  (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi)  * Pre_Hx );
}
#else
/* ****************************************************************************
Functionname:     trUpdateJacobiMatrix                      */ /*!
Description:

 
        @param[in]        void
       
          @return           void
          @pre              -
          @post             -
          @author           Michael Walter
*****************************************************************************/
void trUpdateJacobiMatrix(void)
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
        /*
        Simplified Version                     
        Pre_ax =  du - sinTheta * g;                
        Pre_ay =  dv + cosTheta_sinPhi * g;    
        Pre_az =  dw + cosTheta_cosPhi * g;      
        */
       
        /*--- VARIABLES ---*/
        f32_t Phi, Theta, Psi;
        f32_t g = 9.81F;
       
        Phi   = status.Phi;
        Theta = status.Theta;
        Psi   = status.Psi;
       
        sinPhi = sin(Phi);
        cosPhi = cos(Phi);
        sinTheta = sin(Theta);
        cosTheta = cos(Theta);
       
        //Pre_ax =  du - sinTheta * g;                 
        //matSetFull(matC, _ax, _Phi   , 0);
        matSetFull(matC, _ax, _Theta, -cosTheta * g);
        //matSetFull(matC, _ax, _Psi   , 0);
       
        //Pre_ay =  dv + cosTheta_sinPhi * g;    
        matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);
        matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);
        //matSetFull(matC, _ay, _Psi   , 0);
       
        //Pre_az =  dw + cosTheta_cosPhi * g;  
        matSetFull(matC, _az, _Phi   , -cosTheta * sinPhi * g);
        matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);
        //matSetFull(matC, _az, _Psi   , 0);
       
        matSetFull(matC, _compass, _Psi , 1.0F);
}
#endif


#ifdef USE_Extended_MM3_Measurement_Model
/*****************************************************************************
Functionname:     trPredictMeasurement                      */ /*!
Description:                    Predict Measurements: AX, AY, AZ, HX, HY, HZ

 
        @param[in]        void
       
          @return           void
          @pre              -
          @post             -
          @author           Michael Walter
*****************************************************************************/
void trPredictMeasurement(void)
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*
    Simplified Version  
        Pre_ax =  du - sinTheta * g;    
    Pre_ay =  dv + cosTheta_sinPhi * g;    
    Pre_az =  dw + cosTheta_cosPhi * g;  
        */
       
        /*--- VARIABLES ---*/
    f32_t g = 9.81F;
        f32_t Pre_ax, Pre_ay,  Pre_az;    
        f32_t Phi, Theta, Psi;

        f32_t Pre_mx;
        f32_t Pre_my;
        f32_t Pre_mz;

        f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */
        f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */

        f32_t cosTheta_cosPsi, sinPhi_cosTheta, cosPhi_sinPsi, sinPhi_sinTheta_cosPsi, cos45_cosTheta_cosPsi;
        f32_t cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi,  cos45_sinPhi_cosTheta, cos45_sinTheta;
       
        matGetFull(matXs, _Phi, 0, &Phi);
    matGetFull(matXs, _Theta, 0, &Theta);
    matGetFull(matXs, _Psi, 0, &Psi);
               
        sinPhi_P = sin(Phi);
    cosPhi_P = cos(Phi);
    sinTheta_P = sin(Theta);
    cosTheta_P = cos(Theta);
        sinPsi_P = sin(Psi);
        cosPsi_P = cos(Psi);

        cosTheta_cosPsi = cosTheta_P * cosPsi_P;
        sinPhi_cosTheta = sinPhi_P * cosTheta_P;
        cosPhi_sinPsi = cosPhi_P * sinPsi_P;
        sinPhi_sinTheta_cosPsi = sinPhi_P * sinTheta_P * cosPsi_P;

        cos45_cosTheta_cosPsi = cos45 * (cosTheta_cosPsi);
        cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi = cos45 * (-cosPhi_sinPsi + sinPhi_sinTheta_cosPsi);
        cos45_sinPhi_cosTheta = cos45 * sinPhi_cosTheta;
        cos45_sinTheta = cos45 * sinTheta_P;

    /* Simplified Version */   
        Pre_ax =  -sinTheta_P * g;    
        Pre_ay =  cosTheta_P * sinPhi_P * g;    
        Pre_az =  cosTheta_P * cosPhi_P * g;  
       
        matSetFull(matYs, _ax, 0, Pre_ax);
        matSetFull(matYs, _ay, 0, Pre_ay);
        matSetFull(matYs, _az, 0, Pre_az);
       
        //Pre_mx = (cos45 * (cosTheta_P * cosPsi_P) +  sin45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P) )* Pre_Hx   +  (-cos45 * sinTheta_P + sin45 * sinPhi_P * cosTheta_P) * Pre_Hz;
        //Pre_my = (-sin45 * (cosTheta_P * cosPsi_P) +   cos45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P)) * Pre_Hx  +  (sin45 *  sinTheta_P + cos45 * sinPhi_P * cosTheta_P) * Pre_Hz;
        //Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P)  * Pre_Hx  + cosPhi_P * cosTheta_P  * Pre_Hz;

        Pre_mx = (cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi)* Pre_Hx   +  (-cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz;
        Pre_my = (-cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi) * Pre_Hx  +  (cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz;
        Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P)  * Pre_Hx  + cosPhi_P * cosTheta_P  * Pre_Hz;        

        //DebugOut.Analog[3] =  Pre_mx * 100;
        //DebugOut.Analog[4]  = Pre_my * 100;
        //DebugOut.Analog[5]  = Pre_mz * 100;
       
        matSetFull(matYs, _mx, 0, Pre_mx);
        matSetFull(matYs, _my, 0, Pre_my);
        matSetFull(matYs, _mz, 0, Pre_mz);
}

#else
/*****************************************************************************
Functionname:     trPredictMeasurement                      */ /*!
Description:                    Predict Measurements: AX, AY, AZ, Compass

 
        @param[in]        void
       
          @return           void
          @pre              -
          @post             -
          @author           Michael Walter
*****************************************************************************/
void trPredictMeasurement(void)
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*
    Simplified Version  
        Pre_ax =  du - sinTheta * g;    
    Pre_ay =  dv + cosTheta_sinPhi * g;    
    Pre_az =  dw + cosTheta_cosPhi * g;  
        */
       
        /*--- VARIABLES ---*/
    f32_t g = 9.81F;
        f32_t Pre_ax, Pre_ay,  Pre_az;    
        f32_t Phi, Theta, Psi;
       
        matGetFull(matXs, _Phi, 0, &Phi);
    matGetFull(matXs, _Theta, 0, &Theta);
    matGetFull(matXs, _Psi, 0, &Psi);
       
        sinPhi_P = sin(Phi);
    cosPhi_P = cos(Phi);
    sinTheta_P = sin(Theta);
    cosTheta_P = cos(Theta);
       
    /* Simplified Version */   
        Pre_ax =  -sinTheta_P * g;    
        Pre_ay =  cosTheta_P * sinPhi_P * g;    
        Pre_az =  cosTheta_P * cosPhi_P * g;  
       
        matSetFull(matYs, _ax, 0, Pre_ax);
        matSetFull(matYs, _ay, 0, Pre_ay);
        matSetFull(matYs, _az, 0, Pre_az);
        matSetFull(matYs, _compass, 0, Psi);
}
#endif 

#ifdef USE_Extended_MM3_Measurement_Model
/* ****************************************************************************
Functionname:     trMeasure                      */ /*!
Description:      

  @param[in]        
 
        @return           void
        @pre              -
        @post             -
        @author           Michael Walter
**************************************************************************** */
void trMeasure()
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*--- VARIABLES ---*/

        f32_t ADC_ax = 0.0F;
        f32_t ADC_ay = 0.0F;
        f32_t ADC_az = 0.0F;
       
        fYValid[_ax] = 1;
        fYValid[_ay] = 1;
        fYValid[_az] = 1;

        ADC_ax = AdWertAccNick * 0.047F;
        ADC_ay = -AdWertAccRoll * 0.047F;      
        ADC_az = AdWertAccHoch * 0.047F;       

        matSetFull(matY, _ax, 0, ADC_ax);
        matSetFull(matY, _ay, 0, ADC_ay);
        matSetFull(matY, _az, 0, ADC_az);
       
        //if (MM3_Ready == 1)
        if (1)
        {
                MM3_Heading();
                matSetFull(matY, _mx, 0, MM3_Hx);
                matSetFull(matY, _my, 0, MM3_Hy);
                matSetFull(matY, _mz, 0, MM3_Hz);

                fYValid[_mx] = 1;
                fYValid[_my] = 1;
                fYValid[_mz] = 1;
                //MM3_Ready = 0;
       
                DebugOut.Analog[13] =  MM3_Hx * 100;
                DebugOut.Analog[14]  = MM3_Hy * 100;
                DebugOut.Analog[15]  = MM3_Hz * 100;
        }
        else
        {
                fYValid[_mx] = 0;
                fYValid[_my] = 0;
                fYValid[_mz] = 0;
        }
}
#else
/* ****************************************************************************
Functionname:     trMeasure                      */ /*!
Description:      

  @param[in]        
 
        @return           void
        @pre              -
        @post             -
        @author           Michael Walter
**************************************************************************** */
void trMeasure()
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*--- VARIABLES ---*/

        f32_t ADC_ax = 0.0F;
        f32_t ADC_ay = 0.0F;
        f32_t ADC_az = 0.0F;
       
        fYValid[_ax] = 1;
        fYValid[_ay] = 1;
        fYValid[_az] = 1;

        ADC_ax = AdWertAccNick * 0.047F;
        ADC_ay = -AdWertAccRoll * 0.047F;      
        ADC_az = AdWertAccHoch * 0.047F;       

        matSetFull(matY, _ax, 0, ADC_ax);
        matSetFull(matY, _ay, 0, ADC_ay);
        matSetFull(matY, _az, 0, ADC_az);
       
        static uint8_t updCompass = 0;
        f32_t Psi, Compass;
               
        if (!updCompass--)
        {
                updCompass = 10;
                KompassValue = MM3_Heading();
        }
       
        Compass = KompassValue / 180.F * PI;
       
    matGetFull(matXs, _Psi, 0, &Psi);

        if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi))
        {
                Compass += 2 * PI;
        }
        else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi))
        {
                Compass -= 2 * PI;
        }
       
        matSetFull(matY, _compass, 0, Compass);

    /* Roll and Yaw angle are smaller 8 Deg*/
        if ((abs(status.iPhi10)  < 80 ) && (abs(status.iTheta10) < 80))
        {
                fYValid[_compass] = 1;
        }
        else
        {
                fYValid[_compass] = 0;
        }
}
#endif

/* ****************************************************************************
Functionname:     trUpdateBU                      */ /*!
Description:         Update control matrix B and vector U

 
        @param[in]        
       
          @return           void
          @pre              -
          @post             -
          @author          Michael Walter
**************************************************************************** */
void trUpdateBU()
{
        /*--- (SYMBOLIC) CONSTANTS ---*/
       
        /*--- VARIABLES ---*/
        /*
        dPhi      | 1      sinPhi_tanTheta    cosPhi_tanTheta  |   p  
        dTheta =  | 0      cosPhi                     -sinPhi  | * q
        dPsi      | 0      sinPhi_secTheta    cosPhi_secTheta  |   w
        */
         
        f32_t ADC_p = 0.0F;
        f32_t ADC_q = 0.0F;
        f32_t ADC_r = 0.0F;
       
        /* store predicted state vector in current state */
        matGetFull(matXd, _Phi   , 0, &Phi);
        matGetFull(matXd, _Theta , 0, &Theta);
       
        sinPhi = sin(Phi);
        cosPhi = cos(Phi);
         
        tanTheta = tan(Theta);
        secTheta = 1.0F / cos(Theta);
       
        matSetFull(matB, _Phi, _p, 1.0F);
        matSetFull(matB, _Phi, _q, sinPhi * tanTheta );
        matSetFull(matB, _Phi, _r, cosPhi * tanTheta );
       
        //matSetFull(matB, _Theta, _p, 0.0F);
        matSetFull(matB, _Theta, _q, cosPhi );
        matSetFull(matB, _Theta, _r, -sinPhi );
       
        //matSetFull(matB, _Psi, _p, 0.0F);
        matSetFull(matB, _Psi, _q, sinPhi * secTheta );
        matSetFull(matB, _Psi, _r, cosPhi * secTheta );  

        ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime;
        ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime;
        ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime;
               
        matSetFull(matU, _p, 0, ADC_p );
        matSetFull(matU, _q, 0, ADC_q);
        matSetFull(matU, _r, 0, ADC_r);
}


void AttitudeEstimation()
{
        trPredictMeasurement();
   
        trUpdateJacobiMatrix();
       
        /*  Extract measurements and store them in vector matY.
        Measurement validity is indicated by fYValid[] */
        trMeasure();
       
        /* Innovation: calculate Xd, and Pd  */
        trInnovate();
       
        /* Update transition matrix Phi  */
        trUpdatePhi();
       
        /* Update control matrix B */
        trUpdateBU();
       
        /* Predict new state Xs and Ps */
        KAFIPrediction(p_kafi);
       
        /* store innovated state vector in current state */
        matGetFull(matXs, _Phi   , 0, &status.Phi);
        matGetFull(matXs, _Theta , 0, &status.Theta);
        matGetFull(matXs, _Psi   , 0, &status.Psi);
               
        trLimitAngles();
}



/* ****************************************************************************
  Functionname:     trEstimateVelocity                      */ /*!
  Description:      

  @param[in]        

  @return           void
  @pre              -
  @post             -
  @author          
**************************************************************************** */
void trEstimateVelocity()
{
   /*--- (SYMBOLIC) CONSTANTS ---*/

   /*--- VARIABLES ---*/
 
}


/* ****************************************************************************
  Functionname:     trLimitAngles                      */ /*!
  Description:      

  @param[in]        

  @return           void
  @pre              -
  @post             -
  @author          
**************************************************************************** */
void trLimitAngles()
{
   /*--- (SYMBOLIC) CONSTANTS ---*/

   /*--- VARIABLES ---*/
   f32_t Psi;

   if (status.Phi > 2.0F * PI)
   {
      status.Phi -= 2.0F * PI;
      matSetFull(matXs,  _Phi, 0, status.Phi);
   }
   if (status.Phi < -2.0F * PI)
   {
      status.Phi += 2.0F * PI;
      matSetFull(matXs,  _Phi,  0, status.Phi);          
   }
   if (status.Theta > 2.0F * PI)
   {
      status.Theta -= 2.0F * PI;
      matSetFull(matXs, _Theta, 0, status.Theta);
   }
   if (status.Theta < -2.0F * PI)
   {
      status.Theta += 2.0F * PI;
      matSetFull(matXs, _Theta, 0, status.Theta);
   }
   if (status.Psi > 2.0F * PI)
   {
      status.Psi -= 2.0F * PI;
          sollGier -= 3600;
 
      matGetFull(matXs, _Psi, 0, &Psi);
      matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI);
      matGetFull(matXd, _Psi, 0, &Psi);
      matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI);
  }
   if (status.Psi < 0.0F * PI)
   {
      status.Psi += 2.0F * PI;
          sollGier += 3600;
         
      matGetFull(matXs, _Psi, 0, &Psi);
      matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI);
      matGetFull(matXd, _Psi, 0, &Psi);
      matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI);
   }
         
        status.iPhi10 = (int) (status.Phi * 573.0F);
        status.iTheta10 = (int) (status.Theta * 573.0F);
        status.iPsi10 = (int) (status.Psi * 573.0F);
       
        if ((sollGier - status.iPsi10) > 3600)
        {
                sollGier -= 3600;
        }

        if ((sollGier - status.iPsi10) < -3600)
        {
                sollGier += 3600;
        }      
}