move vehicle at rest detection ekf2 -> land_detector

- move vehicle at reset detection ekf2 -> land_detector
 - ekf_unit: reduce init period
   - Fake fusion is when at rest is quite strong and makes the variance reduce rapidly. Reduce the amount of time we wait before checking if the variances are still large enough.
 - ekf_unit: reduce minimum vel/pos variance required after init
   - Fake pos fusion has a low observation noise, making the vel/pos variances reduce quickly.

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

msg/ekf_gps_drift.msg

@@ -2,4 +2,5 @@ uint64 timestamp		# time since system start (microseconds)
 float32 hpos_drift_rate		# Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s)
 float32 vpos_drift_rate		# Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s)
 float32 hspd			# Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s)
-bool blocked			# true when drift calculation is blocked due to IMU movement check controlled by EKF2_MOVE_TEST
+
+bool blocked			# true when drift calculation is blocked due to IMU movement check

msg/estimator_status.msg

@@ -1,11 +1,6 @@
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[3] vibe			# IMU vibration metrics in the following array locations
-# 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
-# 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
-# 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
-
 float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m)
 
 uint16 gps_check_fail_flags     # Bitmask to indicate status of GPS checks - see definition below

msg/vehicle_land_detected.msg

@@ -1,11 +1,18 @@
 uint64 timestamp	# time since system start (microseconds)
+
 bool freefall		# true if vehicle is currently in free-fall
 bool ground_contact	# true if vehicle has ground contact but is not landed (1. stage)
 bool maybe_landed	# true if the vehicle might have landed (2. stage)
 bool landed		# true if vehicle is currently landed on the ground (3. stage)
+
 bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing).
 bool in_descend
+
 bool has_low_throttle
+
 bool vertical_movement
 bool horizontal_movement
+
 bool close_to_ground_or_skipped_check
+
+bool at_rest

src/modules/ekf2/EKF/common.h

@@ -374,9 +374,6 @@ struct parameters {
 	const float auxvel_noise{0.5f};		///< minimum observation noise, uses reported noise if greater (m/s)
 	const float auxvel_gate{5.0f};		///< velocity fusion innovation consistency gate size (STD)
 
-	// control of on-ground movement check
-	float is_moving_scaler{1.0f};		///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive.
-
 	// compute synthetic magnetomter Z value if possible
 	int32_t synthesize_mag_z{0};
 	int32_t check_mag_strength{0};

src/modules/ekf2/EKF/ekf.cpp

@@ -82,6 +82,8 @@ void Ekf::reset()
 
 	_prev_dvel_bias_var.zero();
 
+	_control_status.flags.vehicle_at_rest = true;
+
 	resetGpsDriftCheckFilters();
 }
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -75,9 +75,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 
 	_newest_high_rate_imu_sample = imu_sample;
 
-	// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
-	_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt, imu_sample);
-
 	_imu_updated = _imu_down_sampler.update(imu_sample);
 
 	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
@@ -96,25 +93,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 	}
 }
 
-bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
-{
-	// detect if the vehicle is not moving when on ground
-	if (!_control_status.flags.in_air) {
-		if (((_vibe_metrics(1) * 4.0e4f > _params.is_moving_scaler) || (_vibe_metrics(2) * 2.1e2f > _params.is_moving_scaler))
-		    && ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
-
-			_time_last_move_detect_us = imu.time_us;
-		}
-
-		return ((imu.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
-
-	} else {
-		_time_last_move_detect_us = imu.time_us;
-		return false;
-	}
-}
-
-
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
 	if (!_initialised) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -81,17 +81,6 @@ public:
 
 	void setIMUData(const imuSample &imu_sample);
 
-	/*
-	Returns  following IMU vibration metrics in the following array locations
-	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
-	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
-	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
-	*/
-	const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
-	void setDeltaAngleConingMetric(float delta_angle_coning_metric) { _vibe_metrics(0) = delta_angle_coning_metric; }
-	void setDeltaAngleHighFrequencyVibrationMetric(float delta_angle_vibration_metric) { _vibe_metrics(1) = delta_angle_vibration_metric; }
-	void setDeltaVelocityHighFrequencyVibrationMetric(float delta_velocity_vibration_metric) { _vibe_metrics(2) = delta_velocity_vibration_metric; }
-
 	void setMagData(const magSample &mag_sample);
 
 	void setGpsData(const gps_message &gps);
@@ -127,6 +116,8 @@ public:
 		_control_status.flags.in_air = in_air;
 	}
 
+	void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; }
+
 	// return true if the attitude is usable
 	bool attitude_valid() const { return PX4_ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
 
@@ -248,8 +239,6 @@ public:
 	const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
 	void clear_information_events() { _information_events.value = 0; }
 
-	bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
-
 	// Getter for the average imu update period in s
 	float get_dt_imu_avg() const { return _dt_imu_avg; }
 
@@ -363,7 +352,7 @@ protected:
 					// [0] Horizontal position drift rate (m/s)
 					// [1] Vertical position drift rate (m/s)
 					// [2] Filtered horizontal velocity (m/s)
-	uint64_t _time_last_move_detect_us{0};	// timestamp of last movement detection event in microseconds
+
 	uint64_t _time_last_on_ground_us{0};	///< last time we were on the ground (uSec)
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
 	bool _gps_drift_updated{false};	// true when _gps_drift_metrics has been updated and is ready for retrieval
@@ -422,26 +411,14 @@ private:
 
 	inline void setDragData(const imuSample &imu);
 
-	inline void computeVibrationMetric(const imuSample &imu);
-	inline bool checkIfVehicleAtRest(float dt, const imuSample &imu);
-
 	void printBufferAllocationFailed(const char *buffer_name);
 
 	ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
 
 	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
 
-	// IMU vibration and movement monitoring
-	Vector3f _vibe_metrics{};	// IMU vibration metrics
-					// [0] Level of coning vibration in the IMU delta angles (rad^2)
-					// [1] high frequency vibration level in the IMU delta angle data (rad)
-					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
-
 	// Used by the multi-rotor specific drag force fusion
 	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
 	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
-
-	// observation buffer final allocation failed
-	uint64_t _last_allocation_fail_print{0};
 };
 #endif // !EKF_ESTIMATOR_INTERFACE_H

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -105,7 +105,7 @@ void Ekf::fuseFakePosition()
 	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
 		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
 
-	} else if (_control_status.flags.vehicle_at_rest) {
+	} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Accelerate tilt fine alignment by fusing more
 		// aggressively when the vehicle is at rest
 		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.1f);

src/modules/ekf2/EKF2.cpp

@@ -160,7 +160,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
 	_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
 	_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
-	_param_ekf2_move_test(_params->is_moving_scaler),
 	_param_ekf2_mag_check(_params->check_mag_strength),
 	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
@@ -385,8 +384,6 @@ void EKF2::Run()
 				_gyro_calibration_count = imu.gyro_calibration_count;
 
 			} else {
-				bool reset_actioned = false;
-
 				if ((imu.accel_calibration_count != _accel_calibration_count)
 				    || (imu.accel_device_id != _device_id_accel)) {
 
@@ -398,8 +395,6 @@ void EKF2::Run()
 
 					// reset bias learning
 					_accel_cal = {};
-
-					reset_actioned = true;
 				}
 
 				if ((imu.gyro_calibration_count != _gyro_calibration_count)
@@ -413,12 +408,6 @@ void EKF2::Run()
 
 					// reset bias learning
 					_gyro_cal = {};
-
-					reset_actioned = true;
-				}
-
-				if (reset_actioned) {
-					SelectImuStatus();
 				}
 			}
 		}
@@ -460,8 +449,6 @@ void EKF2::Run()
 
 					// reset bias learning
 					_accel_cal = {};
-
-					SelectImuStatus();
 				}
 
 				if (_device_id_gyro != sensor_selection.gyro_device_id) {
@@ -472,8 +459,6 @@ void EKF2::Run()
 
 					// reset bias learning
 					_gyro_cal = {};
-
-					SelectImuStatus();
 				}
 			}
 		}
@@ -482,8 +467,6 @@ void EKF2::Run()
 	if (imu_updated) {
 		const hrt_abstime now = imu_sample_new.time_us;
 
-		UpdateImuStatus();
-
 		// push imu data into estimator
 		_ekf.setIMUData(imu_sample_new);
 		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
@@ -534,6 +517,7 @@ void EKF2::Run()
 			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
 				const bool was_in_air = _ekf.control_status_flags().in_air;
 				_ekf.set_in_air_status(!vehicle_land_detected.landed);
+				_ekf.set_vehicle_at_rest(vehicle_land_detected.at_rest);
 
 				if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
 					if (!_had_valid_terrain) {
@@ -1194,7 +1178,6 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 
 	_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
 	_ekf.get_ekf_soln_status(&status.solution_status_flags);
-	_ekf.getImuVibrationMetrics().copyTo(status.vibe);
 
 	// reset counters
 	status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
@@ -1401,48 +1384,6 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 	return amsl_hgt + _wgs84_hgt_offset;
 }
 
-void EKF2::SelectImuStatus()
-{
-	for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) {
-		uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
-
-		vehicle_imu_status_s imu_status{};
-		imu_status_sub.copy(&imu_status);
-
-		if (imu_status.accel_device_id == _device_id_accel) {
-			_vehicle_imu_status_sub.ChangeInstance(imu_instance);
-			return;
-		}
-	}
-
-	PX4_WARN("%d - IMU status not found for accel %" PRId32 ", gyro %" PRId32, _instance, _device_id_accel,
-		 _device_id_gyro);
-}
-
-void EKF2::UpdateImuStatus()
-{
-	vehicle_imu_status_s imu_status;
-
-	if (_vehicle_imu_status_sub.update(&imu_status)) {
-		if (imu_status.accel_device_id != _device_id_accel) {
-			SelectImuStatus();
-			return;
-		}
-
-		const float dt = _ekf.get_dt_imu_avg();
-
-		// accel -> delta velocity
-		_ekf.setDeltaVelocityHighFrequencyVibrationMetric(imu_status.accel_vibration_metric * dt);
-
-		// gyro -> delta angle
-		_ekf.setDeltaAngleHighFrequencyVibrationMetric(imu_status.gyro_vibration_metric * dt);
-
-		// note: the coning metric uses the cross product of two consecutive angular velocities
-		// this is why the conversion to delta angle requires dt^2
-		_ekf.setDeltaAngleConingMetric(imu_status.gyro_coning_vibration * dt * dt);
-	}
-}
-
 void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF airspeed sample

src/modules/ekf2/EKF2.hpp

@@ -86,7 +86,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_imu.h>
-#include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
@@ -154,8 +153,6 @@ private:
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
-	void SelectImuStatus();
-
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -164,7 +161,6 @@ private:
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
-	void UpdateImuStatus();
 
 	void UpdateAccelCalibration(const hrt_abstime &timestamp);
 	void UpdateGyroCalibration(const hrt_abstime &timestamp);
@@ -257,7 +253,6 @@ private:
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
@@ -536,10 +531,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
 		_param_ekf2_pcoef_z,	///< static pressure position error coefficient along the Z body axis
 
-		// Test used to determine if the vehicle is static or moving
-		(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
-		_param_ekf2_move_test,	///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
-
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
 		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
 		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)

src/modules/ekf2/ekf2_params.c

@@ -159,9 +159,9 @@ PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5);
  * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH
  * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV
  * 4 : Maximum allowed speed error set by EKF2_REQ_SACC
- * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
- * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
- * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
+ * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary.
+ * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check will only run when the vehicle is on ground and stationary.
+ * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary.
  * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT
  *
  * @group EKF2
@@ -480,7 +480,7 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
  * If set to 'Magnetic heading' magnetic heading fusion is used at all times
  * If set to '3-axis' 3-axis field fusion is used at all times.
  * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
- * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests controlled by the EKF2_MOVE_TEST parameter indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
+ * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
  * If set to 'None' the magnetometer will not be used under any circumstance. If no external source of yaw is available, it is possible to use post-takeoff horizontal movement combined with GPS velocity measurements to align the yaw angle with the timer required (depending on the amount of movement and GPS data quality). Other external sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
  * @group EKF2
  * @value 0 Automatic
@@ -700,7 +700,7 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
 /**
  * Expected range finder reading when on ground.
  *
- * If the vehicle is on ground, is not moving as determined by the motion test controlled by EKF2_MOVE_TEST and the range finder is returning invalid or no data, then an assumed range value of EKF2_MIN_RNG will be used by the terrain estimator so that a terrain height estimate is avilable at the start of flight in situations where the range finder may be inside its minimum measurements distance when on ground.
+ * If the vehicle is on ground, is not moving as determined by the motion test and the range finder is returning invalid or no data, then an assumed range value of EKF2_MIN_RNG will be used by the terrain estimator so that a terrain height estimate is avilable at the start of flight in situations where the range finder may be inside its minimum measurements distance when on ground.
  *
  * @group EKF2
  * @min 0.01
@@ -1320,18 +1320,6 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f);
 
-/**
- * Vehicle movement test threshold
- *
- * Scales the threshold tests applied to IMU data used to determine if the vehicle is static or moving. See parameter descriptions for EKF2_GPS_CHECK and EKF2_MAG_TYPE for further information on the functionality affected by this parameter.
- *
- * @group EKF2
- * @min 0.1
- * @max 10.0
- * @decimal 1
- */
-PARAM_DEFINE_FLOAT(EKF2_MOVE_TEST, 1.0f);
-
 /**
  * Required GPS health time on startup
  *

src/modules/ekf2/test/sensor_simulator/imu.h

@@ -55,6 +55,11 @@ public:
 	void setAccelData(const Vector3f &accel);
 	void setGyroData(const Vector3f &gyro);
 
+	bool moving()
+	{
+		return ((fabsf(_accel_data.norm() - CONSTANTS_ONE_G) > 0.01f) || (_gyro_data.norm() > 0.01f));
+	}
+
 private:
 	Vector3f _accel_data;
 	Vector3f _gyro_data;

src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp

@@ -154,6 +154,7 @@ void SensorSimulator::runMicroseconds(uint32_t duration)
 
 		if (update_imu) {
 			// Update at IMU rate
+			_ekf->set_vehicle_at_rest(!_imu.moving());
 			_ekf->update();
 		}
 	}
@@ -193,6 +194,7 @@ void SensorSimulator::runReplayMicroseconds(uint32_t duration)
 		updateSensors();
 
 		if (update_imu) {
+			_ekf->set_vehicle_at_rest(!_imu.moving());
 			_ekf->update();
 		}
 	}
@@ -316,6 +318,7 @@ void SensorSimulator::runTrajectoryMicroseconds(uint32_t duration)
 		updateSensors();
 
 		if (update_imu) {
+			_ekf->set_vehicle_at_rest(!_imu.moving());
 			_ekf->update();
 		}
 	}

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -50,7 +50,7 @@ public:
 	SensorSimulator _sensor_simulator;
 	EkfWrapper _ekf_wrapper;
 
-	const float _init_tilt_period = 1.0; // seconds
+	const float _init_tilt_period = 0.3f; // seconds
 
 	// GTests is calling this
 	void SetUp() override
@@ -106,12 +106,12 @@ public:
 		const Vector3f pos_var = _ekf->getPositionVariance();
 		const Vector3f vel_var = _ekf->getVelocityVariance();
 
-		const float pos_variance_limit = 0.1f;
+		const float pos_variance_limit = 0.01f; // Fake fusion obs var when at rest
 		EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(0)" << pos_var(0);
 		EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(1)" << pos_var(1);
 		EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(2)" << pos_var(2);
 
-		const float vel_variance_limit = 0.3f;
+		const float vel_variance_limit = 0.16f;
 		EXPECT_TRUE(vel_var(0) > vel_variance_limit) << "vel_var(0)" << vel_var(0);
 		EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(1)" << vel_var(1);
 		EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(2)" << vel_var(2);

src/modules/land_detector/LandDetector.cpp

@@ -48,7 +48,18 @@ namespace land_detector
 LandDetector::LandDetector() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
-{}
+{
+	_land_detected.ground_contact = true;
+	_land_detected.maybe_landed = true;
+	_land_detected.landed = true;
+	_land_detected.in_ground_effect = true;
+	_land_detected.in_descend = false;
+	_land_detected.has_low_throttle = false;
+	_land_detected.vertical_movement = false;
+	_land_detected.horizontal_movement = false;
+	_land_detected.close_to_ground_or_skipped_check = true;
+	_land_detected.at_rest = true;
+}
 
 LandDetector::~LandDetector()
 {
@@ -91,6 +102,18 @@ void LandDetector::Run()
 		_acceleration = matrix::Vector3f{vehicle_acceleration.xyz};
 	}
 
+	vehicle_angular_velocity_s vehicle_angular_velocity{};
+
+	if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
+		_angular_velocity = matrix::Vector3f{vehicle_angular_velocity.xyz};
+
+		static constexpr float GYRO_NORM_MAX = math::radians(3.f); // 3 degrees/second
+
+		if (_angular_velocity.norm() > GYRO_NORM_MAX) {
+			_time_last_move_detect_us = vehicle_angular_velocity.timestamp_sample;
+		}
+	}
+
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 	_vehicle_status_sub.update(&_vehicle_status);
 
@@ -124,13 +147,18 @@ void LandDetector::Run()
 	const bool landDetected = _landed_hysteresis.get_state();
 	const bool in_ground_effect = _ground_effect_hysteresis.get_state();
 
+	UpdateVehicleAtRest();
+
+	const bool at_rest = landDetected && _at_rest;
+
 	// 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) ||
 	    (_land_detected.freefall != freefallDetected) ||
 	    (_land_detected.maybe_landed != maybe_landedDetected) ||
 	    (_land_detected.ground_contact != ground_contactDetected) ||
-	    (_land_detected.in_ground_effect != in_ground_effect)) {
+	    (_land_detected.in_ground_effect != in_ground_effect) ||
+	    (_land_detected.at_rest != at_rest)) {
 
 		if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
 			// We did take off
@@ -147,6 +175,7 @@ void LandDetector::Run()
 		_land_detected.horizontal_movement = _get_horizontal_movement();
 		_land_detected.vertical_movement = _get_vertical_movement();
 		_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
+		_land_detected.at_rest = at_rest;
 		_land_detected.timestamp = hrt_absolute_time();
 		_vehicle_land_detected_pub.publish(_land_detected);
 	}
@@ -178,4 +207,51 @@ void LandDetector::Run()
 	}
 }
 
+void LandDetector::UpdateVehicleAtRest()
+{
+	if (_sensor_selection_sub.updated()) {
+		sensor_selection_s sensor_selection{};
+		_sensor_selection_sub.copy(&sensor_selection);
+
+		if (sensor_selection.gyro_device_id != _device_id_gyro) {
+
+			bool gyro_status_found = false;
+
+			// find corresponding vehicle_imu_status instance
+			for (uint8_t imu_instance = 0; imu_instance < 4; imu_instance++) {
+				uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
+
+				vehicle_imu_status_s imu_status{};
+				imu_status_sub.copy(&imu_status);
+
+				if ((imu_status.gyro_device_id != 0) && (imu_status.gyro_device_id == sensor_selection.gyro_device_id)) {
+					_vehicle_imu_status_sub.ChangeInstance(imu_instance);
+					_device_id_gyro = sensor_selection.gyro_device_id;
+					gyro_status_found = true;
+					break;
+				}
+			}
+
+			if (!gyro_status_found) {
+				PX4_WARN("IMU status not found for gyro %" PRId32, sensor_selection.gyro_device_id);
+			}
+		}
+	}
+
+	vehicle_imu_status_s imu_status;
+
+	if (_vehicle_imu_status_sub.update(&imu_status)) {
+		static constexpr float GYRO_VIBE_METRIC_MAX = 0.02f; // gyro_vibration_metric * dt * 4.0e4f > is_moving_scaler)
+		static constexpr float ACCEL_VIBE_METRIC_MAX = 1.2f; // accel_vibration_metric * dt * 2.1e2f > is_moving_scaler
+
+		if ((imu_status.gyro_vibration_metric > GYRO_VIBE_METRIC_MAX)
+		    || (imu_status.accel_vibration_metric > ACCEL_VIBE_METRIC_MAX)) {
+
+			_time_last_move_detect_us = imu_status.timestamp;
+		}
+	}
+
+	_at_rest = (hrt_elapsed_time(&_time_last_move_detect_us) > 1_s);
+}
+
 } // namespace land_detector

src/modules/land_detector/LandDetector.h

@@ -59,7 +59,10 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_status.h>
@@ -148,6 +151,7 @@ protected:
 	vehicle_status_s         _vehicle_status{};
 
 	matrix::Vector3f _acceleration{};
+	matrix::Vector3f _angular_velocity{};
 
 	bool _armed{false};
 	bool _previous_armed_state{false};	///< stores the previous actuator_armed.armed state
@@ -156,17 +160,14 @@ protected:
 private:
 	void Run() override;
 
-	vehicle_land_detected_s _land_detected = {
-		.timestamp = 0,
-		.freefall = false,
-		.ground_contact = true,
-		.maybe_landed = true,
-		.landed = true,
-	};
+	void UpdateVehicleAtRest();
 
+	vehicle_land_detected_s _land_detected{};
 	hrt_abstime _takeoff_time{0};
 	hrt_abstime _total_flight_time{0};	///< total vehicle flight time in microseconds
 
+	hrt_abstime _time_last_move_detect_us{0};	// timestamp of last movement detection event in microseconds
+
 	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)};
@@ -174,11 +175,18 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
+	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
+	uint32_t _device_id_gyro{0};
+
+	bool _at_rest{true};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		ModuleParams,
 		(ParamInt<px4::params::LND_FLIGHT_T_HI>) _param_total_flight_time_high,

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -285,11 +285,10 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	}
 
 	// Next look if all rotation angles are not moving.
-	vehicle_angular_velocity_s vehicle_angular_velocity{};
-	_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
-	const Vector2f angular_velocity{vehicle_angular_velocity.xyz[0], vehicle_angular_velocity.xyz[1]};
 	const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
 
+	matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)};
+
 	if (angular_velocity.norm() > max_rotation_scaled) {
 		return false;
 	}

src/modules/land_detector/MulticopterLandDetector.h

@@ -44,7 +44,6 @@
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/hover_thrust_estimate.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/takeoff_status.h>
@@ -110,7 +109,7 @@ private:
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};