diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index afbc115297..0f9b2e0738 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -74,6 +74,18 @@ bool FlightTaskManualAcceleration::update() _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); _constraints.want_takeoff = _checkTakeoff(); + + // check if an external yaw handler is active and if yes, let it update the yaw setpoints + if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + + // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) + if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { + // vehicle is steady + _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + } + } + return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 36d360ca81..b608dd9022 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -52,10 +52,17 @@ public: bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool update() override; + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + private: StickAccelerationXY _stick_acceleration_xy; StickYaw _stick_yaw; void _ekfResetHandlerPositionXY() override; void _ekfResetHandlerVelocityXY() override; + + WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ };