ArduCopter: bug fix to LAND flight mode not actually landing if initiated from failsafe

An hidden bit of failsafe functionality in the set_mode function was
switching the throttle mode back to THROTTLE_AUTO instead of
THROTTLE_LAND
This commit is contained in:
Randy Mackay 2013-01-12 14:48:41 +09:00 committed by Andrew Tridgell
parent 175f69a64f
commit 08dc730d40

View File

@ -383,8 +383,8 @@ static void set_mode(uint8_t mode)
// used to stop fly_aways
// set to false if we have low throttle
motors.auto_armed(g.rc_3.control_in > 0);
set_auto_armed(g.rc_3.control_in > 0);
motors.auto_armed(g.rc_3.control_in > 0 || ap.failsafe);
set_auto_armed(g.rc_3.control_in > 0 || ap.failsafe);
// if we change modes, we must clear landed flag
set_land_complete(false);
@ -545,17 +545,6 @@ static void set_mode(uint8_t mode)
break;
}
if(ap.failsafe) {
// this is to allow us to fly home without interactive throttle control
set_throttle_mode(THROTTLE_AUTO);
ap.manual_throttle = false;
// does not wait for us to be in high throttle, since the
// Receiver will be outputting low throttle
motors.auto_armed(true);
set_auto_armed(true);
}
if(ap.manual_attitude) {
// We are under manual attitude control
// remove the navigation from roll and pitch command