From 9e4628af587558ef9ead4d96292dd426cea6d897 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 14 Nov 2016 13:46:47 +1100
Subject: [PATCH] Copter: precision loiter

---
 ArduCopter/Copter.h           |  2 ++
 ArduCopter/control_loiter.cpp | 39 +++++++++++++++++++++++++++++++++++
 2 files changed, 41 insertions(+)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index efa65a675e..6f1dae8e83 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -883,6 +883,8 @@ private:
     bool landing_with_GPS();
     bool loiter_init(bool ignore_checks);
     void loiter_run();
+    bool do_precision_loiter() const;
+    void precision_loiter_xy();
     bool poshold_init(bool ignore_checks);
     void poshold_run();
     void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp
index 2bbff1adaf..d1448afd1b 100644
--- a/ArduCopter/control_loiter.cpp
+++ b/ArduCopter/control_loiter.cpp
@@ -35,6 +35,41 @@ bool Copter::loiter_init(bool ignore_checks)
     }
 }
 
+bool Copter::do_precision_loiter() const
+{
+#if PRECISION_LANDING != ENABLED
+    return false;
+#else
+    if (ap.land_complete_maybe) {
+        return false;        // don't move on the ground
+    }
+    // if the pilot *really* wants to move the vehicle, let them....
+    if (wp_nav.get_pilot_desired_acceleration().length() > 50.0f) {
+        return false;
+    }
+    if (!precland.target_acquired()) {
+        return false; // we don't have a good vector
+    }
+    return true;
+#endif
+}
+
+void Copter::precision_loiter_xy()
+{
+    wp_nav.clear_pilot_desired_acceleration();
+    Vector2f target_pos, target_vel_rel;
+    if (!precland.get_target_position_cm(target_pos)) {
+        target_pos.x = inertial_nav.get_position().x;
+        target_pos.y = inertial_nav.get_position().y;
+    }
+    if (!precland.get_target_velocity_relative_cms(target_vel_rel)) {
+        target_vel_rel.x = -inertial_nav.get_velocity().x;
+        target_vel_rel.y = -inertial_nav.get_velocity().y;
+    }
+    pos_control.set_xy_target(target_pos.x, target_pos.y);
+    pos_control.override_vehicle_velocity_xy(-target_vel_rel);
+}
+
 // loiter_run - runs the loiter controller
 // should be called at 100hz or more
 void Copter::loiter_run()
@@ -158,6 +193,10 @@ void Copter::loiter_run()
         // set motors to full range
         motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
+        if (do_precision_loiter()) {
+            precision_loiter_xy();
+        }
+
         // run loiter controller
         wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);