fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 13:13:33 +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/common.h
+1
View File
@@ -336,6 +336,7 @@ struct parameters {
float arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
// synthetic sideslip fusion
int32_t beta_fusion_enabled{0};
float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
float beta_noise{0.3f}; ///< synthetic sideslip noise (rad)
const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec)
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);
}
}
}
}
src/modules/ekf2/EKF/estimator_interface.h
-3
View File
@@ -132,9 +132,6 @@ public:
// set vehicle is fixed wing status
void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
// set flag if synthetic sideslip measurement should be fused
void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
// set flag if static pressure rise due to ground effect is expected
// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
// flag will clear after GNDEFFECT_TIMEOUT uSec