MulticopterLandDetector: Make land detection time configurable

The tree stages used arbitrary 350, 250 300ms totally 900ms So this changes it to each stage to a third of the parameter. Default it is 1 second -> 333ms per stage.
PX4 · Apr 30, 2021 · 3f2c631 · 3f2c631
1 parent ba51e37
commit 3f2c631
Show file tree

Hide file tree

Showing 7 changed files with 25 additions and 27 deletions.
diff --git a/src/modules/land_detector/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h
@@ -55,7 +55,6 @@ class AirshipLandDetector : public LandDetector
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
 	void _set_hysteresis_factor(const int factor) override {};
-
 };
 
 } // namespace land_detector
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
@@ -100,16 +100,13 @@ void LandDetector::Run()
 		// 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 {
-		if (!_high_hysteresis_active && !_vehicle_local_position.dist_bottom_valid) {
-			_set_hysteresis_factor(3);
-			_high_hysteresis_active = true;
-
-		} else if (_high_hysteresis_active && _vehicle_local_position.dist_bottom_valid) {
-			_set_hysteresis_factor(1);
-			_high_hysteresis_active = false;
-		}
+		_set_hysteresis_factor(1);
 	}
 
 	const hrt_abstime now_us = hrt_absolute_time();

diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
@@ -173,8 +173,6 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
 	hrt_abstime _takeoff_time{0};
 	hrt_abstime _total_flight_time{0};	///< total vehicle flight time in microseconds
 
-	bool _high_hysteresis_active{false};
-
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
 	uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -80,9 +80,6 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
-
-	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
-	_set_hysteresis_factor(1);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -370,9 +367,9 @@ bool MulticopterLandDetector::_is_close_to_ground()
 
 void MulticopterLandDetector::_set_hysteresis_factor(const int factor)
 {
-	_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US * factor);
-	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US * factor);
-	_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US * 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);
 }
 

diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
@@ -87,15 +87,6 @@ class MulticopterLandDetector : public LandDetector
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
 
-	/** Time in us that ground contact condition have to hold before triggering contact ground */
-	static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms;
-
-	/** Time in us that almost landing conditions have to hold before triggering almost landed . */
-	static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms;
-
-	/** Time in us that landing conditions have to hold before triggering a land. */
-	static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms;
-
 	/** Time interval in us in which wider acceptance thresholds are used after landed. */
 	static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
 
@@ -148,6 +139,7 @@ class MulticopterLandDetector : public LandDetector
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		LandDetector,
+		(ParamFloat<px4::params::LNDMC_TRIG_TIME>)   _param_lndmc_trig_time,
 		(ParamFloat<px4::params::LNDMC_ALT_MAX>)    _param_lndmc_alt_max,
 		(ParamFloat<px4::params::LNDMC_ROT_MAX>)    _param_lndmc_rot_max,
 		(ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,

diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h
@@ -56,7 +56,6 @@ class RoverLandDetector : public LandDetector
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
 	void _set_hysteresis_factor(const int factor) override {};
-
 };
 
 } // namespace land_detector
diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c
@@ -31,6 +31,22 @@
  *
  ****************************************************************************/
 
+/**
+ * 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 max climb rate
  *