From b6040878b48e3fdf2859c108cfee734272e029a1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 11 Mar 2012 23:17:05 +1100 Subject: [PATCH] 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 | 37 +++++++++++++++++++ libraries/AP_Math/AP_Math.h | 5 +++ .../AP_Math/examples/rotations/rotations.pde | 28 ++++++++++++++ libraries/AP_Math/matrix3.cpp | 1 + libraries/AP_Math/rotations.h | 9 ++++- libraries/AP_Math/vector3.cpp | 1 + 6 files changed, 80 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index ab16fbe833..abb8a360c8 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/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 // 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 %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"); } diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 9122be5316..133c7bdd6e 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -44,6 +44,7 @@ void Matrix3::rotation(enum Rotation r) { switch (r) { case ROTATION_NONE: + case ROTATION_MAX: *this = MATRIX_ROTATION_NONE; break; case ROTATION_YAW_45: diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 839119b9e8..ca648bde54 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -17,6 +17,12 @@ * with this program. If not, see . */ + +// 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 }; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 9e0be0630c..cd373488df 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -29,6 +29,7 @@ void Vector3::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);