Copter: CPU failsafe logs an error

This commit is contained in:
Jonathan Challinger 2014-10-29 21:23:09 -07:00 committed by Randy Mackay
parent 31087e4f20
commit 80ba40d149
2 changed files with 8 additions and 2 deletions

View File

@ -328,6 +328,7 @@ enum FlipState {
#define ERROR_SUBSYSTEM_EKFINAV_CHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17
#define ERROR_SUBSYSTEM_BARO 18
#define ERROR_SUBSYSTEM_CPU 19
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1

View File

@ -39,15 +39,20 @@ void failsafe_check()
// the main loop is running, all is OK
failsafe_last_mainLoop_count = mainLoop_count;
failsafe_last_timestamp = tnow;
in_failsafe = false;
if (in_failsafe) {
in_failsafe = false;
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_RESOLVED);
}
return;
}
if (failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
// motors are running but we have gone 2 second since the
// main loop ran. That means we're in trouble and should
// disarm the motors.
in_failsafe = true;
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_OCCURRED);
}
if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {