LandDetectorMC: enforce that LNDMC_Z_VEL_MAX is larger than MPC_LAND_CRWL/MPC_LAND_SPEED (#20614)

* LandDetectorMC: enforce that LNDMC_Z_VEL_MAX * 1.2 is below *MPC_LAND_CRWL/MPC_LAND_SPEED

Otherwise the _in_descend flag doesn't get set correctly during the last part
of the landing, where the descend speed is at MPC_LAND_CRWL or LAND_SPEED.
The _in_descend flag is set it the velocity setpoint is >1.1*LNDMC_Z_VEL_MAX.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -79,6 +79,8 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
+	_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);
 }
 
@@ -118,6 +120,19 @@ void MulticopterLandDetector::_update_params()
 {
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
+	param_get(_paramHandle.landSpeed, &_params.landSpeed);
+	param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
+
+	// 1.2 corresponds to the margin between the default parameters LNDMC_Z_VEL_MAX = MPC_LAND_CRWL / 1.2
+	const float lndmc_upper_threshold = math::min(_params.crawlSpeed, _params.landSpeed) / 1.2f;
+
+	if (_param_lndmc_z_vel_max.get() > lndmc_upper_threshold) {
+		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_CRWL or MPC_LAND_SPEED, updating %.3f -> %.3f",
+			(double)_param_lndmc_z_vel_max.get(), (double)(lndmc_upper_threshold));
+
+		_param_lndmc_z_vel_max.set(lndmc_upper_threshold);
+		_param_lndmc_z_vel_max.commit_no_notification();
+	}
 
 	int32_t use_hover_thrust_estimate = 0;
 	param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);

src/modules/land_detector/MulticopterLandDetector.h

@@ -87,12 +87,13 @@ private:
 	/** 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;
 
-	/** Handles for interesting parameters. **/
 	struct {
 		param_t minThrottle;
 		param_t hoverThrottle;
 		param_t minManThrottle;
 		param_t useHoverThrustEstimate;
+		param_t landSpeed;
+		param_t crawlSpeed;
 	} _paramHandle{};
 
 	struct {
@@ -100,6 +101,8 @@ private:
 		float hoverThrottle;
 		float minManThrottle;
 		bool useHoverThrustEstimate;
+		float landSpeed;
+		float crawlSpeed;
 	} _params{};
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};

src/modules/land_detector/land_detector_params_mc.c

@@ -51,12 +51,13 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);
  * Multicopter vertical velocity threshold
  *
  * Vertical velocity threshold to detect landing.
- * Should be set lower than the expected minimal speed for landing
- * so MPC_LAND_SPEED for autonomous landing and MPC_LAND_CRWL
- * if distance sensor is present and slowing down close to ground.
+ * Has to be set lower than the expected minimal speed for landing,
+ * which is either MPC_LAND_SPEED or MPC_LAND_CRWL.
+ * This is enforced by an automatic check.
  *
  * @unit m/s
- * @decimal 1
+ * @min 0
+ * @decimal 2
  *
  * @group Land Detector
  */