Copter: use velocity output from AC_PrecLand

ArduCopter/Copter.h

@@ -293,8 +293,6 @@ private:
         uint32_t start_ms;
     } takeoff_state;
 
-    uint32_t precland_last_update_ms;
-
     // altitude below which we do no navigation in auto takeoff
     float auto_takeoff_no_nav_alt_cm;
     

ArduCopter/Log.cpp

@@ -651,12 +651,11 @@ struct PACKED log_Precland {
     LOG_PACKET_HEADER;
     uint64_t time_us;
     uint8_t healthy;
-    float bf_angle_x;
-    float bf_angle_y;
-    float ef_angle_x;
-    float ef_angle_y;
+    uint8_t target_acquired;
     float pos_x;
     float pos_y;
+    float vel_x;
+    float vel_y;
 };
 
 // Write an optical flow packet
@@ -668,19 +667,20 @@ void Copter::Log_Write_Precland()
         return;
     }
 
-    const Vector2f &bf_angle = precland.last_bf_angle_to_target();
-    const Vector2f &ef_angle = precland.last_ef_angle_to_target();
-    const Vector3f &target_pos_ofs = precland.last_target_pos_offset();
+    Vector2f target_pos_rel = Vector2f(0.0f,0.0f);
+    Vector2f target_vel_rel = Vector2f(0.0f,0.0f);
+    precland.get_target_position_relative_cm(target_pos_rel);
+    precland.get_target_velocity_relative_cms(target_vel_rel);
+
     struct log_Precland pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG),
         time_us         : AP_HAL::micros64(),
         healthy         : precland.healthy(),
-        bf_angle_x      : degrees(bf_angle.x),
-        bf_angle_y      : degrees(bf_angle.y),
-        ef_angle_x      : degrees(ef_angle.x),
-        ef_angle_y      : degrees(ef_angle.y),
-        pos_x           : target_pos_ofs.x,
-        pos_y           : target_pos_ofs.y
+        target_acquired : precland.target_acquired(),
+        pos_x           : target_pos_rel.x,
+        pos_y           : target_pos_rel.y,
+        vel_x           : target_vel_rel.x,
+        vel_y           : target_vel_rel.y
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
  #endif     // PRECISION_LANDING == ENABLED
@@ -833,7 +833,7 @@ const struct LogStructure Copter::log_structure[] = {
     { LOG_HELI_MSG, sizeof(log_Heli),
       "HELI",  "Qff",         "TimeUS,DRRPM,ERRPM" },
     { LOG_PRECLAND_MSG, sizeof(log_Precland),
-      "PL",    "QBffffff",    "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
+      "PL",    "QBBffff",    "TimeUS,Heal,TAcq,pX,pY,vX,vY" },
     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
       "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
     { LOG_THROW_MSG, sizeof(log_Throw),

ArduCopter/control_land.cpp

@@ -174,10 +174,10 @@ void Copter::land_run_vertical_control(bool pause_descent)
 #endif
 
     // compute desired velocity
-    const float precland_acceptable_error = 25.0f;
-    const float precland_min_descent_speed = -10.0f;
+    const float precland_acceptable_error = 15.0f;
+    const float precland_min_descent_speed = 10.0f;
     int32_t alt_above_ground = land_get_alt_above_ground();
-    
+
     float cmb_rate = 0;
     if (!pause_descent) {
         float max_land_descent_velocity;
@@ -196,9 +196,10 @@ void Copter::land_run_vertical_control(bool pause_descent)
         // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
         cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
 
-        if (doing_precision_landing && alt_above_ground < 300.0f) {
-            float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(abs(g.land_speed)/precland_acceptable_error));
-            cmb_rate = MIN(precland_min_descent_speed, cmb_rate+land_slowdown);
+        if (doing_precision_landing && alt_above_ground < 200.0f) {
+            float max_descent_speed = abs(g.land_speed)/2.0f;
+            float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(max_descent_speed/precland_acceptable_error));
+            cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
         }
     }
 
@@ -251,12 +252,18 @@ void Copter::land_run_horizontal_control()
 #if PRECISION_LANDING == ENABLED
     bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();
     // run precision landing
-    if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) {
-        Vector3f target_pos;
-        precland.get_target_position(target_pos);
+    if (doing_precision_landing) {
+        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.freeze_ff_xy();
-        precland_last_update_ms = precland.last_update_ms();
+        pos_control.override_vehicle_velocity_xy(-target_vel_rel);
     }
 #else
     bool doing_precision_landing = false;

ArduCopter/precision_landing.cpp

@@ -26,6 +26,6 @@ void Copter::update_precland()
         }
     }
 
-    copter.precland.update(height_above_ground_cm);
+    copter.precland.update(height_above_ground_cm, rangefinder_alt_ok());
 }
 #endif