diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 749944cc86..d46eee375d 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -61,6 +61,11 @@ static void stabilize_run() angle_target.x = target_roll; angle_target.y = target_pitch; + // get pilot's desired yaw rate + if (!failsafe.radio && !ap.land_complete) { + rate_stab_ef_target.z = get_pilot_desired_yaw_rate(g.rc_4.control_in); + } + // set target heading to current heading while landed if (ap.land_complete) { angle_target.z = ahrs.yaw_sensor; @@ -73,11 +78,6 @@ static void stabilize_run() attitude_control.angle_to_rate_ef_roll(); attitude_control.angle_to_rate_ef_pitch(); - // get pilot's desired yaw rate - if (!failsafe.radio && !ap.land_complete) { - rate_stab_ef_target.z = get_pilot_desired_yaw_rate(g.rc_4.control_in); - } - // set earth-frame rate stabilize target for yaw with pilot's desired yaw // To-Do: this is quite wasteful to update the entire target vector when only yaw is used attitude_control.rate_stab_ef_targets(rate_stab_ef_target);