Math: added a function to combine standard rotations

this will allow us to have an overall board rotation plus a per-sensor
rotation

libraries/AP_Math/AP_Math.cpp

@@ -30,3 +30,40 @@ float safe_sqrt(float v)
 	}
 	return ret;
 }
+
+
+// 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
+// Note that this relies the set of rotations being complete. The
+// optional 'found' parameter is for the test suite to ensure that it is.
+enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found)
+{
+	Vector3f tv1, tv2;
+	enum Rotation r;
+	tv1(1,2,3);
+	tv1.rotate(r1);
+	tv1.rotate(r2);
+
+	for (r=ROTATION_NONE; r<ROTATION_MAX;
+	     r = (enum Rotation)((uint8_t)r+1)) {
+		Vector3f diff;
+		tv2(1,2,3);
+		tv2.rotate(r);
+		diff = tv1 - tv2;
+		if (diff.length() < 1.0e-6) {
+			// we found a match
+			if (found) {
+				*found = true;
+			}
+			return r;
+		}
+	}
+
+	// we found no matching rotation. Someone has edited the
+	// rotations list and broken its completeness property ...
+	if (found) {
+		*found = false;
+	}
+	return ROTATION_NONE;
+}

libraries/AP_Math/AP_Math.h

@@ -24,4 +24,9 @@ float safe_asin(float v);
 // a varient of sqrt() that always gives a valid answer.
 float safe_sqrt(float v);
 
+// 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

libraries/AP_Math/examples/rotations/rotations.pde

@@ -25,6 +25,11 @@ AP_Baro_BMP085_HIL      barometer;
 AP_Compass_HIL     compass;
 #endif
 
+#if AUTOMATIC_DECLINATION == ENABLED
+// this is in an #if to avoid the static data
+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#endif
+
 
 // standard rotation matrices (these are the originals from the old code)
 #define MATRIX_ROTATION_NONE               Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
@@ -162,6 +167,28 @@ static void test_vectors(void)
 }
 
 
+// test combinations of rotations
+static void test_combinations(void)
+{
+    enum Rotation r1, r2, r3;
+    bool found;
+
+	for (r1=ROTATION_NONE; r1<ROTATION_MAX;
+	     r1 = (enum Rotation)((uint8_t)r1+1)) {
+        for (r2=ROTATION_NONE; r2<ROTATION_MAX;
+             r2 = (enum Rotation)((uint8_t)r2+1)) {
+            r3 = rotation_combination(r1, r2, &found);
+            if (found) {
+                Serial.printf("rotation: %u + %u -> %u\n",
+                              (unsigned)r1, (unsigned)r2, (unsigned)r3);
+            } else {
+                Serial.printf("ERROR rotation: no combination for %u + %u\n",
+                              (unsigned)r1, (unsigned)r2);
+            }
+        }
+    }
+}
+
 /*
   rotation tests
  */
@@ -171,6 +198,7 @@ void setup(void)
     Serial.println("rotation unit tests\n");
     test_matrices();
     test_vectors();
+    test_combinations();
     Serial.println("rotation unit tests done\n");
 }
 

libraries/AP_Math/matrix3.cpp

@@ -44,6 +44,7 @@ void Matrix3<T>::rotation(enum Rotation r)
 {
     switch (r) {
     case ROTATION_NONE:
+    case ROTATION_MAX:
 	    *this = MATRIX_ROTATION_NONE;
 	    break;
     case ROTATION_YAW_45:

libraries/AP_Math/rotations.h

@@ -17,6 +17,12 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+
+// these rotations form a full set - every rotation in the following
+// list when combined with another in the list forms an entry which is
+// also in the list. This is an important property. Please run the
+// rotations test suite if you add to the list.
+
 // these rotation values are stored to EEPROM, so be careful not to
 // change the numbering of any existing entry when adding a new entry.
 enum Rotation {
@@ -35,5 +41,6 @@ enum Rotation {
 	ROTATION_PITCH_180,
 	ROTATION_ROLL_180_YAW_225,
 	ROTATION_ROLL_180_YAW_270,
-	ROTATION_ROLL_180_YAW_315
+	ROTATION_ROLL_180_YAW_315,
+    ROTATION_MAX
 };

libraries/AP_Math/vector3.cpp

@@ -29,6 +29,7 @@ void Vector3<T>::rotate(enum Rotation rotation)
     T tmp;
     switch (rotation) {
     case ROTATION_NONE:
+    case ROTATION_MAX:
         return;
     case ROTATION_YAW_45: {
         tmp = HALF_SQRT_2*(x - y);