Moved failsafe check to more logical place

This commit is contained in:
Jason Short 2012-01-09 16:55:51 -08:00
parent f240888b49
commit 5d28e97339
1 changed files with 7 additions and 3 deletions

View File

@ -153,12 +153,16 @@ static void throttle_failsafe(uint16_t pwm)
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC);
//
}else if(failsafeCounter == 10) {
// Don't enter Failsafe if we are not armed
if(motor_armed == true)
// home distance is in meters
// This is to prevent accidental RTL
if((motor_armed == true) && (home_distance > 10) && (current_loc.alt > 400)){
SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC);
set_failsafe(true);
}
}else if (failsafeCounter > 10){
failsafeCounter = 11;
}