From c53a0fcfd91bbc4bfb8c3c08ec022f260dc1ecd8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 17 Apr 2014 16:14:18 +0900 Subject: [PATCH] Copter: conditional yaw fix waypoint command was setting auto_yaw_mode when it was run after the do-cmd yaw_look_at_heading was being set to current heading which was overwriting the caller's desired heading --- ArduCopter/commands_logic.pde | 8 +++++++- ArduCopter/control_auto.pde | 4 ++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 5d2345643f..56ddac5f7a 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -755,7 +755,13 @@ static bool verify_within_distance() // verify_yaw - return true if we have reached the desired heading static bool verify_yaw() { - if( labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200 ) { + // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) + if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { + set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + } + + // check if we are within 2 degrees of the target heading + if (labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200) { return true; }else{ return false; diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index e996189846..4fa80988ca 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -403,8 +403,8 @@ void set_auto_yaw_mode(uint8_t yaw_mode) break; case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed - yaw_look_at_heading = ahrs.yaw_sensor; + // keep heading pointing in the direction held in yaw_look_at_heading + // caller should set the yaw_look_at_heading break; case AUTO_YAW_LOOK_AHEAD: