fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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:45:50 -08:00
parent 92dc41e885
commit a779d45cd0
2 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.cpp
View File

@ -87,7 +87,6 @@ bool Ekf::update()
return false;
}
}
//printStates();
//printStatesFast();
// 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) ||
_dt_imu_avg * _imu_ticks >= 0.02f){
_imu_sample_new = {
imu = {
delta_ang : _q_down_sampled.to_axis_angle(),
delta_vel : _imu_down_sampled.delta_vel,
delta_ang_dt : _imu_down_sampled.delta_ang_dt,

3
EKF/estimator_base.cpp
View File

@ -74,8 +74,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
if (_time_last_imu > 0) {
_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
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_ticks++;
if (collect_imu(imu_sample_new)) {
_imu_buffer.push(imu_sample_new);
_imu_ticks = 0;