AC_AttitudeControl: input_rate_step_bf_roll_pitch_yaw() must update t…

…he angle velocity target

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -628,19 +628,17 @@ 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);
-
+    _ang_vel_target.zero();
     // 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_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
-
+
+    Vector3f ang_vel_body {roll_rate_step_bf_cd, pitch_rate_step_bf_cd, yaw_rate_step_bf_cd};
+    ang_vel_body = ang_vel_body * 0.01f * DEG_TO_RAD;
     // finally update the attitude target
-    _ang_vel_body = _ang_vel_target;
+    _ang_vel_body = ang_vel_body;
 }
 
 // Command a thrust vector and heading rate