fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 08:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FWPosControl: Only use accleration input for TECS if local_position has valid velocity

Browse Source 
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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-14 16:40:45 +01:00
parent 2fb8b58652
commit 330c8be053
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+8 -3
View File
@@ -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,