AC_AttitudeControl: check for zero rate Y max before taking min

ArduPilot · Aug 25, 2021 · bae7955 · bae7955
1 parent e8066aa
commit bae7955
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -614,7 +614,9 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect
         _ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, get_slew_yaw_rads(), _dt);
 
         // Limit the angular velocity
-        ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads()));
+        // a zero _angle_vel_yaw_max means that setting is disabled
+        const float ang_vel_yaw_max_rads = is_zero(_ang_vel_yaw_max) ? get_slew_yaw_rads() : MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads());
+        ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), ang_vel_yaw_max_rads);
     } else {
         // set persisted quaternion target attitude
         _attitude_target = desired_attitude_quat;