diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index edc82d6c98..608e4443c3 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -66,7 +66,8 @@ void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint } _yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get()); - yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get())); + const float yawspeed_scale = math::min(math::radians(_param_mpc_man_y_max.get()), _yawspeed_constraint); + yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * yawspeed_scale); yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev); } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index 185c490110..f15b4dd574 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -52,6 +52,7 @@ public: void ekfResetHandler(float delta_yaw); void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime, float unaided_yaw = NAN); + void setYawspeedConstraint(float yawspeed) { _yawspeed_constraint = yawspeed; }; private: AlphaFilter _yawspeed_filter; @@ -78,6 +79,8 @@ private: */ float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const; + float _yawspeed_constraint{INFINITY}; + DEFINE_PARAMETERS( (ParamFloat) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection (ParamFloat) _param_mpc_man_y_tau ///< time constant for yaw speed filtering