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