AC_AttitudeControl: incorporate sqrt controller in rate shaping

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -287,7 +287,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
 
         // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
         // the output rate towards the input rate.
-        _euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt);
+        _euler_rate_target.z = input_shaping_rate((euler_yaw_rate - _euler_rate_target.z), _rate_y_tc, euler_accel.z, _euler_rate_target.z, _dt);
 
         // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
         euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
@@ -388,9 +388,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
 
         // When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
         // the output rate towards the input rate.
-        _euler_rate_target.x = input_shaping_ang_vel(_euler_rate_target.x, euler_roll_rate, euler_accel.x, _dt);
-        _euler_rate_target.y = input_shaping_ang_vel(_euler_rate_target.y, euler_pitch_rate, euler_accel.y, _dt);
-        _euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt);
+        _euler_rate_target.x = input_shaping_rate((euler_roll_rate - _euler_rate_target.x), _rate_rp_tc, euler_accel.x, _euler_rate_target.x, _dt);
+        _euler_rate_target.y = input_shaping_rate((euler_pitch_rate - _euler_rate_target.y), _rate_rp_tc, euler_accel.y, _euler_rate_target.y, _dt);
+        _euler_rate_target.z = input_shaping_rate((euler_yaw_rate - _euler_rate_target.z), _rate_y_tc, euler_accel.z, _euler_rate_target.z, _dt);
 
         // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
         euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
@@ -428,9 +428,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
         // Compute acceleration-limited body frame rates
         // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
         // the output rate towards the input rate.
-        _ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
-        _ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
-        _ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
+        _ang_vel_target.x = input_shaping_rate((roll_rate_rads - _ang_vel_target.x), _rate_rp_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
+        _ang_vel_target.y = input_shaping_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
+        _ang_vel_target.z = input_shaping_rate((yaw_rate_rads - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
 
         // Convert body-frame angular velocity into euler angle derivative of desired attitude
         ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
@@ -461,9 +461,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
     // Compute acceleration-limited body frame rates
     // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
     // the output rate towards the input rate.
-    _ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
-    _ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
-    _ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
+    _ang_vel_target.x = input_shaping_rate((roll_rate_rads - _ang_vel_target.x), _rate_rp_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
+    _ang_vel_target.y = input_shaping_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
+    _ang_vel_target.z = input_shaping_rate((yaw_rate_rads - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
 
     // Update the unused targets attitude based on current attitude to condition mode change
     _ahrs.get_quat_body_to_ned(_attitude_target);
@@ -500,9 +500,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
     // Compute acceleration-limited body frame rates
     // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
     // the output rate towards the input rate.
-    _ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
-    _ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
-    _ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
+    _ang_vel_target.x = input_shaping_rate((roll_rate_rads - _ang_vel_target.x), _rate_rp_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
+    _ang_vel_target.y = input_shaping_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
+    _ang_vel_target.z = input_shaping_rate((yaw_rate_rads - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
 
     // Retrieve quaternion body attitude
     Quaternion attitude_body;
@@ -834,6 +834,19 @@ float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desi
     }
 }
 
+// calculates the accleration correction from an rate error. The angular acceleration is limited.
+float AC_AttitudeControl::input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt)
+{
+    float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt);
+
+    if (is_positive(accel_max)) {
+        desired_ang_accel = constrain_float(desired_ang_accel, -accel_max, accel_max);
+    }
+    target_ang_vel += desired_ang_accel * dt;
+
+    return target_ang_vel;
+}
+
 // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
 // This function can be used to predict the delay associated with angle requests.
 void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -309,6 +309,8 @@ public:
     // limits the acceleration and deceleration of a velocity request
     static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);
 
+    static float input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt);
+
     // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
     // This function can be used to predict the delay associated with angle requests.
     void input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const;
@@ -370,6 +372,12 @@ public:
     // get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts
     void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate);
     
+    // Sets the roll and pitch rate shaping time constant
+    void set_roll_pitch_rate_tc(float input_tc) { _rate_rp_tc = input_tc; }
+
+    // Sets the yaw rate shaping time constant
+    void set_yaw_rate_tc(float input_tc) { _rate_y_tc = input_tc; }
+
     // User settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -485,6 +493,10 @@ protected:
     // Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections
     float               _feedforward_scalar = 1.0f;
 
+    // rate controller input smoothing time constant
+    float               _rate_rp_tc;
+    float               _rate_y_tc;
+
     // References to external libraries
     const AP_AHRS_View&  _ahrs;
     const AP_Vehicle::MultiCopter &_aparm;