diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index 18ca105830..63de1d7776 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -61,17 +61,13 @@ public: const matrix::Vector2f &velIvar); void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att); - void get_wind(float wind[2]) - { - wind[0] = _state(INDEX_W_N); - wind[1] = _state(INDEX_W_E); - } - bool is_estimate_valid() { return _initialised; } bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, bool &reinit_filter); + matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; } + // invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10] float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); } float get_tas_scale_var() { return _P(2, 2); } @@ -79,11 +75,7 @@ public: float get_tas_innov_var() { return _tas_innov_var; } float get_beta_innov() { return _beta_innov; } float get_beta_innov_var() { return _beta_innov_var; } - void get_wind_var(float wind_var[2]) - { - wind_var[0] = _P(0, 0); - wind_var[1] = _P(1, 1); - } + matrix::Vector2f get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; } bool get_wind_estimator_reset() { return _wind_estimator_reset; } void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 0a630006f2..d9cd621f3a 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -57,8 +57,9 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q); update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); check_airspeed_data_stuck(input_data.timestamp); - check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio); check_load_factor(input_data.accel_z); + check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, + input_data.ground_velocity); update_airspeed_valid_status(input_data.timestamp); } @@ -95,14 +96,10 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) airspeed_wind_s wind_est = {}; wind_est.timestamp = timestamp; - float wind[2]; - _wind_estimator.get_wind(wind); - wind_est.windspeed_north = wind[0]; - wind_est.windspeed_east = wind[1]; - float wind_cov[2]; - _wind_estimator.get_wind_var(wind_cov); - wind_est.variance_north = wind_cov[0]; - wind_est.variance_east = wind_cov[1]; + wind_est.windspeed_north = _wind_estimator.get_wind()(0); + wind_est.windspeed_east = _wind_estimator.get_wind()(1); + wind_est.variance_north = _wind_estimator.get_wind_var()(0); + wind_est.variance_east = _wind_estimator.get_wind_var()(1); wind_est.tas_innov = _wind_estimator.get_tas_innov(); wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var(); wind_est.beta_innov = _wind_estimator.get_beta_innov(); @@ -217,7 +214,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) void AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio) + float estimator_status_mag_test_ratio, matrix::Vector3f vI) { // Check normalised innovation levels with requirement for continuous data and use of hysteresis // to prevent false triggering. @@ -227,7 +224,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ } // reset states if we are not flying or wind estimator was just initialized/reset - if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s + if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s || _tas_innov_integ_threshold <= 0.f) { _innovations_check_failed = false; _time_last_tas_pass = time_now; @@ -241,13 +238,13 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ } else { // nav velocity data is likely good so airspeed innovations are able to be used - // compute the ratio of innovation to gate size - const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [100,5] Hz - const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov() - / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var()); + const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [5,100] Hz + matrix::Vector2f wind_2d(_wind_estimator.get_wind()); + matrix::Vector3f air_vel = vI - matrix::Vector3f {wind_2d(0), wind_2d(1), 0.f}; + const float tas_innov = abs(_TAS - air_vel.norm()); - if (tas_test_ratio > _tas_innov_threshold) { - _apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance + if (tas_innov > _tas_innov_threshold) { + _apsd_innov_integ_state += dt_s * (tas_innov - _tas_innov_threshold); // integrate exceedance } else { // reset integrator used to trigger and record pass if integrator check is disabled diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 3cda6bfb5f..0c92b42427 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -178,7 +178,7 @@ private: void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio); + float estimator_status_mag_test_ratio, matrix::Vector3f vI); void check_load_factor(float accel_z); void update_airspeed_valid_status(const uint64_t timestamp); void reset(); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index a27444d24e..0e04a1dbc3 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -505,14 +505,10 @@ void AirspeedModule::update_wind_estimator_sideslip() } _wind_estimate_sideslip.timestamp = _time_now_usec; - float wind[2]; - _wind_estimator_sideslip.get_wind(wind); - _wind_estimate_sideslip.windspeed_north = wind[0]; - _wind_estimate_sideslip.windspeed_east = wind[1]; - float wind_cov[2]; - _wind_estimator_sideslip.get_wind_var(wind_cov); - _wind_estimate_sideslip.variance_north = wind_cov[0]; - _wind_estimate_sideslip.variance_east = wind_cov[1]; + _wind_estimate_sideslip.windspeed_north = _wind_estimator_sideslip.get_wind()(0); + _wind_estimate_sideslip.windspeed_east = _wind_estimator_sideslip.get_wind()(1); + _wind_estimate_sideslip.variance_north = _wind_estimator_sideslip.get_wind_var()(0); + _wind_estimate_sideslip.variance_east = _wind_estimator_sideslip.get_wind_var()(1); _wind_estimate_sideslip.tas_innov = NAN; _wind_estimate_sideslip.tas_innov_var = NAN; _wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov(); diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 159ba45683..c6c40afa10 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -167,34 +167,34 @@ PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1); PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0); /** - * Airspeed failsafe consistency threshold + * Airspeed failure innovation threshold * - * This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive, - * smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the - * inconsistency between predicted and measured airspeed is large enough to cause the wind EKF to reject airspeed measurements. + * This specifies the minimum airspeed innovation required to trigger a failsafe. Larger values make the check less sensitive, + * smaller values make it more sensitive. Large innovations indicate an inconsistency between predicted (groundspeed - windspeeed) + * and measured airspeed. * The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter. -* + * + * @unit m/s * @min 0.5 - * @max 3.0 + * @max 10.0 * @decimal 1 * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f); +PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 5.f); /** - * Airspeed failsafe consistency delay + * Airspeed failure innovation integral threshold * - * This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe. - * For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will - * rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values - * make it more sensitive. + * This sets the time integral of airspeed innovation exceedance above ASPD_FS_INNOV required to trigger a failsafe. + * Larger values make the check less sensitive, smaller positive values make it more sensitive. * - * @unit s - * @max 30.0 + * @unit m + * @min 0.0 + * @max 50.0 * @decimal 1 * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f); +PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 10.f); /** * Airspeed failsafe stop delay