AC_AttitudeControl: limit_target_thrust_angle handle zero

ArduPilot · Oct 6, 2024 · 5bfb7ee · 5bfb7ee
1 parent 3744826
commit 5bfb7ee
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -278,6 +278,13 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed)
 // limit_target_thrust_angle - 
 bool AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
 {
+    if (is_zero(thrust_angle_max_cd)) {
+        _attitude_target.zero();
+        _ang_vel_target.zero();
+        _euler_rate_target.zero();
+        return true;
+    }
+
     float thrust_angle_max_rad = radians(thrust_angle_max_cd * 0.01f);
 
     // attitude_body represents a quaternion rotation in NED frame to the body
@@ -288,7 +295,7 @@ bool AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
 
     // angle_delta is the estimated rotation that the aircraft will experience during the correction
     Vector3f angle_delta = Vector3f{gyro.x / get_angle_roll_p().kP(), gyro.y / get_angle_pitch_p().kP(), gyro.z / get_angle_yaw_p().kP() };
-    
+
     // attitude_update represents a quaternion rotation representing the expected rotation in the next time_constant
     Quaternion attitude_update;
     attitude_update.from_axis_angle(angle_delta);