From 38ff3816208e438fd8aa244fa10c2fdd6dc7a2be Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 28 Jul 2012 23:05:05 +0900 Subject: [PATCH] ArduCopter: reset target yaw when throttle is zero (except if failsafe has been triggered) --- ArduCopter/ArduCopter.pde | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ade0596af1..11132ec2b3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1589,7 +1589,9 @@ void update_yaw_mode(void) nav_yaw = ahrs.yaw_sensor; } }else{ - if(motors.armed() == false) + // reset target yaw to current yaw if the motors are disarmed or throttle is zero + // Note: we do not want to reset yaw if failsafe has been triggered even though throttle maybe zero (in fact, normally throttle is zero in failsafe) + if(motors.armed() == false || (g.rc_3.control_in == 0 && !failsafe) ) nav_yaw = ahrs.yaw_sensor; g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);