From ef0e37b478fcb344824419055ccb5ee7ddd0d3f3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Sep 2014 12:20:40 +0900 Subject: [PATCH] Copter: bugfix to condition-yaw for relative angles Thanks to roque-canales for raising the issue and paradisephil for finding the specific piece of code that went wrong and suggesting the fix. --- ArduCopter/GCS_Mavlink.pde | 4 ++-- ArduCopter/commands_logic.pde | 1 + ArduCopter/control_auto.pde | 7 +++++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index bfe017f269..c32962d52e 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1078,12 +1078,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] // param2 : speed during change [deg per second] - // param3 : direction (not supported) + // param3 : direction (-1:ccw, +1:cw) // param4 : relative offset (1) or absolute angle (0) if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && ((packet.param4 == 0) || (packet.param4 == 1))) { - set_auto_yaw_look_at_heading(packet.param1, packet.param2, (uint8_t)packet.param4); + set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 7559d71904..9e0421d933 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -812,6 +812,7 @@ static void do_yaw(const AP_Mission::Mission_Command& cmd) set_auto_yaw_look_at_heading( cmd.content.yaw.angle_deg, cmd.content.yaw.turn_rate_dps, + cmd.content.yaw.direction, cmd.content.yaw.relative_angle); } diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 1a7a55f437..ce7e744b86 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -475,7 +475,7 @@ void set_auto_yaw_mode(uint8_t yaw_mode) } // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, uint8_t relative_angle) +static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target int32_t curr_yaw_target = attitude_control.angle_ef_targets().z; @@ -486,7 +486,10 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, u yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle - yaw_look_at_heading = wrap_360_cd(angle_deg * 100); + if (direction < 0) { + angle_deg = -angle_deg; + } + yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); } // get turn speed