Rev 331 | Rev 339 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 331 | Rev 338 | ||
---|---|---|---|
Line 136... | Line 136... | ||
136 | { |
136 | { |
137 | static s32 no_error_delay = 0; |
137 | static s32 no_error_delay = 0; |
138 | s32 newErrorCode = 0; |
138 | s32 newErrorCode = 0; |
139 | UART_VersionInfo.HardwareError[0] = 0; |
139 | UART_VersionInfo.HardwareError[0] = 0; |
Line 140... | Line 140... | ||
140 | 140 | ||
141 | if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08; |
141 | if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS; |
Line 142... | Line 142... | ||
142 | else DebugOut.Status[1] &= ~0x08; // MK3Mag green status |
142 | else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status |
143 | 143 | ||
- | 144 | if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL; |
|
- | 145 | else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status |
|
- | 146 | ||
Line 144... | Line 147... | ||
144 | if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] |= 0x02; |
147 | if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC; |
145 | else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status |
148 | else DebugOut.StatusRed &= ~AMPEL_NC; |
146 | 149 | ||
147 | if(CheckDelay(SPI0_Timeout)) |
150 | if(CheckDelay(SPI0_Timeout)) |
148 | { |
151 | { |
149 | LED_RED_ON; |
152 | LED_RED_ON; |
150 | sprintf(ErrorMSG,"no FC communication "); |
153 | sprintf(ErrorMSG,"no FC communication "); |
151 | newErrorCode = 3; |
154 | newErrorCode = 3; |
152 | StopNavigation = 1; |
155 | StopNavigation = 1; |
153 | DebugOut.Status[0] &= ~0x01; // status of FC Present |
156 | DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present |
154 | DebugOut.Status[0] &= ~0x02; // status of BL Present |
157 | DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present |
155 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
158 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
156 | } |
159 | } |
Line 162... | Line 165... | ||
162 | I2C1_Deinit(); |
165 | I2C1_Deinit(); |
163 | I2C1_Init(); |
166 | I2C1_Init(); |
164 | newErrorCode = 4; |
167 | newErrorCode = 4; |
165 | StopNavigation = 1; |
168 | StopNavigation = 1; |
166 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
169 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
167 | DebugOut.Status[1] |= 0x08; |
170 | DebugOut.StatusRed |= AMPEL_COMPASS; |
168 | } |
171 | } |
169 | else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
172 | else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
170 | { |
173 | { |
171 | LED_RED_ON; |
174 | LED_RED_ON; |
172 | #ifndef FOLLOW_ME |
175 | #ifndef FOLLOW_ME |
Line 175... | Line 178... | ||
175 | sprintf(ErrorMSG,"! FollowMe only ! "); |
178 | sprintf(ErrorMSG,"! FollowMe only ! "); |
176 | #endif |
179 | #endif |
177 | newErrorCode = 1; |
180 | newErrorCode = 1; |
178 | StopNavigation = 1; |
181 | StopNavigation = 1; |
179 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
182 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
- | 183 | DebugOut.StatusRed |= AMPEL_NC; |
|
180 | } |
184 | } |
Line 181... | Line 185... | ||
181 | 185 | ||
182 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
186 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
183 | { |
187 | { |
Line 288... | Line 292... | ||
288 | else if(ErrorDisturbedEarthMagnetField) |
292 | else if(ErrorDisturbedEarthMagnetField) |
289 | { |
293 | { |
290 | LED_RED_ON; |
294 | LED_RED_ON; |
291 | sprintf(ErrorMSG,"Magnet error "); |
295 | sprintf(ErrorMSG,"Magnet error "); |
292 | newErrorCode = 22; |
296 | newErrorCode = 22; |
- | 297 | DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC; |
|
- | 298 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
|
- | 299 | } |
|
- | 300 | else if(BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
|
- | 301 | { |
|
- | 302 | LED_RED_ON; |
|
- | 303 | sprintf(ErrorMSG,"ERR:Motor restart "); |
|
- | 304 | newErrorCode = 23; |
|
293 | DebugOut.Status[1] |= 0x08; |
305 | DebugOut.StatusRed |= AMPEL_BL; |
- | 306 | } |
|
- | 307 | else if(BL_MinOfMaxPWM != 255 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
|
- | 308 | { |
|
- | 309 | LED_RED_ON; |
|
- | 310 | sprintf(ErrorMSG,"ERR:BL Limitation "); |
|
- | 311 | newErrorCode = 24; |
|
- | 312 | DebugOut.StatusRed |= AMPEL_BL; |
|
294 | } |
313 | } |
295 | else // no error occured |
314 | else // no error occured |
296 | { |
315 | { |
297 | StopNavigation = 0; |
316 | StopNavigation = 0; |
298 | LED_RED_OFF; |
317 | LED_RED_OFF; |
Line 301... | Line 320... | ||
301 | { |
320 | { |
302 | sprintf(ErrorMSG,"No Error "); |
321 | sprintf(ErrorMSG,"No Error "); |
303 | ErrorCode = 0; |
322 | ErrorCode = 0; |
304 | } |
323 | } |
305 | } |
324 | } |
306 | if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04; |
- | |
307 | else DebugOut.Status[1] &= ~0x04; |
- | |
Line 308... | Line 325... | ||
308 | 325 | ||
309 | if(newErrorCode) |
326 | if(newErrorCode) |
310 | { |
327 | { |
311 | if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running |
328 | if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running |
Line 385... | Line 402... | ||
385 | TimerCheckError = SetDelay(3000); |
402 | TimerCheckError = SetDelay(3000); |
386 | UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
403 | UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
387 | UART1_PutString("\n\r Version information:"); |
404 | UART1_PutString("\n\r Version information:"); |
Line 388... | Line 405... | ||
388 | 405 | ||
389 | GetNaviCtrlVersion(); |
406 | GetNaviCtrlVersion(); |
390 | DebugOut.Status[0] = 0x04 | 0x08; // NC and MK3Mag |
407 | DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag |
Line 391... | Line 408... | ||
391 | DebugOut.Status[1] = 0x00; |
408 | DebugOut.StatusRed = 0x00; |
Line 392... | Line 409... | ||
392 | 409 | ||
393 | Compass_Init(); |
410 | Compass_Init(); |