AC_AttControl: add step input for autotune

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -376,6 +376,32 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
     attitude_controller_run_quat();
 }
 
+// Command an angular step (i.e change) in body frame angle
+// Used to command a step in angle without exciting the orthogonal axis during autotune
+void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd)
+{
+    // Convert from centidegrees on public interface to radians
+    float roll_step_rads = radians(roll_angle_step_bf_cd*0.01f);
+    float pitch_step_rads = radians(pitch_angle_step_bf_cd*0.01f);
+    float yaw_step_rads = radians(yaw_angle_step_bf_cd*0.01f);
+
+    // rotate attitude target by desired step
+    Quaternion attitude_target_update_quat;
+    attitude_target_update_quat.from_axis_angle(Vector3f(roll_step_rads, pitch_step_rads, yaw_step_rads));
+    _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
+    _attitude_target_quat.normalize();
+
+    // calculate the attitude target euler angles
+    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
+
+    // Set rate feedforward requests to zero
+    _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
+    _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
+
+    // Call quaternion attitude controller
+    attitude_controller_run_quat();
+}
+
 // Calculates the body frame angular velocities to follow the target attitude
 void AC_AttitudeControl::attitude_controller_run_quat()
 {

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -128,6 +128,9 @@ public:
     // Command an angular velocity with angular velocity feedforward and smoothing
     virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
 
+    // Command an angular step (i.e change) in body frame angle
+    virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
+
     // Run angular velocity controller and send outputs to the motors
     virtual void rate_controller_run() = 0;