Rev 2137 | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2137 | Rev 2142 | ||
---|---|---|---|
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 | // disable interrupts global |
37 | // disable interrupts global |
38 | cli(); |
38 | cli(); |
39 | 39 | ||
40 | // wdt_enable(WDTO_120MS); |
40 | // wdt_enable(WDTO_120MS); |
41 | wdt_disable(); |
41 | wdt_disable(); |
42 | 42 | ||
43 | // analyze hardware environment |
43 | // analyze hardware environment |
44 | setCPUType(); |
44 | setCPUType(); |
45 | setBoardRelease(); |
45 | setBoardRelease(); |
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 | //if (CPUType == ATMEGA644P);// usart1_Init(); |
52 | //if (CPUType == ATMEGA644P);// usart1_Init(); |
53 | RC_Init(); |
53 | RC_Init(); |
54 | analog_init(); |
54 | analog_init(); |
55 | 55 | ||
56 | // Parameter Set handling |
56 | // Parameter Set handling |
57 | IMUConfig_readOrDefault(); |
57 | IMUConfig_readOrDefault(); |
58 | channelMap_readOrDefault(); |
58 | channelMap_readOrDefault(); |
59 | rcTrim_readOrDefault(); |
59 | rcTrim_readOrDefault(); |
60 | paramSet_readOrDefault(); |
60 | paramSet_readOrDefault(); |
61 | 61 | ||
62 | // enable interrupts global |
62 | // enable interrupts global |
63 | sei(); |
63 | sei(); |
64 | 64 | ||
65 | controlMixer_setNeutral(); |
65 | controlMixer_setNeutral(); |
66 | 66 | ||
67 | // Cal. attitude sensors and reset integrals. |
67 | // Cal. attitude sensors and reset integrals. |
68 | attitude_setNeutral(); |
68 | attitude_setNeutral(); |
69 | 69 | ||
70 | // Init flight parameters |
70 | // Init flight parameters |
71 | // flight_setNeutral(); |
71 | // flight_setNeutral(); |
72 | 72 | ||
73 | // This is not a busy-wait operation and should be OK. |
73 | // This is not a busy-wait operation and should be OK. |
74 | beep(2000); |
74 | beep(2000); |
75 | 75 | ||
76 | while (1) { |
76 | while (1) { |
77 | if (runFlightControl) { // control interval |
77 | if (runFlightControl) { // control interval |
78 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
78 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
79 | 79 | ||
80 | if (!resetFlag) { |
80 | if (!resetFlag) { |
81 | wdt_reset(); |
81 | wdt_reset(); |
82 | } |
82 | } |
83 | 83 | ||
84 | if (!analogDataReady) { |
84 | if (!analogDataReady) { |
85 | // Analog data should have been ready but is not!! |
85 | // Analog data should have been ready but is not!! |
86 | debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
86 | debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
87 | } else { |
87 | } else { |
88 | debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
88 | debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
89 | } |
89 | } |
90 | // This is probably the correct order: |
90 | // This is probably the correct order: |
91 | // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
91 | // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
92 | // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
92 | // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
93 | // Flight control uses results from both. |
93 | // Flight control uses results from both. |
94 | #ifdef DO_PROFILE |
94 | #ifdef DO_PROFILE |
95 | startProfileTimer(ATTITUDE); |
95 | startProfileTimer(ATTITUDE); |
96 | #endif |
96 | #endif |
97 | calculateFlightAttitude(); |
97 | calculateFlightAttitude(); |
98 | #ifdef DO_PROFILE |
98 | #ifdef DO_PROFILE |
99 | stopProfileTimer(ATTITUDE); |
99 | stopProfileTimer(ATTITUDE); |
100 | #endif |
100 | #endif |
101 | 101 | ||
102 | #ifdef DO_PROFILE |
102 | #ifdef DO_PROFILE |
103 | startProfileTimer(CONTROLMIXER); |
103 | startProfileTimer(CONTROLMIXER); |
104 | #endif |
104 | #endif |
105 | controlMixer_periodicTask(); |
105 | controlMixer_periodicTask(); |
106 | #ifdef DO_PROFILE |
106 | #ifdef DO_PROFILE |
107 | stopProfileTimer(CONTROLMIXER); |
107 | stopProfileTimer(CONTROLMIXER); |
108 | #endif |
108 | #endif |
109 | 109 | ||
110 | #ifdef DO_PROFILE |
110 | #ifdef DO_PROFILE |
111 | startProfileTimer(COMMANDS); |
111 | startProfileTimer(COMMANDS); |
112 | #endif |
112 | #endif |
113 | commands_handleCommands(); |
113 | commands_handleCommands(); |
114 | #ifdef DO_PROFILE |
114 | #ifdef DO_PROFILE |
115 | stopProfileTimer(COMMANDS); |
115 | stopProfileTimer(COMMANDS); |
116 | #endif |
116 | #endif |
117 | 117 | ||
118 | #ifdef DO_PROFILE |
118 | #ifdef DO_PROFILE |
119 | startProfileTimer(FLIGHT); |
119 | startProfileTimer(FLIGHT); |
120 | #endif |
120 | #endif |
121 | flight_control(); |
121 | flight_control(); |
122 | #ifdef DO_PROFILE |
122 | #ifdef DO_PROFILE |
123 | stopProfileTimer(FLIGHT); |
123 | stopProfileTimer(FLIGHT); |
124 | #endif |
124 | #endif |
125 | 125 | ||
126 | // Allow Serial Data Transmit if motors must not updated or motors are not running |
126 | // Allow Serial Data Transmit if motors must not updated or motors are not running |
127 | if (!runFlightControl || !isFlying) { |
127 | if (!runFlightControl || !isFlying) { |
128 | usart0_transmitTxData(); |
128 | usart0_transmitTxData(); |
129 | } |
129 | } |
130 | 130 | ||
131 | usart0_processRxData(); |
131 | usart0_processRxData(); |
132 | 132 | ||
133 | static uint8_t aboveWarningLimitVoltageSeen = 0; |
133 | static uint8_t aboveWarningLimitVoltageSeen = 0; |
134 | 134 | ||
135 | if (checkDelay(timer)) { |
135 | if (checkDelay(timer)) { |
136 | if (UBat >= staticParams.batteryWarningVoltage) { |
136 | if (UBat >= staticParams.batteryWarningVoltage) { |
137 | aboveWarningLimitVoltageSeen = 1; |
137 | aboveWarningLimitVoltageSeen = 1; |
138 | } else { // If we are above USB voltage, or if we have once been above warning voltage |
138 | } else { // If we are above USB voltage, or if we have once been above warning voltage |
139 | if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) { |
139 | if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) { |
140 | beepBatteryAlarm(); |
140 | beepBatteryAlarm(); |
141 | } |
141 | } |
142 | } |
142 | } |
143 | calculateFeaturedServoValues(); |
143 | //calculateFeaturedServoValues(); |
144 | timer = setDelay(20); // every 20 ms |
144 | timer = setDelay(20); // every 20 ms |
145 | } |
145 | } |
146 | 146 | ||
147 | output_update(); |
147 | output_update(); |
148 | 148 | ||
149 | if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
149 | if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
150 | debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
150 | debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
151 | } else { |
151 | } else { |
152 | debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
152 | debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
153 | } |
153 | } |
154 | } |
154 | } |
155 | } |
155 | } |
156 | return (1); |
156 | return (1); |
157 | } |
157 | } |
158 | 158 |