From d7d83303032dedc4998d4f9924a072bc6f387b1d Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Sun, 12 Oct 2014 01:06:51 -0700
Subject: [PATCH] Copter: loiter copter without accepting user input at end of
 auto mission

---
 ArduCopter/commands_logic.pde |  2 +-
 ArduCopter/control_auto.pde   | 44 +++++++++++++++++++++++++++++++++++
 ArduCopter/defines.h          |  3 ++-
 3 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 20adc167ae..8ebd7e8481 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -281,7 +281,7 @@ static void exit_mission()
     // if we are not on the ground switch to loiter or land
     if(!ap.land_complete) {
         // try to enter loiter but if that fails land
-        if (!set_mode(LOITER)) {
+        if(!auto_loiter_start()) {
             set_mode(LAND);
         }
     }else{
diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde
index 10a902e4c3..87a68e3740 100644
--- a/ArduCopter/control_auto.pde
+++ b/ArduCopter/control_auto.pde
@@ -21,6 +21,8 @@
 static bool auto_init(bool ignore_checks)
 {
     if ((GPS_ok() && inertial_nav.position_ok() && mission.num_commands() > 1) || ignore_checks) {
+        auto_mode = Auto_Loiter;
+
         // stop ROI from carrying over from previous runs of the mission
         // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
         if (auto_yaw_mode == AUTO_YAW_ROI) {
@@ -79,6 +81,10 @@ static void auto_run()
         auto_nav_guided_run();
         break;
 #endif
+
+    case Auto_Loiter:
+        auto_loiter_run();
+        break;
     }
 }
 
@@ -415,6 +421,44 @@ void auto_nav_guided_run()
 }
 #endif  // NAV_GUIDED
 
+bool auto_loiter_start()
+{
+    if (!GPS_ok()) {
+        return false;
+    }
+    auto_mode = Auto_Loiter;
+
+    Vector3f origin = inertial_nav.get_position();
+
+    Vector3f stopping_point;
+    pos_control.get_stopping_point_xy(stopping_point);
+    pos_control.get_stopping_point_z(stopping_point);
+
+    wp_nav.set_wp_origin_and_destination(origin, stopping_point);
+
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
+
+    return true;
+}
+
+void auto_loiter_run() {
+    if(!ap.auto_armed || ap.land_complete) {
+        attitude_control.relax_bf_rate_controller();
+        attitude_control.set_yaw_target_to_current_heading();
+        attitude_control.set_throttle_out(0, false);
+        return;
+    }
+
+    float target_yaw_rate = 0;
+    if(!failsafe.radio) {
+        target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+    }
+
+    wp_nav.update_wpnav();
+    pos_control.update_z_controller();
+    attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+}
+
 // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
 // set rtl parameter to true if this is during an RTL
 uint8_t get_default_auto_yaw_mode(bool rtl)
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 4d617fe597..12b74a2f3e 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -179,7 +179,8 @@ enum AutoMode {
     Auto_CircleMoveToEdge,
     Auto_Circle,
     Auto_Spline,
-    Auto_NavGuided
+    Auto_NavGuided,
+    Auto_Loiter
 };
 
 // Guided modes