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 "main.h"
#include "kafi.h"
#include "mymath.h"
#include "mm3.h"
#include "FlightControl.h"

/*****************************************************************************
(SYMBOLIC) CONSTANTS
*****************************************************************************/

#define sin45 -0.707106
#define cos45 0.707106

/*****************************************************************************
  VARIABLES
*****************************************************************************/
extern int RCQuality;
int RemoteLinkLost = 0;
int InvalidAttitude = 0;
extern unsigned long maxDistance;
unsigned char EEPromArray[E2END+1] EEMEM;

void TestRemote(void);
void IMU_Main(void);
void TestBattery(void);
void SendDebugData(void);
void InitPorts(void);
void GenerateDefaults(void);
void TestIC2Link(void);
void GetAirPressureOffset(void);
void DeployRescue(void);

extern void InitOSD(void);
extern void InitGPS(void);
extern void SendOSD(void);
extern void RemoteControl(void);  
extern void SendMotorData(void);
extern void GetMeasurements(void);
extern void AttitudeEstimation(void);
extern void PD_Regler(void);
extern void SetNeutral(void);

/* ****************************************************************************
Functionname:     main                      */ /*!
Description:      Hauptprogramm

  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
int main (void)
{
  /* Controler Init */
  InitPorts();
  Kafi_Init();
  Timer_Init();
  UART_Init();
  InitGPS();
  rc_sum_init();
  ADC_Init();
  i2c_init();
#ifdef USE_COMPASS
  MM3_Init();
#endif
  /* Enable Interrupts */
  sei();

  /* Generate Default Values */
  GenerateDefaults ();
  /* Get Air Pressure Offset */
  GetAirPressureOffset();
  /* Determine Gyro / ACC Offsets */
  SetNeutral();

  DebugIn.Analog[1] = 1000;
  DebugIn.Digital[0] = 0x55;
 
#ifdef USE_COMPASS
  /* Calibrate the MM3 Compass? */
  if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) &&
    (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) &&
    (MotorenEin == 0))
  {
    printf("\n\rCalibrating Compass");
    MM3_Calibrate();
  }
#endif

#ifdef USE_OSD
  /* Init the Bob-4 OnScreen Display */
  InitOSD();
#endif

  /* Start the main Task */
  IMU_Main();
  return (1);
}


/* ****************************************************************************
  Functionname:     IMU_Main                      */ /*!
  Description:      

  @param[in]        

  @return           void
  @pre              -
  @post             -
  @author         Michael Walter  
**************************************************************************** */
void IMU_Main()
{
  ROT_ON
  I2CTimeout = 5000;
 
  while (1)
  {
    static i32_t OldTime = 0;
    if (UpdateMotor)
    {
      UpdateMotor=0;

#ifdef USE_OSD
      /* G e n e r a t e   O S D   D a t a */
      static char CntOSD = 0;
      if (CntOSD % 6 == 1)
      {
        SendOSD();
      }
      CntOSD++;
#endif

      /* Set the cycle Time to 120ms / 125ms */
      if (OldTime != 0)
      {
        while (((Count8Khz - OldTime) *10) / 8 < 120);
        DebugOut.Analog[14] = ((Count8Khz - OldTime) *10) / 8;
      }
      OldTime = Count8Khz;
     
      /* GetMeasurements()*/
      GetMeasurements();

      /* EstimateFlightAttitude */
      FlightAttitudeEstimation();

      /* Set Nominal Value */
      RemoteControl();  

      /* PD Control */
      PD_Regler();
     
      /* Send Motor Data */
      SendMotorData();
     
      /* TestRemote */
      TestRemote();
     
      /* Test IC2- / RC-Link */
      TestIC2Link();
    }
    /* Send Debug Data over RS232 */
    SendDebugData();
    /* Check the Batery for Undervoltage */
    TestBattery();
    /* DeployRescue */
    DeployRescue();
  }
}

/* ****************************************************************************
Functionname:     DeployRescue                      */ /*!
Description:       Deploy a rescue parachute using a servo

  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
void DeployRescue()
{
  /* Yaw or pitch are greater than 90 Deg abs */
  if (((abs(status.iTheta10) > 900) || (abs(status.iPhi10) > 900)) &&
     ((abs(AverageRoll) > 400) ||
      (abs(AverageNick) > 400) ||
      (abs(AverageGier) > 400) ))
  {
    //MotorenEin = 0;
    Delay_ms(500);
    InvalidAttitude = 1;
  }
 
  if (InvalidAttitude)
  {
    ServoValue = 150;
  }
  else
  {
    ServoValue = 0;
  }
}

/* ****************************************************************************
Functionname:     ReadParameterSet                      */ /*!
Description:      -- Parametersatz aus EEPROM lesen ---
                  number [0..5]

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
{
  if (number > 5)
  {
    number = 5;
  }
  eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
}

/* ****************************************************************************
Functionname:     WriteParameterSet                      */ /*!
Description:      -- Parametersatz ins EEPROM schreiben ---
                  number [0..5]

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
{
  if(number > 5)
  {
    number = 5;
  }
  eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
  eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number);      // diesen Parametersatz als aktuell merken
}

/* ****************************************************************************
Functionname:     GetActiveParamSetNumber                      */ /*!
Description:      

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
unsigned char GetActiveParamSetNumber(void)
{
  unsigned char set;
  set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]);
  if(set > 5)
  {
    set = 2;  
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set);      // diesen Parametersatz als aktuell merken
  }
  return(set);
}

/* ****************************************************************************
Functionname:     GetAirPressureOffset                      */ /*!
Description:      

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void GetAirPressureOffset(void)
{
 unsigned int timer;
 ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
  printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
  if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
  {
    printf("\n\rAbgleich Luftdrucksensor..");
    timer = SetDelay(1000);  
    SucheLuftruckOffset();
    while (!CheckDelay(timer));
    printf("OK\n\r");
  }
}

/* ****************************************************************************
Functionname:     GenerateDefaults                      */ /*!
Description:      Generate the Default Paramter

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void  GenerateDefaults()
{
  VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
  VersionInfo.Nebenversion = VERSION_NEBENVERSION;
  VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
 
#define EE_DATENREVISION 66 // wird angepasst, wenn sich die EEPROM-Daten geändert haben
  if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
  {
    printf("\n\rInit. EEPROM: Generiere Default-Parameter...");
    DefaultKonstanten1();
    for (unsigned char i=0;i<6;i++)  
    {
      if(i==2) DefaultKonstanten2(); // Kamera
      if(i==3) DefaultKonstanten3(); // Beginner
      if(i>3)  DefaultKonstanten2(); // Kamera
      WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
    }
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
  }
}


/* ****************************************************************************
Functionname:     InitPorts                      */ /*!
Description:      Init the IO Ports

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void  InitPorts()
{
  unsigned int timer = 0;
  DDRB  = 0x00;
  PORTB = 0x00;
  for(timer = 0; timer < 1000; timer++); // verzögern
  DDRC  = 0x81; // SCL
  PORTC = 0xff; // Pullup SDA
  DDRB  = 0x1B; // LEDs und Druckoffset
  PORTB = 0x01; // LED_Rot
  DDRD  = 0x3E; // Speaker & TXD & J3 J4 J5
  DDRD  |=0x80; // J7
  PORTD = 0xF7; // LED
 
  MCUSR &=~(1<<WDRF);
  WDTCSR |= (1<<WDCE)|(1<<WDE);
  WDTCSR = 0;
}

/* ****************************************************************************
Functionname:     TestIC2Link                      */ /*!
Description:      Test IC2- / RC-Link

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void TestIC2Link()
{
  if(PcZugriff)
  {
    PcZugriff--;
  }
 
  if(SenderOkay)  
  {
    SenderOkay--;
  }
 
  if(!I2CTimeout)
  {
    I2CTimeout = 5;
    i2c_reset();
    if((BeepMuster == 0xffff) && MotorenEin)
    {
      beeptime = 10000;
      BeepMuster = 0x0080;
    }
  }
  else
  {
    I2CTimeout--;
  }
}


/* ****************************************************************************
Functionname:     SendDebugData                      */ /*!
Description:      Send Debug Data over RS232

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void SendDebugData()
{
  if(SIO_DEBUG)
  {
    DatenUebertragung();
    BearbeiteRxDaten();
  }
  else
  {
    BearbeiteRxDaten();
  }
}


/* ****************************************************************************
Functionname:     TestBattery                      */ /*!
Description:      Check the Battery Voltage

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void  TestBattery()
{
  unsigned int timer = 0;
  if(CheckDelay(timer))
  {  /* Voltage is Below Threshhod ? */
    if(UBat < EE_Parameter.UnterspannungsWarnung)
    {    
      if(BeepMuster == 0xffff)
      {
        beeptime = 6000;
        BeepMuster = 0x0300;
      }
    }
    timer = SetDelay(100);  
  }
  DebugOut.Analog[15] =  UBat;
}

/* ****************************************************************************
  Functionname:     TestRemote                      */ /*!
  Description:      

  @param[in]        

  @return           void
  @pre              -
  @post             -
  @author         Michael Walter  
**************************************************************************** */
void TestRemote()
{
  /*--- (SYMBOLIC) CONSTANTS ---*/
 
  /*--- VARIABLES ---*/
  static unsigned int TimeSinceRC_BelowThreshhold = 0;
  static unsigned int TimeSinceRC_AboveThreshhold = 0;
 
  if (MotorenEin == 1)
  {
    if (RemoteLinkLost == 0)
    {
      if (RCQuality < 60)
      {
        if (TimeSinceRC_BelowThreshhold < 10000)
        {
          TimeSinceRC_BelowThreshhold++;
        }
      }
      else
      {
        TimeSinceRC_BelowThreshhold = 0;
      }
      if ((TimeSinceRC_BelowThreshhold > 500) || /* aprox. 5 seconds */
        (SenderOkay < 100))
      {
        RemoteLinkLost = 1;
        TimeSinceRC_AboveThreshhold = 0;
      }
    }
    else
    {
      if (RCQuality > 80)
      {
        if (TimeSinceRC_AboveThreshhold < 10000)
        {
          TimeSinceRC_AboveThreshhold++;
        }
      }
      else
      {
        TimeSinceRC_AboveThreshhold = 0;
      }
      if (TimeSinceRC_AboveThreshhold > 100) /* aprox. 1 seconds */
      {
        RemoteLinkLost = 0;
        TimeSinceRC_BelowThreshhold = 0;    
      }
    }
  }
  else
  {
    RemoteLinkLost = 0;
    TimeSinceRC_BelowThreshhold = 0;
    TimeSinceRC_AboveThreshhold = 0;
  }
  /*DebugOut.Analog[14] = TimeSinceRC_BelowThreshhold;*/
  /*DebugOut.Analog[15] = TimeSinceRC_AboveThreshhold;*/
}