Rev 254 | Rev 256 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 254 | Rev 255 | ||
---|---|---|---|
Line 87... | Line 87... | ||
87 | u8 ErrorCode = 0; |
87 | u8 ErrorCode = 0; |
88 | u16 BeepTime; |
88 | u16 BeepTime; |
89 | u8 NCFlags = 0; |
89 | u8 NCFlags = 0; |
90 | s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
90 | s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
Line 91... | Line 91... | ||
91 | 91 | ||
92 | u8 ClearFCFlags = 0; |
92 | u8 ClearFCStatusFlags = 0; |
93 | u8 StopNavigation = 0; |
93 | u8 StopNavigation = 0; |
94 | Param_t Parameter; |
94 | Param_t Parameter; |
Line 95... | Line 95... | ||
95 | volatile FC_t FC; |
95 | volatile FC_t FC; |
Line 127... | Line 127... | ||
127 | sprintf(msg,"\n\r NaviCtrl V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH); |
127 | sprintf(msg,"\n\r NaviCtrl V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH); |
128 | UART1_PutString(msg); |
128 | UART1_PutString(msg); |
129 | } |
129 | } |
Line 130... | Line 130... | ||
130 | 130 | ||
131 | //---------------------------------------------------------------------------------------------------- |
- | |
132 | #define DEFEKT_G_NICK 0x01 |
- | |
133 | #define DEFEKT_G_ROLL 0x02 |
- | |
134 | #define DEFEKT_G_GIER 0x04 |
- | |
135 | #define DEFEKT_A_NICK 0x08 |
- | |
136 | #define DEFEKT_A_ROLL 0x10 |
- | |
137 | #define DEFEKT_A_Z 0x20 |
- | |
138 | #define DEFEKT_PRESSURE 0x40 |
- | |
139 | #define DEFEKT_CAREFREE_ERR 0x80 |
- | |
140 | - | ||
141 | #define DEFEKT_I2C 0x01 |
- | |
142 | #define DEFEKT_BL_MISSING 0x02 |
- | |
143 | #define DEFEKT_SPI_RX_ERR 0x04 |
- | |
144 | #define DEFEKT_PPM_ERR 0x08 |
- | |
145 | #define DEFEKT_MIXER_ERR 0x10 |
- | |
146 | //---------------------------------------------------------------------------------------------------- |
131 | //---------------------------------------------------------------------------------------------------- |
147 | #define ERROR_FC_COMMUNICATION 0x01 |
132 | #define ERROR_FC_COMMUNICATION 0x01 |
148 | #define ERROR_MK3_COMMUNICATION 0x02 |
133 | #define ERROR_MK3_COMMUNICATION 0x02 |
149 | #define ERROR_FC_INCOMPATIBLE 0x04 |
134 | #define ERROR_FC_INCOMPATIBLE 0x04 |
150 | #define ERROR_MK3_INCOMPATIBLE 0x08 |
135 | #define ERROR_MK3_INCOMPATIBLE 0x08 |
Line 157... | Line 142... | ||
157 | UART_VersionInfo.HardwareError[0] = 0; |
142 | UART_VersionInfo.HardwareError[0] = 0; |
Line 158... | Line 143... | ||
158 | 143 | ||
159 | if(/*(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) ||*/ CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08; |
144 | if(/*(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) ||*/ CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08; |
Line 160... | Line 145... | ||
160 | else DebugOut.Status[1] &= ~0x08; // MK3Mag green status |
145 | else DebugOut.Status[1] &= ~0x08; // MK3Mag green status |
161 | 146 | ||
Line 162... | Line 147... | ||
162 | if((FC_ErrorCode[1] & DEFEKT_I2C) || (FC_ErrorCode[1] & DEFEKT_BL_MISSING)) DebugOut.Status[1] |= 0x02; |
147 | if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] |= 0x02; |
163 | else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status |
148 | else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status |
164 | 149 | ||
Line 191... | Line 176... | ||
191 | ErrorCode = 1; |
176 | ErrorCode = 1; |
192 | StopNavigation = 1; |
177 | StopNavigation = 1; |
193 | UART_VersionInfo.HardwareError[0] |= ERROR_FC_INCOMPATIBLE; |
178 | UART_VersionInfo.HardwareError[0] |= ERROR_FC_INCOMPATIBLE; |
194 | } |
179 | } |
Line 195... | Line 180... | ||
195 | 180 | ||
196 | else if(FC_ErrorCode[0] & DEFEKT_G_NICK) |
181 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
197 | { |
182 | { |
198 | LED_RED_ON; |
183 | LED_RED_ON; |
199 | sprintf(ErrorMSG,"ERR: FC Nick Gyro"); |
184 | sprintf(ErrorMSG,"ERR: FC Nick Gyro"); |
200 | ErrorCode = 10; |
185 | ErrorCode = 10; |
201 | } |
186 | } |
202 | else if(FC_ErrorCode[0] & DEFEKT_G_ROLL) |
187 | else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL) |
203 | { |
188 | { |
204 | LED_RED_ON; |
189 | LED_RED_ON; |
205 | sprintf(ErrorMSG,"ERR: FC Roll Gyro"); |
190 | sprintf(ErrorMSG,"ERR: FC Roll Gyro"); |
206 | ErrorCode = 11; |
191 | ErrorCode = 11; |
207 | } |
192 | } |
208 | else if(FC_ErrorCode[0] & DEFEKT_G_GIER) |
193 | else if(FC.Error[0] & FC_ERROR0_GYRO_YAW) |
209 | { |
194 | { |
210 | LED_RED_ON; |
195 | LED_RED_ON; |
211 | sprintf(ErrorMSG,"ERR: FC Yaw Gyro"); |
196 | sprintf(ErrorMSG,"ERR: FC Yaw Gyro"); |
212 | ErrorCode = 12; |
197 | ErrorCode = 12; |
213 | } |
198 | } |
214 | else if(FC_ErrorCode[0] & DEFEKT_A_NICK) |
199 | else if(FC.Error[0] & FC_ERROR0_ACC_NICK) |
215 | { |
200 | { |
216 | LED_RED_ON; |
201 | LED_RED_ON; |
217 | sprintf(ErrorMSG,"ERR: FC Nick ACC"); |
202 | sprintf(ErrorMSG,"ERR: FC Nick ACC"); |
218 | ErrorCode = 13; |
203 | ErrorCode = 13; |
219 | } |
204 | } |
220 | else if(FC_ErrorCode[0] & DEFEKT_A_ROLL) |
205 | else if(FC.Error[0] & FC_ERROR0_ACC_ROLL) |
221 | { |
206 | { |
222 | LED_RED_ON; |
207 | LED_RED_ON; |
223 | sprintf(ErrorMSG,"ERR: FC Roll ACC"); |
208 | sprintf(ErrorMSG,"ERR: FC Roll ACC"); |
224 | ErrorCode = 14; |
209 | ErrorCode = 14; |
225 | } |
210 | } |
226 | else if(FC_ErrorCode[0] & DEFEKT_A_Z) |
211 | else if(FC.Error[0] & FC_ERROR0_ACC_TOP) |
227 | { |
212 | { |
228 | LED_RED_ON; |
213 | LED_RED_ON; |
229 | sprintf(ErrorMSG,"ERR: FC Z-ACC"); |
214 | sprintf(ErrorMSG,"ERR: FC Z-ACC"); |
230 | ErrorCode = 15; |
215 | ErrorCode = 15; |
231 | } |
216 | } |
232 | else if(FC_ErrorCode[0] & DEFEKT_PRESSURE) |
217 | else if(FC.Error[0] & FC_ERROR0_PRESSURE) |
233 | { |
218 | { |
234 | LED_RED_ON; |
219 | LED_RED_ON; |
235 | sprintf(ErrorMSG,"ERR: Pressure sensor"); |
220 | sprintf(ErrorMSG,"ERR: Pressure sensor"); |
236 | ErrorCode = 16; |
221 | ErrorCode = 16; |
237 | } |
222 | } |
238 | else if(FC_ErrorCode[1] & DEFEKT_I2C) |
223 | else if(FC.Error[1] & FC_ERROR1_I2C) |
239 | { |
224 | { |
240 | LED_RED_ON; |
225 | LED_RED_ON; |
241 | sprintf(ErrorMSG,"ERR: FC I2C"); |
226 | sprintf(ErrorMSG,"ERR: FC I2C"); |
242 | ErrorCode = 17; |
227 | ErrorCode = 17; |
243 | } |
228 | } |
244 | else if(FC_ErrorCode[1] & DEFEKT_BL_MISSING) |
229 | else if(FC.Error[1] & FC_ERROR1_BL_MISSING) |
245 | { |
230 | { |
246 | LED_RED_ON; |
231 | LED_RED_ON; |
247 | sprintf(ErrorMSG,"ERR: Bl Missing"); |
232 | sprintf(ErrorMSG,"ERR: Bl Missing"); |
248 | ErrorCode = 18; |
233 | ErrorCode = 18; |
249 | } |
234 | } |
250 | else if(FC_ErrorCode[1] & DEFEKT_MIXER_ERR) |
235 | else if(FC.Error[1] & FC_ERROR1_MIXER) |
251 | { |
236 | { |
252 | LED_RED_ON; |
237 | LED_RED_ON; |
253 | sprintf(ErrorMSG,"Mixer Error"); |
238 | sprintf(ErrorMSG,"Mixer Error"); |
254 | ErrorCode = 19; |
239 | ErrorCode = 19; |
Line 275... | Line 260... | ||
275 | sprintf(ErrorMSG,"bad compass value "); |
260 | sprintf(ErrorMSG,"bad compass value "); |
276 | ErrorCode = 6; |
261 | ErrorCode = 6; |
277 | StopNavigation = 1; |
262 | StopNavigation = 1; |
278 | UART_VersionInfo.HardwareError[0] |= ERROR_COMPASS_VALUE; |
263 | UART_VersionInfo.HardwareError[0] |= ERROR_COMPASS_VALUE; |
279 | } |
264 | } |
280 | else if(FC.Flags & FCFLAG_SPI_RX_ERR) |
265 | else if((FC.Error[1] & FC_ERROR1_SPI_RX)) |
281 | { |
266 | { |
282 | LED_RED_ON; |
267 | LED_RED_ON; |
283 | sprintf(ErrorMSG,"FC spi rx error "); |
268 | sprintf(ErrorMSG,"FC spi rx error "); |
284 | ErrorCode = 8; |
269 | ErrorCode = 8; |
285 | StopNavigation = 1; |
270 | StopNavigation = 1; |
286 | } |
271 | } |
287 | else if(FC_ErrorCode[0] & DEFEKT_CAREFREE_ERR) |
272 | else if(FC.Error[0] & FC_ERROR0_CAREFREE) |
288 | { |
273 | { |
289 | LED_RED_ON; |
274 | LED_RED_ON; |
290 | sprintf(ErrorMSG,"FC: Carefree Error"); |
275 | sprintf(ErrorMSG,"FC: Carefree Error"); |
291 | ErrorCode = 20; |
276 | ErrorCode = 20; |
292 | } |
277 | } |
293 | else if(FC.RC_Quality < 100) |
278 | else if(FC.Error[1] & FC_ERROR1_PPM) |
294 | { |
279 | { |
295 | LED_RED_ON; |
280 | LED_RED_ON; |
296 | sprintf(ErrorMSG,"RC Signal lost "); |
281 | sprintf(ErrorMSG,"RC Signal lost "); |
297 | ErrorCode = 7; |
282 | ErrorCode = 7; |
298 | } |
283 | } |
Line 455... | Line 440... | ||
455 | if(CheckDelay(TimerCheckError)) |
440 | if(CheckDelay(TimerCheckError)) |
456 | { |
441 | { |
457 | TimerCheckError = SetDelay(1000); |
442 | TimerCheckError = SetDelay(1000); |
458 | if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected |
443 | if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected |
459 | CheckErrors(); |
444 | CheckErrors(); |
460 | if(FC.Flags & FCFLAG_FLY) NaviData.FlyingTime++; // we want to count the battery-time |
445 | if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time |
461 | // else NaviData.FlyingTime = 0; // not the time per flight |
446 | // else NaviData.FlyingTime = 0; // not the time per flight |
462 | if(SerialLinkOkay) SerialLinkOkay--; |
447 | if(SerialLinkOkay) SerialLinkOkay--; |
463 | if(SerialLinkOkay < 250 - 5) NCFlags |= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication |
448 | if(SerialLinkOkay < 250 - 5) NCFlags |= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication |
464 | else NCFlags &= ~NC_FLAG_NOSERIALLINK; |
449 | else NCFlags &= ~NC_FLAG_NOSERIALLINK; |
465 | if(StopNavigation && (Parameter.NaviGpsModeControl >= 50)) BeepTime = 1000; |
450 | if(StopNavigation && (Parameter.NaviGpsModeControl >= 50)) BeepTime = 1000; |