fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: push fuse beta config into backend

Browse Source
This commit is contained in:
Daniel Agar
2022-11-02 10:09:26 -04:00
parent 0996e5319f
commit 298cc61e07
5 changed files with 24 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+21 -20
View File
@@ -570,30 +570,31 @@ void Ekf::controlAirDataFusion()
void Ekf::controlBetaFusion()
{
if (_control_status.flags.fake_pos) {
return;
}
_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
if (_control_status.flags.fuse_beta) {
if (beta_fusion_time_triggered &&
_control_status.flags.fuse_beta &&
_control_status.flags.in_air) {
updateSideslip(_aid_src_sideslip);
_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the timeout timers to prevent repeated resets
_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
resetWind();
}
if (beta_fusion_time_triggered) {
if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
fuseSideslip(_aid_src_sideslip);
updateSideslip(_aid_src_sideslip);
_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the timeout timers to prevent repeated resets
_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
resetWind();
}
if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
fuseSideslip(_aid_src_sideslip);
}
}
}
}