From 56ceb230cb462a6e532472e99c44fc0f298f98e7 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Sun, 9 Dec 2012 18:04:31 +0900
Subject: [PATCH] ArduCopter: add YAW_OVR_BEHAVE to allow control of when
 autopilot takes back control of yaw after pilot overrides it during a mission

---
 ArduCopter/Parameters.h       | 2 ++
 ArduCopter/Parameters.pde     | 7 +++++++
 ArduCopter/commands_logic.pde | 5 +++++
 ArduCopter/defines.h          | 4 ++++
 4 files changed, 18 insertions(+)

diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 4bc6585a03..3eb19b7104 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -72,6 +72,7 @@ public:
         k_param_crosstrack_min_distance,
         k_param_rssi_pin,
         k_param_throttle_accel_enabled,
+        k_param_yaw_override_behaviour,
 
         // 65: AP_Limits Library
         k_param_limits = 65,
@@ -271,6 +272,7 @@ public:
     AP_Int8         battery_curr_pin;
     AP_Int8         rssi_pin;
     AP_Int8         throttle_accel_enabled;      // enable/disable accel based throttle controller
+    AP_Int8         yaw_override_behaviour;     // controls when autopilot takes back normal control of yaw after pilot overrides
 
     // Waypoints
     //
diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index 18427db378..920b59ed7b 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -151,6 +151,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(throttle_accel_enabled,  "THR_ACC_ENABLE",   1),
 
+    // @Param: YAW_OVR_BEHAVE
+    // @DisplayName: Yaw override behaviour
+    // @Description: Controls when autopilot takes back normal control of yaw after pilot overrides
+    // @Values: 0:At Next WP, 1:On Mission Restart
+    // @User: Advanced
+    GSCALAR(yaw_override_behaviour,  "YAW_OVR_BEHAVE",   YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT),
+
     GSCALAR(waypoint_mode,  "WP_MODE",          0),
     GSCALAR(command_total,  "WP_TOTAL",         0),
     GSCALAR(command_index,  "WP_INDEX",         0),
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 7a6cd31beb..2bea8eff85 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -284,6 +284,11 @@ static void do_nav_wp()
     if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false) {
         wp_verify_byte |= NAV_ALTITUDE;
     }
+
+    // reset control of yaw
+    if( g.yaw_override_behaviour == YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT ) {
+        set_yaw_mode(YAW_LOOK_AT_NEXT_WP);
+    }
 }
 
 // do_land - initiate landing procedure
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 1816763f26..70fb00a30e 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -192,6 +192,10 @@
 #define CIRCLE_MODE 3
 #define NO_NAV_MODE 4
 
+// Yaw override behaviours - used for setting yaw_override_behaviour
+#define YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT     0   // auto pilot takes back yaw control at next waypoint
+#define YAW_OVERRIDE_BEHAVIOUR_AT_MISSION_RESTART   1   // auto pilot tkaes back control only when mission is restarted
+
 // TOY mixing options
 #define TOY_LOOKUP_TABLE 0
 #define TOY_LINEAR_MIXER 1