AP_Math: added rotation_equal()

libraries/AP_Math/AP_Math.cpp

@@ -255,3 +255,21 @@ bool is_valid_octal(uint16_t octal)
     }
     return true;
 }
+
+/*
+  return true if two rotations are equivalent
+  This copes with the fact that we have some duplicates, like ROLL_180_YAW_90 and PITCH_180_YAW_270
+ */
+bool rotation_equal(enum Rotation r1, enum Rotation r2)
+{
+    if (r1 == r2) {
+        return true;
+    }
+    Vector3f v(1,2,3);
+    Vector3f v1 = v;
+    Vector3f v2 = v;
+    v1.rotate(r1);
+    v2.rotate(r2);
+    return (v1 - v2).length() < 0.001;
+}
+

libraries/AP_Math/AP_Math.h

@@ -258,3 +258,5 @@ Vector3f rand_vec3f(void);
 // confirm a value is a valid octal value
 bool is_valid_octal(uint16_t octal);
 
+// return true if two rotations are equal
+bool rotation_equal(enum Rotation r1, enum Rotation r2);