fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 11:59:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: don't store _auxvel_sample_delayed

Browse Source
This commit is contained in:
Daniel Agar 2022-02-01 09:48:03 -05:00
parent 452b5e94b4
commit 8a0581516c
2 changed files with 14 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/ekf2/EKF/control.cpp
View File

@ -1068,27 +1068,27 @@ void Ekf::controlDragFusion()
void Ekf::controlAuxVelFusion()
{
bool data_ready = false;
if (_auxvel_buffer) {
data_ready = _auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
}
auxVelSample auxvel_sample_delayed;
if (data_ready && isHorizontalAidingActive()) {
if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
if (isHorizontalAidingActive()) {
_last_vel_obs = _auxvel_sample_delayed.vel;
_aux_vel_innov = _state.vel - _last_vel_obs;
_last_vel_obs_var = _aux_vel_innov_var;
const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
_aux_vel_innov_var, _aux_vel_test_ratio);
_last_vel_obs = auxvel_sample_delayed.vel;
_aux_vel_innov = _state.vel - _last_vel_obs;
_last_vel_obs_var = _aux_vel_innov_var;
// Can be enabled after bit for this is added to EKF_AID_MASK
// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
// _aux_vel_innov_var, _aux_vel_test_ratio);
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
_aux_vel_innov_var, _aux_vel_test_ratio);
// Can be enabled after bit for this is added to EKF_AID_MASK
// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
// _aux_vel_innov_var, _aux_vel_test_ratio);
}
}
}
}

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -296,7 +296,6 @@ protected:
extVisionSample _ev_sample_delayed{};
extVisionSample _ev_sample_delayed_prev{};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
auxVelSample _auxvel_sample_delayed{};
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)