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

ekf2: fix bug causing faulty imu data storage

Browse Source
This commit is contained in:
bugobliterator
2016-01-29 23:56:24 -08:00
parent 92dc41e885
commit a779d45cd0
2 changed files with 2 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+1 -2
View File
@@ -87,7 +87,6 @@ bool Ekf::update()
return false; return false;
} }
} }
//printStates(); //printStates();
//printStatesFast(); //printStatesFast();
// prediction // prediction
@@ -289,7 +288,7 @@ bool Ekf::collect_imu(imuSample &imu)
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled) || if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled) ||
_dt_imu_avg * _imu_ticks >= 0.02f){ _dt_imu_avg * _imu_ticks >= 0.02f){
_imu_sample_new = { imu = {
delta_ang : _q_down_sampled.to_axis_angle(), delta_ang : _q_down_sampled.to_axis_angle(),
delta_vel : _imu_down_sampled.delta_vel, delta_vel : _imu_down_sampled.delta_vel,
delta_ang_dt : _imu_down_sampled.delta_ang_dt, delta_ang_dt : _imu_down_sampled.delta_ang_dt,
EKF/estimator_base.cpp
+1 -2
View File
@@ -74,8 +74,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
if (_time_last_imu > 0) { if (_time_last_imu > 0) {
_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
} }
delta_ang_dt = delta_ang_dt / 1e6f;
delta_vel_dt = delta_vel_dt / 1e6f;
// copy data // copy data
imuSample imu_sample_new = {}; imuSample imu_sample_new = {};
@@ -88,6 +86,7 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
imu_sample_new.time_us = time_usec; imu_sample_new.time_us = time_usec;
_imu_ticks++; _imu_ticks++;
if (collect_imu(imu_sample_new)) { if (collect_imu(imu_sample_new)) {
_imu_buffer.push(imu_sample_new); _imu_buffer.push(imu_sample_new);
_imu_ticks = 0; _imu_ticks = 0;