From 98b847fbe670e62f04da1f5bd78168722f81013b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 15 Mar 2023 10:51:01 +0100 Subject: [PATCH] FWPositionControl: add extra check for low wind variance to use TAS rate estimate in TECS Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 14 ++++++------- .../FixedwingPositionControl.hpp | 7 ++++--- .../fw_path_navigation_params.c | 20 +++++++++++++++++++ 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index d604377a43..507745058a 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -296,13 +296,11 @@ FixedwingPositionControl::wind_poll() _wind_sub.update(&wind); // assumes wind is valid if finite - _wind_valid = PX4_ISFINITE(wind.windspeed_north) - && PX4_ISFINITE(wind.windspeed_east); - + _wind_valid = PX4_ISFINITE(wind.windspeed_north) && PX4_ISFINITE(wind.windspeed_east); _time_wind_last_received = hrt_absolute_time(); - _wind_vel(0) = wind.windspeed_north; - _wind_vel(1) = wind.windspeed_east; + _wind_vel = {wind.windspeed_north, wind.windspeed_east}; + _wind_var = {wind.variance_north, wind.variance_east}; } else { // invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout @@ -310,8 +308,7 @@ FixedwingPositionControl::wind_poll() } if (!_wind_valid) { - _wind_vel(0) = 0.f; - _wind_vel(1) = 0.f; + _wind_vel.setZero(); } } @@ -2556,6 +2553,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva const Vector3f local_pos_a = Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; const Vector3f groundspeed_minus_wind = Vector3f{_local_pos.vx - _wind_vel(0), _local_pos.vy - _wind_vel(1), _local_pos.vz}; + // extra check: wind variance of each component max FW_WIND_VAR_THLD + const bool wind_valid_extra = _wind_valid && math::max(_wind_var(0), _wind_var(1)) < _param_fw_wind_var_thld.get(); + // additionally only use it if local position has valid velocitis and we're currently in a position-controlled mode, as // then we can assume that local_position has valid fields const bool use_groundspeed_acceleration = wind_valid_extra && _local_pos.v_xy_valid && _local_pos.v_z_valid diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index 0067f890d4..ac4c5a9032 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -378,8 +378,8 @@ private: // WIND - // [m/s] wind velocity vector - Vector2f _wind_vel{0.0f, 0.0f}; + Vector2f _wind_vel{0.f, 0.f}; // [m/s] wind velocity vector + Vector2f _wind_var{0.f, 0.f}; // [(m/s)^2] wind velocity variance bool _wind_valid{false}; @@ -954,7 +954,8 @@ private: (ParamFloat) _param_fw_tko_airspd, (ParamFloat) _param_rwto_psp, - (ParamBool) _param_fw_laun_detcn_on + (ParamBool) _param_fw_laun_detcn_on, + (ParamFloat) _param_fw_wind_var_thld ) }; diff --git a/src/modules/fw_path_navigation/fw_path_navigation_params.c b/src/modules/fw_path_navigation/fw_path_navigation_params.c index dfabdec05d..8964ce545f 100644 --- a/src/modules/fw_path_navigation/fw_path_navigation_params.c +++ b/src/modules/fw_path_navigation/fw_path_navigation_params.c @@ -1063,3 +1063,23 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f); + +/** + * Wind variance threshold for usage of TAS rate estimate + * + * The wind estimate is used to determine a 3D airspeed vector which is then used to estimate the + * current airspeed derivative, which in turn is an input for TECS. + * + * To pass the check both the north and east component of the wind estimate need to have reported + * variances below this threshold. + * Set to 0 if the airspeed rate estimate shouldn't be used at all (not possible to estimate without wind). + * If airspeed rate is not estimated, it is assumed to be 0. TECS is then generally slower to reduce + * airspeed errors. + * + * @unit (m/s)^2 + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WIND_VAR_THLD, 0.1f);