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);