From 1acd33e290a11acc7734aa281344e878325dd108 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Feb 2014 22:02:59 +0900 Subject: [PATCH] Copter: do_yaw takes yaw target from AttControl --- ArduCopter/commands_logic.pde | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 46bf4b91c9..804fe8ef1c 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -517,13 +517,16 @@ static void do_within_distance() static void do_yaw() { + // get current yaw target + int32_t curr_yaw_target = attitude_control.angle_ef_targets().z; + // get final angle, 1 = Relative, 0 = Absolute if( command_cond_queue.lng == 0 ) { // absolute angle yaw_look_at_heading = wrap_360_cd(command_cond_queue.alt * 100); }else{ // relative angle - yaw_look_at_heading = wrap_360_cd(control_yaw + command_cond_queue.alt * 100); + yaw_look_at_heading = wrap_360_cd(curr_yaw_target + command_cond_queue.alt * 100); } // get turn speed @@ -531,7 +534,7 @@ static void do_yaw() // default to regular auto slew rate yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; }else{ - int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - control_yaw) / 100) / command_cond_queue.lat; + int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / command_cond_queue.lat; yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec }