mc_hover_thrust_estimator: validity flag and other small improvements/fixes

- track and publish validity based on hover thrust variance, innovation test ratio, and hysteresis
 - only publish on actual updates or becoming inactive
 - fix dt (previous timestamp wasn't being saved)
 - use local position timestamp (corresponding) to accel data rather than current time to avoid unnecessary timing jitter
 - check local position validity before using
 - mc_hover_thrust_estimator: move from wq:lp_default -> wq:nav_and_controllers to ensure the hover thrust estimator runs after the position controller and uses the same vehicle_local_position data
 - land_detector: check hover thrust estimate validity and adjust low throttle thresholds if hover thrust is available
 - mc_pos_control: only use hover thrust estimate if valid

msg/hover_thrust_estimate.msg

@@ -1,4 +1,5 @@
 uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # time of corresponding sensor data last used for this estimate
 
 float32 hover_thrust		# estimated hover thrust [0.1, 0.9]
 float32 hover_thrust_var	# estimated hover thrust variance
@@ -8,3 +9,5 @@ float32 accel_innov_var		# innovation variance of the last acceleration fusion
 float32 accel_innov_test_ratio	# normalized innovation squared test ratio
 
 float32 accel_noise_var		# vertical acceleration noise variance estimated form innovation residual
+
+bool valid

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -106,8 +106,12 @@ void MulticopterLandDetector::_update_topics()
 		hover_thrust_estimate_s hte;
 
 		if (_hover_thrust_estimate_sub.update(&hte)) {
+			if (hte.valid) {
 				_params.hoverThrottle = hte.hover_thrust;
 			}
+
+			_hover_thrust_estimate_valid = hte.valid;
+		}
 	}
 }
 
@@ -182,9 +186,9 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	}
 
 
-	// low thrust: 30% of throttle range between min and hover
+	// low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available
-	const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f;
+	const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f;
-
+	const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
 	bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
 
 	// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,

src/modules/land_detector/MulticopterLandDetector.h

@@ -114,6 +114,7 @@ private:
 
 	bool _flag_control_climb_rate_enabled{false};
 	bool _hover_thrust_initialized{false};
+	bool _hover_thrust_estimate_valid{false};
 
 	float _actuator_controls_throttle{0.f};
 

src/modules/mc_hover_thrust_estimator/CMakeLists.txt

@@ -39,10 +39,13 @@ px4_add_library(zero_order_hover_thrust_ekf
 px4_add_module(
 	MODULE modules__mc_hover_thrust_estimator
 	MAIN mc_hover_thrust_estimator
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		MulticopterHoverThrustEstimator.cpp
 		MulticopterHoverThrustEstimator.hpp
 	DEPENDS
+		hysteresis
 		mathlib
 		px4_work_queue
 		zero_order_hover_thrust_ekf

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -41,10 +41,13 @@
 
 #include <mathlib/mathlib.h>
 
+using namespace time_literals;
+
 MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_valid_hysteresis.set_hysteresis_time_from(false, 2_s);
 	updateParams();
 	reset();
 }
@@ -93,6 +96,11 @@ void MulticopterHoverThrustEstimator::Run()
 		return;
 	}
 
+	// new local position estimate and setpoint needed every iteration
+	if (!_vehicle_local_pos_sub.updated() || !_vehicle_local_position_setpoint_sub.updated()) {
+		return;
+	}
+
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -105,73 +113,112 @@ void MulticopterHoverThrustEstimator::Run()
 
 	perf_begin(_cycle_perf);
 
+	if (_vehicle_land_detected_sub.updated()) {
 		vehicle_land_detected_s vehicle_land_detected;
-	vehicle_status_s vehicle_status;
-	vehicle_local_position_s local_pos;
 
-	if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
 			_landed = vehicle_land_detected.landed;
 		}
-
-	if (_vehicle_status_sub.update(&vehicle_status)) {
-		_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 	}
 
-	if (_vehicle_local_pos_sub.update(&local_pos)) {
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
+
+	// always copy latest position estimate
+	vehicle_local_position_s local_pos{};
+
+	if (_vehicle_local_pos_sub.copy(&local_pos)) {
 		// This is only necessary because the landed
 		// flag of the land detector does not guarantee that
 		// the vehicle does not touch the ground anymore
 		// TODO: improve the landed flag
+		if (local_pos.dist_bottom_valid) {
 			_in_air = local_pos.dist_bottom > 1.f;
 		}
+	}
 
-	ZeroOrderHoverThrustEkf::status status{};
+	const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f;
+	_timestamp_last = local_pos.timestamp;
 
-	if (_armed && !_landed && _in_air) {
+	if (_armed && !_landed && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
-		vehicle_local_position_setpoint_s local_pos_sp;
-
-		if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
-
-			const hrt_abstime now = hrt_absolute_time();
-			const float dt = math::constrain((now - _timestamp_last) / 1e6f, 0.002f, 0.2f);
 
 		_hover_thrust_ekf.predict(dt);
 
-			if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) {
+		vehicle_local_position_setpoint_s local_pos_sp;
+
+		if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
+			if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
 				// Inform the hover thrust estimator about the measured vertical
 				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
+				ZeroOrderHoverThrustEkf::status status;
 				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
+
+				const bool valid = _in_air && (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f);
+				_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
+				_valid = _valid_hysteresis.get_state();
+
+				publishStatus(local_pos.timestamp, status);
 			}
 		}
 
 	} else {
+		_valid_hysteresis.set_state_and_update(false, hrt_absolute_time());
+
 		if (!_armed) {
 			reset();
 		}
 
-		status.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate();
+		if (_valid) {
-		status.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar();
+			// only publish a single message to invalidate
-		status.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar();
+			publishInvalidStatus();
-		status.innov = NAN;
-		status.innov_var = NAN;
-		status.innov_test_ratio = NAN;
-	}
 
-	publishStatus(status);
+			_valid = false;
+		}
+	}
 
 	perf_end(_cycle_perf);
 }
 
-void MulticopterHoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status)
+void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample,
+		const ZeroOrderHoverThrustEkf::status &status)
 {
-	hover_thrust_estimate_s status_msg{};
+	hover_thrust_estimate_s status_msg;
+
+	status_msg.timestamp_sample = timestamp_sample;
+
 	status_msg.hover_thrust = status.hover_thrust;
 	status_msg.hover_thrust_var = status.hover_thrust_var;
+
 	status_msg.accel_innov = status.innov;
 	status_msg.accel_innov_var = status.innov_var;
 	status_msg.accel_innov_test_ratio = status.innov_test_ratio;
 	status_msg.accel_noise_var = status.accel_noise_var;
+
+	status_msg.valid = _valid;
+
 	status_msg.timestamp = hrt_absolute_time();
+
+	_hover_thrust_ekf_pub.publish(status_msg);
+}
+
+void MulticopterHoverThrustEstimator::publishInvalidStatus()
+{
+	hover_thrust_estimate_s status_msg{};
+
+	status_msg.hover_thrust = NAN;
+	status_msg.hover_thrust_var = NAN;
+	status_msg.accel_innov = NAN;
+	status_msg.accel_innov_var = NAN;
+	status_msg.accel_innov_test_ratio = NAN;
+	status_msg.accel_noise_var = NAN;
+
+	status_msg.timestamp = hrt_absolute_time();
+
 	_hover_thrust_ekf_pub.publish(status_msg);
 }
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -42,6 +42,7 @@
 #pragma once
 
 #include <drivers/drv_hrt.h>
+#include <lib/hysteresis/hysteresis.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -86,7 +87,9 @@ private:
 	void updateParams() override;
 
 	void reset();
-	void publishStatus(ZeroOrderHoverThrustEkf::status &status);
+
+	void publishStatus(const hrt_abstime &timestamp_sample, const ZeroOrderHoverThrustEkf::status &status);
+	void publishInvalidStatus();
 
 	ZeroOrderHoverThrustEkf _hover_thrust_ekf{};
 
@@ -105,6 +108,10 @@ private:
 	bool _landed{false};
 	bool _in_air{false};
 
+	bool _valid{false};
+
+	systemlib::Hysteresis _valid_hysteresis{false};
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
 
 	DEFINE_PARAMETERS(

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -435,9 +435,11 @@ MulticopterPositionControl::poll_subscriptions()
 		hover_thrust_estimate_s hte;
 
 		if (_hover_thrust_estimate_sub.update(&hte)) {
+			if (hte.valid) {
 				_control.updateHoverThrust(hte.hover_thrust);
 			}
 		}
+	}
 }
 
 void