EKF: Add on ground movement detector

EKF/estimator_interface.cpp

@@ -87,6 +87,19 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 	_delta_vel_prev = imu_sample_new.delta_vel;
 	_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
 
+	// detect if the vehicle is not moving when on ground
+	if (!_control_status.flags.in_air) {
+		if ((_vibe_metrics[1] * 4.0E4f > 1.0f)
+				|| (_vibe_metrics[2] * 2.1E2f > 1.0f)
+				|| ((imu_sample_new.delta_ang.norm() / dt) > 0.05f)) {
+			_time_last_move_detect_us = _imu_sample_new.time_us;
+		}
+		_vehicle_at_rest = ((_imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
+	} else {
+		_time_last_move_detect_us = _imu_sample_new.time_us;
+		_vehicle_at_rest = false;
+	}
+
 	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
 	if (collect_imu(imu_sample_new)) {
 		_imu_buffer.push(imu_sample_new);

EKF/estimator_interface.h

@@ -457,13 +457,15 @@ protected:
 	bool _deadreckon_time_exceeded{false};	// true if the horizontal nav solution has been deadreckoning for too long and is invalid
 	bool _is_wind_dead_reckoning{false};	// true if we are navigating reliant on wind relative measurements
 
-	// IMU vibration monitoring
+	// IMU vibration and movement monitoring
 	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
 	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
 	float _vibe_metrics[3] {};	// IMU vibration metrics
 					// [0] Level of coning vibration in the IMU delta angles (rad^2)
 					// [1] high frequency vibraton level in the IMU delta angle data (rad)
 					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
+	bool _vehicle_at_rest{false};	// true when the vehicle is at rest
+	uint64_t _time_last_move_detect_us{0};	// timestamp of last movement detection event in microseconds
 
 	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer;