|
|
@ -310,11 +310,11 @@ bool Ekf::collect_imu(imuSample &imu) |
|
|
|
|
|
|
|
|
|
|
|
// store the new sample for the complementary filter prediciton
|
|
|
|
// store the new sample for the complementary filter prediciton
|
|
|
|
_imu_sample_new = { |
|
|
|
_imu_sample_new = { |
|
|
|
delta_ang : imu.delta_ang, |
|
|
|
.delta_ang = imu.delta_ang, |
|
|
|
delta_vel : imu.delta_vel, |
|
|
|
.delta_vel = imu.delta_vel, |
|
|
|
delta_ang_dt : imu.delta_ang_dt, |
|
|
|
.delta_ang_dt = imu.delta_ang_dt, |
|
|
|
delta_vel_dt : imu.delta_vel_dt, |
|
|
|
.delta_vel_dt = imu.delta_vel_dt, |
|
|
|
time_us : imu.time_us |
|
|
|
.time_us = imu.time_us |
|
|
|
}; |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; |
|
|
|
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; |
|
|
@ -333,11 +333,11 @@ bool Ekf::collect_imu(imuSample &imu) |
|
|
|
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
|
|
|
|
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
|
|
|
|
_dt_imu_avg * _imu_ticks >= 0.02f){ |
|
|
|
_dt_imu_avg * _imu_ticks >= 0.02f){ |
|
|
|
imu = { |
|
|
|
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, |
|
|
|
delta_vel_dt : _imu_down_sampled.delta_vel_dt, |
|
|
|
.delta_vel_dt = _imu_down_sampled.delta_vel_dt, |
|
|
|
time_us : imu.time_us |
|
|
|
.time_us = imu.time_us |
|
|
|
}; |
|
|
|
}; |
|
|
|
_imu_down_sampled.delta_ang.setZero(); |
|
|
|
_imu_down_sampled.delta_ang.setZero(); |
|
|
|
_imu_down_sampled.delta_vel.setZero(); |
|
|
|
_imu_down_sampled.delta_vel.setZero(); |
|
|
|