Subversion Repositories FlightCtrl

Rev

Rev 2130 | Rev 2133 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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