fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 23:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add on ground movement detector

Browse Source
This commit is contained in:
Paul Riseborough
2018-06-09 22:23:55 +02:00
committed by Lorenz Meier
parent 2d3b65231b
commit e10798bfdf
2 changed files with 16 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+13
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);