AP_Math: support some more rotation combinations

libraries/AP_Math/AP_Math.cpp

@@ -31,7 +31,7 @@ float safe_sqrt(float v)
     return ret;
 }
 
-
+#if ROTATION_COMBINATION_SUPPORT
 // find a rotation that is the combination of two other
 // rotations. This is used to allow us to add an overall board
 // rotation to an existing rotation of a sensor such as the compass
@@ -67,6 +67,7 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou
     }
     return ROTATION_NONE;
 }
+#endif
 
 // constrain a value
 float constrain(float amt, float low, float high) {

libraries/AP_Math/AP_Math.h

@@ -25,6 +25,7 @@
 // acceleration due to gravity in m/s/s
 #define GRAVITY_MSS 9.80665
 
+#define ROTATION_COMBINATION_SUPPORT 0
 
 // define AP_Param types AP_Vector3f and Ap_Matrix3f
 AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
@@ -36,10 +37,12 @@ float           safe_asin(float v);
 // a varient of sqrt() that always gives a valid answer.
 float           safe_sqrt(float v);
 
+#if ROTATION_COMBINATION_SUPPORT
 // find a rotation that is the combination of two other
 // rotations. This is used to allow us to add an overall board
 // rotation to an existing rotation of a sensor such as the compass
 enum Rotation           rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);
+#endif
 
 // return distance in meters between two locations
 float                   get_distance(const struct Location *loc1, const struct Location *loc2);

libraries/AP_Math/examples/rotations/Makefile

@@ -1,4 +1 @@
 include ../../../../mk/apm.mk
-
-sitl:
-	make -f ../../../../libraries/Desktop/Desktop.mk

libraries/AP_Math/examples/rotations/rotations.pde

@@ -2,15 +2,24 @@
 //
 // Unit tests for the AP_Math rotations code
 //
+#include <AP_HAL.h>
 #include <stdlib.h>
 #include <AP_Common.h>
 #include <AP_Progmem.h>
 #include <AP_Param.h>
-#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include <AP_HAL_Empty.h>
+#include <AP_HAL_PX4.h>
 #include <AP_Math.h>
+#include <Filter.h>
+#include <AP_ADC.h>
+#include <SITL.h>
+#include <AP_Compass.h>
+#include <AP_Baro.h>
+#include <GCS_MAVLink.h>
 #include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
 
-#include <AP_HAL_AVR.h>
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
 // standard rotation matrices (these are the originals from the old code)
@@ -30,6 +39,10 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 #define MATRIX_ROTATION_ROLL_180_YAW_225   Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
 #define MATRIX_ROTATION_ROLL_180_YAW_270   Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
 #define MATRIX_ROTATION_ROLL_180_YAW_315   Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
+#define MATRIX_ROTATION_ROLL_90            Matrix3f(1, 0,  0,  0, 0, -1,  0,  1, 0)
+#define MATRIX_ROTATION_ROLL_270           Matrix3f(1, 0,  0,  0, 0,  1,  0, -1, 0)
+#define MATRIX_ROTATION_PITCH_90           Matrix3f(0, 0,  1,  0, 1,  0, -1,  0, 0)
+#define MATRIX_ROTATION_PITCH_270          Matrix3f(0, 0, -1,  0, 1,  0,  1,  0, 0)
 
 static void print_matrix(Matrix3f &m)
 {
@@ -39,6 +52,12 @@ static void print_matrix(Matrix3f &m)
                   m.c.x, m.c.y, m.c.z);
 }
 
+static void print_vector(Vector3f &v)
+{
+    hal.console->printf("[%.2f %.2f %.2f]\n",
+                        v.x, v.y, v.z);
+}
+
 // test one matrix
 static void test_matrix(enum Rotation rotation, Matrix3f m)
 {
@@ -75,6 +94,10 @@ static void test_matrices(void)
     test_matrix(ROTATION_ROLL_180_YAW_225, MATRIX_ROTATION_ROLL_180_YAW_225);
     test_matrix(ROTATION_ROLL_180_YAW_270, MATRIX_ROTATION_ROLL_180_YAW_270);
     test_matrix(ROTATION_ROLL_180_YAW_315, MATRIX_ROTATION_ROLL_180_YAW_315);
+    test_matrix(ROTATION_ROLL_90,   MATRIX_ROTATION_ROLL_90);
+    test_matrix(ROTATION_ROLL_270,  MATRIX_ROTATION_ROLL_270);
+    test_matrix(ROTATION_PITCH_90,  MATRIX_ROTATION_PITCH_90);
+    test_matrix(ROTATION_PITCH_270, MATRIX_ROTATION_PITCH_270);
 }
 
 // test rotation of vectors
@@ -149,6 +172,12 @@ static void test_vectors(void)
 }
 
 
+static void new_combination(enum Rotation r1, enum Rotation r2)
+{
+    
+}
+
+#if ROTATION_COMBINATION_SUPPORT
 // test combinations of rotations
 static void test_combinations(void)
 {
@@ -166,10 +195,13 @@ static void test_combinations(void)
             } else {
                 hal.console->printf("ERROR rotation: no combination for %u + %u\n",
                               (unsigned)r1, (unsigned)r2);
+                new_combination(r1, r2);
             }
         }
     }
 }
+#endif
+
 
 // test rotation method accuracy
 static void test_rotation_accuracy(void)
@@ -204,6 +236,67 @@ static void test_rotation_accuracy(void)
     }
 }
 
+static void test_euler(enum Rotation rotation, float roll, float pitch, float yaw)
+{
+    Vector3f v, v1, v2, diff;
+    Matrix3f rotmat;
+    const float accuracy = 1.0e-6;
+
+    v.x = 1;
+    v.y = 2;
+    v.z = 3;
+    v1 = v;
+
+    v1.rotate(rotation);
+    
+    rotmat.from_euler(radians(roll), radians(pitch), radians(yaw));
+    v2 = v;
+    v2 = rotmat * v2;
+
+    diff = (v2 - v1);
+    if (diff.length() > accuracy) {
+        hal.console->printf("euler test %u incorrect\n", (unsigned)rotation);
+        print_vector(v);
+        print_vector(v1);
+        print_vector(v2);
+    }
+#if 0
+    if (rotation >= ROTATION_ROLL_90_YAW_45)
+        print_matrix(rotmat);
+#endif
+}
+
+static void test_eulers(void)
+{
+    hal.console->println("euler tests");
+    test_euler(ROTATION_NONE,               0,   0,   0);
+    test_euler(ROTATION_YAW_45,             0,   0,  45);
+    test_euler(ROTATION_YAW_90,             0,   0,  90);
+    test_euler(ROTATION_YAW_135,            0,   0, 135);
+    test_euler(ROTATION_YAW_180,            0,   0, 180);
+    test_euler(ROTATION_YAW_225,            0,   0, 225);
+    test_euler(ROTATION_YAW_270,            0,   0, 270);
+    test_euler(ROTATION_YAW_315,            0,   0, 315);
+    test_euler(ROTATION_ROLL_180,         180,   0,   0);
+    test_euler(ROTATION_ROLL_180_YAW_45,  180,   0,  45);
+    test_euler(ROTATION_ROLL_180_YAW_90,  180,   0,  90);
+    test_euler(ROTATION_ROLL_180_YAW_135, 180,   0, 135);
+    test_euler(ROTATION_PITCH_180,          0, 180,   0);
+    test_euler(ROTATION_ROLL_180_YAW_225, 180,   0, 225);
+    test_euler(ROTATION_ROLL_180_YAW_270, 180,   0, 270);
+    test_euler(ROTATION_ROLL_180_YAW_315, 180,   0, 315);
+    test_euler(ROTATION_ROLL_90,           90,   0,   0);
+    test_euler(ROTATION_ROLL_90_YAW_45,    90,   0,  45);
+    test_euler(ROTATION_ROLL_90_YAW_90,    90,   0,  90);
+    test_euler(ROTATION_ROLL_90_YAW_135,   90,   0, 135);
+    test_euler(ROTATION_ROLL_270,         270,   0,   0);
+    test_euler(ROTATION_ROLL_270_YAW_45,  270,   0,  45);
+    test_euler(ROTATION_ROLL_270_YAW_90,  270,   0,  90);
+    test_euler(ROTATION_ROLL_270_YAW_135, 270,   0, 135);
+    test_euler(ROTATION_PITCH_90,           0,  90,   0);
+    test_euler(ROTATION_PITCH_270,          0, 270,   0);    
+}
+
 /*
  *  rotation tests
  */
@@ -212,8 +305,11 @@ void setup(void)
     hal.console->println("rotation unit tests\n");
     test_matrices();
     test_vectors();
+#if ROTATION_COMBINATION_SUPPORT
     test_combinations();
+#endif
     test_rotation_accuracy();
+    test_eulers();
     hal.console->println("rotation unit tests done\n");
 }
 

libraries/AP_Math/matrix3.cpp

@@ -37,6 +37,16 @@
 #define MATRIX_ROTATION_ROLL_180_YAW_225   Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1)
 #define MATRIX_ROTATION_ROLL_180_YAW_270   Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
 #define MATRIX_ROTATION_ROLL_180_YAW_315   Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1)
+#define MATRIX_ROTATION_ROLL_90            Matrix3f(1, 0,  0,  0, 0, -1,  0,  1, 0)
+#define MATRIX_ROTATION_ROLL_90_YAW_45     Matrix3f(HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, 0, 1, 0)
+#define MATRIX_ROTATION_ROLL_90_YAW_90     Matrix3f(0, 0, 1, 1, 0, 0, 0, 1, 0)
+#define MATRIX_ROTATION_ROLL_90_YAW_135    Matrix3f(-HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, 0, 1, 0)
+#define MATRIX_ROTATION_ROLL_270           Matrix3f(1, 0,  0,  0, 0,  1,  0, -1, 0)
+#define MATRIX_ROTATION_ROLL_270_YAW_45    Matrix3f(HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, 0, -1, 0)
+#define MATRIX_ROTATION_ROLL_270_YAW_90    Matrix3f(0, 0, -1, 1, 0, 0, 0, -1, 0)
+#define MATRIX_ROTATION_ROLL_270_YAW_135   Matrix3f(-HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, 0, -1, 0)
+#define MATRIX_ROTATION_PITCH_90           Matrix3f(0, 0,  1,  0, 1,  0, -1,  0, 0)
+#define MATRIX_ROTATION_PITCH_270          Matrix3f(0, 0, -1,  0, 1,  0,  1,  0, 0)
 
 // fill in a matrix with a standard rotation
 template <typename T>
@@ -92,6 +102,36 @@ void Matrix3<T>::rotation(enum Rotation r)
     case ROTATION_ROLL_180_YAW_315:
         *this = MATRIX_ROTATION_ROLL_180_YAW_315;
         break;
+    case ROTATION_ROLL_90:
+        *this = MATRIX_ROTATION_ROLL_90;
+        break;
+    case ROTATION_ROLL_90_YAW_45:
+        *this = MATRIX_ROTATION_ROLL_90_YAW_45;
+        break;
+    case ROTATION_ROLL_90_YAW_90:
+        *this = MATRIX_ROTATION_ROLL_90_YAW_90;
+        break;
+    case ROTATION_ROLL_90_YAW_135:
+        *this = MATRIX_ROTATION_ROLL_90_YAW_135;
+        break;
+    case ROTATION_ROLL_270:
+        *this = MATRIX_ROTATION_ROLL_270;
+        break;
+    case ROTATION_ROLL_270_YAW_45:
+        *this = MATRIX_ROTATION_ROLL_270_YAW_45;
+        break;
+    case ROTATION_ROLL_270_YAW_90:
+        *this = MATRIX_ROTATION_ROLL_270_YAW_90;
+        break;
+    case ROTATION_ROLL_270_YAW_135:
+        *this = MATRIX_ROTATION_ROLL_270_YAW_135;
+        break;
+    case ROTATION_PITCH_90:
+        *this = MATRIX_ROTATION_PITCH_90;
+        break;
+    case ROTATION_PITCH_270:
+        *this = MATRIX_ROTATION_PITCH_270;
+        break;
     }
 }
 

libraries/AP_Math/rotations.h

@@ -42,5 +42,15 @@ enum Rotation {
     ROTATION_ROLL_180_YAW_225,
     ROTATION_ROLL_180_YAW_270,
     ROTATION_ROLL_180_YAW_315,
+    ROTATION_ROLL_90,
+    ROTATION_ROLL_90_YAW_45,
+    ROTATION_ROLL_90_YAW_90,
+    ROTATION_ROLL_90_YAW_135,
+    ROTATION_ROLL_270,
+    ROTATION_ROLL_270_YAW_45,
+    ROTATION_ROLL_270_YAW_90,
+    ROTATION_ROLL_270_YAW_135,
+    ROTATION_PITCH_90,
+    ROTATION_PITCH_270,
     ROTATION_MAX
 };

libraries/AP_Math/vector3.cpp

@@ -106,6 +106,60 @@ void Vector3<T>::rotate(enum Rotation rotation)
         x = tmp; z = -z;
         return;
     }
+    case ROTATION_ROLL_90: {
+        tmp = z; z = y; y = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_45: {
+        tmp = z; z = y; y = -tmp;
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_90: {
+        tmp = z; z = y; y = -tmp;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_135: {
+        tmp = z; z = y; y = -tmp;
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270: {
+        tmp = z; z = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_45: {
+        tmp = z; z = -y; y = tmp;
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_90: {
+        tmp = z; z = -y; y = tmp;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_135: {
+        tmp = z; z = -y; y = tmp;
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_PITCH_90: {
+        tmp = z; z = -x; x = tmp;
+        return;
+    }
+    case ROTATION_PITCH_270: {
+        tmp = z; z = x; x = -tmp;
+        return;
+    }
     }
 }