From f4c300af251e7a6eabbc4b2c4761decdf1a572dc Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 22 Feb 2022 10:10:45 +0100 Subject: [PATCH] FlightTaskAuto: Nudging: only set yawrate_sp if WV is disabled or stick out of dead-zone Otherwise the setpoint from weather vane is constantly overwritten by it, even if the yaw stick is not moved. Signed-off-by: Silvan Fuhrer --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 8cf850fdd8..5e0b09a432 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -249,8 +249,14 @@ void FlightTaskAuto::_prepareLandSetpoints() // Stick full up -1 -> stop, stick full down 1 -> double the speed vertical_speed *= (1 + _sticks.getPositionExpo()(2)); - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, - _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); + // Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone + const bool weather_vane_active = (_ext_yaw_handler != nullptr) && _ext_yaw_handler->is_active(); + + if (!weather_vane_active || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) { + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); + } + _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);