Rev 327 | Rev 329 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 327 | Rev 328 | ||
---|---|---|---|
Line 87... | Line 87... | ||
87 | u32 TimerCheckError; |
87 | u32 TimerCheckError; |
88 | u8 ErrorCode = 0; |
88 | u8 ErrorCode = 0; |
89 | u16 BeepTime; |
89 | u16 BeepTime; |
90 | u8 NCFlags = 0; |
90 | u8 NCFlags = 0; |
91 | s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
91 | s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
- | 92 | u8 ErrorGpsFixLost = 0; |
|
Line 92... | Line 93... | ||
92 | 93 | ||
93 | u8 ClearFCStatusFlags = 0; |
94 | u8 ClearFCStatusFlags = 0; |
94 | u8 StopNavigation = 0; |
95 | u8 StopNavigation = 0; |
95 | Param_t Parameter; |
96 | Param_t Parameter; |
Line 131... | Line 132... | ||
131 | 132 | ||
Line 132... | Line 133... | ||
132 | //---------------------------------------------------------------------------------------------------- |
133 | //---------------------------------------------------------------------------------------------------- |
133 | 134 | ||
- | 135 | void CheckErrors(void) |
|
- | 136 | { |
|
134 | void CheckErrors(void) |
137 | static no_error_delay = 0; |
Line 135... | Line 138... | ||
135 | { |
138 | s32 newErrorCode = 0; |
136 | UART_VersionInfo.HardwareError[0] = 0; |
139 | UART_VersionInfo.HardwareError[0] = 0; |
Line 143... | Line 146... | ||
143 | 146 | ||
144 | if(CheckDelay(SPI0_Timeout)) |
147 | if(CheckDelay(SPI0_Timeout)) |
145 | { |
148 | { |
146 | LED_RED_ON; |
149 | LED_RED_ON; |
147 | sprintf(ErrorMSG,"no FC communication "); |
150 | sprintf(ErrorMSG,"no FC communication "); |
148 | ErrorCode = 3; |
151 | newErrorCode = 3; |
149 | StopNavigation = 1; |
152 | StopNavigation = 1; |
150 | DebugOut.Status[0] &= ~0x01; // status of FC Present |
153 | DebugOut.Status[0] &= ~0x01; // status of FC Present |
151 | DebugOut.Status[0] &= ~0x02; // status of BL Present |
154 | DebugOut.Status[0] &= ~0x02; // status of BL Present |
152 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
155 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
Line 156... | Line 159... | ||
156 | LED_RED_ON; |
159 | LED_RED_ON; |
157 | sprintf(ErrorMSG,"no compass communication "); |
160 | sprintf(ErrorMSG,"no compass communication "); |
158 | //Reset I2CBus |
161 | //Reset I2CBus |
159 | I2C1_Deinit(); |
162 | I2C1_Deinit(); |
160 | I2C1_Init(); |
163 | I2C1_Init(); |
161 | ErrorCode = 4; |
164 | newErrorCode = 4; |
162 | StopNavigation = 1; |
165 | StopNavigation = 1; |
163 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
166 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
164 | DebugOut.Status[1] |= 0x08; |
167 | DebugOut.Status[1] |= 0x08; |
165 | } |
168 | } |
166 | else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
169 | else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
Line 169... | Line 172... | ||
169 | #ifndef FOLLOW_ME |
172 | #ifndef FOLLOW_ME |
170 | sprintf(ErrorMSG,"FC not compatible "); |
173 | sprintf(ErrorMSG,"FC not compatible "); |
171 | #else |
174 | #else |
172 | sprintf(ErrorMSG,"! FollowMe only ! "); |
175 | sprintf(ErrorMSG,"! FollowMe only ! "); |
173 | #endif |
176 | #endif |
174 | ErrorCode = 1; |
177 | newErrorCode = 1; |
175 | StopNavigation = 1; |
178 | StopNavigation = 1; |
176 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
179 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
177 | } |
180 | } |
Line 178... | Line 181... | ||
178 | 181 | ||
179 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
182 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
180 | { |
183 | { |
181 | LED_RED_ON; |
184 | LED_RED_ON; |
182 | sprintf(ErrorMSG,"ERR: FC Nick Gyro"); |
185 | sprintf(ErrorMSG,"ERR: FC Nick Gyro"); |
183 | ErrorCode = 10; |
186 | newErrorCode = 10; |
184 | } |
187 | } |
185 | else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL) |
188 | else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL) |
186 | { |
189 | { |
187 | LED_RED_ON; |
190 | LED_RED_ON; |
188 | sprintf(ErrorMSG,"ERR: FC Roll Gyro"); |
191 | sprintf(ErrorMSG,"ERR: FC Roll Gyro"); |
189 | ErrorCode = 11; |
192 | newErrorCode = 11; |
190 | } |
193 | } |
191 | else if(FC.Error[0] & FC_ERROR0_GYRO_YAW) |
194 | else if(FC.Error[0] & FC_ERROR0_GYRO_YAW) |
192 | { |
195 | { |
193 | LED_RED_ON; |
196 | LED_RED_ON; |
194 | sprintf(ErrorMSG,"ERR: FC Yaw Gyro"); |
197 | sprintf(ErrorMSG,"ERR: FC Yaw Gyro"); |
195 | ErrorCode = 12; |
198 | newErrorCode = 12; |
196 | } |
199 | } |
197 | else if(FC.Error[0] & FC_ERROR0_ACC_NICK) |
200 | else if(FC.Error[0] & FC_ERROR0_ACC_NICK) |
198 | { |
201 | { |
199 | LED_RED_ON; |
202 | LED_RED_ON; |
200 | sprintf(ErrorMSG,"ERR: FC Nick ACC"); |
203 | sprintf(ErrorMSG,"ERR: FC Nick ACC"); |
201 | ErrorCode = 13; |
204 | newErrorCode = 13; |
202 | } |
205 | } |
203 | else if(FC.Error[0] & FC_ERROR0_ACC_ROLL) |
206 | else if(FC.Error[0] & FC_ERROR0_ACC_ROLL) |
204 | { |
207 | { |
205 | LED_RED_ON; |
208 | LED_RED_ON; |
206 | sprintf(ErrorMSG,"ERR: FC Roll ACC"); |
209 | sprintf(ErrorMSG,"ERR: FC Roll ACC"); |
207 | ErrorCode = 14; |
210 | newErrorCode = 14; |
208 | } |
211 | } |
209 | else if(FC.Error[0] & FC_ERROR0_ACC_TOP) |
212 | else if(FC.Error[0] & FC_ERROR0_ACC_TOP) |
210 | { |
213 | { |
211 | LED_RED_ON; |
214 | LED_RED_ON; |
212 | sprintf(ErrorMSG,"ERR:FC Z-ACC"); |
215 | sprintf(ErrorMSG,"ERR:FC Z-ACC"); |
213 | ErrorCode = 15; |
216 | newErrorCode = 15; |
214 | } |
217 | } |
215 | else if(FC.Error[0] & FC_ERROR0_PRESSURE) |
218 | else if(FC.Error[0] & FC_ERROR0_PRESSURE) |
216 | { |
219 | { |
217 | LED_RED_ON; |
220 | LED_RED_ON; |
218 | sprintf(ErrorMSG,"ERR:Pressure sensor"); |
221 | sprintf(ErrorMSG,"ERR:Pressure sensor"); |
219 | ErrorCode = 16; |
222 | newErrorCode = 16; |
220 | } |
223 | } |
221 | else if(FC.Error[1] & FC_ERROR1_I2C) |
224 | else if(FC.Error[1] & FC_ERROR1_I2C) |
222 | { |
225 | { |
223 | LED_RED_ON; |
226 | LED_RED_ON; |
224 | sprintf(ErrorMSG,"ERR:I2C FC to BL"); |
227 | sprintf(ErrorMSG,"ERR:I2C FC to BL"); |
225 | ErrorCode = 17; |
228 | newErrorCode = 17; |
226 | } |
229 | } |
227 | else if(FC.Error[1] & FC_ERROR1_BL_MISSING) |
230 | else if(FC.Error[1] & FC_ERROR1_BL_MISSING) |
228 | { |
231 | { |
229 | LED_RED_ON; |
232 | LED_RED_ON; |
230 | sprintf(ErrorMSG,"ERR: Bl Missing"); |
233 | sprintf(ErrorMSG,"ERR: Bl Missing"); |
231 | ErrorCode = 18; |
234 | newErrorCode = 18; |
232 | } |
235 | } |
233 | else if(FC.Error[1] & FC_ERROR1_MIXER) |
236 | else if(FC.Error[1] & FC_ERROR1_MIXER) |
234 | { |
237 | { |
235 | LED_RED_ON; |
238 | LED_RED_ON; |
236 | sprintf(ErrorMSG,"Mixer Error"); |
239 | sprintf(ErrorMSG,"Mixer Error"); |
237 | ErrorCode = 19; |
240 | newErrorCode = 19; |
238 | } |
241 | } |
239 | else if(CheckDelay(UBX_Timeout)) |
242 | else if(CheckDelay(UBX_Timeout)) |
240 | { |
243 | { |
241 | LED_RED_ON; |
244 | LED_RED_ON; |
242 | if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected "); |
245 | if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected "); |
243 | else |
246 | else |
244 | { |
247 | { |
245 | sprintf(ErrorMSG,"no GPS communication "); |
248 | sprintf(ErrorMSG,"no GPS communication "); |
246 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
249 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
247 | ErrorCode = 5; |
250 | newErrorCode = 5; |
248 | } |
251 | } |
249 | StopNavigation = 1; |
252 | StopNavigation = 1; |
250 | // UBX_Timeout = SetDelay(500); |
253 | // UBX_Timeout = SetDelay(500); |
251 | } |
254 | } |
252 | else if(Compass_Heading < 0) |
255 | else if(Compass_Heading < 0) |
253 | { |
256 | { |
254 | LED_RED_ON; |
257 | LED_RED_ON; |
255 | sprintf(ErrorMSG,"bad compass value "); |
258 | sprintf(ErrorMSG,"bad compass value "); |
256 | ErrorCode = 6; |
259 | newErrorCode = 6; |
257 | StopNavigation = 1; |
260 | StopNavigation = 1; |
258 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
261 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
259 | } |
262 | } |
260 | else if((FC.Error[1] & FC_ERROR1_SPI_RX)) |
263 | else if((FC.Error[1] & FC_ERROR1_SPI_RX)) |
261 | { |
264 | { |
262 | LED_RED_ON; |
265 | LED_RED_ON; |
263 | sprintf(ErrorMSG,"FC spi rx error "); |
266 | sprintf(ErrorMSG,"FC spi rx error "); |
264 | ErrorCode = 8; |
267 | newErrorCode = 8; |
265 | StopNavigation = 1; |
268 | StopNavigation = 1; |
266 | } |
269 | } |
267 | else if(FC.Error[0] & FC_ERROR0_CAREFREE) |
270 | else if(FC.Error[0] & FC_ERROR0_CAREFREE) |
268 | { |
271 | { |
269 | LED_RED_ON; |
272 | LED_RED_ON; |
270 | sprintf(ErrorMSG,"FC: Carefree Error"); |
273 | sprintf(ErrorMSG,"FC: Carefree Error"); |
271 | ErrorCode = 20; |
274 | newErrorCode = 20; |
272 | } |
275 | } |
273 | else if(FC.Error[1] & FC_ERROR1_PPM) |
276 | else if(FC.Error[1] & FC_ERROR1_PPM) |
274 | { |
277 | { |
275 | LED_RED_ON; |
278 | LED_RED_ON; |
276 | sprintf(ErrorMSG,"RC Signal lost "); |
279 | sprintf(ErrorMSG,"RC Signal lost "); |
- | 280 | newErrorCode = 7; |
|
- | 281 | } |
|
- | 282 | else if(ErrorGpsFixLost) |
|
- | 283 | { |
|
- | 284 | LED_RED_ON; |
|
- | 285 | sprintf(ErrorMSG,"GPS Fix lost "); |
|
277 | ErrorCode = 7; |
286 | newErrorCode = 21; |
278 | } |
287 | } |
279 | else // no error occured |
288 | else // no error occured |
280 | { |
- | |
281 | sprintf(ErrorMSG,"No Error "); |
- | |
282 | ErrorCode = 0; |
289 | { |
283 | StopNavigation = 0; |
290 | StopNavigation = 0; |
- | 291 | LED_RED_OFF; |
|
- | 292 | if(no_error_delay) { no_error_delay--; } |
|
- | 293 | else |
|
- | 294 | { |
|
- | 295 | sprintf(ErrorMSG,"No Error "); |
|
- | 296 | ErrorCode = 0; |
|
284 | LED_RED_OFF; |
297 | } |
285 | } |
298 | } |
286 | if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04; |
299 | if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04; |
- | 300 | else DebugOut.Status[1] &= ~0x04; |
|
- | 301 | ||
- | 302 | if(newErrorCode) |
|
- | 303 | { |
|
- | 304 | if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running |
|
- | 305 | ErrorCode = newErrorCode; |
|
- | 306 | } |
|
- | 307 | FC.Error[0] = 0; |
|
- | 308 | FC.Error[1] = 0; |
|
- | 309 | FC.Error[2] = 0; |
|
- | 310 | FC.Error[3] = 0; |
|
- | 311 | FC.Error[4] = 0; |
|
287 | else DebugOut.Status[1] &= ~0x04; |
312 | ErrorGpsFixLost = 0; |
Line 288... | Line 313... | ||
288 | } |
313 | } |
289 | 314 | ||
290 | // the handler will be cyclic called by the timer 1 ISR |
315 | // the handler will be cyclic called by the timer 1 ISR |