Rev 2130 | Rev 2133 | 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 | |
| 2132 | - | 40 | wdt_enable(WDTO_120MS); |
| 2108 | - | 41 | |
| 2132 | - | 42 | // initalize modules |
| 43 | output_init(); |
||
| 44 | timer0_init(); |
||
| 45 | timer2_init(); |
||
| 46 | usart0_init(); |
||
| 47 | RC_Init(); |
||
| 48 | analog_init(); |
||
| 2108 | - | 49 | |
| 2132 | - | 50 | // Parameter Set handling |
| 51 | IMUConfig_readOrDefault(); |
||
| 52 | channelMap_readOrDefault(); |
||
| 53 | rcTrim_readOrDefault(); |
||
| 54 | paramSet_readOrDefault(); |
||
| 2108 | - | 55 | |
| 2132 | - | 56 | // enable interrupts global |
| 57 | sei(); |
||
| 2108 | - | 58 | |
| 2132 | - | 59 | controlMixer_setNeutral(); |
| 2108 | - | 60 | |
| 2132 | - | 61 | // Cal. attitude sensors and reset integrals. |
| 62 | attitude_setNeutral(); |
||
| 2108 | - | 63 | |
| 2132 | - | 64 | // This is not a busy-wait operation and should be OK. |
| 65 | beep(2000); |
||
| 2108 | - | 66 | |
| 2132 | - | 67 | while (1) { |
| 68 | if (runFlightControl) { // control interval |
||
| 69 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
||
| 2108 | - | 70 | |
| 2132 | - | 71 | if (!resetFlag) { |
| 72 | wdt_reset(); |
||
| 73 | } |
||
| 2108 | - | 74 | |
| 2132 | - | 75 | if (sensorDataReady != ALL_DATA_READY) { |
| 76 | // Analog data should have been ready but is not!! |
||
| 77 | debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
||
| 78 | } else { |
||
| 79 | debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
||
| 80 | } |
||
| 81 | PD2HIGH; |
||
| 82 | // This is probably the correct order: |
||
| 83 | // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
||
| 84 | // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
||
| 85 | // Flight control uses results from both. |
||
| 86 | calculateFlightAttitude(); |
||
| 87 | controlMixer_periodicTask(); |
||
| 88 | commands_handleCommands(); |
||
| 89 | flight_control(); |
||
| 90 | PD2LOW; |
||
| 2108 | - | 91 | |
| 2132 | - | 92 | // Allow Serial Data Transmit if motors must not updated or motors are not running |
| 93 | if (!runFlightControl || !isFlying) { |
||
| 94 | usart0_transmitTxData(); |
||
| 95 | } |
||
| 2108 | - | 96 | |
| 2132 | - | 97 | usart0_processRxData(); |
| 2108 | - | 98 | |
| 2132 | - | 99 | static uint8_t aboveWarningLimitVoltageSeen = 0; |
| 2109 | - | 100 | |
| 2132 | - | 101 | if (checkDelay(timer)) { |
| 102 | if (UBat >= staticParams.batteryWarningVoltage) { |
||
| 103 | aboveWarningLimitVoltageSeen = 1; |
||
| 104 | } else { // If we are above USB voltage, or if we have once been above warning voltage |
||
| 105 | if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) { |
||
| 106 | beepBatteryAlarm(); |
||
| 107 | } |
||
| 108 | } |
||
| 109 | calculateFeaturedServoValues(); |
||
| 110 | timer = setDelay(20); // every 20 ms |
||
| 111 | } |
||
| 112 | |||
| 113 | output_update(); |
||
| 114 | |||
| 115 | if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
||
| 116 | debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
||
| 117 | } else { |
||
| 118 | debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
||
| 119 | } |
||
| 120 | } |
||
| 121 | } |
||
| 122 | return (1); |
||
| 2108 | - | 123 | } |