AP_Math: implement double versions of some position control methods

libraries/AP_Math/control.cpp

@@ -39,7 +39,7 @@ void update_vel_accel(float& vel, float accel, float dt, float limit)
 
 // update_pos_vel_accel - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
 // the position and velocity is not moved in the direction of limit if limit is not set to zero
-void update_pos_vel_accel(float& pos, float& vel, float accel, float dt, float limit)
+void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit)
 {
     // move position and velocity forward by dt if it does not increase error when limited.
     float delta_pos = vel * dt + accel * 0.5f * sq(dt);
@@ -68,7 +68,7 @@ void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, Vector2
 
 // update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
 // the position and velocity is not moved in the direction of limit if limit is not set to zero
-void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit)
+void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit)
 {
     // move position and velocity forward by dt.
     Vector2f delta_pos = vel * dt + accel * 0.5f * sq(dt);
@@ -81,7 +81,7 @@ void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel
         }
     }
 
-    pos += delta_pos;
+    pos += delta_pos.topostype();
 
     update_vel_accel_xy(vel, accel, dt, limit);
 }
@@ -245,8 +245,8 @@ void shape_vel_accel_xy(const Vector2f &vel_input1, const Vector2f& accel_input,
  The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input
  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(const float pos_input, const float vel_input, const float accel_input,
-                         const float pos, const float vel, float& accel,
+void shape_pos_vel_accel(const postype_t pos_input, const float vel_input, const float accel_input,
+                         const postype_t 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)
 {
@@ -277,8 +277,8 @@ void shape_pos_vel_accel(const float pos_input, const float vel_input, const flo
 }
 
 // 2D version
-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,
+void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
+                            const Vector2p& 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)) {
@@ -290,7 +290,7 @@ void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input
     const float accel_tc_max = accel_max*(1.0f - 1.0f/CONTROL_TIME_CONSTANT_RATIO);
 
     // position error to be corrected
-    Vector2f pos_error = pos_input - pos;
+    Vector2f pos_error = (pos_input - pos).tofloat();
 
     // velocity to correct position
     Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);

libraries/AP_Math/control.h

@@ -29,13 +29,13 @@ void update_vel_accel(float& vel, float accel, float dt, float limit);
 
 // update_vel_accel projects the velocity, vel, forward in time based on a time step of dt and acceleration of accel.
 // update_vel_accel - single axis projection.
-void update_pos_vel_accel(float& pos, float& vel, float accel, float dt, float limit);
+void update_pos_vel_accel(postype_t & pos, float& vel, float accel, float dt, float limit);
 
 // update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
 void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
 
 // update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
-void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
+void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
 
 /* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
  The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@@ -84,12 +84,12 @@ void shape_vel_accel_xy(const Vector2f &vel_input, const Vector2f& accel_input,
  The time constant must be positive.
  The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input
 */
-void shape_pos_vel_accel(const float pos_input, const float vel_input, const float accel_input,
-                         const float pos, const float vel, float& accel,
+void shape_pos_vel_accel(const postype_t pos_input, const float vel_input, const float accel_input,
+                         const postype_t 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);
-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,
+void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
+                            const Vector2p& 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

libraries/AP_Math/vector2.cpp

@@ -448,18 +448,6 @@ void Vector2<T>::rotate(float angle_rad)
     y = ry;
 }
 
-template <typename T>
-Vector2<double> Vector2<T>::todouble(void) const
-{
-    return Vector2d{x,y};
-}
-
-template <typename T>
-Vector2<float> Vector2<T>::tofloat(void) const
-{
-    return Vector2f{float(x),float(y)};
-}
-
 // define for float and double
 template class Vector2<float>;
 template class Vector2<double>;

libraries/AP_Math/vector2.h

@@ -165,8 +165,12 @@ struct Vector2
     /*
       conversion to/from double
      */
-    Vector2<float> tofloat() const;
-    Vector2<double> todouble() const;
+    Vector2<float> tofloat() const {
+        return Vector2<float>{float(x),float(y)};
+    }
+    Vector2<double> todouble() const {
+        return Vector2<double>{x,y};
+    }
     
     // given a position p1 and a velocity v1 produce a vector
     // perpendicular to v1 maximising distance from p1

libraries/AP_Math/vector3.h

@@ -268,8 +268,12 @@ public:
     /*
       conversion to/from double
      */
-    Vector3<float> tofloat() const;
-    Vector3<double> todouble() const;
+    Vector3<float> tofloat() const {
+        return Vector3<float>{float(x),float(y),float(z)};
+    }
+    Vector3<double> todouble() const {
+        return Vector3<double>{x,y,z};
+    }
 
     // given a position p1 and a velocity v1 produce a vector
     // perpendicular to v1 maximising distance from p1.  If p1 is the