diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 962fa0e625..2b422b5247 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -284,12 +284,6 @@ void FlightTaskManualAltitude::_updateHeadingSetpoints() } } } - - // check if an external yaw handler is active and if yes, let it compute the yaw setpoints - if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { - _yaw_setpoint = NAN; - _yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate(); - } } void FlightTaskManualAltitude::_updateSetpoints() diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index b40dd1df38..491a17c215 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -50,11 +50,6 @@ public: bool updateInitialize() override; bool update() override; - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *ext_yaw_handler) override { _ext_yaw_handler = ext_yaw_handler; } - protected: void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ virtual void _updateSetpoints(); /**< updates all setpoints */ @@ -101,9 +96,6 @@ private: void _respectMaxAltitude(); - WeatherVane *_ext_yaw_handler = - nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ float _max_speed_up = 10.0f; float _min_speed_down = 1.0f; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 44c05ec4c3..12bc4586de 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -147,6 +147,13 @@ void FlightTaskManualPosition::_updateXYlock() void FlightTaskManualPosition::_updateSetpoints() { FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction + + // check if an external yaw handler is active and if yes, let it update the yaw setpoints + if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + } + _thrust_setpoint.setAll(NAN); // don't require any thrust setpoints _updateXYlock(); // check for position lock } diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 4b0b9ed46f..04a2064fdf 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -51,6 +51,11 @@ public: bool activate() override; bool updateInitialize() 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; } + protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -65,4 +70,8 @@ protected: private: float _velocity_scale{0.0f}; //scales the stick input to velocity uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + + WeatherVane *_weathervane_yaw_handler = + nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + };