diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 868e8b46a9..750847c707 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -57,7 +57,7 @@ void Ekf::fuseAirspeed() const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); - // determine if we need the sideslip fusion to correct states other than wind + // determine if we need the airspeed fusion to correct states other than wind const bool update_wind_only = !_is_wind_dead_reckoning; // Intermediate variables @@ -165,7 +165,6 @@ void Ekf::fuseAirspeed() if (is_fused) { _time_last_arsp_fuse = _time_last_imu; - _control_status.flags.fuse_aspd = true; } } diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 8d4b8d5428..c865d24d8e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1302,29 +1302,53 @@ void Ekf::controlAirDataFusion() const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6); - if (_control_status.flags.wind && - (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) { + if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) { _control_status.flags.wind = false; } - if (_control_status.flags.fuse_aspd && airspeed_timed_out) { - _control_status.flags.fuse_aspd = false; + if (_params.arsp_thr <= 0.f) { + stopAirspeedFusion(); + return; } - // Always try to fuse airspeed data if available and we are in flight - if (!_using_synthetic_position && _tas_data_ready && _control_status.flags.in_air) { - // 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 timout timer to prevent repeated resets + if (_tas_data_ready) { + const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position; + const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr; + const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant; + + if (_control_status.flags.fuse_aspd) { + if (continuing_conditions_passing) { + if (is_airspeed_significant) { + fuseAirspeed(); + } + + const bool is_fusion_failing = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); + + if (is_fusion_failing) { + stopAirspeedFusion(); + } + + } else { + stopAirspeedFusion(); + } + + } else if (starting_conditions_passing) { + // 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 wind speed states and corresponding covariances + resetWindStates(); + resetWindCovariance(); + } + + startAirspeedFusion(); _time_last_arsp_fuse = _time_last_imu; - // reset the wind speed states and corresponding covariances - resetWindStates(); - resetWindCovariance(); } - fuseAirspeed(); + } else if (_control_status.flags.fuse_aspd && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us > (uint64_t) 1e6)) { + ECL_WARN("Airspeed data stopped"); + stopAirspeedFusion(); } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1f1c05d5d4..7f0f0aca63 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -954,6 +954,9 @@ private: return sensor_timestamp + acceptance_interval > _time_last_imu; } + void startAirspeedFusion(); + void stopAirspeedFusion(); + void startGpsFusion(); void stopGpsFusion(); void stopGpsPosFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index c522304e38..5f0f8962c3 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1480,6 +1480,14 @@ void Ekf::loadMagCovData() P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat; } +void Ekf::startAirspeedFusion() { + _control_status.flags.fuse_aspd = true; +} + +void Ekf::stopAirspeedFusion() { + _control_status.flags.fuse_aspd = false; +} + void Ekf::startGpsFusion() { resetHorizontalPositionToGps(); @@ -1723,13 +1731,15 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M void Ekf::runYawEKFGSF() { - float TAS; + float TAS = 0.f; - if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) { - TAS = _params.EKFGSF_tas_default; + if (_control_status.flags.fixed_wing) { + if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000)) { + TAS = _params.EKFGSF_tas_default; - } else { - TAS = _airspeed_sample_delayed.true_airspeed; + } else if (_airspeed_sample_delayed.true_airspeed >= _params.arsp_thr) { + TAS = _airspeed_sample_delayed.true_airspeed; + } } const Vector3f imu_gyro_bias = getGyroBias(); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a67290e0c3..7acf148846 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -140,6 +140,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_ev_pos_x(_params->ev_pos_body(0)), _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_tau_vel(_params->vel_Tau), _param_ekf2_tau_pos(_params->pos_Tau), _param_ekf2_gbias_init(_params->switch_on_gyro_bias), @@ -1360,16 +1361,12 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) // via the wind estimator that uses EKF velocity estimates. const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor; - // only set airspeed data if condition for airspeed fusion are met - if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (true_airspeed_m_s > _param_ekf2_arsp_thr.get())) { - - airspeedSample airspeed_sample { - .time_us = airspeed.timestamp, - .true_airspeed = true_airspeed_m_s, - .eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, - }; - _ekf.setAirspeedData(airspeed_sample); - } + airspeedSample airspeed_sample { + .time_us = airspeed.timestamp, + .true_airspeed = true_airspeed_m_s, + .eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, + }; + _ekf.setAirspeedData(airspeed_sample); ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 57c339d95e..fd5dbf60fa 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -471,7 +471,7 @@ private: _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) // control of airspeed and sideslip fusion - (ParamFloat) + (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) _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables