diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index dd21360366..df6902a032 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 4794a84fef..df640ffad3 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: + void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; + void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; + StickAccelerationXY _stick_acceleration_xy; StickYaw _stick_yaw; - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; + WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ };