diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index dbc5b55680..e1ae0dc117 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index fc259a51b1..0235dbdcd3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -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); + } } } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 69cb788907..23bee11ca0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b5ef696ad1..bf4de3456f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -145,6 +145,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_ev_pos_y(_params->ev_pos_body(1)), _param_ekf2_ev_pos_z(_params->ev_pos_body(2)), _param_ekf2_arsp_thr(_params->arsp_thr), + _param_ekf2_fuse_beta(_params->beta_fusion_enabled), _param_ekf2_tau_vel(_params->vel_Tau), _param_ekf2_tau_pos(_params->pos_Tau), _param_ekf2_gbias_init(_params->switch_on_gyro_bias), @@ -532,9 +533,6 @@ void EKF2::Run() if (_status_sub.copy(&vehicle_status)) { const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); - // only fuse synthetic sideslip measurements if conditions are met - _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1)); - // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) _ekf.set_is_fixed_wing(is_fixed_wing); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 652e3f6b6f..08bf9d30b1 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -569,7 +569,7 @@ private: // control of airspeed and sideslip fusion (ParamExtFloat) _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) - (ParamInt) + (ParamExtInt) _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables // output predictor filter time constants