Rev 2112 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2112 | Rev 2158 | ||
---|---|---|---|
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 <util/delay.h> |
4 | #include <util/delay.h> |
5 | 5 | ||
6 | #include "timer0.h" |
6 | #include "timer0.h" |
7 | #include "timer2.h" |
7 | #include "timer2.h" |
8 | #include "uart0.h" |
8 | #include "uart0.h" |
9 | #include "uart1.h" |
9 | #include "uart1.h" |
10 | #include "output.h" |
10 | #include "output.h" |
11 | #include "menu.h" |
11 | #include "menu.h" |
12 | #include "attitude.h" |
12 | #include "attitude.h" |
13 | #include "commands.h" |
13 | #include "commands.h" |
14 | #include "flight.h" |
14 | #include "flight.h" |
15 | #include "rc.h" |
15 | #include "rc.h" |
16 | #include "analog.h" |
16 | #include "analog.h" |
17 | #include "configuration.h" |
17 | #include "configuration.h" |
18 | #include "printf_P.h" |
18 | #include "printf_P.h" |
19 | #include "twimaster.h" |
19 | #include "twimaster.h" |
20 | #include "controlMixer.h" |
20 | #include "controlMixer.h" |
21 | #ifdef USE_NAVICTRL |
21 | #ifdef USE_NAVICTRL |
22 | #include "spi.h" |
22 | #include "spi.h" |
23 | #endif |
23 | #endif |
24 | #ifdef USE_MK3MAG |
24 | #ifdef USE_MK3MAG |
25 | #include "mk3mag.h" |
25 | #include "mk3mag.h" |
26 | #endif |
26 | #endif |
27 | #include "eeprom.h" |
27 | #include "eeprom.h" |
28 | 28 | ||
29 | int16_t main(void) { |
29 | int16_t main(void) { |
30 | uint16_t timer; |
30 | uint16_t timer; |
31 | 31 | ||
32 | // disable interrupts global |
32 | // disable interrupts global |
33 | cli(); |
33 | cli(); |
34 | 34 | ||
35 | // analyze hardware environment |
35 | // analyze hardware environment |
36 | setCPUType(); |
36 | setCPUType(); |
37 | setBoardRelease(); |
37 | setBoardRelease(); |
38 | 38 | ||
39 | // disable watchdog |
39 | // disable watchdog |
40 | MCUSR &= ~(1 << WDRF); |
40 | MCUSR &= ~(1 << WDRF); |
41 | WDTCSR |= (1 << WDCE) | (1 << WDE); |
41 | WDTCSR |= (1 << WDCE) | (1 << WDE); |
42 | WDTCSR = 0; |
42 | WDTCSR = 0; |
43 | 43 | ||
44 | // This is strange: It should NOT be necessarty to do. But the call of the same, |
44 | // This is strange: It should NOT be necessarty to do. But the call of the same, |
45 | // in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
45 | // in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
46 | channelMap_default(); |
46 | channelMap_default(); |
47 | 47 | ||
48 | // initalize modules |
48 | // initalize modules |
49 | output_init(); |
49 | output_init(); |
50 | timer0_init(); |
50 | timer0_init(); |
51 | timer2_init(); |
51 | timer2_init(); |
52 | usart0_init(); |
52 | usart0_init(); |
53 | //if (CPUType == ATMEGA644P);// usart1_Init(); |
53 | //if (CPUType == ATMEGA644P);// usart1_Init(); |
54 | RC_Init(); |
54 | RC_Init(); |
55 | analog_init(); |
55 | analog_init(); |
56 | I2C_init(); |
56 | I2C_init(); |
57 | #ifdef USE_NAVICTRL |
57 | #ifdef USE_NAVICTRL |
58 | SPI_MasterInit(); |
58 | SPI_MasterInit(); |
59 | #endif |
59 | #endif |
60 | #ifdef USE_MK3MAG |
60 | #ifdef USE_MK3MAG |
61 | MK3MAG_init(); |
61 | MK3MAG_init(); |
62 | #endif |
62 | #endif |
63 | #ifdef USE_DIRECT_GPS |
63 | #ifdef USE_DIRECT_GPS |
64 | usart1_init(); |
64 | usart1_init(); |
65 | #endif |
65 | #endif |
66 | 66 | ||
67 | // Parameter Set handling |
67 | // Parameter Set handling |
68 | IMUConfig_readOrDefault(); |
68 | IMUConfig_readOrDefault(); |
69 | channelMap_readOrDefault(); |
69 | channelMap_readOrDefault(); |
70 | mixerMatrix_readOrDefault(); |
70 | motorMixer_readOrDefault(); |
71 | paramSet_readOrDefault(); |
71 | paramSet_readOrDefault(); |
72 | 72 | ||
73 | // enable interrupts global |
73 | // enable interrupts global |
74 | sei(); |
74 | sei(); |
75 | 75 | ||
76 | printf("\n\r==================================="); |
76 | printf("\n\r==================================="); |
77 | printf("\n\rFlightControl"); |
77 | printf("\n\rFlightControl"); |
78 | printf("\n\rHardware: Custom"); |
78 | printf("\n\rHardware: Custom"); |
79 | printf("\n\r CPU: Atmega644"); |
79 | printf("\n\r CPU: Atmega644"); |
80 | if (CPUType == ATMEGA644P) |
80 | if (CPUType == ATMEGA644P) |
81 | printf("p"); |
81 | printf("p"); |
82 | printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
82 | printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
83 | printf("\n\r==================================="); |
83 | printf("\n\r==================================="); |
84 | 84 | ||
85 | // Wait for a short time (otherwise the RC channel check won't work below) |
85 | // Wait for a short time (otherwise the RC channel check won't work below) |
86 | // timer = SetDelay(500); |
86 | // timer = SetDelay(500); |
87 | // while(!CheckDelay(timer)); |
87 | // while(!CheckDelay(timer)); |
88 | 88 | ||
89 | // Instead, while away the time by flashing the 2 outputs: |
89 | // Instead, while away the time by flashing the 2 outputs: |
90 | // First J16, then J17. Makes it easier to see which is which. |
90 | // First J16, then J17. Makes it easier to see which is which. |
91 | timer = setDelay(200); |
91 | timer = setDelay(200); |
92 | outputSet(0,1); |
92 | outputSet(0,1); |
93 | GRN_OFF; |
93 | GRN_OFF; |
94 | RED_ON; |
94 | RED_ON; |
95 | while (!checkDelay(timer)) |
95 | while (!checkDelay(timer)) |
96 | ; |
96 | ; |
97 | 97 | ||
98 | timer = setDelay(200); |
98 | timer = setDelay(200); |
99 | outputSet(0,0); |
99 | outputSet(0,0); |
100 | outputSet(1,1); |
100 | outputSet(1,1); |
101 | RED_OFF; |
101 | RED_OFF; |
102 | GRN_ON; |
102 | GRN_ON; |
103 | while (!checkDelay(timer)) |
103 | while (!checkDelay(timer)) |
104 | ; |
104 | ; |
105 | 105 | ||
106 | timer = setDelay(200); |
106 | timer = setDelay(200); |
107 | while (!checkDelay(timer)) |
107 | while (!checkDelay(timer)) |
108 | ; |
108 | ; |
109 | outputSet(1,0); |
109 | outputSet(1,0); |
110 | GRN_OFF; |
110 | GRN_OFF; |
111 | 111 | ||
112 | twi_diagnostics(); |
112 | twi_diagnostics(); |
113 | 113 | ||
114 | printf("\n\r==================================="); |
114 | printf("\n\r==================================="); |
115 | 115 | ||
116 | #ifdef USE_NAVICTRL |
116 | #ifdef USE_NAVICTRL |
117 | printf("\n\rSupport for NaviCtrl"); |
117 | printf("\n\rSupport for NaviCtrl"); |
118 | #endif |
118 | #endif |
119 | 119 | ||
120 | #ifdef USE_DIRECT_GPS |
120 | #ifdef USE_DIRECT_GPS |
121 | printf("\n\rDirect (no NaviCtrl) navigation"); |
121 | printf("\n\rDirect (no NaviCtrl) navigation"); |
122 | #endif |
122 | #endif |
123 | 123 | ||
124 | controlMixer_setNeutral(); |
124 | controlMixer_setNeutral(); |
125 | 125 | ||
126 | // Cal. attitude sensors and reset integrals. |
126 | // Cal. attitude sensors and reset integrals. |
127 | attitude_setNeutral(); |
127 | attitude_setNeutral(); |
128 | 128 | ||
129 | // Init flight parameters |
129 | // Init flight parameters |
130 | // flight_setNeutral(); |
130 | // flight_setNeutral(); |
131 | 131 | ||
132 | beep(2000); |
132 | beep(2000); |
133 | 133 | ||
134 | printf("\n\rControl: "); |
134 | printf("\n\rControl: "); |
135 | if (staticParams.bitConfig & CFG_HEADING_HOLD) |
135 | if (staticParams.bitConfig & CFG_HEADING_HOLD) |
136 | printf("Heading Hold"); |
136 | printf("Heading Hold"); |
137 | else printf("RTL Mode"); |
137 | else printf("RTL Mode"); |
138 | 138 | ||
139 | printf("\n\n\r"); |
139 | printf("\n\n\r"); |
140 | 140 | ||
141 | LCD_clear(); |
141 | LCD_clear(); |
142 | 142 | ||
143 | I2CTimeout = 5000; |
143 | I2CTimeout = 5000; |
144 | 144 | ||
145 | while (1) { |
145 | while (1) { |
146 | if (runFlightControl) { // control interval |
146 | if (runFlightControl) { // control interval |
147 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
147 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
148 | if (!analogDataReady) { |
148 | if (!analogDataReady) { |
149 | // Analog data should have been ready but is not!! |
149 | // Analog data should have been ready but is not!! |
150 | debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
150 | debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
151 | } else { |
151 | } else { |
152 | debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
152 | debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
153 | 153 | ||
154 | J4HIGH; |
154 | J4HIGH; |
155 | // This is probably the correct order: |
155 | // This is probably the correct order: |
156 | // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
156 | // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
157 | // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
157 | // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
158 | // Flight control uses results from both. |
158 | // Flight control uses results from both. |
159 | calculateFlightAttitude(); |
159 | calculateFlightAttitude(); |
160 | controlMixer_periodicTask(); |
160 | controlMixer_periodicTask(); |
161 | commands_handleCommands(); |
161 | commands_handleCommands(); |
162 | flight_control(); |
162 | flight_control(); |
163 | J4LOW; |
163 | J4LOW; |
164 | 164 | ||
165 | if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout |
165 | if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout |
166 | if (!I2CTimeout) { |
166 | if (!I2CTimeout) { |
167 | I2C_Reset(); |
167 | I2C_Reset(); |
168 | I2CTimeout = 5; |
168 | I2CTimeout = 5; |
169 | } |
169 | } |
170 | beepI2CAlarm(); |
170 | beepI2CAlarm(); |
171 | } |
171 | } |
172 | 172 | ||
173 | // Allow serial data transmission if there is still time, or if we are not flying anyway. |
173 | // Allow serial data transmission if there is still time, or if we are not flying anyway. |
174 | if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
174 | if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
175 | usart0_transmitTxData(); |
175 | usart0_transmitTxData(); |
176 | } |
176 | } |
177 | 177 | ||
178 | usart0_processRxData(); |
178 | usart0_processRxData(); |
179 | 179 | ||
180 | if (checkDelay(timer)) { |
180 | if (checkDelay(timer)) { |
181 | if (UBat <= UBAT_AT_5V) { |
181 | if (UBat <= UBAT_AT_5V) { |
182 | // Do nothing. The voltage on the input side of the regulator is <5V; |
182 | // Do nothing. The voltage on the input side of the regulator is <5V; |
183 | // we must be running off USB power. Keep it quiet. |
183 | // we must be running off USB power. Keep it quiet. |
184 | } else if (UBat < staticParams.batteryVoltageWarning) { |
184 | } else if (UBat < staticParams.batteryVoltageWarning) { |
185 | beepBatteryAlarm(); |
185 | beepBatteryAlarm(); |
186 | } |
186 | } |
187 | 187 | ||
188 | #ifdef USE_NAVICTRL |
188 | #ifdef USE_NAVICTRL |
189 | SPI_StartTransmitPacket(); |
189 | SPI_StartTransmitPacket(); |
190 | SendSPI = 4; |
190 | SendSPI = 4; |
191 | #endif |
191 | #endif |
192 | timer = setDelay(20); // every 20 ms |
192 | timer = setDelay(20); // every 20 ms |
193 | } |
193 | } |
194 | output_update(); |
194 | output_update(); |
195 | } |
195 | } |
196 | 196 | ||
197 | #ifdef USE_NAVICTRL |
197 | #ifdef USE_NAVICTRL |
198 | if(!SendSPI) { |
198 | if(!SendSPI) { |
199 | // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
199 | // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
200 | // within the SPI_TransmitByte() routine the value is set to 4. |
200 | // within the SPI_TransmitByte() routine the value is set to 4. |
201 | // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
201 | // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
202 | // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
202 | // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
203 | SPI_TransmitByte(); |
203 | SPI_TransmitByte(); |
204 | } |
204 | } |
205 | #endif |
205 | #endif |
206 | 206 | ||
207 | calculateServoValues(); |
207 | calculateServoValues(); |
208 | 208 | ||
209 | if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
209 | if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
210 | debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
210 | debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
211 | } else { |
211 | } else { |
212 | debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
212 | debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
213 | } |
213 | } |
214 | } |
214 | } |
215 | } |
215 | } |
216 | return (1); |
216 | return (1); |
217 | } |
217 | } |
218 | 218 |