move hover_thrust_estimator to new module (mc_hover_thrust_estimator)

* MC_HTE: unitialize with hover_thrust parameter
* MC_HTE: constrain hover thrust setter between 0.1 and 0.9
* MC_HTE: integrate with land detector and velocity controller
* MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.

* MC_HTE: use altitude agl threshold to start the estimator
local_position.z is relative to the origin of the EKF while dist_bottom
is above ground

Co-authored-by: bresch <brescianimathieu@gmail.com>

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -76,6 +76,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.landSpeed      = param_find("MPC_LAND_SPEED");
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_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).
@@ -93,6 +94,14 @@ void MulticopterLandDetector::_update_topics()
 	_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
 	_vehicle_control_mode_sub.update(&_vehicle_control_mode);
 	_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
+
+	if (_params.useHoverThrustEstimate) {
+		hover_thrust_estimate_s hte;
+
+		if (_hover_thrust_estimate_sub.update(&hte)) {
+			_params.hoverThrottle = hte.hover_thrust;
+		}
+	}
 }
 
 void MulticopterLandDetector::_update_params()
@@ -102,9 +111,16 @@ void MulticopterLandDetector::_update_params()
 	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get()));
 
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
-	param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 	param_get(_paramHandle.landSpeed, &_params.landSpeed);
+
+	int32_t use_hover_thrust_estimate = 0;
+	param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
+	_params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1);
+
+	if (!_params.useHoverThrustEstimate) {
+		param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
+	}
 }
 
 bool MulticopterLandDetector::_get_freefall_state()

src/modules/land_detector/MulticopterLandDetector.h

@@ -48,6 +48,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/hover_thrust_estimate.h>
 
 #include "LandDetector.h"
 
@@ -102,6 +103,7 @@ private:
 		param_t hoverThrottle;
 		param_t minManThrottle;
 		param_t landSpeed;
+		param_t useHoverThrustEstimate;
 	} _paramHandle{};
 
 	struct {
@@ -109,6 +111,7 @@ private:
 		float hoverThrottle;
 		float minManThrottle;
 		float landSpeed;
+		bool useHoverThrustEstimate;
 	} _params{};
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
@@ -118,6 +121,7 @@ private:
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 
 	actuator_controls_s               _actuator_controls {};
 	battery_status_s                  _battery_status {};