AC_AttitudeControl: Fix rate step command

lthall · Sep 18, 2024 · 7262b20 · 7262b20
1 parent 375628c
commit 7262b20
Showing 1 changed file with 20 additions and 5 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -625,19 +625,34 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
 // Done as a single thread-safe function to avoid intermediate zero values being seen by the attitude controller
 void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd)
 {
-    // Set x-axis angular velocity in centidegrees/s
-    _ang_vel_target.x = radians(roll_rate_step_bf_cd * 0.01f);
-    _ang_vel_target.y = radians(pitch_rate_step_bf_cd * 0.01f);
-    _ang_vel_target.z = radians(yaw_rate_step_bf_cd * 0.01f);
+    Vector3f ang_vel_body;
+    if ( is_zero(roll_rate_step_bf_cd) ) {
+        ang_vel_body.x = input_shaping_ang_vel(_ang_vel_body.x, 0.0, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
+    } else {
+        ang_vel_body.x = radians(roll_rate_step_bf_cd * 0.01f);
+    }
+
+    if ( is_zero(pitch_rate_step_bf_cd) ) {
+        ang_vel_body.y = input_shaping_ang_vel(_ang_vel_body.y, 0.0, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
+    } else {
+        ang_vel_body.y = radians(pitch_rate_step_bf_cd * 0.01f);
+    }
+
+    if ( is_zero(yaw_rate_step_bf_cd) ) {
+        ang_vel_body.z = input_shaping_ang_vel(_ang_vel_body.z, 0.0, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
+    } else {
+        ang_vel_body.z = radians(yaw_rate_step_bf_cd * 0.01f);
+    }
 
     // Update the unused targets attitude based on current attitude to condition mode change
     _ahrs.get_quat_body_to_ned(_attitude_target);
     _attitude_target.to_euler(_euler_angle_target);
     // Convert body-frame angular velocity into euler angle derivative of desired attitude
+    _ang_vel_target = ang_vel_body;
     ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
 
     // finally update the attitude target
-    _ang_vel_body = _ang_vel_target;
+    _ang_vel_body = ang_vel_body;
 }
 
 // Command a thrust vector and heading rate