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