Land detector: Remove LNDMC_TRIG_TIME parameter (#25251)

* LandDetector: remove unused LNDMC_TRIG_TIME parameter

* LandDetector: remove unnecessarily complicated global set_hysteresis_factor() function

* Apply suggestions from code review

Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

* Apply suggestions from code review

* remove param

---------

Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>

src/modules/land_detector/AirshipLandDetector.h

@@ -54,7 +54,6 @@ public:
 protected:
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
-	void _set_hysteresis_factor(const int factor) override {};
 };
 
 } // namespace land_detector

src/modules/land_detector/FixedwingLandDetector.h

@@ -63,7 +63,6 @@ public:
 protected:
 
 	bool _get_landed_state() override;
-	void _set_hysteresis_factor(const int factor) override {};
 
 private:
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};

src/modules/land_detector/LandDetector.cpp

@@ -122,20 +122,6 @@ void LandDetector::Run()
 
 	_update_topics();
 
-	if (!_dist_bottom_is_observable) {
-		// we consider the distance to the ground observable if the system is using a range sensor
-		_dist_bottom_is_observable = _vehicle_local_position.dist_bottom_sensor_bitfield &
-					     vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE;
-	}
-
-	// Increase land detection time if not close to ground
-	if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) {
-		_set_hysteresis_factor(3);
-
-	} else {
-		_set_hysteresis_factor(1);
-	}
-
 	const hrt_abstime now_us = hrt_absolute_time();
 
 	_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);

src/modules/land_detector/LandDetector.h

@@ -142,7 +142,6 @@ protected:
 	virtual bool _get_vertical_movement() { return false; }
 	virtual bool _get_rotational_movement() { return false; }
 	virtual bool _get_close_to_ground_or_skipped_check() {  return false; }
-	virtual void _set_hysteresis_factor(const int factor) = 0;
 
 	systemlib::Hysteresis _freefall_hysteresis{false};
 	systemlib::Hysteresis _landed_hysteresis{true};
@@ -158,7 +157,6 @@ protected:
 
 	bool _armed{false};
 	bool _previous_armed_state{false};	///< stores the previous actuator_armed.armed state
-	bool _dist_bottom_is_observable{false};
 
 private:
 	void Run() override;

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -44,7 +44,7 @@
  *State 2 (=maybe_landed):
  *maybe_landed can only occur if the internal ground_contact hysteresis state is true. maybe_landed criteria requires to have no motion in x and y,
  *no rotation and a thrust below 0.1 of the thrust_range (thrust_hover - thrust_min). In addition, the mc_pos_control turns off the thrust_sp in
- *body frame along x and y which helps to detect maybe_landed. The criteria for maybe_landed needs to be true for (LNDMC_TRIG_TIME / 3) seconds.
+ *body frame along x and y which helps to detect maybe_landed. The criteria for maybe_landed needs to be true for 1/3 seconds.
  *
  *State 3 (=landed)
  *landed can only be detected if maybe_landed is true for LAND_DETECTOR_TRIGGER_TIME_US. No farther criteria is tested, but the mc_pos_control goes into
@@ -82,6 +82,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 	_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
 	_minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s);
+	_freefall_hysteresis.set_hysteresis_time_from(false, 300_ms);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -114,6 +115,24 @@ void MulticopterLandDetector::_update_topics()
 	if (_takeoff_status_sub.update(&takeoff_status)) {
 		_takeoff_state = takeoff_status.takeoff_state;
 	}
+
+	// Update distance sensor observability
+	if (!_dist_bottom_is_observable) {
+		// we consider the distance to the ground observable if the system is using a range sensor
+		_dist_bottom_is_observable =
+			_vehicle_local_position.dist_bottom_sensor_bitfield & vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE;
+	}
+
+	// Increase land detection time if not close to ground
+	hrt_abstime land_detection_time = 1_s;
+
+	if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) {
+		land_detection_time = 3_s;
+	}
+
+	_ground_contact_hysteresis.set_hysteresis_time_from(false, land_detection_time / 3);
+	_landed_hysteresis.set_hysteresis_time_from(false, land_detection_time / 3);
+	_maybe_landed_hysteresis.set_hysteresis_time_from(false, land_detection_time / 3);
 }
 
 void MulticopterLandDetector::_update_params()
@@ -306,19 +325,11 @@ bool MulticopterLandDetector::_get_ground_effect_state()
 bool MulticopterLandDetector::_is_close_to_ground()
 {
 	if (_vehicle_local_position.dist_bottom_valid) {
-		return _vehicle_local_position.dist_bottom < DIST_FROM_GROUND_THRESHOLD;
+		return _vehicle_local_position.dist_bottom < 1.f;
 
 	} else {
 		return false;
 	}
 }
 
-void MulticopterLandDetector::_set_hysteresis_factor(const int factor)
-{
-	_ground_contact_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor);
-	_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor);
-	_maybe_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor);
-	_freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US);
-}
-
 } // namespace land_detector

src/modules/land_detector/MulticopterLandDetector.h

@@ -77,16 +77,9 @@ protected:
 	bool _get_rotational_movement() override { return _rotational_movement; }
 	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
 
-	void _set_hysteresis_factor(const int factor) override;
 private:
 	bool _is_close_to_ground();
 
-	/** Time in us that freefall has to hold before triggering freefall */
-	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
-
-	/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
-	static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;
-
 	struct {
 		param_t minThrottle;
 		param_t hoverThrottle;
@@ -124,6 +117,8 @@ private:
 
 	systemlib::Hysteresis _minimum_thrust_8s_hysteresis{false};
 
+	bool _dist_bottom_is_observable{false}; ///< there was a valid distance to bottom estimate before so we have a downwards facing distance sensor
+
 	bool _in_descend{false};		///< vehicle is commanded to desend
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
 	bool _vertical_movement{false};
@@ -134,7 +129,6 @@ private:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		LandDetector,
-		(ParamFloat<px4::params::LNDMC_TRIG_TIME>)   _param_lndmc_trig_time,
 		(ParamFloat<px4::params::LNDMC_ROT_MAX>)    _param_lndmc_rot_max,
 		(ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,
 		(ParamFloat<px4::params::LNDMC_Z_VEL_MAX>)  _param_lndmc_z_vel_max,

src/modules/land_detector/RoverLandDetector.h

@@ -58,7 +58,6 @@ public:
 protected:
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
-	void _set_hysteresis_factor(const int factor) override {};
 
 private:
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};

src/modules/land_detector/land_detector_params_mc.c

@@ -31,22 +31,6 @@
  *
  ****************************************************************************/
 
-/**
- * Multicopter land detection trigger time
- *
- * Total time it takes to go through all three land detection stages:
- * ground contact, maybe landed, landed
- * when all necessary conditions are constantly met.
- *
- * @unit s
- * @min 0.1
- * @max 10.0
- * @decimal 1
- *
- * @group Land Detector
- */
-PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);
-
 /**
  * Multicopter vertical velocity threshold
  *