AP_Math: Control: Add heading shaping

libraries/AP_Math/control.cpp

@@ -359,6 +359,62 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
     shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total);
 }
 
+/* shape_angle_vel_accel calculate a jerk limited path from the current angle, angular velocity and angular acceleration to an input angle, angular velocity and angular acceleration.           
+ The function takes the current angle, angular velocity, and angular acceleration and calculates the required jerk limited adjustment to the angular acceleration for the next time dt.
+ The kinematic path is constrained by :
+    maximum angular velocity - angle_vel_max,
+    maximum angular acceleration - angle_accel_max,
+    maximum angular jerk - angle_jerk_max.
+ The function alters the variable accel to follow a jerk limited kinematic path to angle_input, angle_vel_input and angle_accel_input.
+ The angle_vel_max limit can be removed by setting the desired limit to zero.
+ The correction angular acceleration can is limited to angle_accel_max. If limit_total_accel is true the total angular acceleration is limited to angle_accel_max.
+*/            
+void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input,
+                         float angle, float angle_vel, float& angle_accel,
+                         float angle_vel_max, float angle_accel_max,
+                         float angle_jerk_max, float dt, bool limit_total)
+{
+    // sanity check accel_max
+    if (!is_positive(angle_accel_max)) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+        return;
+    }
+
+    // calculate angle error to be corrected
+    // unwrap angle error to achieve fastest path
+    float stopping_time = fabsf(angle_vel / angle_accel_max);
+    float angle_error = angle_input - angle - angle_vel * stopping_time;
+    angle_error = wrap_PI(angle_error);
+    angle_error += angle_vel * stopping_time;
+
+    // Calculate time constants and limits to ensure stable operation
+    // The negative acceleration limit is used here because the square root controller
+    // manages the approach to the setpoint. Therefore the acceleration is in the opposite
+    // direction to the position error.
+    float angle_accel_tc_max;
+    float KPv;
+    angle_accel_tc_max = 0.5 * angle_accel_max;
+    KPv = 0.5 * angle_jerk_max / angle_accel_max;
+
+    // velocity to correct position
+    float angle_vel_target = sqrt_controller(angle_error, KPv, angle_accel_tc_max, dt);
+
+    // limit velocity to vel_max
+    if (is_positive(angle_vel_max)){
+        angle_vel_target = constrain_float(angle_vel_target, -angle_vel_max, angle_vel_max);
+    }
+
+    // velocity correction with input velocity
+    angle_vel_target += angle_vel_input;
+
+    // limit velocity to vel_max
+    if (limit_total && is_positive(angle_vel_max)){
+        angle_vel_target = constrain_float(angle_vel_target, -angle_vel_max, angle_vel_max);
+    }
+
+    shape_vel_accel(angle_vel_target, angle_accel_input, angle_vel, angle_accel, -angle_accel_max, angle_accel_max, angle_jerk_max, dt, limit_total);
+}
+
 /* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
  Input parameters are:
     vel is the velocity vector used to define the direction acceleration limit is biased in.

libraries/AP_Math/control.h

@@ -103,6 +103,22 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
                             float vel_max, float accel_max,
                             float jerk_max, float dt, bool limit_total);
 
+/* shape_angle_vel_accel calculate a jerk limited path from the current angle, angular velocity and angular acceleration to an input angle, angular velocity and angular acceleration.           
+ The function takes the current angle, angular velocity, and angular acceleration and calculates the required jerk limited adjustment to the angular acceleration for the next time dt.
+ The kinematic path is constrained by :
+    maximum angular velocity - angle_vel_max,
+    maximum angular acceleration - angle_accel_max,
+    maximum angular jerk - angle_jerk_max.
+ The function alters the variable accel to follow a jerk limited kinematic path to angle_input, angle_vel_input and angle_accel_input.
+ The angle_vel_max limit can be removed by setting the desired limit to zero.
+ The correction angular velocity is limited to angle_vel_max. If limit_total is true the total angular velocity is limited to angle_vel_max.
+ The correction angular acceleration can is limited to angle_accel_max. If limit_total is true the total angular acceleration is limited to angle_accel_max.
+*/            
+void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input,
+                         float angle, float angle_vel, float& angle_accel,
+                         float angle_vel_max, float angle_accel_max,
+                         float angle_jerk_max, float dt, bool limit_total);
+
 
 /* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
  Input parameters are: