land_detector: multicopter ground contact robustness, fixes, and cleanup

 - if "landed" and "maybe_landed" states are false then both the "hit_ground" and the "low_thrust" condition need to be true in order to detect landing
 - ground contact MC NAN setpoint workaround
 - ground contact additionally check acceleration setpoint
 - schedule with vehicle_local_position updates (most updates require valid local position)
 - don't allow LNDMC_Z_VEL_MAX to exceed MPC_LAND_SPEED
 - ground contact horizontal and vertical movement checks default to
failed if estimates aren't available

Author: dagar

src/modules/commander/Commander.cpp

@@ -1575,7 +1575,6 @@ Commander::run()
 		/* Update land detector */
 		if (_land_detector_sub.updated()) {
 			_was_landed = _land_detector.landed;
-			bool was_falling = _land_detector.freefall;
 			_land_detector_sub.copy(&_land_detector);
 
 			// Only take actions if armed
@@ -1596,12 +1595,6 @@ Commander::run()
 						_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
 					}
 				}
-
-				if (was_falling != _land_detector.freefall) {
-					if (_land_detector.freefall) {
-						mavlink_log_info(&mavlink_log_pub, "Freefall detected");
-					}
-				}
 			}
 		}
 

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -51,16 +51,10 @@ FixedwingLandDetector::FixedwingLandDetector()
 	_landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US);
 }
 
-void FixedwingLandDetector::_update_topics()
-{
-	LandDetector::_update_topics();
-	_airspeed_validated_sub.update(&_airspeed_validated);
-}
-
 bool FixedwingLandDetector::_get_landed_state()
 {
 	// Only trigger flight conditions if we are armed.
-	if (!_actuator_armed.armed) {
+	if (!_armed) {
 		return true;
 	}
 
@@ -83,19 +77,20 @@ bool FixedwingLandDetector::_get_landed_state()
 			_velocity_z_filtered = val;
 		}
 
+		airspeed_validated_s airspeed_validated{};
+		_airspeed_validated_sub.copy(&airspeed_validated);
+
 		// set _airspeed_filtered to 0 if airspeed data is invalid
-		if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) {
+		if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
 			_airspeed_filtered = 0.0f;
 
 		} else {
-			_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
+			_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
 		}
 
 		// A leaking lowpass prevents biases from building up, but
 		// gives a mostly correct response for short impulses.
-		const matrix::Vector3f accel{_vehicle_acceleration.xyz};
-		const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));
-
+		const float acc_hor = sqrtf(_acceleration(0) * _acceleration(0) + _acceleration(1) * _acceleration(1));
 		_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
 
 		// Crude land detector for fixedwing.

src/modules/land_detector/FixedwingLandDetector.h

@@ -61,7 +61,7 @@ class FixedwingLandDetector final : public LandDetector
 protected:
 
 	bool _get_landed_state() override;
-	void _update_topics() override;
+	void _update_topics() override {};
 
 private:
 
@@ -71,8 +71,6 @@ class FixedwingLandDetector final : public LandDetector
 
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
-	airspeed_validated_s _airspeed_validated{};
-
 	float _airspeed_filtered{0.0f};
 	float _velocity_xy_filtered{0.0f};
 	float _velocity_z_filtered{0.0f};

src/modules/land_detector/LandDetector.cpp

@@ -57,21 +57,52 @@ LandDetector::~LandDetector()
 
 void LandDetector::start()
 {
-	_update_params();
-	ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
+	ScheduleDelayed(50_ms);
+	_vehicle_local_position_sub.registerCallback();
 }
 
 void LandDetector::Run()
 {
+	// push backup schedule
+	ScheduleDelayed(50_ms);
+
 	perf_begin(_cycle_perf);
 
-	if (_parameter_update_sub.updated()) {
+	if (_parameter_update_sub.updated() || (_land_detected.timestamp == 0)) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
 		_update_params();
+
+		_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
+		_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
+	}
+
+	actuator_armed_s actuator_armed;
+
+	if (_actuator_armed_sub.update(&actuator_armed)) {
+		_armed = actuator_armed.armed;
 	}
 
-	_actuator_armed_sub.update(&_actuator_armed);
+	vehicle_acceleration_s vehicle_acceleration;
+
+	if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
+		_acceleration = matrix::Vector3f{vehicle_acceleration.xyz};
+	}
+
+	_vehicle_local_position_sub.update(&_vehicle_local_position);
+	_vehicle_status_sub.update(&_vehicle_status);
+
 	_update_topics();
-	_update_state();
+
+	const hrt_abstime now_us = hrt_absolute_time();
+
+	_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);
+	_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us);
+	_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us);
+	_landed_hysteresis.set_state_and_update(_get_landed_state(), now_us);
+	_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us);
 
 	const bool freefallDetected = _freefall_hysteresis.get_state();
 	const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
@@ -80,8 +111,6 @@ void LandDetector::Run()
 	const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY;
 	const bool in_ground_effect = _ground_effect_hysteresis.get_state();
 
-	const hrt_abstime now = hrt_absolute_time();
-
 	// publish at 1 Hz, very first time, or when the result has changed
 	if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) ||
 	    (_land_detected.landed != landDetected) ||
@@ -93,24 +122,23 @@ void LandDetector::Run()
 
 		if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
 			// We did take off
-			_takeoff_time = now;
+			_takeoff_time = now_us;
 		}
 
-		_land_detected.timestamp = hrt_absolute_time();
 		_land_detected.landed = landDetected;
 		_land_detected.freefall = freefallDetected;
 		_land_detected.maybe_landed = maybe_landedDetected;
 		_land_detected.ground_contact = ground_contactDetected;
 		_land_detected.alt_max = alt_max;
 		_land_detected.in_ground_effect = in_ground_effect;
-
+		_land_detected.timestamp = hrt_absolute_time();
 		_vehicle_land_detected_pub.publish(_land_detected);
 	}
 
 	// set the flight time when disarming (not necessarily when landed, because all param changes should
 	// happen on the same event and it's better to set/save params while not in armed state)
-	if (_takeoff_time != 0 && !_actuator_armed.armed && _previous_armed_state) {
-		_total_flight_time += now - _takeoff_time;
+	if (_takeoff_time != 0 && !_armed && _previous_armed_state) {
+		_total_flight_time += now_us - _takeoff_time;
 		_takeoff_time = 0;
 
 		uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
@@ -124,7 +152,7 @@ void LandDetector::Run()
 		_param_total_flight_time_low.commit_no_notification();
 	}
 
-	_previous_armed_state = _actuator_armed.armed;
+	_previous_armed_state = _armed;
 
 	perf_end(_cycle_perf);
 
@@ -134,36 +162,4 @@ void LandDetector::Run()
 	}
 }
 
-void LandDetector::_update_params()
-{
-	parameter_update_s param_update;
-	_parameter_update_sub.copy(&param_update);
-
-	updateParams();
-	_update_total_flight_time();
-}
-
-void LandDetector::_update_state()
-{
-	const hrt_abstime now_us = hrt_absolute_time();
-	_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);
-	_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us);
-	_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us);
-	_landed_hysteresis.set_state_and_update(_get_landed_state(), now_us);
-	_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us);
-}
-
-void LandDetector::_update_topics()
-{
-	_actuator_armed_sub.update(&_actuator_armed);
-	_vehicle_acceleration_sub.update(&_vehicle_acceleration);
-	_vehicle_local_position_sub.update(&_vehicle_local_position);
-}
-
-void LandDetector::_update_total_flight_time()
-{
-	_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
-	_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
-}
-
 } // namespace land_detector

src/modules/land_detector/LandDetector.h

@@ -56,11 +56,13 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
 
 using namespace time_literals;
 
@@ -70,11 +72,9 @@ namespace land_detector
 class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::ScheduledWorkItem
 {
 public:
-
 	LandDetector();
 	virtual ~LandDetector();
 
-
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[])
 	{
@@ -99,12 +99,12 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
 	/**
 	 * Updates parameters.
 	 */
-	virtual void _update_params();
+	virtual void _update_params() {};
 
 	/**
 	 * Updates subscribed uORB topics.
 	 */
-	virtual void _update_topics();
+	virtual void _update_topics() = 0;
 
 	/**
 	 * @return true if UAV is in a landed state.
@@ -136,17 +136,22 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
 	 */
 	virtual bool _get_ground_effect_state() { return false; }
 
-	/** Run main land detector loop at this interval. */
-	static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms;
-
 	systemlib::Hysteresis _freefall_hysteresis{false};
 	systemlib::Hysteresis _landed_hysteresis{true};
 	systemlib::Hysteresis _maybe_landed_hysteresis{true};
 	systemlib::Hysteresis _ground_contact_hysteresis{true};
 	systemlib::Hysteresis _ground_effect_hysteresis{false};
 
-	actuator_armed_s         _actuator_armed{};
-	vehicle_acceleration_s   _vehicle_acceleration{};
+	vehicle_local_position_s _vehicle_local_position{};
+	vehicle_status_s         _vehicle_status{};
+
+	matrix::Vector3f _acceleration{};
+
+	bool _armed{false};
+	bool _previous_armed_state{false};	///< stores the previous actuator_armed.armed state
+
+private:
+	void Run() override;
 
 	vehicle_land_detected_s _land_detected = {
 		.timestamp = 0,
@@ -157,29 +162,19 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
 		.landed = true,
 	};
 
-	vehicle_local_position_s _vehicle_local_position{};
-
-	uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};
-
-private:
-
-	void Run() override;
-
-	void _update_state();
-
-	void _update_total_flight_time();
-
-	bool _previous_armed_state{false};	///< stores the previous actuator_armed.armed state
-
 	hrt_abstime _takeoff_time{0};
 	hrt_abstime _total_flight_time{0};	///< total vehicle flight time in microseconds
 
-	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")};
+	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)};
 
 	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		ModuleParams,