QuadPlane: Precision Landing(v2)

ArduCopter/precision_landing.cpp

@@ -13,13 +13,11 @@ void Copter::init_precland()
 
 void Copter::update_precland()
 {
-    int32_t height_above_ground_cm = current_loc.alt;
-
-    // use range finder altitude if it is valid, otherwise use home alt
-    if (rangefinder_alt_ok()) {
-        height_above_ground_cm = rangefinder_state.alt_cm_glitch_protected;
+    if (!rangefinder_alt_ok() ||
+        rangefinder_state.alt_cm_glitch_protected == 0) {
+        return precland.update(0, false);
     }
-
-    precland.update(height_above_ground_cm, rangefinder_alt_ok());
+    return precland.update(rangefinder_state.alt_cm_glitch_protected,
+                           true);
 }
 #endif

ArduPlane/ArduPlane.cpp

@@ -75,6 +75,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if AP_AIRSPEED_AUTOCAL_ENABLE
     SCHED_TASK(airspeed_ratio_update,   1,    100),
 #endif // AP_AIRSPEED_AUTOCAL_ENABLE
+#if PRECISION_LANDING == ENABLED
+    SCHED_TASK(update_precland,      400,     50),
+#endif
 #if HAL_MOUNT_ENABLED
     SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100),
 #endif // HAL_MOUNT_ENABLED

ArduPlane/GCS_Mavlink.cpp

@@ -670,6 +670,16 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c
     plane.reset_offset_altitude();
 }
 
+/*
+  handle a LANDING_TARGET command. The timestamp has been jitter corrected
+*/
+void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
+{
+#if PRECISION_LANDING == ENABLED
+    plane.g2.precland.handle_msg(packet, timestamp_ms);
+#endif
+}
+
 
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_long_t &packet)
 {

ArduPlane/GCS_Mavlink.h

@@ -25,6 +25,8 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK
     MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
     MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet) override;
 
+    void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
+
     void send_position_target_global_int() override;
 
     virtual bool in_hil_mode() const override;

ArduPlane/GCS_Plane.cpp

@@ -20,6 +20,16 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
     }
 
+#if PRECISION_LANDING == ENABLED
+    if (plane.g2.precland.enabled()) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
+    }
+    if (plane.g2.precland.enabled() && plane.g2.precland.healthy()) {
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
+    }
+#endif
+
     // reverse thrust
     if (plane.have_reverse_thrust()) {
         control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;

ArduPlane/Parameters.cpp

@@ -1238,6 +1238,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID),
 #endif // OFFBOARD_GUIDED == ENABLED
 
+#if PRECISION_LANDING == ENABLED
+    // @Group: PLND_
+    // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp
+    AP_SUBGROUPINFO(precland, "PLND_", 29, ParametersG2, AC_PrecLand),
+#endif
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -575,6 +575,10 @@ class ParametersG2 {
 
     // min initial climb in RTL
     AP_Int16        rtl_climb_min;
+
+#if PRECISION_LANDING == ENABLED
+    AC_PrecLand precland;
+#endif
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -110,6 +110,10 @@
 #include <AP_Scripting/AP_Scripting.h>
 #endif
 
+#if PRECISION_LANDING == ENABLED
+#include <AC_PrecLand/AC_PrecLand.h>
+#endif
+
 #include "RC_Channel.h"     // RC Channel Library
 #include "Parameters.h"
 #if HAL_ADSB_ENABLED
@@ -1078,6 +1082,10 @@ class Plane : public AP_Vehicle {
 #if HAL_SOARING_ENABLED
     void update_soaring();
 #endif
+
+    // precision_landing.cpp
+    void init_precland();
+    void update_precland();
 
     // vehicle specific waypoint info helpers
     bool get_wp_distance_m(float &distance) const override;

ArduPlane/RC_Channel.cpp

@@ -188,6 +188,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
         break;
 
     case AUX_FUNC::CROW_SELECT:
+    case AUX_FUNC::PRECISION_LOITER:
         do_aux_function(ch_option, ch_flag);
         break;
 
@@ -305,6 +306,22 @@ case AUX_FUNC::ARSPD_CALIBRATE:
             break;
         case AuxSwitchPos::LOW:
             plane.airspeed.set_calibration_enabled(false);
+        break;
+       }
+#endif
+        break;
+
+    case AUX_FUNC::PRECISION_LOITER:
+#if PRECISION_LANDING == ENABLED
+        switch (ch_flag) {
+        case AuxSwitchPos::HIGH:
+            plane.quadplane.set_precision_loiter_enabled(true);
+            break;
+        case AuxSwitchPos::MIDDLE:
+            // nothing
+            break;
+        case AuxSwitchPos::LOW:
+            plane.quadplane.set_precision_loiter_enabled(false);
             break;
         }
 #endif

ArduPlane/config.h

@@ -24,6 +24,13 @@
  # define MAV_SYSTEM_ID          1
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Precision Landing system
+//
+#ifndef PRECISION_LANDING
+#define PRECISION_LANDING !HAL_MINIMIZE_FEATURES
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Advanced Failsafe support
 //

ArduPlane/precision_landing.cpp

@@ -0,0 +1,22 @@
+//
+// functions to support precision landing
+//
+
+#include "Plane.h"
+
+#if PRECISION_LANDING == ENABLED
+void Plane::init_precland()
+{
+    plane.g2.precland.init(400); // this must match the number on the scheduler table
+}
+
+void Plane::update_precland()
+{
+    if (plane.g.rangefinder_landing &&
+        rangefinder_state.in_range) {
+        return g2.precland.update(rangefinder_state.height_estimate,
+                                  true);
+    }
+    return g2.precland.update(0, false);
+}
+#endif

ArduPlane/quadplane.cpp

@@ -1418,12 +1418,40 @@ bool QuadPlane::is_flying_vtol(void) const
  */
 float QuadPlane::landing_descent_rate_cms(float height_above_ground) const
 {
-    float ret = linear_interpolate(land_speed_cms, wp_nav->get_default_speed_down(),
+    float speed_down = wp_nav->get_default_speed_down();
+    float ret = linear_interpolate(land_speed_cms, speed_down,
                                    height_above_ground,
                                    land_final_alt, land_final_alt+6);
+#if PRECISION_LANDING == ENABLED
+    if (precland_active()) {
+        const float precland_acceptable_error = 15.0f;
+        const float precland_min_descent_speed = 10.0f;
+        float land_slowdown = MAX(0.0f, pos_control->get_pos_error_xy()*(speed_down/precland_acceptable_error));
+        ret = MAX(precland_min_descent_speed, ret - land_slowdown);
+    }
+#endif
     return ret;
 }
 
+// return true if precision landing is active
+bool QuadPlane::precland_active(void) const
+{
+#if PRECISION_LANDING == ENABLED
+    if (plane.control_mode != &plane.mode_qland &&
+        plane.control_mode != &plane.mode_qloiter &&
+        !in_vtol_land_descent()) {
+        return false;
+    }
+    if (plane.control_mode == &plane.mode_qloiter &&
+        !_precision_loiter_enabled) {
+        return false;
+    }
+    AC_PrecLand &precland = plane.g2.precland;
+    return pos_control->is_active_xy() && precland.target_acquired();
+#else
+    return false;
+#endif
+}
 
 // run quadplane loiter controller
 void QuadPlane::control_loiter()
@@ -1466,6 +1494,24 @@ void QuadPlane::control_loiter()
     // run loiter controller
     loiter_nav->update();
 
+#if PRECISION_LANDING == ENABLED
+    if (precland_active()) {
+        AC_PrecLand &precland = plane.g2.precland;
+
+        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);
+    }
+#endif
+
     // nav roll and pitch are controller by loiter controller
     plane.nav_roll_cd = loiter_nav->get_roll();
     plane.nav_pitch_cd = loiter_nav->get_pitch();
@@ -2580,8 +2626,7 @@ void QuadPlane::vtol_position_controller(void)
         plane.nav_controller->update_waypoint(plane.prev_WP_loc, loc);
         FALLTHROUGH;
 
-    case QPOS_LAND_FINAL:
-
+    case QPOS_LAND_FINAL: {
         // set position controller desired velocity and acceleration to zero
         pos_control->set_desired_velocity_xy(0.0f,0.0f);
         pos_control->set_desired_accel_xy(0.0f,0.0f);
@@ -2592,6 +2637,24 @@ void QuadPlane::vtol_position_controller(void)
         } else {
             pos_control->set_xy_target(poscontrol.target.x, poscontrol.target.y);
         }
+
+#if PRECISION_LANDING == ENABLED
+        // run precision landing
+        if (precland_active()) {
+            Vector2f target_pos, target_vel_rel;
+            AC_PrecLand &precland = plane.g2.precland;
+            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);
+        }
+#endif
         pos_control->update_xy_controller();
 
         // nav roll and pitch are controller by position controller
@@ -2603,6 +2666,7 @@ void QuadPlane::vtol_position_controller(void)
                                                                       plane.nav_pitch_cd,
                                                                       get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
         break;
+    }
 
     case QPOS_LAND_COMPLETE:
         // nothing to do
@@ -2655,7 +2719,7 @@ void QuadPlane::vtol_position_controller(void)
             ahrs.setTouchdownExpected(true);
         }
         break;
-        
+
     case QPOS_LAND_COMPLETE:
         break;
     }
@@ -3075,7 +3139,8 @@ bool QuadPlane::verify_vtol_land(void)
         return true;
     }
     if (poscontrol.state == QPOS_POSITION2 &&
-        plane.auto_state.wp_distance < 2) {
+        ((plane.auto_state.wp_distance < 2) || precland_active())) {
+
         poscontrol.state = QPOS_LAND_DESCEND;
 #if AC_FENCE == ENABLED
         plane.fence.auto_disable_fence_for_landing();

ArduPlane/quadplane.h

@@ -10,6 +10,7 @@
 #include <AC_Fence/AC_Fence.h>
 #include <AC_Avoidance/AC_Avoid.h>
 #include <AP_Proximity/AP_Proximity.h>
+#include "config.h"
 #include "qautotune.h"
 #include "defines.h"
 
@@ -51,6 +52,10 @@ class QuadPlane
     static const struct AP_Param::GroupInfo var_info[];
     static const struct AP_Param::GroupInfo var_info2[];
 
+#if PRECISION_LANDING == ENABLED
+    void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
+#endif
+
     void control_run(void);
     void control_auto(void);
     bool init_mode(void);
@@ -62,7 +67,7 @@ class QuadPlane
     void waypoint_controller(void);
 
     void update_throttle_mix(void);
-    
+
     // update transition handling
     void update(void);
 
@@ -297,7 +302,12 @@ class QuadPlane
 
     // calculate a stopping distance for fixed-wing to vtol transitions
     float stopping_distance(void);
-
+
+#if PRECISION_LANDING == ENABLED
+    bool _precision_loiter_enabled;
+#endif
+    bool precland_active() const;
+
     AP_Int16 transition_time_ms;
 
     // transition deceleration, m/s/s

ArduPlane/system.cpp

@@ -111,6 +111,11 @@ void Plane::init_ardupilot()
     camera_mount.init();
 #endif
 
+#if PRECISION_LANDING == ENABLED
+    // initialise precision landing
+    init_precland();
+#endif
+
 #if LANDING_GEAR_ENABLED == ENABLED
     // initialise landing gear position
     g2.landing_gear.init();
@@ -390,7 +395,7 @@ void Plane::startup_INS_ground(void)
     ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
     ahrs.set_wind_estimation(true);
 
-    ins.init(scheduler.get_loop_rate_hz());
+    ins.init(400);  // this must be the same number as in the scheduler table
     ahrs.reset();
 
     // read Baro pressure at ground

ArduPlane/wscript

@@ -12,6 +12,7 @@ def build(bld):
             'AP_Avoidance',
             'AP_Arming',
             'AP_Camera',
+            'AP_IRLock',
             'AP_L1_Control',
             'AP_Navigation',
             'AP_RCMapper',
@@ -30,6 +31,7 @@ def build(bld):
             'AP_OSD',
             'AC_AutoTune',
             'AP_KDECAN',
+            'AC_PrecLand',
         ],
     )