Rev 2158 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2158 | Rev 2189 | ||
---|---|---|---|
Line 1... | Line 1... | ||
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 | #include <stdio.h> |
|
Line 5... | Line 6... | ||
5 | 6 | ||
6 | #include "timer0.h" |
7 | #include "timer0.h" |
7 | #include "timer2.h" |
8 | #include "timer2.h" |
8 | #include "uart0.h" |
9 | #include "uart0.h" |
9 | #include "uart1.h" |
10 | //#include "uart1.h" |
10 | #include "output.h" |
- | |
11 | #include "menu.h" |
11 | #include "output.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 "profiler.h" |
15 | #include "rc.h" |
16 | #include "rc.h" |
16 | #include "analog.h" |
17 | #include "analog.h" |
17 | #include "configuration.h" |
- | |
18 | #include "printf_P.h" |
18 | #include "configuration.h" |
19 | #include "twimaster.h" |
19 | #include "twimaster.h" |
20 | #include "controlMixer.h" |
20 | #include "controlMixer.h" |
21 | #ifdef USE_NAVICTRL |
21 | #include "eeprom.h" |
22 | #include "spi.h" |
- | |
23 | #endif |
22 | #include "beeper.h" |
24 | #ifdef USE_MK3MAG |
23 | #ifdef USE_MK3MAG |
25 | #include "mk3mag.h" |
24 | #include "mk3mag.h" |
26 | #endif |
- | |
Line 27... | Line 25... | ||
27 | #include "eeprom.h" |
25 | #endif |
28 | 26 | ||
Line 29... | Line 27... | ||
29 | int16_t main(void) { |
27 | int16_t main(void) { |
Line 34... | Line 32... | ||
34 | 32 | ||
35 | // analyze hardware environment |
33 | // analyze hardware environment |
36 | setCPUType(); |
34 | setCPUType(); |
Line -... | Line 35... | ||
- | 35 | setBoardRelease(); |
|
- | 36 | ||
37 | setBoardRelease(); |
37 | fdevopen(uart_putchar, NULL); |
38 | 38 | ||
39 | // disable watchdog |
39 | // disable watchdog |
40 | MCUSR &= ~(1 << WDRF); |
40 | MCUSR &= ~(1 << WDRF); |
Line 41... | Line -... | ||
41 | WDTCSR |= (1 << WDCE) | (1 << WDE); |
- | |
42 | WDTCSR = 0; |
- | |
43 | - | ||
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,.... |
41 | WDTCSR |= (1 << WDCE) | (1 << WDE); |
46 | channelMap_default(); |
42 | WDTCSR = 0; |
47 | 43 | ||
48 | // initalize modules |
44 | // initalize modules |
49 | output_init(); |
45 | output_init(); |
50 | timer0_init(); |
46 | timer0_init(); |
51 | timer2_init(); |
47 | // timer2_init(); |
52 | usart0_init(); |
- | |
53 | //if (CPUType == ATMEGA644P);// usart1_Init(); |
48 | usart0_init(); |
54 | RC_Init(); |
- | |
55 | analog_init(); |
49 | //if (CPUType == ATMEGA644P);// usart1_Init(); |
56 | I2C_init(); |
- | |
57 | #ifdef USE_NAVICTRL |
50 | RC_Init(); |
58 | SPI_MasterInit(); |
51 | I2C_init(); |
59 | #endif |
52 | analog_init(); |
60 | #ifdef USE_MK3MAG |
53 | #ifdef USE_MK3MAG |
61 | MK3MAG_init(); |
54 | MK3MAG_init(); |
62 | #endif |
55 | #endif |
Line 63... | Line 56... | ||
63 | #ifdef USE_DIRECT_GPS |
56 | #ifdef USE_DIRECT_GPS |
64 | usart1_init(); |
57 | usart1_init(); |
65 | #endif |
58 | #endif |
66 | 59 | ||
67 | // Parameter Set handling |
60 | // Parameter Set handling |
Line 68... | Line 61... | ||
68 | IMUConfig_readOrDefault(); |
61 | IMUConfig_readOrDefault(); |
69 | channelMap_readOrDefault(); |
62 | channelMap_readOrDefault(); |
Line 77... | Line 70... | ||
77 | printf("\n\rFlightControl"); |
70 | printf("\n\rFlightControl"); |
78 | printf("\n\rHardware: Custom"); |
71 | printf("\n\rHardware: Custom"); |
79 | printf("\n\r CPU: Atmega644"); |
72 | printf("\n\r CPU: Atmega644"); |
80 | if (CPUType == ATMEGA644P) |
73 | if (CPUType == ATMEGA644P) |
81 | printf("p"); |
74 | printf("p"); |
82 | printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
75 | printf("\n\rSoftware: V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, |
- | 76 | VERSION_PATCH + 'a'); |
|
83 | printf("\n\r==================================="); |
77 | printf("\n\r==================================="); |
Line 84... | Line 78... | ||
84 | 78 | ||
85 | // Wait for a short time (otherwise the RC channel check won't work below) |
79 | // Wait for a short time (otherwise the RC channel check won't work below) |
86 | // timer = SetDelay(500); |
80 | // timer = SetDelay(500); |
Line 87... | Line 81... | ||
87 | // while(!CheckDelay(timer)); |
81 | // while(!CheckDelay(timer)); |
88 | 82 | ||
89 | // Instead, while away the time by flashing the 2 outputs: |
83 | // Instead, while away the time by flashing the 2 outputs: |
90 | // First J16, then J17. Makes it easier to see which is which. |
84 | // First J16, then J17. Makes it easier to see which is which. |
91 | timer = setDelay(200); |
85 | timer = setDelay(200); |
92 | outputSet(0,1); |
86 | output_setLED(0, 1); |
93 | GRN_OFF; |
87 | GRN_OFF; |
94 | RED_ON; |
88 | RED_ON; |
Line 95... | Line 89... | ||
95 | while (!checkDelay(timer)) |
89 | while (!checkDelay(timer)) |
96 | ; |
90 | ; |
97 | 91 | ||
98 | timer = setDelay(200); |
92 | timer = setDelay(200); |
99 | outputSet(0,0); |
93 | output_setLED(0, 0); |
100 | outputSet(1,1); |
94 | output_setLED(1, 1); |
101 | RED_OFF; |
95 | RED_OFF; |
Line 102... | Line 96... | ||
102 | GRN_ON; |
96 | GRN_ON; |
103 | while (!checkDelay(timer)) |
97 | while (!checkDelay(timer)) |
104 | ; |
98 | ; |
105 | 99 | ||
106 | timer = setDelay(200); |
100 | timer = setDelay(200); |
Line 107... | Line 101... | ||
107 | while (!checkDelay(timer)) |
101 | while (!checkDelay(timer)) |
Line 108... | Line 102... | ||
108 | ; |
102 | ; |
Line 121... | Line 115... | ||
121 | printf("\n\rDirect (no NaviCtrl) navigation"); |
115 | printf("\n\rDirect (no NaviCtrl) navigation"); |
122 | #endif |
116 | #endif |
Line 123... | Line 117... | ||
123 | 117 | ||
Line -... | Line 118... | ||
- | 118 | controlMixer_setNeutral(); |
|
- | 119 | ||
124 | controlMixer_setNeutral(); |
120 | I2CTimeout = 5000; |
125 | 121 | ||
Line 126... | Line 122... | ||
126 | // Cal. attitude sensors and reset integrals. |
122 | // Cal. attitude sensors and reset integrals. |
127 | attitude_setNeutral(); |
123 | attitude_setNeutral(); |
Line 128... | Line 124... | ||
128 | 124 | ||
129 | // Init flight parameters |
- | |
130 | // flight_setNeutral(); |
- | |
131 | - | ||
132 | beep(2000); |
- | |
133 | - | ||
134 | printf("\n\rControl: "); |
- | |
135 | if (staticParams.bitConfig & CFG_HEADING_HOLD) |
125 | // Init flight parameters |
Line 136... | Line 126... | ||
136 | printf("Heading Hold"); |
126 | // flight_setNeutral(); |
137 | else printf("RTL Mode"); |
- | |
138 | - | ||
Line -... | Line 127... | ||
- | 127 | ||
- | 128 | beep(2000); |
|
- | 129 | printf("\n\n\r"); |
|
- | 130 | ||
139 | printf("\n\n\r"); |
131 | timer2_init(); |
140 | - | ||
141 | LCD_clear(); |
- | |
142 | 132 | ||
143 | I2CTimeout = 5000; |
- | |
144 | - | ||
145 | while (1) { |
- | |
146 | if (runFlightControl) { // control interval |
- | |
Line 147... | Line -... | ||
147 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
- | |
148 | if (!analogDataReady) { |
- | |
149 | // Analog data should have been ready but is not!! |
133 | /* |
150 | debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
- | |
151 | } else { |
- | |
152 | debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
134 | * Main loop updates attitude and does some nonessential tasks |
153 | - | ||
154 | J4HIGH; |
- | |
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) |
- | |
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. |
- | |
159 | calculateFlightAttitude(); |
- | |
160 | controlMixer_periodicTask(); |
- | |
161 | commands_handleCommands(); |
- | |
162 | flight_control(); |
- | |
163 | J4LOW; |
- | |
164 | - | ||
165 | if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout |
- | |
166 | if (!I2CTimeout) { |
- | |
167 | I2C_Reset(); |
- | |
168 | I2CTimeout = 5; |
- | |
169 | } |
- | |
Line 170... | Line -... | ||
170 | beepI2CAlarm(); |
- | |
171 | } |
- | |
172 | - | ||
173 | // Allow serial data transmission if there is still time, or if we are not flying anyway. |
135 | * like beeping alarms and computing servo values. |
174 | if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
136 | */ |
175 | usart0_transmitTxData(); |
137 | while (1) { |
176 | } |
- | |
177 | - | ||
178 | usart0_processRxData(); |
- | |
179 | - | ||
180 | if (checkDelay(timer)) { |
- | |
181 | if (UBat <= UBAT_AT_5V) { |
138 | attitude_update(); |
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. |
- | |
184 | } else if (UBat < staticParams.batteryVoltageWarning) { |
- | |
185 | beepBatteryAlarm(); |
- | |
186 | } |
- | |
187 | - | ||
188 | #ifdef USE_NAVICTRL |
- | |
189 | SPI_StartTransmitPacket(); |
- | |
190 | SendSPI = 4; |
- | |
191 | #endif |
- | |
192 | timer = setDelay(20); // every 20 ms |
- | |
193 | } |
- | |
194 | output_update(); |
- | |
195 | } |
- | |
196 | - | ||
197 | #ifdef USE_NAVICTRL |
- | |
198 | if(!SendSPI) { |
- | |
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. |
- | |
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. |
- | |
203 | SPI_TransmitByte(); |
139 | |
204 | } |
140 | // This is fair to leave here - servo values only need update after a change in attitude anyway. |
205 | #endif |
141 | // calculateServoValues(); |
206 | 142 | ||
207 | calculateServoValues(); |
143 | if (UBat <= UBAT_AT_5V || UBat >= staticParams.batteryVoltageWarning) { |
208 | 144 | // Do nothing. The voltage on the input side of the regulator is <5V; |
|
209 | if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
145 | // we must be running off USB power. Keep it quiet. |