Subversion Repositories FlightCtrl

/*

*/

/*****************************************************************************

(SYMBOLIC) CONSTANTS

*****************************************************************************/

/*****************************************************************************

  VARIABLES

*****************************************************************************/

extern int RCQuality;

int RemoteLinkLost;

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);

/* ****************************************************************************

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();  

      /* PID 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()

{

#if 0

  /* Yaw or pitch are greater than  60 Deg abs */

  if (((abs(status.iTheta10) > 600) || (abs(status.iPhi10) > 600)) &&

            ((abs(AverageRoll_X) > Threshhold) ||

            (abs(AverageRoll_Y) > Threshhold) ||

            (abs(AverageRoll_Z) > Threshhold) ))

  {

    MotorenEin = 0;

    Delay_ms(1000);

    ReleaseServo();

  }

#endif

}

/* ****************************************************************************

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");

  }

    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;