From 7a47147d441296bce3c885177470abf0647fbc2a Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 8 Feb 2023 13:45:11 +0100 Subject: [PATCH] FixedwingPositionControl: Explicitly set wind to zero when it is not valid. --- .../fw_path_navigation/FixedwingPositionControl.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 713ce21e8d9a..df6ef6dc792c 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -303,6 +303,11 @@ FixedwingPositionControl::wind_poll() // invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout _wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT; } + + if (!_wind_valid) { + _wind_vel(0) = 0.f; + _wind_vel(1) = 0.f; + } } void @@ -1095,7 +1100,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : 0.0f; navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, - _wind_vel, curvature); + _wind_vel, curvature); } else { navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); @@ -1242,7 +1247,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, - _wind_vel); + _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas;