From a63f1b71fe86944a56f932bb760b53f364376bf2 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 25 Jul 2022 11:29:52 +0300 Subject: [PATCH] wind_estimator: added simple check for validity of synthetic airspeed - synthetic airspeed will only be declared valid as soon as the wind variance has dropped below a parameterized threshold. This is useful for vehicles without an airspeed sensor which rely on synthetic airspeed but only once the vehicle has turned sufficiently for the wind estimates to be reliable. Signed-off-by: RomanBapst --- src/lib/wind_estimator/WindEstimator.cpp | 4 +-- src/lib/wind_estimator/WindEstimator.hpp | 2 ++ .../airspeed_selector_main.cpp | 31 ++++++++++++------- .../airspeed_selector_params.c | 14 +++++++++ 4 files changed, 38 insertions(+), 13 deletions(-) diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 1118cb0d34..098cdd3e49 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -78,8 +78,8 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f & // get an estimate of the state covariance matrix given the estimated variance of ground velocity // and measured airspeed _P.setZero(); - _P(INDEX_W_N, INDEX_W_N) = velIvar(0); - _P(INDEX_W_E, INDEX_W_E) = velIvar(1); + _P(INDEX_W_N, INDEX_W_N) = INITIAL_WIND_VAR; + _P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR; _P(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = 0.0001f; _P = L * _P * L.transpose(); diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index b1ba8b1ea7..bf885f2949 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -97,6 +97,8 @@ private: INDEX_TAS_SCALE }; ///< enum which can be used to access state. + static constexpr float INITIAL_WIND_VAR = 5.f; // initial variance for each wind state + matrix::Vector3f _state{0.f, 0.f, 1.f}; matrix::Matrix3f _P; ///< state covariance matrix diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 1e05a232c7..dc72c1feae 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -147,8 +147,8 @@ private: bool _initialized{false}; /**< module initialized*/ bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */ bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ - float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */ - float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */ + float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */ + float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */ bool _armed_prev{false}; hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {}; @@ -184,7 +184,8 @@ private: (ParamInt) _checks_fail_delay, /**< delay to declare airspeed invalid */ (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ - (ParamFloat) _param_fw_airspd_stall + (ParamFloat) _param_fw_airspd_stall, + (ParamFloat) _param_wind_var_threshold ) void init(); /**< initialization of the airspeed validator instances */ @@ -373,7 +374,7 @@ AirspeedModule::Run() // takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed if (_in_takeoff_situation && (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() || - _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) { + (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()))) { _in_takeoff_situation = false; } @@ -540,13 +541,21 @@ void AirspeedModule::update_wind_estimator_sideslip() void AirspeedModule::update_ground_minus_wind_airspeed() { - // calculate airspeed estimate based on groundspeed-windspeed to use as fallback - const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north; - const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east; - const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z - _ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down); - _ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa, - _vehicle_air_data.baro_temp_celcius); + const float wind_variance = sqrtf(_wind_estimate_sideslip.variance_north * _wind_estimate_sideslip.variance_north + + _wind_estimate_sideslip.variance_east * _wind_estimate_sideslip.variance_east); + + if (wind_variance < _param_wind_var_threshold.get()) { + // calculate airspeed estimate based on groundspeed-windspeed + const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north; + const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east; + const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z + _ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down); + _ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa, + _vehicle_air_data.baro_temp_celcius); + + } else { + _ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN; + } } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 8e9de8e86d..2a3394aa6f 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -227,3 +227,17 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2); * @max 1000 */ PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); + +/** + * Horizontal wind variance threshold for synthetic airspeed. + * + * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid + * as soon as the wind variance drops below this value. + * + * @unit (m/s)^2 + * @min 0.001 + * @max 5 + * @decimal 3 + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ASPD_WVAR_THR, 0.3f);