fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 08:51:36 +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-02 17:43:25 -05:00
parent 452b5e94b4
commit 8a0581516c
2 changed files with 14 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+14 -14
View File
@@ -1068,27 +1068,27 @@ void Ekf::controlDragFusion()
void Ekf::controlAuxVelFusion() void Ekf::controlAuxVelFusion()
{ {
bool data_ready = false;
if (_auxvel_buffer) { 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; const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
_aux_vel_innov = _state.vel - _last_vel_obs;
_last_vel_obs_var = _aux_vel_innov_var;
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar, _last_vel_obs = auxvel_sample_delayed.vel;
_aux_vel_innov_var, _aux_vel_test_ratio); _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 fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar, _aux_vel_innov_var, _aux_vel_test_ratio);
// _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);
}
}
} }
} }
src/modules/ekf2/EKF/estimator_interface.h
-1
View File
@@ -296,7 +296,6 @@ protected:
extVisionSample _ev_sample_delayed{}; extVisionSample _ev_sample_delayed{};
extVisionSample _ev_sample_delayed_prev{}; extVisionSample _ev_sample_delayed_prev{};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) 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) float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)