From e10798bfdf6c8152c12470ce1d3d2366af344113 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 16 May 2018 10:31:22 +1000 Subject: [PATCH] EKF: Add on ground movement detector --- EKF/estimator_interface.cpp | 13 +++++++++++++ EKF/estimator_interface.h | 4 +++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 40a55bbdef..0cb1db08a1 100644 --- a/EKF/estimator_interface.cpp +++ b/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); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 3543ff9144..bae75c5046 100644 --- a/EKF/estimator_interface.h +++ b/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 _imu_buffer;