Subversion Repositories FlightCtrl

Rev

Rev 2119 | Rev 2132 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2108 - 1
#include <avr/boot.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
4
#include <util/delay.h>
5
 
6
#include "timer0.h"
7
#include "timer2.h"
8
#include "uart0.h"
9
#include "output.h"
10
#include "attitude.h"
11
#include "commands.h"
12
#include "flight.h"
13
#include "rc.h"
14
#include "analog.h"
15
#include "configuration.h"
16
#include "controlMixer.h"
17
#include "eeprom.h"
18
#include "printf_P.h"
19
 
20
int16_t main(void) {
21
  uint16_t timer;
2130 - 22
  static uint8_t profileTimer;
2108 - 23
 
24
  // disable interrupts global
25
  cli();
26
 
27
  // disable watchdog
28
  MCUSR &= ~(1 << WDRF);
29
  WDTCSR |= (1 << WDCE) | (1 << WDE);
30
  WDTCSR = 0;
31
 
32
// This is strange: It should NOT be necessarty to do. But the call of the same,
33
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
2130 - 34
// channelMap_default();
2108 - 35
 
36
  // initalize modules
37
  output_init();
38
  timer0_init();
39
  timer2_init();
40
  usart0_init();
41
  RC_Init();
42
  analog_init();
43
 
44
  // Parameter Set handling
45
  IMUConfig_readOrDefault();
46
  channelMap_readOrDefault();
2130 - 47
  rcTrim_readOrDefault();
2108 - 48
  paramSet_readOrDefault();
49
 
50
  // enable interrupts global
51
  sei();
52
 
53
  controlMixer_setNeutral();
54
 
55
  // Cal. attitude sensors and reset integrals.
56
  attitude_setNeutral();
57
 
2130 - 58
  // This is not a busy-wait operation and should be OK.
2108 - 59
  beep(2000);
60
 
2109 - 61
  while (1) {
2108 - 62
    if (runFlightControl) { // control interval
63
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
64
      if (sensorDataReady != ALL_DATA_READY) {
65
        // Analog data should have been ready but is not!!
66
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
67
      } else {
68
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
2109 - 69
      }
2110 - 70
      PD2HIGH;
2109 - 71
      // This is probably the correct order:
72
      // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
73
      // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
74
      // Flight control uses results from both.
75
      calculateFlightAttitude();
76
      controlMixer_periodicTask();
77
      commands_handleCommands();
78
      flight_control();
2110 - 79
      PD2LOW;
2108 - 80
 
2109 - 81
      // Allow Serial Data Transmit if motors must not updated or motors are not running
82
      if (!runFlightControl || !isFlying) {
83
        usart0_transmitTxData();
84
      }
2108 - 85
 
2109 - 86
      usart0_processRxData();
2108 - 87
 
2130 - 88
        static uint8_t aboveWarningLimitVoltageSeen = 0;
2108 - 89
 
2130 - 90
        if (checkDelay(timer)) {
91
          if (UBat >= staticParams.batteryWarningVoltage) {
92
            aboveWarningLimitVoltageSeen = 1;
93
          } else { // If we are above USB voltage, or if we have once been above warning voltage
94
            if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
95
              beepBatteryAlarm();
96
            }
97
          }
98
          calculateFeaturedServoValues();
99
          timer = setDelay(20); // every 20 ms
100
        }
2108 - 101
 
2130 - 102
    output_update();
2109 - 103
 
104
    if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
105
      debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
106
    } else {
107
      debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
2108 - 108
    }
109
  }
110
  return (1);
111
}