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.
*/


/*****************************************************************************
  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;
extern int InvalidAttitude;
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;
 
  InvalidAttitude = 0;
  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
}