159,14 → 159,7 |
|
//if(I2C_CompassPort == I2C_EXTERN_0) LED_RED_OFF_T; |
|
if((FCCalibActive || CompassCalState) && FC_Version.Hardware) |
{ |
sprintf(ErrorMSG,"Calibrate... "); |
newErrorCode = 0; |
ErrorCode = 0; |
no_error_delay = 1; |
} |
else if(CheckDelay(I2C1_Timeout) && (I2C_CompassPort == I2C_INTERN_1)) |
if(CheckDelay(I2C1_Timeout) && (I2C_CompassPort == I2C_INTERN_1)) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"no compass communica"); |
192,7 → 185,33 |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
DebugOut.StatusRed |= AMPEL_COMPASS; |
} |
else if(CompassValueErrorCount > 30) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"compass sensor error"); |
newErrorCode = 34; |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
if(I2C_CompassPort == I2C_INTERN_1) |
{ |
I2C1_Deinit(); |
I2C1_Init(); |
} |
else |
{ |
I2C0_Deinit(); |
I2C0_Init(); |
} |
} |
else |
if((FCCalibActive || CompassCalState) && FC_Version.Hardware) |
{ |
sprintf(ErrorMSG,"Calibrate... "); |
newErrorCode = 0; |
ErrorCode = 0; |
no_error_delay = 1; |
} |
else |
if(CheckDelay(SPI0_Timeout)) |
{ |
LED_RED_ON; |
305,14 → 324,6 |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
} |
else if(CompassValueErrorCount > 30) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"compass sensor error"); |
newErrorCode = 34; |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
} |
else if(Compass_Heading < 0) |
{ |
LED_RED_ON; |
460,6 → 471,8 |
UART1_TransmitTxData(); // PC send answer |
UART2_TransmitTxData(); // FC send answer |
// ---------------- Error Check Timing ---------------------------- |
|
//DebugOut.Analog[16] = CompassValueErrorCount; |
if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start |
{ |
if(CheckDelay(TimerCheckError)) |