fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 13:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add AUX velocity aid src status

Browse Source 
- also includes velocity and position helpers for using estimator aid
   source status messages that will later be used for GPS, EV, etc
This commit is contained in:
Daniel Agar
2022-07-29 12:02:31 -04:00
parent a397c09e59
commit 41d9c3dd2a
8 changed files with 146 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+6 -11
View File
@@ -1131,18 +1131,13 @@ void Ekf::controlAuxVelFusion()
if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
if (isHorizontalAidingActive()) {
const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f);
_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;
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);
_aid_src_aux_vel.fusion_enabled[0] = PX4_ISFINITE(auxvel_sample_delayed.vel(0));
_aid_src_aux_vel.fusion_enabled[1] = PX4_ISFINITE(auxvel_sample_delayed.vel(1));
_aid_src_aux_vel.fusion_enabled[2] = PX4_ISFINITE(auxvel_sample_delayed.vel(2));
fuseVelocity(_aid_src_aux_vel);
}
}
}