Copter: Use new control methods for prec loiter

ArduCopter/mode.h

@@ -1079,6 +1079,7 @@ private:
 
 #if PRECISION_LANDING == ENABLED
     bool _precision_loiter_enabled;
+    bool _precision_loiter_active; // true if user has switched on prec loiter
 #endif
 
 };

ArduCopter/mode_loiter.cpp

@@ -34,6 +34,10 @@ bool ModeLoiter::init(bool ignore_checks)
     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
     pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
 
+#if PRECISION_LANDING == ENABLED
+    _precision_loiter_active = false;
+#endif
+
     return true;
 }
 
@@ -59,15 +63,19 @@ bool ModeLoiter::do_precision_loiter()
 void ModeLoiter::precision_loiter_xy()
 {
     loiter_nav->clear_pilot_desired_acceleration();
-    Vector2f target_pos, target_vel_rel;
+    Vector2f target_pos, target_vel;
     if (!copter.precland.get_target_position_cm(target_pos)) {
         target_pos = inertial_nav.get_position_xy_cm();
     }
-    if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
-        target_vel_rel = -inertial_nav.get_velocity_xy_cms();
-    }
-    pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y);
-    pos_control->override_vehicle_velocity_xy(-target_vel_rel);
+    // get the velocity of the target
+    copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel);
+
+    Vector2f zero;
+    Vector2p landing_pos = target_pos.topostype();
+    // target vel will remain zero if landing target is stationary
+    pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero);
+    // run pos controller
+    pos_control->update_xy_controller();
 }
 #endif
 
@@ -158,13 +166,24 @@ void ModeLoiter::run()
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
 #if PRECISION_LANDING == ENABLED
+        bool precision_loiter_old_state = _precision_loiter_active;
         if (do_precision_loiter()) {
             precision_loiter_xy();
+            _precision_loiter_active = true;
+        } else {
+            _precision_loiter_active = false;
         }
-#endif
-
-        // run loiter controller
+        if (precision_loiter_old_state && !_precision_loiter_active) {
+            // prec loiter was active, not any more, let's init again as user takes control
+            loiter_nav->init_target();
+        }
+        // run loiter controller if we are not doing prec loiter
+        if (!_precision_loiter_active) {
+            loiter_nav->update();
+        }
+#else
         loiter_nav->update();
+#endif
 
         // call attitude controller
         attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);