diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp
index 423f6b05bc..49b1977b55 100644
--- a/libraries/AP_Math/control.cpp
+++ b/libraries/AP_Math/control.cpp
@@ -94,11 +94,11 @@ void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
+ The function alters the input accel to be jerk limited
 */
-void shape_accel(float accel_input, float& accel,
-    float accel_min, float accel_max,
-    float tc, float dt)
+void shape_accel(const float accel_input, float& accel,
+                 const float accel_min, const float accel_max,
+                 const float tc, const float dt)
 {
     // sanity check tc
     if (!is_positive(tc)) {
@@ -177,14 +177,15 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
+ The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel
+ to be jerk limited
  The accel_max limit can be removed by setting it to zero.
 */
-void shape_vel_accel(float &vel_input, float accel_input,
-                     float vel, float& accel,
-                     float vel_min, float vel_max,
-                     float accel_min, float accel_max,
-                     float tc, float dt)
+void shape_vel_accel(float &vel_input, const float accel_input,
+                     const float vel, float& accel,
+                     const float vel_min, const float vel_max,
+                     const float accel_min, const float accel_max,
+                     const float tc, const float dt)
 {
     // sanity check tc
     if (!is_positive(tc)) {
@@ -220,12 +221,13 @@ void shape_vel_accel(float &vel_input, float accel_input,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
+ The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited
  This function operates on the x and y axis of both Vector2f or Vector3f inputs.
  The accel_max limit can be removed by setting it to zero.
 */
 void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
-                        const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float tc, float dt)
+                        const Vector2f& vel, Vector2f& accel,
+                        const float vel_max, const float accel_max, const float tc, const float dt)
 {
     // sanity check tc
     if (!is_positive(tc)) {
@@ -261,13 +263,13 @@ void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
+ The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited
  The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
 */
-void shape_pos_vel_accel(float &pos_input, float vel_input, float accel_input,
-                         float pos, float vel, float& accel,
-                         float vel_correction_max, float vel_min, float vel_max,
-                         float accel_min, float accel_max, float tc, float dt)
+void shape_pos_vel_accel(float &pos_input, const float vel_input, const float accel_input,
+                         const float pos, const float vel, float& accel,
+                         const float vel_correction_max, const float vel_min, const float vel_max,
+                         const float accel_min, const float accel_max, const float tc, const float dt)
 {
     // sanity check tc
     if (!is_positive(tc)) {
@@ -304,13 +306,12 @@ void shape_pos_vel_accel(float &pos_input, float vel_input, float accel_input,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
- This function operates on the x and y axis of both Vector2f or Vector3f inputs.
+ The function alters the accel to be jerk limited
  The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
 */
 void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
-    const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
-    float vel_correction_max, float vel_max, float accel_max, float tc, float dt)
+                            const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
+                            const float vel_correction_max, const float vel_max, const float accel_max, const float tc, const float dt)
 {
     if (!is_positive(tc)) {
         return;
diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h
index d7309c23dd..cb7edf9127 100644
--- a/libraries/AP_Math/control.h
+++ b/libraries/AP_Math/control.h
@@ -26,14 +26,14 @@ void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
+ The function alters the input accel to be jerk limited
 */
-void shape_accel(float accel_input, float& accel,
-                 float accel_min, float accel_max,
-                 float tc, float dt);
+void shape_accel(const float accel_input, float& accel,
+                 const float accel_min, const float accel_max,
+                 const float tc, const float dt);
 
 void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
-                    float accel_max, float tc, float dt);
+                    const float accel_max, const float tc, const float dt);
 
 /* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity.
  The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@@ -44,13 +44,14 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
+ The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel
+ to be jerk limited
 */
 void shape_vel_accel(float &vel_input, const float accel_input,
-                     float vel, float& accel,
-                     float vel_min, float vel_max,
-                     float accel_min, float accel_max,
-                     float tc, float dt);
+                     const float vel, float& accel,
+                     const float vel_min, const float vel_max,
+                     const float accel_min, const float accel_max,
+                     const float tc, const float dt);
 
 /* shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
  The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@@ -61,12 +62,13 @@ void shape_vel_accel(float &vel_input, const float accel_input,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
+ The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited
  This function operates on the x and y axis of both Vector2f or Vector3f inputs.
  The accel_max limit can be removed by setting it to zero.
 */
 void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
-                        const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float tc, float dt);
+                        const Vector2f& vel, Vector2f& accel,
+                        const float vel_max, const float accel_max, const float tc, const float dt);
 
 /* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
  The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@@ -77,12 +79,12 @@ void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
+ The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited
 */
 void shape_pos_vel_accel(float &pos_input, const float vel_input, const float accel_input,
-                         float pos, float vel, float& accel,
-                         float vel_correction_max, float vel_min, float vel_max,
-                         float accel_min, float accel_max, float tc, float dt);
+                         const float pos, const float vel, float& accel,
+                         const float vel_correction_max, const float vel_min, const float vel_max,
+                         const float accel_min, const float accel_max, const float tc, const float dt);
 
 /* shape_pos_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
  The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@@ -93,13 +95,12 @@ void shape_pos_vel_accel(float &pos_input, const float vel_input, const float ac
  The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
  The time constant also defines the time taken to achieve the maximum acceleration.
  The time constant must be positive.
- The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
- This function operates only on the x and y axis of the Vector2f or Vector3f inputs.
+ The function alters the accel to be jerk limited
  The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
 */
 void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
-    const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
-    float vel_correction_max, float vel_max, float accel_max, float tc, float dt);
+                            const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
+                            const float vel_correction_max, const float vel_max, const float accel_max, const float tc, const float dt);
 
 // proportional controller with piecewise sqrt sections to constrain second derivative
 float sqrt_controller(float error, float p, float second_ord_lim, float dt);