WebSVN - FlightCtrl - Diff - Rev 973 and 983 - /branches/KalmanFilter MikeW/FlightControl.c


/*
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.
*/
 
 
/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "kafi.h"
#include "FlightControl.h"
#include "main.h"
#include "mm3.h"
#include "main.h"
#include "eeprom.c"
 
/*****************************************************************************
(SYMBOLIC) CONSTANTS
*****************************************************************************/
#define MAX_GAS 250
#define MIN_GAS    15
 
#define sin45 sin(45.F / 180.F * PI) 
#define cos45 cos(45.F / 180.F * PI)
 
/*****************************************************************************
  VARIABLES
*****************************************************************************/
 
extern void GPSupdate(void);
 
f32_t AdNeutralNick = 0.0F, AdNeutralRoll = 0.0F, AdNeutralGier = 0.0F;
int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;
int AverageRoll = 0, AverageNick = 0, AverageGier = 0;
int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 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;
 
int DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, InitialAltitude = 0;
int SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;
int GierMischanteil, GasMischanteil;
 
unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
unsigned char MotorWert[5];
unsigned char SenderOkay = 0;
unsigned int I2CTimeout = 100;
char MotorenEin = 0;
unsigned int  modell_fliegt = 0;
 
extern unsigned long maxDistance;
extern signed int GPS_Nick,  GPS_Roll;
extern int RemoteLinkLost;
 
struct mk_param_struct EE_Parameter;
 
/* ****************************************************************************
Functionname:     SetNeutral                      */ /*!
Description:    
 
@param[in]        
 
  @return           void
  @pre              -
  @post             -
  @author         Michael Walter  
**************************************************************************** */
void SetNeutral(void)
{
  beeptime = 2000;
  Delay_ms(1000);
  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 = (f32_t) AdWertNick_Raw;
  AdNeutralRoll = (f32_t) AdWertRoll_Raw;
  AdNeutralGier = (f32_t) 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 = 0;
  AccumulatedRoll_cnt = 0;
  AccumulatedNick = 0;
  AccumulatedNick_cnt = 0;
  AccumulatedGier = 0;
  AccumulatedGier_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 = 2000;
}
 
 
/* ****************************************************************************
Functionname:    GetMeasurements                      */ /*!
Description:    CalculateAverage
 
@param[in]        
 
  @return           void
  @pre              -
  @post             -
  @author         Michael Walter  
**************************************************************************** */
void GetMeasurements(void)
{
  ANALOG_OFF;
  AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt;
  AverageNick = AccumulatedNick / AccumulatedNick_cnt;
  AverageGier = AccumulatedGier / AccumulatedGier_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(modell_fliegt < 0x250)
  {
    //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10))
    AdNeutralNick = 0.998F * AdNeutralNick + 0.002F * AdWertNick_Raw;
    AdNeutralRoll = 0.998F * AdNeutralRoll + 0.002F * AdWertRoll_Raw;
    if (abs(StickGier) < 15 || MotorenEin == 0)
    {
      AdNeutralGier = 0.998F * AdNeutralGier + 0.002F * AdWertGier_Raw;
    }
  }
  else if(modell_fliegt < 0x2000)
  {
    //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10))
    AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw;
    AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw;
    if (abs(StickGier) < 15 || MotorenEin == 0)
    {
      AdNeutralGier = 0.999F * AdNeutralGier + 0.001F * AdWertGier_Raw;
    }
  }
  else
  {
    AdNeutralNick = 0.9995F * AdNeutralNick + 0.0005F * AdWertNick_Raw;
    AdNeutralRoll = 0.9995F * AdNeutralRoll + 0.0005F * AdWertRoll_Raw;
    if (abs(StickGier) < 15 || MotorenEin == 0)
    {
      AdNeutralGier = 0.9995F * AdNeutralGier + 0.0005F * AdWertGier_Raw;
    }
  }
 
#if 1
  DebugOut.Analog[6] = AdWertNick_Raw;
  DebugOut.Analog[7] = AdWertRoll_Raw;
  DebugOut.Analog[8] = AdWertGier_Raw;
 
  DebugOut.Analog[9] = AdNeutralNick;
  DebugOut.Analog[10] = AdNeutralRoll;
  DebugOut.Analog[11] = AdNeutralGier;
#endif
 
  AccumulatedRoll = 0;
  AccumulatedRoll_cnt = 0;
  AccumulatedNick = 0;
  AccumulatedNick_cnt = 0;
  AccumulatedGier = 0;
  AccumulatedGier_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;
}
 
/* ****************************************************************************
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:     RemoteControl                      */ /*!
Description:      
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void RemoteControl(void)
{
  static unsigned char delay_neutral = 0;
  static unsigned char delay_einschalten = 0,delay_ausschalten = 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;
      }
    }
  }
  else
  {
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Emfang gut
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay > 140)
    {
      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:     PD_Regler                      */ /*!
Description:
 
  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
void PD_Regler(void)
{
  int StickNick45,StickRoll45;
  int DiffNick,DiffRoll, DiffGier;    
  int motorwert = 0;
  int pd_ergebnis;
 
  /*****************************************************************************
  Update noimial attitude
  **************************************************************************** */
  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
 
#ifdef USE_GPS
 /* G P S   U p d a t e */
  GPSupdate();
#endif
 
  static float AverageGasMischanteil = 50.F;
  if((GasMischanteil > 30) &&
    (MotorenEin == 1) &&
    (RemoteLinkLost == 0) )
  {  /* Average the average throttle to find the hover setting */
    AverageGasMischanteil = 0.999F * AverageGasMischanteil + 0.001F * GasMischanteil;
  }
  /* Overide GasMischanteil */
  static unsigned int DescentCnt = 32000;
  if ((RemoteLinkLost == 1) &&
    (MotorenEin == 1))
  {
    if ((UBat < 100) || /* Start to descent in case of low loltage or in case*/
    (maxDistance / 10 < 12)) /* we reached our target position */
    {
      if (DescentCnt > 0)
      {
        DescentCnt--;
      }
      else
      {  /* We reached our target (hopefully) */
        MotorenEin = 0;
        RemoteLinkLost = 0;
      }
    }
    else
    {
      DescentCnt = 32000;
    }
    /* Bias the throttle for a slow descent */
    GasMischanteil = (int) ((AverageGasMischanteil + 5.0F) * (DescentCnt / 32000.F));
  }
  else
  {
    DescentCnt = 32000;
  }
 
  //DebugOut.Analog[13] = (int) GasMischanteil;
 
#ifdef X_FORMATION
  /* Overide in case the remote link got lost */
  if (RemoteLinkLost == 0)
  { /* We are flying in X-Formation */
    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));
  }
  else
  {  /* GPS overide is aktive */
    StickRoll45 = (int) (0.707F *  (float)(-GPS_Roll) - 0.707F * (float)(-GPS_Nick));
    StickNick45 = (int) (0.707F *  (float)(-GPS_Roll) + 0.707F * (float)(-GPS_Nick));
    StickGier = 0;
  }
#else
  /* Overide in case the remote link got lost */
  if (RemoteLinkLost == 0)
  { /* We are flying in X-Formation */
    StickRoll45 = StickRoll - GPS_Roll;
    StickNick45 = StickNick - GPS_Nick;
  }
  else
  {  /* GPS overide is aktive */
    StickRoll45 = -GPS_Roll;
    StickNick45 = -GPS_Nick;
    StickGier = 0;
  }
#endif
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //  Yaw
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
  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_Raw - (int) AdNeutralGier)) / 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;
  }
 
  /*****************************************************************************
  PD-Control
  **************************************************************************** */
  int scale_p;
  int scale_d;
 
  scale_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
  scale_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 20);  
 
  scale_p = 6;
  scale_d = 7;
 
  //DebugOut.Analog[14] =  scale_p;
  //DebugOut.Analog[15] =  scale_d;
 
  /* Pitch */
  DiffNick = -(status.iTheta10 + StickNick45);
  /* R o l l */
  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_Raw - (int) AdNeutralNick)) / 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_Raw - (int) AdNeutralRoll)) / 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;
#endif
}
 

Rev 973	Rev 983
1	/*	1	/*
2	Copyright 2008, by Michael Walter	2	Copyright 2008, by Michael Walter
3		3
4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser	4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but	5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public	7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
8	License along with this program. If not, see <http://www.gnu.org/licenses/>.	8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
9		9
10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that	10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de	11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
12	unless it is stated otherwise.	12	unless it is stated otherwise.
13	*/	13	*/
14		14
15		15
16	/*****************************************************************************	16	/*****************************************************************************
17	INCLUDES	17	INCLUDES
18	**************************************************************************** */	18	**************************************************************************** */
19	#include "kafi.h"	19	#include "kafi.h"
20	#include "FlightControl.h"	20	#include "FlightControl.h"
21	#include "main.h"	21	#include "main.h"
22	#include "mm3.h"	22	#include "mm3.h"
23	#include "main.h"	23	#include "main.h"
24	#include "eeprom.c"	24	#include "eeprom.c"
25		25
26	/*****************************************************************************	26	/*****************************************************************************
27	(SYMBOLIC) CONSTANTS	27	(SYMBOLIC) CONSTANTS
28	*****************************************************************************/	28	*****************************************************************************/
29	#define MAX_GAS 250	29	#define MAX_GAS 250
30	#define MIN_GAS 15	30	#define MIN_GAS 15
31		31
32	#define sin45 sin(45.F / 180.F * PI)	32	#define sin45 sin(45.F / 180.F * PI)
33	#define cos45 cos(45.F / 180.F * PI)	33	#define cos45 cos(45.F / 180.F * PI)
34		34
35	/*****************************************************************************	35	/*****************************************************************************
36	VARIABLES	36	VARIABLES
37	*****************************************************************************/	37	*****************************************************************************/
38		38
39	extern void GPSupdate(void);	39	extern void GPSupdate(void);
40		40
41	f32_t AdNeutralNick = 0.0F, AdNeutralRoll = 0.0F, AdNeutralGier = 0.0F;	41	f32_t AdNeutralNick = 0.0F, AdNeutralRoll = 0.0F, AdNeutralGier = 0.0F;
42	int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;	42	int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;
43	int AverageRoll = 0, AverageNick = 0, AverageGier = 0;	43	int AverageRoll = 0, AverageNick = 0, AverageGier = 0;
44	int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;	44	int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
45		45
46	f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F;	46	f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F;
47	f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F, ACC_Z_Offset = 0.F;	47	f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F, ACC_Z_Offset = 0.F;
48		48
49	int DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, InitialAltitude = 0;	49	int DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, InitialAltitude = 0;
50	int SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;	50	int SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;
51	int GierMischanteil, GasMischanteil;	51	int GierMischanteil, GasMischanteil;
52		52
53	unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;	53	unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
54	unsigned char MotorWert[5];	54	unsigned char MotorWert[5];
55	unsigned char SenderOkay = 0;	55	unsigned char SenderOkay = 0;
56	unsigned int I2CTimeout = 100;	56	unsigned int I2CTimeout = 100;
57	char MotorenEin = 0;	57	char MotorenEin = 0;
-		58	unsigned int modell_fliegt = 0;
58		59
59	extern unsigned long maxDistance;	60	extern unsigned long maxDistance;
60	extern signed int GPS_Nick, GPS_Roll;	61	extern signed int GPS_Nick, GPS_Roll;
61	extern int RemoteLinkLost;	62	extern int RemoteLinkLost;
62		63
63	struct mk_param_struct EE_Parameter;	64	struct mk_param_struct EE_Parameter;
64		65
65	/* ****************************************************************************	66	/* ****************************************************************************
66	Functionname: SetNeutral / /!	67	Functionname: SetNeutral / /!
67	Description:	68	Description:
68		69
69	@param[in]	70	@param[in]
70		71
71	@return void	72	@return void
72	@pre -	73	@pre -
73	@post -	74	@post -
74	@author Michael Walter	75	@author Michael Walter
75	**************************************************************************** */	76	**************************************************************************** */
76	void SetNeutral(void)	77	void SetNeutral(void)
77	{	78	{
-		79	beeptime = 2000;
78	Delay_ms(1000);	80	Delay_ms(1000);
79	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	81	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
80	{	82	{
81	if((AdWertAirPressure_Raw > 950) \|\| (AdWertAirPressure_Raw < 750))	83	if((AdWertAirPressure_Raw > 950) \|\| (AdWertAirPressure_Raw < 750))
82	{	84	{
83	SucheLuftruckOffset();	85	SucheLuftruckOffset();
84	}	86	}
85	}	87	}
86		88
87	LastAltitude = CurrentAltitude;	89	LastAltitude = CurrentAltitude;
88	Delay_ms_Mess(300);	90	Delay_ms_Mess(300);
89		91
90	ANALOG_OFF;	92	ANALOG_OFF;
91	AdNeutralNick = (f32_t) AdWertNick_Raw;	93	AdNeutralNick = (f32_t) AdWertNick_Raw;
92	AdNeutralRoll = (f32_t) AdWertRoll_Raw;	94	AdNeutralRoll = (f32_t) AdWertRoll_Raw;
93	AdNeutralGier = (f32_t) AdWertGier_Raw;	95	AdNeutralGier = (f32_t) AdWertGier_Raw;
94	NeutralAccY = AdWertAccRoll_Raw;	96	NeutralAccY = AdWertAccRoll_Raw;
95	NeutralAccX = AdWertAccNick_Raw;	97	NeutralAccX = AdWertAccNick_Raw;
96	NeutralAccZ = AdWertAccHoch_Raw;	98	NeutralAccZ = AdWertAccHoch_Raw;
97		99
98	Roll_X_Offset = 0.F;	100	Roll_X_Offset = 0.F;
99	Roll_Y_Offset = 0.F;	101	Roll_Y_Offset = 0.F;
100	Roll_Z_Offset = 0.F;	102	Roll_Z_Offset = 0.F;
101	ACC_X_Offset = 0.F;	103	ACC_X_Offset = 0.F;
102	ACC_Y_Offset = 0.F;	104	ACC_Y_Offset = 0.F;
103	ACC_Z_Offset = 0.F;	105	ACC_Z_Offset = 0.F;
104		106
105	AccumulatedACC_X = 0;	107	AccumulatedACC_X = 0;
106	AccumulatedACC_X_cnt = 0;	108	AccumulatedACC_X_cnt = 0;
107	AccumulatedACC_Y = 0;	109	AccumulatedACC_Y = 0;
108	AccumulatedACC_Y_cnt = 0;	110	AccumulatedACC_Y_cnt = 0;
109	AccumulatedACC_Z = 0;	111	AccumulatedACC_Z = 0;
110	AccumulatedACC_Z_cnt = 0;	112	AccumulatedACC_Z_cnt = 0;
111		113
112	AccumulatedRoll = 0;	114	AccumulatedRoll = 0;
113	AccumulatedRoll_cnt = 0;	115	AccumulatedRoll_cnt = 0;
114	AccumulatedNick = 0;	116	AccumulatedNick = 0;
115	AccumulatedNick_cnt = 0;	117	AccumulatedNick_cnt = 0;
116	AccumulatedGier = 0;	118	AccumulatedGier = 0;
117	AccumulatedGier_cnt = 0;	119	AccumulatedGier_cnt = 0;
118		120
119	AveragerACC_X = 0;	121	AveragerACC_X = 0;
120	AveragerACC_Y = 0;	122	AveragerACC_Y = 0;
121	AveragerACC_Z = 0;	123	AveragerACC_Z = 0;
122		124
123	AccumulatedACC_X = 0;	125	AccumulatedACC_X = 0;
124	AccumulatedACC_X_cnt = 0;	126	AccumulatedACC_X_cnt = 0;
125	AccumulatedACC_Y = 0;	127	AccumulatedACC_Y = 0;
126	AccumulatedACC_Y_cnt = 0;	128	AccumulatedACC_Y_cnt = 0;
127	AccumulatedACC_Z = 0;	129	AccumulatedACC_Z = 0;
128	AccumulatedACC_Z_cnt = 0;	130	AccumulatedACC_Z_cnt = 0;
129	ANALOG_ON;	131	ANALOG_ON;
130		132
131	SummeRoll = 0;	133	SummeRoll = 0;
132	SummeNick = 0;	134	SummeNick = 0;
133		135
134	sollGier = status.iPsi10;	136	sollGier = status.iPsi10;
135	InitialAltitude = AdWertAirPressure_Raw;	137	InitialAltitude = AdWertAirPressure_Raw;
136	beeptime = 2000;	138	beeptime = 2000;
137	}	139	}
138		140
139		141
140	/* ****************************************************************************	142	/* ****************************************************************************
141	Functionname: GetMeasurements / /!	143	Functionname: GetMeasurements / /!
142	Description: CalculateAverage	144	Description: CalculateAverage
143		145
144	@param[in]	146	@param[in]
145		147
146	@return void	148	@return void
147	@pre -	149	@pre -
148	@post -	150	@post -
149	@author Michael Walter	151	@author Michael Walter
150	**************************************************************************** */	152	**************************************************************************** */
151	void GetMeasurements(void)	153	void GetMeasurements(void)
152	{	154	{
153	ANALOG_OFF;	155	ANALOG_OFF;
154	AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt;	156	AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt;
155	AverageNick = AccumulatedNick / AccumulatedNick_cnt;	157	AverageNick = AccumulatedNick / AccumulatedNick_cnt;
156	AverageGier = AccumulatedGier / AccumulatedGier_cnt;	158	AverageGier = AccumulatedGier / AccumulatedGier_cnt;
157		159
158	/* Get Pressure Differenz */	160	/* Get Pressure Differenz */
159	CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;	161	CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;
160	AccumulatedAirPressure_cnt = 0;	162	AccumulatedAirPressure_cnt = 0;
161	AccumulatedAirPressure = 0;	163	AccumulatedAirPressure = 0;
162		164
163	static char AirPressureCnt = 0;	165	static char AirPressureCnt = 0;
164	if (AirPressureCnt % 25 == 1)	166	if (AirPressureCnt % 25 == 1)
165	{	167	{
166	DeltaAltitude = CurrentAltitude - LastAltitude;	168	DeltaAltitude = CurrentAltitude - LastAltitude;
167	LastAltitude = CurrentAltitude;	169	LastAltitude = CurrentAltitude;
168	}	170	}
169	AirPressureCnt++;	171	AirPressureCnt++;
170		172
-		173	if(modell_fliegt < 0x250)
-		174	{
171	//if ((GPS_Roll == 0 && GPS_Nick == 0) \|\| (maxDistance / 10 > 10))	175	//if ((GPS_Roll == 0 && GPS_Nick == 0) \|\| (maxDistance / 10 > 10))
-		176	AdNeutralNick = 0.998F * AdNeutralNick + 0.002F * AdWertNick_Raw;
-		177	AdNeutralRoll = 0.998F * AdNeutralRoll + 0.002F * AdWertRoll_Raw;
-		178	if (abs(StickGier) < 15 \|\| MotorenEin == 0)
-		179	{
-		180	AdNeutralGier = 0.998F * AdNeutralGier + 0.002F * AdWertGier_Raw;
-		181	}
-		182	}
-		183	else if(modell_fliegt < 0x2000)
172	{	184	{
-		185	//if ((GPS_Roll == 0 && GPS_Nick == 0) \|\| (maxDistance / 10 > 10))
173	AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw;	186	AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw;
174	AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw;	187	AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw;
175	if (abs(StickGier) < 15 \|\| MotorenEin == 0)	188	if (abs(StickGier) < 15 \|\| MotorenEin == 0)
176	{	189	{
177	AdNeutralGier = 0.999F * AdNeutralGier + 0.001F * AdWertGier_Raw;	190	AdNeutralGier = 0.999F * AdNeutralGier + 0.001F * AdWertGier_Raw;
-		191	}
-		192	}
-		193	else
-		194	{
-		195	AdNeutralNick = 0.9995F * AdNeutralNick + 0.0005F * AdWertNick_Raw;
-		196	AdNeutralRoll = 0.9995F * AdNeutralRoll + 0.0005F * AdWertRoll_Raw;
-		197	if (abs(StickGier) < 15 \|\| MotorenEin == 0)
-		198	{
-		199	AdNeutralGier = 0.9995F * AdNeutralGier + 0.0005F * AdWertGier_Raw;
178	}	200	}
179	}	201	}
180		202
181	#if 1	203	#if 1
182	DebugOut.Analog[6] = AdWertNick_Raw;	204	DebugOut.Analog[6] = AdWertNick_Raw;
183	DebugOut.Analog[7] = AdWertRoll_Raw;	205	DebugOut.Analog[7] = AdWertRoll_Raw;
184	DebugOut.Analog[8] = AdWertGier_Raw;	206	DebugOut.Analog[8] = AdWertGier_Raw;
185		207
186	DebugOut.Analog[9] = AdNeutralNick;	208	DebugOut.Analog[9] = AdNeutralNick;
187	DebugOut.Analog[10] = AdNeutralRoll;	209	DebugOut.Analog[10] = AdNeutralRoll;
188	DebugOut.Analog[11] = AdNeutralGier;	210	DebugOut.Analog[11] = AdNeutralGier;
189	#endif	211	#endif
190		212
191	AccumulatedRoll = 0;	213	AccumulatedRoll = 0;
192	AccumulatedRoll_cnt = 0;	214	AccumulatedRoll_cnt = 0;
193	AccumulatedNick = 0;	215	AccumulatedNick = 0;
194	AccumulatedNick_cnt = 0;	216	AccumulatedNick_cnt = 0;
195	AccumulatedGier = 0;	217	AccumulatedGier = 0;
196	AccumulatedGier_cnt = 0;	218	AccumulatedGier_cnt = 0;
197		219
198	#if 0	220	#if 0
199	ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt);	221	ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt);
200	ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt);	222	ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt);
201	ACC_Z_Offset = 0.9999F * ACC_Z_Offset + 0.0001F * ((float) AccumulatedACC_Z / (float) AccumulatedACC_Z_cnt);	223	ACC_Z_Offset = 0.9999F * ACC_Z_Offset + 0.0001F * ((float) AccumulatedACC_Z / (float) AccumulatedACC_Z_cnt);
202	#endif	224	#endif
203		225
204	AveragerACC_X = AccumulatedACC_X / AccumulatedACC_X_cnt;	226	AveragerACC_X = AccumulatedACC_X / AccumulatedACC_X_cnt;
205	AveragerACC_Y = AccumulatedACC_Y / AccumulatedACC_Y_cnt;	227	AveragerACC_Y = AccumulatedACC_Y / AccumulatedACC_Y_cnt;
206	AveragerACC_Z = AccumulatedACC_Z / AccumulatedACC_Z_cnt;	228	AveragerACC_Z = AccumulatedACC_Z / AccumulatedACC_Z_cnt;
207		229
208	AccumulatedACC_X = 0;	230	AccumulatedACC_X = 0;
209	AccumulatedACC_X_cnt = 0;	231	AccumulatedACC_X_cnt = 0;
210	AccumulatedACC_Y = 0;	232	AccumulatedACC_Y = 0;
211	AccumulatedACC_Y_cnt = 0;	233	AccumulatedACC_Y_cnt = 0;
212	AccumulatedACC_Z = 0;	234	AccumulatedACC_Z = 0;
213	AccumulatedACC_Z_cnt = 0;	235	AccumulatedACC_Z_cnt = 0;
214		236
215	ANALOG_ON;	237	ANALOG_ON;
216	}	238	}
217		239
218	/* ****************************************************************************	240	/* ****************************************************************************
219	Functionname: SendMotorData / /!	241	Functionname: SendMotorData / /!
220	Description: Senden der Motorwerte per I2C-Bus	242	Description: Senden der Motorwerte per I2C-Bus
221		243
222	@return void	244	@return void
223	@pre -	245	@pre -
224	@post -	246	@post -
225	@author H. Buss / I. Busker	247	@author H. Buss / I. Busker
226	**************************************************************************** */	248	**************************************************************************** */
227	void SendMotorData(void)	249	void SendMotorData(void)
228	{	250	{
229	if(!MotorenEin)	251	if(!MotorenEin)
230	{	252	{
231	Motor_Hinten = 0;	253	Motor_Hinten = 0;
232	Motor_Vorne = 0;	254	Motor_Vorne = 0;
233	Motor_Rechts = 0;	255	Motor_Rechts = 0;
234	Motor_Links = 0;	256	Motor_Links = 0;
235	if(MotorTest[0]) Motor_Vorne = MotorTest[0];	257	if(MotorTest[0]) Motor_Vorne = MotorTest[0];
236	if(MotorTest[1]) Motor_Hinten = MotorTest[1];	258	if(MotorTest[1]) Motor_Hinten = MotorTest[1];
237	if(MotorTest[2]) Motor_Links = MotorTest[2];	259	if(MotorTest[2]) Motor_Links = MotorTest[2];
238	if(MotorTest[3]) Motor_Rechts = MotorTest[3];	260	if(MotorTest[3]) Motor_Rechts = MotorTest[3];
239	}	261	}
240		262
241	//Start I2C Interrupt Mode	263	//Start I2C Interrupt Mode
242	twi_state = 0;	264	twi_state = 0;
243	motor = 0;	265	motor = 0;
244	i2c_start();	266	i2c_start();
245	}	267	}
246		268
247		269
248	/* ****************************************************************************	270	/* ****************************************************************************
249	Functionname: RemoteControl / /!	271	Functionname: RemoteControl / /!
250	Description:	272	Description:
251		273
252	@return void	274	@return void
253	@pre -	275	@pre -
254	@post -	276	@post -
255	@author H. Buss / I. Busker	277	@author H. Buss / I. Busker
256	**************************************************************************** */	278	**************************************************************************** */
257	void RemoteControl(void)	279	void RemoteControl(void)
258	{	280	{
259	static unsigned char delay_neutral = 0;	281	static unsigned char delay_neutral = 0;
260	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;	282	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
261	static unsigned int modell_fliegt = 0;	-
262		283
263	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	284	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
264	// Gaswert ermitteln	285	// Gaswert ermitteln
265	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	286	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
266	GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;	287	GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
267	if(GasMischanteil < 0)	288	if(GasMischanteil < 0)
268	{	289	{
269	GasMischanteil = 0;	290	GasMischanteil = 0;
270	}	291	}
271	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	292	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
272	// Emfang schlecht	293	// Emfang schlecht
273	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	294	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
274	if(SenderOkay < 100)	295	if(SenderOkay < 100)
275	{	296	{
276	if(!PcZugriff)	297	if(!PcZugriff)
277	{	298	{
278	if(BeepMuster == 0xffff)	299	if(BeepMuster == 0xffff)
279	{	300	{
280	beeptime = 15000;	301	beeptime = 15000;
281	BeepMuster = 0x0c00;	302	BeepMuster = 0x0c00;
282	}	303	}
283	}	304	}
284	}	305	}
285	else	306	else
286	{	307	{
287	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	308	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
288	// Emfang gut	309	// Emfang gut
289	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	310	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
290	if(SenderOkay > 140)	311	if(SenderOkay > 140)
291	{	312	{
292	if(GasMischanteil > 40)	313	if(GasMischanteil > 40)
293	{	314	{
294	if(modell_fliegt < 0xffff)	315	if(modell_fliegt < 0xffff)
295	{	316	{
296	modell_fliegt++;	317	modell_fliegt++;
297	}	318	}
298	}	319	}
299		320
300		321
301	if((GasMischanteil > 200) &&	322	if((GasMischanteil > 200) &&
302	(MotorenEin == 0))	323	(MotorenEin == 0))
303	{	324	{
304	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	325	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
305	// auf Nullwerte kalibrieren	326	// auf Nullwerte kalibrieren
306	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	327	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
307	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte	328	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
308	{	329	{
309	if(++delay_neutral > 50) // nicht sofort	330	if(++delay_neutral > 50) // nicht sofort
310	{	331	{
311	MotorenEin = 0;	332	MotorenEin = 0;
312	delay_neutral = 0;	333	delay_neutral = 0;
313	modell_fliegt = 0;	334	modell_fliegt = 0;
314	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	335	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
315	{	336	{
316	if((AdWertAirPressure_Raw > 950) \|\| (AdWertAirPressure_Raw < 750))	337	if((AdWertAirPressure_Raw > 950) \|\| (AdWertAirPressure_Raw < 750))
317	{	338	{
318	SucheLuftruckOffset();	339	SucheLuftruckOffset();
319	}	340	}
320	}	341	}
321	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	342	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
322	SetNeutral();	343	SetNeutral();
323	}	344	}
324	}	345	}
325	else	346	else
326	{	347	{
327	delay_neutral = 0;	348	delay_neutral = 0;
328	}	349	}
329	}	350	}
330	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	351	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
331	// Gas ist unten	352	// Gas ist unten
332	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	353	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
333	if(GasMischanteil < 35)	354	if(GasMischanteil < 35)
334	{	355	{
335	// Starten	356	// Starten
336	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)	357	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
337	{	358	{
338	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	359	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
339	// Einschalten	360	// Einschalten
340	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	361	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
341	if(++delay_einschalten > 100)	362	if(++delay_einschalten > 100)
342	{	363	{
343	MotorenEin = 1;	364	MotorenEin = 1;
344	modell_fliegt = 1;	365	modell_fliegt = 1;
345	delay_einschalten = 100;	366	delay_einschalten = 100;
346	}	367	}
347	}	368	}
348	else	369	else
349	{	370	{
350	delay_einschalten = 0;	371	delay_einschalten = 0;
351	}	372	}
352	//Auf Neutralwerte setzen	373	//Auf Neutralwerte setzen
353	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	374	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
354	// Auschalten	375	// Auschalten
355	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	376	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
356	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)	377	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
357	{	378	{
358	if(++delay_ausschalten > 50) // nicht sofort	379	if(++delay_ausschalten > 50) // nicht sofort
359	{	380	{
360	MotorenEin = 0;	381	MotorenEin = 0;
361	modell_fliegt = 0;	382	modell_fliegt = 0;
362	delay_ausschalten = 50;	383	delay_ausschalten = 50;
363	}	384	}
364	}	385	}
365	else	386	else
366	{	387	{
367	delay_ausschalten = 0;	388	delay_ausschalten = 0;
368	}	389	}
369	}	390	}
370	}	391	}
371	}	392	}
372	}	393	}
373		394
374		395
375		396
376		397
377	/* ****************************************************************************	398	/* ****************************************************************************
378	Functionname: PD_Regler / /!	399	Functionname: PD_Regler / /!
379	Description:	400	Description:
380		401
381	@return void	402	@return void
382	@pre -	403	@pre -
383	@post -	404	@post -
384	@author Michael Walter	405	@author Michael Walter
385	**************************************************************************** */	406	**************************************************************************** */
386	void PD_Regler(void)	407	void PD_Regler(void)
387	{	408	{
388	int StickNick45,StickRoll45;	409	int StickNick45,StickRoll45;
389	int DiffNick,DiffRoll, DiffGier;	410	int DiffNick,DiffRoll, DiffGier;
390	int motorwert = 0;	411	int motorwert = 0;
391	int pd_ergebnis;	412	int pd_ergebnis;
392		413
393	/*****************************************************************************	414	/*****************************************************************************
394	Update noimial attitude	415	Update noimial attitude
395	**************************************************************************** */	416	**************************************************************************** */
396	if(!NewPpmData--)	417	if(!NewPpmData--)
397	{	418	{
398	StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;	419	StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
399	StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;	420	StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
400	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];	421	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
401	} // Ende neue Funken-Werte	422	} // Ende neue Funken-Werte
402		423
403	#ifdef USE_GPS	424	#ifdef USE_GPS
404	/* G P S U p d a t e */	425	/* G P S U p d a t e */
405	GPSupdate();	426	GPSupdate();
406	#endif	427	#endif
407		428
408	static float AverageGasMischanteil = 50.F;	429	static float AverageGasMischanteil = 50.F;
409	if((GasMischanteil > 30) &&	430	if((GasMischanteil > 30) &&
410	(MotorenEin == 1) &&	431	(MotorenEin == 1) &&
411	(RemoteLinkLost == 0) )	432	(RemoteLinkLost == 0) )
412	{ /* Average the average throttle to find the hover setting */	433	{ /* Average the average throttle to find the hover setting */
413	AverageGasMischanteil = 0.999F * AverageGasMischanteil + 0.001F * GasMischanteil;	434	AverageGasMischanteil = 0.999F * AverageGasMischanteil + 0.001F * GasMischanteil;
414	}	435	}
415	/* Overide GasMischanteil */	436	/* Overide GasMischanteil */
416	static unsigned int DescentCnt = 32000;	437	static unsigned int DescentCnt = 32000;
417	if ((RemoteLinkLost == 1) &&	438	if ((RemoteLinkLost == 1) &&
418	(MotorenEin == 1))	439	(MotorenEin == 1))
419	{	440	{
420	if ((UBat < 100) \|\| /* Start to descent in case of low loltage or in case*/	441	if ((UBat < 100) \|\| /* Start to descent in case of low loltage or in case*/
421	(maxDistance / 10 < 12)) /* we reached our target position */	442	(maxDistance / 10 < 12)) /* we reached our target position */
422	{	443	{
423	if (DescentCnt > 0)	444	if (DescentCnt > 0)
424	{	445	{
425	DescentCnt--;	446	DescentCnt--;
426	}	447	}
427	else	448	else
428	{ /* We reached our target (hopefully) */	449	{ /* We reached our target (hopefully) */
429	MotorenEin = 0;	450	MotorenEin = 0;
430	RemoteLinkLost = 0;	451	RemoteLinkLost = 0;
431	}	452	}
432	}	453	}
433	else	454	else
434	{	455	{
435	DescentCnt = 32000;	456	DescentCnt = 32000;
436	}	457	}
437	/* Bias the throttle for a slow descent */	458	/* Bias the throttle for a slow descent */
438	GasMischanteil = (int) ((AverageGasMischanteil + 5.0F) * (DescentCnt / 32000.F));	459	GasMischanteil = (int) ((AverageGasMischanteil + 5.0F) * (DescentCnt / 32000.F));
439	}	460	}
440	else	461	else
441	{	462	{
442	DescentCnt = 32000;	463	DescentCnt = 32000;
443	}	464	}
444		465
445	//DebugOut.Analog[13] = (int) GasMischanteil;	466	//DebugOut.Analog[13] = (int) GasMischanteil;
446		467
447	#ifdef X_FORMATION	468	#ifdef X_FORMATION
448	/* Overide in case the remote link got lost */	469	/* Overide in case the remote link got lost */
449	if (RemoteLinkLost == 0)	470	if (RemoteLinkLost == 0)
450	{ /* We are flying in X-Formation */	471	{ /* We are flying in X-Formation */
451	StickRoll45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) - 0.707F * (float)(StickNick - GPS_Nick));	472	StickRoll45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) - 0.707F * (float)(StickNick - GPS_Nick));
452	StickNick45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) + 0.707F * (float)(StickNick - GPS_Nick));	473	StickNick45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) + 0.707F * (float)(StickNick - GPS_Nick));
453	}	474	}
454	else	475	else
455	{ /* GPS overide is aktive */	476	{ /* GPS overide is aktive */
456	StickRoll45 = (int) (0.707F * (float)(-GPS_Roll) - 0.707F * (float)(-GPS_Nick));	477	StickRoll45 = (int) (0.707F * (float)(-GPS_Roll) - 0.707F * (float)(-GPS_Nick));
457	StickNick45 = (int) (0.707F * (float)(-GPS_Roll) + 0.707F * (float)(-GPS_Nick));	478	StickNick45 = (int) (0.707F * (float)(-GPS_Roll) + 0.707F * (float)(-GPS_Nick));
458	StickGier = 0;	479	StickGier = 0;
459	}	480	}
460	#else	481	#else
461	/* Overide in case the remote link got lost */	482	/* Overide in case the remote link got lost */
462	if (RemoteLinkLost == 0)	483	if (RemoteLinkLost == 0)
463	{ /* We are flying in X-Formation */	484	{ /* We are flying in X-Formation */
464	StickRoll45 = StickRoll - GPS_Roll;	485	StickRoll45 = StickRoll - GPS_Roll;
465	StickNick45 = StickNick - GPS_Nick;	486	StickNick45 = StickNick - GPS_Nick;
466	}	487	}
467	else	488	else
468	{ /* GPS overide is aktive */	489	{ /* GPS overide is aktive */
469	StickRoll45 = -GPS_Roll;	490	StickRoll45 = -GPS_Roll;
470	StickNick45 = -GPS_Nick;	491	StickNick45 = -GPS_Nick;
471	StickGier = 0;	492	StickGier = 0;
472	}	493	}
473	#endif	494	#endif
474		495
475	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	496	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
476	// Yaw	497	// Yaw
477	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	498	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
478		499

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/FlightControl.c @ 1698 - Rev 973 → 983