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);