Subversion Repositories FlightCtrl

Rev

Rev 2130 | Rev 2137 | 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;
- 
 
 22 
 
- 
 
 23 
void reset(void) {
- 
 
 24 
        resetFlag = 1;
- 
 
 25 
        wdt_enable(WDTO_15MS);
- 
 
 26 
        while (1)
- 
 
 27 
                ;
- 
 
 28 
}
19 
 
 29 
 
20 
int16_t main(void) {
 30 
int16_t main(void) {
- 
 
 31 
        uint16_t timer = 0;
- 
 
 32 
 
21 
  uint16_t timer;
 33 
#ifdef DO_PROFILE
- 
 
 34 
        static uint8_t profileTimer;
Line 22... Line 35...
22 
  static uint8_t profileTimer;
 35 
#endif
23 
 
 36 
 
Line 24... Line -...
24 
  // disable interrupts global
 - 
 
25 
  cli();
 - 
 
26 
 
 37 
        // disable interrupts global
Line 27... Line 38...
27 
  // analyze hardware environment
 38 
        cli();
28 
  setCPUType();
 39 
 
29 
  setBoardRelease();
 - 
 
30 
 
 40 
        wdt_enable(WDTO_120MS);
Line 31... Line 41...
31 
  // disable watchdog
 41 
 
- 
 
 42 
        // analyze hardware environment
- 
 
 43 
        setCPUType();
- 
 
 44 
        setBoardRelease();
- 
 
 45 
 
32 
  MCUSR &= ~(1 << WDRF);
 46 
        // initalize modules
- 
 
 47 
        output_init();
33 
  WDTCSR |= (1 << WDCE) | (1 << WDE);
 48 
        timer0_init();
Line 34... Line 49...
34 
  WDTCSR = 0;
 49 
        timer2_init();
35 
 
 50 
        usart0_init();
36 
// This is strange: It should NOT be necessarty to do. But the call of the same,
 51 
        //if (CPUType == ATMEGA644P);// usart1_Init();
37 
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
 52 
        RC_Init();
38 
// channelMap_default();
 - 
 
39 
 
 - 
 
40 
  // initalize modules
 - 
 
41 
  output_init();
 53 
        analog_init();
Line 42... Line 54...
42 
  timer0_init();
 54 
 
43 
  timer2_init();
 - 
 
44 
  usart0_init();
 - 
 
45 
  //if (CPUType == ATMEGA644P);// usart1_Init();
 55 
        // Parameter Set handling
46 
  RC_Init();
 - 
 
Line 47... Line 56...
47 
  analog_init();
 56 
        IMUConfig_readOrDefault();
48 
 
 - 
 
Line -... Line 57...
- 
 
 57 
        channelMap_readOrDefault();
49 
  // Parameter Set handling
 58 
        rcTrim_readOrDefault();
Line 50... Line 59...
50 
  IMUConfig_readOrDefault();
 59 
        paramSet_readOrDefault();
51 
  channelMap_readOrDefault();
 60 
 
Line 52... Line 61...
52 
  rcTrim_readOrDefault();
 61 
        // enable interrupts global
53 
  paramSet_readOrDefault();
 62 
        sei();
Line 54... Line -...
54 
 
 - 
 
55 
  // enable interrupts global
 63 
 
- 
 
 64 
        controlMixer_setNeutral();
- 
 
 65 
 
Line 56... Line 66...
56 
  sei();
 66 
        // Cal. attitude sensors and reset integrals.
57 
 
 67 
        attitude_setNeutral();
58 
  controlMixer_setNeutral();
 - 
 
- 
 
 68 
 
Line 59... Line 69...
59 
 
 69 
        // Init flight parameters
60 
  // Cal. attitude sensors and reset integrals.
 70 
        // flight_setNeutral();
61 
  attitude_setNeutral();
 71 
 
62 
 
 72 
        // This is not a busy-wait operation and should be OK.
63 
  // Init flight parameters
 73 
        beep(2000);
64 
  // flight_setNeutral();
 74 
 
65 
 
 75 
        while (1) {
66 
  // This is not a busy-wait operation and should be OK.
 76 
                if (runFlightControl) { // control interval
67 
  beep(2000);
 77 
                        runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
68 
 
 78 
 
69 
  while (1) {
 79 
                        if (!resetFlag) {
70 
    if (runFlightControl) { // control interval
 80 
                                wdt_reset();
71 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 81 
                        }
72 
 
 82 
 
73 
      if (!analogDataReady) {
 83 
                        if (!analogDataReady) {
74 
        // Analog data should have been ready but is not!!
 84 
                                // Analog data should have been ready but is not!!
75 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
 85 
                                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
Line 76... Line 86...
76 
      } else {
 86 
                        } else {
77 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 87 
                                debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
78 
          }
 88 
                        }
79 
        // This is probably the correct order:
 89 
                        // This is probably the correct order:
80 
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
 90 
                        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
81 
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
 91 
                        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
82 
        // Flight control uses results from both.
 92 
                        // Flight control uses results from both.
Line 83... Line 93...
83 
#ifdef DO_PROFILE
 93 
#ifdef DO_PROFILE
84 
      startProfileTimer(ATTITUDE);
 94 
                        startProfileTimer(ATTITUDE);
85 
#endif
 95 
#endif
86 
      calculateFlightAttitude();
 96 
                        calculateFlightAttitude();
87 
#ifdef DO_PROFILE
 97 
#ifdef DO_PROFILE
88 
      stopProfileTimer(ATTITUDE);
 98 
                        stopProfileTimer(ATTITUDE);
89 
#endif
 99 
#endif
Line 90... Line 100...
90 
 
 100 
 
91 
#ifdef DO_PROFILE
 101 
#ifdef DO_PROFILE
92 
      startProfileTimer(CONTROLMIXER);
 102 
                        startProfileTimer(CONTROLMIXER);
93 
#endif
 103 
#endif
94 
      controlMixer_periodicTask();
 104 
                        controlMixer_periodicTask();
95 
#ifdef DO_PROFILE
 105 
#ifdef DO_PROFILE
96 
      stopProfileTimer(CONTROLMIXER);
 106 
                        stopProfileTimer(CONTROLMIXER);
Line 97... Line 107...
97 
#endif
 107 
#endif
98 
 
 108 
 
99 
#ifdef DO_PROFILE
 109 
#ifdef DO_PROFILE
100 
      startProfileTimer(COMMANDS);
 110 
                        startProfileTimer(COMMANDS);
Line 101... Line 111...
101 
#endif
 111 
#endif
Line 102... Line 112...
102 
      commands_handleCommands();
 112 
                        commands_handleCommands();
Line 103... Line 113...
103 
#ifdef DO_PROFILE
 113 
#ifdef DO_PROFILE
104 
      stopProfileTimer(COMMANDS);
 114 
                        stopProfileTimer(COMMANDS);
105 
#endif
 115 
#endif
106 
 
 116 
 
107 
#ifdef DO_PROFILE
 117 
#ifdef DO_PROFILE
108 
      startProfileTimer(FLIGHT);
 118 
                        startProfileTimer(FLIGHT);
109 
#endif
 119 
#endif
110 
      flight_control();
 120 
                        flight_control();
111 
#ifdef DO_PROFILE
 121 
#ifdef DO_PROFILE
112 
      stopProfileTimer(FLIGHT);
 122 
                        stopProfileTimer(FLIGHT);
113 
#endif
 123 
#endif
Line 114... Line 124...
114 
 
 124 
 
Line 115... Line 125...
115 
        // Allow Serial Data Transmit if motors must not updated or motors are not running
 125 
                        // Allow Serial Data Transmit if motors must not updated or motors are not running
116 
        if (!runFlightControl || !isFlying) {
 126 
                        if (!runFlightControl || !isFlying) {
117 
          usart0_transmitTxData();
 127 
                                usart0_transmitTxData();
118 
        }
 128 
                        }
119 
 
 129 
 
120 
        usart0_processRxData();
 130 
                        usart0_processRxData();
121 
 
 131 
 
122 
        static uint8_t aboveWarningLimitVoltageSeen = 0;
 132 
                        static uint8_t aboveWarningLimitVoltageSeen = 0;
123 
 
 133