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