AP_Math: sync for 4.1.0beta releases

libraries/AP_Math/AP_Math.cpp

@@ -244,36 +244,25 @@ long wrap_360_cd(const long angle)
     }
     return res;
 }
-template <typename T>
+
-float wrap_PI(const T radian)
+ftype wrap_PI(const ftype radian)
 {
-    auto res = wrap_2PI(radian);
+    ftype res = wrap_2PI(radian);
     if (res > M_PI) {
         res -= M_2PI;
     }
     return res;
 }
 
-template float wrap_PI<int>(const int radian);
+ftype wrap_2PI(const ftype radian)
-template float wrap_PI<short>(const short radian);
-template float wrap_PI<float>(const float radian);
-template float wrap_PI<double>(const double radian);
-
-template <typename T>
-float wrap_2PI(const T radian)
 {
-    float res = fmodf(static_cast<float>(radian), M_2PI);
+    ftype res = fmodF(radian, M_2PI);
     if (res < 0) {
         res += M_2PI;
     }
     return res;
 }
 
-template float wrap_2PI<int>(const int radian);
-template float wrap_2PI<short>(const short radian);
-template float wrap_2PI<float>(const float radian);
-template float wrap_2PI<double>(const double radian);
-
 template <typename T>
 T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
 {
@@ -423,7 +412,7 @@ void fill_nanf(float *f, uint16_t count)
 void fill_nanf(double *f, uint16_t count)
 {
     while (count--) {
-        *f++ = 0;
+        *f++ = std::numeric_limits<double>::signaling_NaN();
     }
 }
 #endif

libraries/AP_Math/AP_Math.h

@@ -142,14 +142,12 @@ double wrap_360_cd(const double angle);
 /*
   wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
  */
-template <typename T>
+ftype wrap_PI(const ftype radian);
-float wrap_PI(const T radian);
 
 /*
  * wrap an angle in radians to 0..2PI
  */
-template <typename T>
+ftype wrap_2PI(const ftype radian);
-float wrap_2PI(const T radian);
 
 /*
  * Constrain a value to be within the range: low and high
@@ -179,7 +177,7 @@ inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64
 }
 
 // degrees -> radians
-static inline constexpr float radians(float deg)
+static inline constexpr ftype radians(ftype deg)
 {
     return deg * DEG_TO_RAD;
 }

libraries/AP_Math/SCurve.cpp

@@ -738,7 +738,7 @@ void SCurve::add_segments(float L)
 // Vm - maximum constant velocity
 // L - Length of the path
 // t2_out, t4_out, t6_out are the segment durations needed to achieve the kinimatic path specified by the input variables
-void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const
+void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out)
 {
     // init outputs
     Jm_out = 0.0f;
@@ -772,6 +772,7 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl
             // solution = 2 - t6 t4 t2 = 0 1 0
             t2_out = 0.0f;
             t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
+            t4_out = MAX(t4_out, 0.0);
             t6_out = 0.0f;
         }
     } else {
@@ -785,6 +786,7 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl
             // solution = 7 - t6 t4 t2 = 1 1 1
             t2_out = Am / Jm - tj;
             t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
+            t4_out = MAX(t4_out, 0.0);
             t6_out = t2_out;
         }
     }

libraries/AP_Math/SCurve.h

@@ -52,6 +52,16 @@ public:
     // initialise and clear the path
     void init();
 
+    // calculate the segment times for the trigonometric S-Curve path defined by:
+    // tj - duration of the raised cosine jerk profile (aka jerk time)
+    // Jm - maximum value of the raised cosine jerk profile (aka jerk max)
+    // V0 - initial velocity magnitude
+    // Am - maximum constant acceleration
+    // Vm - maximum constant velocity
+    // L - Length of the path
+    // this is an internal function, static for test suite
+    static void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out);
+
     // generate a trigonometric track in 3D space that moves over a straight line
     // between two points defined by the origin and destination
     void calculate_track(const Vector3f &origin, const Vector3f &destination,
@@ -135,15 +145,6 @@ private:
     // generate time segments for straight segment
     void add_segments(float L);
 
-    // calculate the segment times for the trigonometric S-Curve path defined by:
-    // tj - duration of the raised cosine jerk profile (aka jerk time)
-    // Jm - maximum value of the raised cosine jerk profile (aka jerk max)
-    // V0 - initial velocity magnitude
-    // Am - maximum constant acceleration
-    // Vm - maximum constant velocity
-    // L - Length of the path
-    void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const;
-
     // generate three time segments forming the jerk profile
     void add_segments_jerk(uint8_t &seg_pnt, float tj, float Jm, float Tcj);
 

libraries/AP_Math/control.cpp

@@ -155,6 +155,17 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
     }
 }
 
+void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
+    float accel_max, float tc, float dt)
+{
+    const Vector2f accel_input_2f {accel_input.x, accel_input.y};
+    Vector2f accel_2f {accel.x, accel.y};
+
+    shape_accel_xy(accel_input_2f, accel_2f, accel_max, tc, dt);
+    accel.x = accel_2f.x;
+    accel.y = accel_2f.y;
+}
+
 
 /* shape_vel_accel and 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.

libraries/AP_Math/control.h

@@ -2,6 +2,8 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/AP_HAL_Boards.h>
+#include "vector2.h"
+#include "vector3.h"
 
 #ifndef HAL_WITH_POSTYPE_DOUBLE
 #define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024
@@ -53,6 +55,9 @@ void shape_accel(const float accel_input, float& accel,
 void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
                     const float accel_max, const float tc, const float dt);
 
+void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
+    float accel_max, float tc, 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.
  The kinematic path is constrained by :

libraries/AP_Math/definitions.h

@@ -7,7 +7,7 @@
 #ifdef M_PI
 # undef M_PI
 #endif
-#define M_PI      (3.141592653589793f)
+#define M_PI      (3.141592653589793)
 
 #ifdef M_PI_2
 # undef M_PI_2

libraries/AP_Math/ftype.h

@@ -26,6 +26,7 @@ typedef double ftype;
 #define fabsF(x) fabs(x)
 #define ceilF(x) ceil(x)
 #define fminF(x,y) fmin(x,y)
+#define fmodF(x,y) fmod(x,y)
 #define toftype todouble
 #else
 typedef float ftype;
@@ -43,6 +44,7 @@ typedef float ftype;
 #define fabsF(x) fabsf(x)
 #define ceilF(x) ceilf(x)
 #define fminF(x,y) fminf(x,y)
+#define fmodF(x,y) fmodf(x,y)
 #define toftype tofloat
 #endif
 

libraries/AP_Math/quaternion.cpp

@@ -71,33 +71,6 @@ void QuaternionT<T>::rotation_matrix(Matrix3f &m) const
     m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
 }
 
-// return the rotation matrix equivalent for this quaternion after normalization
-template <typename T>
-void QuaternionT<T>::rotation_matrix_norm(Matrix3<T> &m) const
-{
-    const T q1q1 = q1 * q1;
-    const T q1q2 = q1 * q2;
-    const T q1q3 = q1 * q3;
-    const T q1q4 = q1 * q4;
-    const T q2q2 = q2 * q2;
-    const T q2q3 = q2 * q3;
-    const T q2q4 = q2 * q4;
-    const T q3q3 = q3 * q3;
-    const T q3q4 = q3 * q4;
-    const T q4q4 = q4 * q4;
-    const T invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
-
-    m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
-    m.a.y = 2.0f*(q2q3 - q1q4)*invs;
-    m.a.z = 2.0f*(q2q4 + q1q3)*invs;
-    m.b.x = 2.0f*(q2q3 + q1q4)*invs;
-    m.b.y = (-q2q2 + q3q3 - q4q4 + q1q1)*invs;
-    m.b.z = 2.0f*(q3q4 - q1q2)*invs;
-    m.c.x = 2.0f*(q2q4 - q1q3)*invs;
-    m.c.y = 2.0f*(q3q4 + q1q2)*invs;
-    m.c.z = (-q2q2 - q3q3 + q4q4 + q1q1)*invs;
-}
-
 // return the rotation matrix equivalent for this quaternion
 // Thanks to Martin John Baker
 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
@@ -481,9 +454,9 @@ void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
         q2=q3=q4=0.0f;
         return;
     }
-    const T st2 = sinF(theta/2.0f);
+    const T st2 = sinF(0.5*theta);
 
-    q1 = cosF(theta/2.0f);
+    q1 = cosF(0.5*theta);
     q2 = axis.x * st2;
     q3 = axis.y * st2;
     q4 = axis.z * st2;
@@ -507,7 +480,7 @@ void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
     v = Vector3<T>(q2,q3,q4);
     if (!is_zero(l)) {
         v /= l;
-        v *= wrap_PI(2.0f * atan2f(l,q1));
+        v *= wrap_PI(2.0f * atan2F(l,q1));
     }
 }
 
@@ -531,11 +504,11 @@ void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
 template <typename T>
 void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
 {
-    const T t2 = theta/2.0f;
+    const T t2 = 0.5*theta;
     const T sqt2 = sq(t2);
     const T st2 = t2-sqt2*t2/6.0f;
 
-    q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
+    q1 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
     q2 = axis.x * st2;
     q3 = axis.y * st2;
     q4 = axis.z * st2;
@@ -550,13 +523,13 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
     if (is_zero(theta)) {
         return;
     }
-    const T t2 = theta/2.0f;
+    const T t2 = 0.5*theta;
     const T sqt2 = sq(t2);
     T st2 = t2-sqt2*t2/6.0f;
     st2 /= theta;
 
     //"rotation quaternion"
-    const T w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
+    const T w2 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
     const T x2 = v.x * st2;
     const T y2 = v.y * st2;
     const T z2 = v.z * st2;
@@ -578,7 +551,7 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
 template <typename T>
 T QuaternionT<T>::get_euler_roll() const
 {
-    return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
+    return (atan2F(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
 }
 
 // get euler pitch angle
@@ -592,7 +565,7 @@ T QuaternionT<T>::get_euler_pitch() const
 template <typename T>
 T QuaternionT<T>::get_euler_yaw() const
 {
-    return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
+    return atan2F(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
 }
 
 // create eulers from a quaternion
@@ -772,7 +745,7 @@ T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
     const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0);
 
     // calculate and return angular difference
-    return (2.0 * asinf(vec_len_div2));
+    return (2.0 * asinF(vec_len_div2));
 }
 
 // define for float and double

libraries/AP_Math/quaternion.h

@@ -67,9 +67,6 @@ public:
     void        rotation_matrix(Matrix3f &m) const;
     void        rotation_matrix(Matrix3d &m) const;
 
-    // return the rotation matrix equivalent for this quaternion after normalization
-    void        rotation_matrix_norm(Matrix3<T> &m) const;
-
     // return the rotation matrix equivalent for this quaternion
     void		from_rotation_matrix(const Matrix3<T> &m);
 

libraries/AP_Math/tests/test_vector2.cpp

@@ -109,7 +109,7 @@ TEST(Vector2Test, angle)
 {
     EXPECT_FLOAT_EQ(M_PI/2, Vector2f(0, 1).angle());
     EXPECT_FLOAT_EQ(M_PI/4, Vector2f(1, 1).angle());
-    EXPECT_FLOAT_EQ(0.0f, Vector2f(1, 0).angle());
+    EXPECT_FLOAT_EQ(0.0f, Vector2d(1, 0).angle());
     EXPECT_FLOAT_EQ(M_PI*5/4, Vector2f(-1, -1).angle());
     EXPECT_FLOAT_EQ(M_PI*5/4, Vector2f(-5, -5).angle());
 

libraries/AP_Math/vector2.h

@@ -34,6 +34,7 @@
 #endif
 
 #include <cmath>
+#include <float.h>
 #include <AP_Common/AP_Common.h>
 #include "ftype.h"
 

libraries/AP_Math/vector3.h

@@ -87,6 +87,12 @@ public:
         , y(y0)
         , z(z0) {}
 
+    //Create a Vector3 from a Vector2 with z
+    constexpr Vector3<T>(const Vector2<T> &v0, const T z0)
+        : x(v0.x)
+        , y(v0.y)
+        , z(z0) {}
+
     // test for equality
     bool operator ==(const Vector3<T> &v) const;