dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Add on ground movement detector

This commit is contained in:
Paul Riseborough 2018-05-16 10:31:22 +10:00 committed by Lorenz Meier
parent 2d3b65231b
commit e10798bfdf
2 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
EKF/estimator_interface.cpp
View File

@ -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);

4
EKF/estimator_interface.h
View File

@ -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;