Plane: make RNGFND_LANDING a bitmask

this gives greater control of when the rangefinder will be used. A
particularly important one is the ability to disable it for VTOL
assist as false positives can be quite common and drains the battery

Author: tridge

ArduPlane/GCS_Plane.cpp

@@ -116,7 +116,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
     const RangeFinder *rangefinder = RangeFinder::get_singleton();
     if (rangefinder && rangefinder->has_orientation(plane.rangefinder_orientation())) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
-        if (plane.g.rangefinder_landing) {
+        if (uint16_t(plane.g.rangefinder_landing.get()) != 0) {
             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
         if (rangefinder->has_data_orient(plane.rangefinder_orientation())) {

ArduPlane/Parameters.cpp

@@ -785,9 +785,9 @@ const AP_Param::Info Plane::var_info[] = {
     GOBJECT(rangefinder,            "RNGFND", RangeFinder),
 
     // @Param: RNGFND_LANDING
-    // @DisplayName: Enable rangefinder for landing
-    // @Description: This enables the use of a rangefinder for automatic landing. The rangefinder will be used both on the landing approach and for final flare
-    // @Values: 0:Disabled,1:Enabled
+    // @DisplayName: Enable use of rangefinder
+    // @Description: Sets the use of a rangefinder for automatic landing and other use cases. When enabled for landing and takeoff the rangefinder will be used both on the landing approach and for final flare as well as as VTOL landing and for takeoffs and throttle suppression when close to the ground. When enabled for assist the rangefinder will be used for VTOL assistance. When enabled for climb the rangefinder will be used for the initial climb in QRTL and AUTO. Set to 0 to disable use of the rangefinder.
+    // @Bitmask: 0:All, 1:TakeoffAndLanding, 2:Assist, 3:InitialClimb
     // @User: Standard
     GSCALAR(rangefinder_landing,    "RNGFND_LANDING",   0),
 #endif

ArduPlane/Plane.h

@@ -226,6 +226,7 @@ class Plane : public AP_Vehicle {
     Rotation rangefinder_orientation(void) const {
         return Rotation(g2.rangefinder_land_orient.get());
     }
+
 #endif
 
 #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
@@ -896,8 +897,9 @@ class Plane : public AP_Vehicle {
     void adjust_altitude_target();
     void setup_alt_slope(void);
     int32_t get_RTL_altitude_cm() const;
-    float relative_ground_altitude(bool use_rangefinder_if_available);
-    float relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available);
+    bool rangefinder_use(enum RangeFinderUse rangefinder_use) const;
+    float relative_ground_altitude(enum RangeFinderUse rangefinder_use);
+    float relative_ground_altitude(enum RangeFinderUse rangefinder_use, bool use_terrain_if_available);
     void set_target_altitude_current(void);
     void set_target_altitude_location(const Location &loc);
     int32_t relative_target_altitude_cm(void);

ArduPlane/VTOL_Assist.cpp

@@ -106,7 +106,7 @@ bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
         alt_error.reset();
 
     } else {
-        const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
+        const float height_above_ground = plane.relative_ground_altitude(RangeFinderUse::ASSIST);
         if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {
             gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);
         }

ArduPlane/altitude.cpp

@@ -115,7 +115,7 @@ int32_t Plane::get_RTL_altitude_cm() const
   return relative altitude in meters (relative to terrain, if available,
   or home otherwise)
  */
-float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available)
+float Plane::relative_ground_altitude(enum RangeFinderUse use_rangefinder, bool use_terrain_if_available)
 {
 #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
    float height_AGL;
@@ -126,13 +126,13 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool us
 #endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
 
 #if AP_RANGEFINDER_ENABLED
-   if (use_rangefinder_if_available && rangefinder_state.in_range) {
+   if (rangefinder_use(use_rangefinder) && rangefinder_state.in_range) {
         return rangefinder_state.height_estimate;
    }
 #endif
 
 #if HAL_QUADPLANE_ENABLED && AP_RANGEFINDER_ENABLED
-   if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
+   if (rangefinder_use(use_rangefinder) && quadplane.in_vtol_land_final() &&
        rangefinder.status_orient(rangefinder_orientation()) == RangeFinder::Status::OutOfRangeLow) {
        // a special case for quadplane landing when rangefinder goes
        // below minimum. Consider our height above ground to be zero
@@ -163,13 +163,29 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool us
     return relative_altitude;
 }
 
+/*
+  return true if we should use the rangefinder for a specific use case
+ */
+bool Plane::rangefinder_use(enum RangeFinderUse use_rangefinder) const
+{
+    const uint8_t use = uint8_t(g.rangefinder_landing.get());
+    if (use == uint8_t(RangeFinderUse::NONE)) {
+        return false;
+    }
+    if (use & uint8_t(RangeFinderUse::ALL)) {
+        // if ALL bit is set then ignore other bits
+        return true;
+    }
+    return (use & uint8_t(use_rangefinder)) != 0;
+}
+
 // Helper for above method using terrain if the vehicle is currently terrain following
-float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
+float Plane::relative_ground_altitude(enum RangeFinderUse use_rangefinder)
 {
 #if AP_TERRAIN_AVAILABLE
-    return relative_ground_altitude(use_rangefinder_if_available, target_altitude.terrain_following);
+    return relative_ground_altitude(use_rangefinder, target_altitude.terrain_following);
 #else
-    return relative_ground_altitude(use_rangefinder_if_available, false);
+    return relative_ground_altitude(use_rangefinder, false);
 #endif
 }
 
@@ -667,7 +683,7 @@ float Plane::rangefinder_correction(void)
     }
 
     // for now we only support the rangefinder for landing 
-    bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_FixedWing::FlightStage::LAND);
+    bool using_rangefinder = (rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) && flight_stage == AP_FixedWing::FlightStage::LAND);
     if (!using_rangefinder) {
         return 0;
     }
@@ -682,7 +698,7 @@ float Plane::rangefinder_correction(void)
 void Plane::rangefinder_terrain_correction(float &height)
 {
 #if AP_TERRAIN_AVAILABLE
-    if (!g.rangefinder_landing ||
+    if (!rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) ||
         flight_stage != AP_FixedWing::FlightStage::LAND ||
         !terrain_enabled_in_current_mode()) {
         return;
@@ -765,7 +781,7 @@ void Plane::rangefinder_height_update(void)
 #endif
             if (!rangefinder_state.in_use &&
                 flightstage_good_for_rangefinder_landing &&
-                g.rangefinder_landing) {
+                rangefinder_use(RangeFinderUse::TAKEOFF_LANDING)) {
                 rangefinder_state.in_use = true;
                 gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
             }
@@ -795,7 +811,7 @@ void Plane::rangefinder_height_update(void)
         if (now - rangefinder_state.last_correction_time_ms > 5000) {
             rangefinder_state.correction = correction;
             rangefinder_state.initial_correction = correction;
-            if (g.rangefinder_landing) {
+            if (rangefinder_use(RangeFinderUse::TAKEOFF_LANDING)) {
                 landing.set_initial_slope();
             }
             rangefinder_state.last_correction_time_ms = now;
@@ -931,7 +947,7 @@ float Plane::get_landing_height(bool &rangefinder_active)
 #if AP_RANGEFINDER_ENABLED
     // possibly correct with rangefinder
     height -= rangefinder_correction();
-    rangefinder_active = g.rangefinder_landing && rangefinder_state.in_range;
+    rangefinder_active = rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) && rangefinder_state.in_range;
 #endif
 
     return height;

ArduPlane/defines.h

@@ -189,3 +189,15 @@ enum class FenceAutoEnable : uint8_t {
     AutoDisableFloorOnly=2,
     WhenArmed=3
 };
+
+/*
+  bitmask of options for RGFND_LANDING
+ */
+enum class RangeFinderUse : uint8_t {
+    NONE    = 0U,
+    ALL     = (1U<<0),
+    TAKEOFF_LANDING = (1U<<1),
+    ASSIST  = (1U<<2),
+    CLIMB   = (1U<<3),
+};
+

ArduPlane/mode_autoland.cpp

@@ -162,7 +162,7 @@ bool ModeAutoLand::_enter()
 #else
         const bool use_terrain = false;
 #endif
-        const float dist_to_climb = terrain_alt_min - plane.relative_ground_altitude(plane.g.rangefinder_landing, use_terrain);
+        const float dist_to_climb = terrain_alt_min - plane.relative_ground_altitude(RangeFinderUse::CLIMB, use_terrain);
         if (is_positive(dist_to_climb)) {
             // Copy loiter and update target altitude to current altitude plus climb altitude
             cmd_climb = cmd_loiter;

ArduPlane/mode_qland.cpp

@@ -9,7 +9,7 @@ bool ModeQLand::_enter()
     quadplane.throttle_wait = false;
     quadplane.setup_target_position();
     poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND);
-    quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing);
+    quadplane.last_land_final_agl = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
     quadplane.landing_detect.lower_limit_start_ms = 0;
     quadplane.landing_detect.land_start_ms = 0;
 #if AP_LANDINGGEAR_ENABLED

ArduPlane/mode_qloiter.cpp

@@ -157,7 +157,7 @@ void ModeQLoiter::run()
             }
 #endif  // AP_ICENGINE_ENABLED
         }
-        float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
+        float height_above_ground = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
         float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground);
 
         if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {

ArduPlane/mode_qrtl.cpp

@@ -33,7 +33,7 @@ bool ModeQRTL::_enter()
         const bool use_terrain = false;
 #endif
 
-        const float dist_to_climb = target_alt - plane.relative_ground_altitude(plane.g.rangefinder_landing, use_terrain);
+        const float dist_to_climb = target_alt - plane.relative_ground_altitude(RangeFinderUse::CLIMB, use_terrain);
         if (is_positive(dist_to_climb)) {
             // climb before returning, only next waypoint altitude is used
             submode = SubMode::climb;