Copter: Switch PrecLand to use updated Pos Controller

ArduCopter/Copter.h

@@ -357,6 +357,7 @@ private:
             uint8_t unused3                 : 1; // 25      // was compass_init_location; true when the compass's initial location has been set
             uint8_t unused2                 : 1; // 26      // aux switch rc_override is allowed
             uint8_t armed_with_airmode_switch : 1; // 27      // we armed using a arming switch
+            uint8_t prec_land_active        : 1; // 28      // true if precland is active
         };
         uint32_t value;
     } ap_t;

ArduCopter/mode.cpp

@@ -641,29 +641,47 @@ void Mode::land_run_horizontal_control()
         }
     }
 
+    // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
+    bool doing_precision_landing = false;
 #if PRECISION_LANDING == ENABLED
-    bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired();
+    doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired();
     // run precision landing
     if (doing_precision_landing) {
-        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();
+         // 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
+
+    if (!doing_precision_landing) {
+        if (copter.ap.prec_land_active) {
+            // precland isn't active anymore but was active a while back
+            // lets run an init again
+            // set target to stopping point
+            Vector2f stopping_point;
+            loiter_nav->get_stopping_point_xy(stopping_point);
+            loiter_nav->init_target(stopping_point);
         }
-        pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y);
-        pos_control->override_vehicle_velocity_xy(-target_vel_rel);
-    }
-#endif
+        // process roll, pitch inputs
+        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
 
-    // process roll, pitch inputs
-    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
+        // run loiter controller
+        loiter_nav->update();
+    }
 
-    // run loiter controller
-    loiter_nav->update();
+    copter.ap.prec_land_active = doing_precision_landing;
 
-    Vector3f thrust_vector = loiter_nav->get_thrust_vector();
+    Vector3f thrust_vector = pos_control->get_thrust_vector();
 
     if (g2.wp_navalt_min > 0) {
         // user has requested an altitude below which navigation

ArduCopter/mode_auto.cpp

@@ -335,6 +335,12 @@ void ModeAuto::land_start(const Vector2f& destination)
     // disable the fence on landing
     copter.fence.auto_disable_fence_for_landing();
 #endif
+
+    // reset flag indicating if pilot has applied roll or pitch inputs during landing
+    copter.ap.land_repo_active = false;
+
+    // this will be set true if prec land is later active
+    copter.ap.prec_land_active = false;
 }
 
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location

ArduCopter/mode_land.cpp

@@ -28,6 +28,9 @@ bool ModeLand::init(bool ignore_checks)
     // reset flag indicating if pilot has applied roll or pitch inputs during landing
     copter.ap.land_repo_active = false;
 
+    // this will be set true if prec land is later active
+    copter.ap.prec_land_active = false;
+
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
 

ArduCopter/mode_rtl.cpp

@@ -25,6 +25,9 @@ bool ModeRTL::init(bool ignore_checks)
     // reset flag indicating if pilot has applied roll or pitch inputs during landing
     copter.ap.land_repo_active = false;
 
+    // this will be set true if prec land is later active
+    copter.ap.prec_land_active = false;
+
 #if PRECISION_LANDING == ENABLED
     // initialise precland state machine
     copter.precland_statemachine.init();