89,6 → 89,7 |
u16 BeepTime; |
u8 NCFlags = 0; |
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
u8 ErrorGpsFixLost = 0; |
|
u8 ClearFCStatusFlags = 0; |
u8 StopNavigation = 0; |
133,6 → 134,8 |
|
void CheckErrors(void) |
{ |
static no_error_delay = 0; |
s32 newErrorCode = 0; |
UART_VersionInfo.HardwareError[0] = 0; |
|
if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08; |
145,7 → 148,7 |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"no FC communication "); |
ErrorCode = 3; |
newErrorCode = 3; |
StopNavigation = 1; |
DebugOut.Status[0] &= ~0x01; // status of FC Present |
DebugOut.Status[0] &= ~0x02; // status of BL Present |
158,7 → 161,7 |
//Reset I2CBus |
I2C1_Deinit(); |
I2C1_Init(); |
ErrorCode = 4; |
newErrorCode = 4; |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
DebugOut.Status[1] |= 0x08; |
171,7 → 174,7 |
#else |
sprintf(ErrorMSG,"! FollowMe only ! "); |
#endif |
ErrorCode = 1; |
newErrorCode = 1; |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
} |
180,61 → 183,61 |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR: FC Nick Gyro"); |
ErrorCode = 10; |
newErrorCode = 10; |
} |
else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR: FC Roll Gyro"); |
ErrorCode = 11; |
newErrorCode = 11; |
} |
else if(FC.Error[0] & FC_ERROR0_GYRO_YAW) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR: FC Yaw Gyro"); |
ErrorCode = 12; |
newErrorCode = 12; |
} |
else if(FC.Error[0] & FC_ERROR0_ACC_NICK) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR: FC Nick ACC"); |
ErrorCode = 13; |
newErrorCode = 13; |
} |
else if(FC.Error[0] & FC_ERROR0_ACC_ROLL) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR: FC Roll ACC"); |
ErrorCode = 14; |
newErrorCode = 14; |
} |
else if(FC.Error[0] & FC_ERROR0_ACC_TOP) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:FC Z-ACC"); |
ErrorCode = 15; |
newErrorCode = 15; |
} |
else if(FC.Error[0] & FC_ERROR0_PRESSURE) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:Pressure sensor"); |
ErrorCode = 16; |
newErrorCode = 16; |
} |
else if(FC.Error[1] & FC_ERROR1_I2C) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:I2C FC to BL"); |
ErrorCode = 17; |
newErrorCode = 17; |
} |
else if(FC.Error[1] & FC_ERROR1_BL_MISSING) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR: Bl Missing"); |
ErrorCode = 18; |
newErrorCode = 18; |
} |
else if(FC.Error[1] & FC_ERROR1_MIXER) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"Mixer Error"); |
ErrorCode = 19; |
newErrorCode = 19; |
} |
else if(CheckDelay(UBX_Timeout)) |
{ |
244,7 → 247,7 |
{ |
sprintf(ErrorMSG,"no GPS communication "); |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
ErrorCode = 5; |
newErrorCode = 5; |
} |
StopNavigation = 1; |
// UBX_Timeout = SetDelay(500); |
253,7 → 256,7 |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"bad compass value "); |
ErrorCode = 6; |
newErrorCode = 6; |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
} |
261,7 → 264,7 |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"FC spi rx error "); |
ErrorCode = 8; |
newErrorCode = 8; |
StopNavigation = 1; |
} |
else if(FC.Error[0] & FC_ERROR0_CAREFREE) |
268,23 → 271,45 |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"FC: Carefree Error"); |
ErrorCode = 20; |
newErrorCode = 20; |
} |
else if(FC.Error[1] & FC_ERROR1_PPM) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"RC Signal lost "); |
ErrorCode = 7; |
newErrorCode = 7; |
} |
else if(ErrorGpsFixLost) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"GPS Fix lost "); |
newErrorCode = 21; |
} |
else // no error occured |
{ |
sprintf(ErrorMSG,"No Error "); |
ErrorCode = 0; |
StopNavigation = 0; |
LED_RED_OFF; |
if(no_error_delay) { no_error_delay--; } |
else |
{ |
sprintf(ErrorMSG,"No Error "); |
ErrorCode = 0; |
} |
} |
if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04; |
else DebugOut.Status[1] &= ~0x04; |
|
if(newErrorCode) |
{ |
if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 5; // delay the errors if the motors are running |
ErrorCode = newErrorCode; |
} |
FC.Error[0] = 0; |
FC.Error[1] = 0; |
FC.Error[2] = 0; |
FC.Error[3] = 0; |
FC.Error[4] = 0; |
ErrorGpsFixLost = 0; |
} |
|
// the handler will be cyclic called by the timer 1 ISR |