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