diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 22ae8a5ad9..57c63ba692 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -46,7 +46,8 @@ FlightTaskManualStabilized::FlightTaskManualStabilized(control::SuperBlock *pare _tilt_max_man(parent, "MPC_MAN_TILT_MAX", false), _throttle_min(parent, "MPC_THR_MIN", false), _throttle_max(parent, "MPC_THR_MAX", false), - _throttle_hover(parent, "MPC_THR_HOVER", false) + _throttle_hover(parent, "MPC_THR_HOVER", false), + _yaw_rate_max(parent, "MPC_MAN_Y_MAX", false) {} bool FlightTaskManualStabilized::activate() @@ -62,6 +63,7 @@ void FlightTaskManualStabilized::_scaleSticks() /* Scale sticks to yaw and thrust using * linear scale for yaw and piecewise linear map for thrust. */ _yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get()); + _yaw_rate_sp = math::sign(_yaw_rate_sp) * math::min(fabsf(_yaw_rate_sp), math::radians(_yaw_rate_max.get())); _throttle = _throttleCurve(); } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp index d95724d282..e31c0303f4 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp @@ -69,5 +69,6 @@ private: control::BlockParamFloat _tilt_max_man; /**< maximum tilt allowed for manual flight */ control::BlockParamFloat _throttle_min; /**< minimum throttle that always has to be satisfied in flight*/ control::BlockParamFloat _throttle_max; /**< maximum throttle that always has to be satisfied in flight*/ - control::BlockParamFloat _throttle_hover; /**< yhrottle value at which vehicle is at hover equilibrium */ + control::BlockParamFloat _throttle_hover; /**< throttle value at which vehicle is at hover equilibrium */ + control::BlockParamFloat _yaw_rate_max; /** manual maximum yawspeed */ };