From 330c8be053b377acbf44b59e8687e55d756e8c64 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 14 Mar 2023 16:40:45 +0100 Subject: [PATCH] FWPosControl: Only use accleration input for TECS if local_position has valid velocity And additionally only if the control mode flag position is set, as then we can assume that all the fields in local_position are valid. Signed-off-by: Silvan Fuhrer --- .../fw_path_navigation/FixedwingPositionControl.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 5d2aab81c4..d604377a43 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -2554,11 +2554,16 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva throttle_max); const Vector3f local_pos_a = Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; - const Vector3f groundspee_minus_wind = Vector3f{_local_pos.vx - _wind_vel(0), _local_pos.vy - _wind_vel(1), _local_pos.vz}; + const Vector3f groundspeed_minus_wind = Vector3f{_local_pos.vx - _wind_vel(0), _local_pos.vy - _wind_vel(1), _local_pos.vz}; + + // 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 + && _control_mode.flag_control_position_enabled; // only do acceleration control in case wind is valid, and otherwise set it to 0 - const float acceleration_in_direction_of_airspeed_vector = _wind_valid ? groundspee_minus_wind.unit_or_zero().dot( - local_pos_a) : 0.f; + const float acceleration_in_direction_of_airspeed_vector = use_groundspeed_acceleration ? + groundspeed_minus_wind.unit_or_zero().dot(local_pos_a) : 0.f; _tecs.update(_pitch - radians(_param_fw_psp_off.get()), _current_altitude,