diff --git a/libraries/APM_Compass/APM_Compass.cpp b/libraries/APM_Compass/APM_Compass.cpp index 4cbe5d4eba..fb695da044 100644 --- a/libraries/APM_Compass/APM_Compass.cpp +++ b/libraries/APM_Compass/APM_Compass.cpp @@ -38,8 +38,20 @@ extern "C" { #define CompassAddress 0x1E +// constant rotation matrices +const Matrix3f rotation[8] = { + Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ), // COMPONENTS_UP_PINS_BACK = no rotation + Matrix3f( 0, 1, 0, -1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_LEFT = rotation_yaw_270 + Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD = rotation_yaw_180 + Matrix3f( 0, -1, 0, 1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_RIGHT = rotation_yaw_90 + Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK = rotation_roll_180 + Matrix3f( 0, -1, 0, -1, 0, 0, 0, 0, -1 ),// COMPONENTS_DOWN_PINS_LEFT = rotation_roll_180_yaw_270 + Matrix3f( -1, 0, 0, 0, 1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD = rotation_pitch_180 + Matrix3f( 0, 1, 0, 1, 0, 0, 0, 0, -1 ) // COMPONENTS_DOWN_PINS_RIGHT = rotation_roll_180_yaw_90 +}; + // Constructors //////////////////////////////////////////////////////////////// -APM_Compass_Class::APM_Compass_Class() +APM_Compass_Class::APM_Compass_Class() : orientation(0) { } @@ -73,12 +85,12 @@ void APM_Compass_Class::Read() Wire.endTransmission(); //end transmission if (i==6) // All bytes received? - { + { // MSB byte first, then LSB, X,Y,Z Mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis Mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis Mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis - } + } } void APM_Compass_Class::Calculate(float roll, float pitch) @@ -89,15 +101,22 @@ void APM_Compass_Class::Calculate(float roll, float pitch) float sin_roll; float cos_pitch; float sin_pitch; + Vector3f rotMagVec; cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? sin_roll = sin(roll); cos_pitch = cos(pitch); sin_pitch = sin(pitch); + + // rotate the magnetometer values depending upon orientation + rotMagVec = rotation[orientation]*Vector3f(Mag_X,Mag_Y,Mag_Z); + // Tilt compensated Magnetic field X component: - Head_X = Mag_X*cos_pitch+Mag_Y*sin_roll*sin_pitch+Mag_Z*cos_roll*sin_pitch; + //Head_X = Mag_X*cos_pitch+Mag_Y*sin_roll*sin_pitch+Mag_Z*cos_roll*sin_pitch; + Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; // Tilt compensated Magnetic field Y component: - Head_Y = Mag_Y*cos_roll-Mag_Z*sin_roll; + //Head_Y = Mag_Y*cos_roll-Mag_Z*sin_roll; + Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; // Magnetic Heading Heading = atan2(-Head_Y,Head_X); // Optimization for external DCM use. Calculate normalized components @@ -106,5 +125,41 @@ void APM_Compass_Class::Calculate(float roll, float pitch) } +void APM_Compass_Class::SetOrientation(int newOrientation) +{ + orientation = newOrientation; + /* select( orientation ) + { + case APM_COMPASS_COMPONENTS_UP_PINS_BACK: + orientationMatrix = rotation_none; + break; + case APM_COMPASS_COMPONENTS_UP_PINS_LEFT: + orientationMatrix = rotation_yaw_270; + break; + case APM_COMPASS_COMPONENTS_UP_PINS_FORWARD: + orientationMatrix = rotation_yaw_180; + break; + case APM_COMPASS_COMPONENTS_UP_PINS_RIGHT: + orientationMatrix = rotation_yaw_90; + break; + case APM_COMPASS_COMPONENTS_DOWN_PINS_BACK: + orientationMatrix = rotation_roll_180; + break; + case APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT: + orientationMatrix = rotation_roll_180_yaw_270; + break; + case APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD: + orientationMatrix = rotation_pitch_180; + break; + case APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT: + orientationMatrix = rotation_roll_180_yaw_90; + break; + default: + orientationMatrix = rotation_none; + break; + }*/ +} + + // make one instance for the user to use APM_Compass_Class APM_Compass; \ No newline at end of file diff --git a/libraries/APM_Compass/APM_Compass.h b/libraries/APM_Compass/APM_Compass.h index fde6234963..8f7a957e14 100644 --- a/libraries/APM_Compass/APM_Compass.h +++ b/libraries/APM_Compass/APM_Compass.h @@ -1,6 +1,17 @@ #ifndef APM_Compass_h #define APM_Compass_h +#include + +#define APM_COMPASS_COMPONENTS_UP_PINS_BACK 0 +#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 1 +#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 2 +#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 3 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 4 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 5 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 6 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 7 + class APM_Compass_Class { private: @@ -11,11 +22,13 @@ class APM_Compass_Class float Heading; float Heading_X; float Heading_Y; + int orientation; APM_Compass_Class(); // Constructor void Init(); void Read(); void Calculate(float roll, float pitch); + void SetOrientation(int newOrientation); }; extern APM_Compass_Class APM_Compass; diff --git a/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde b/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde index 83f05e82a8..8ff2c1de71 100644 --- a/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde +++ b/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde @@ -5,6 +5,7 @@ #include #include // Compass Library +#include // Math library #define ToDeg(x) (x*57.2957795131) // *180/pi @@ -38,4 +39,4 @@ void loop() Serial.print(APM_Compass.Mag_Z); Serial.println(" )"); } -} \ No newline at end of file +} diff --git a/libraries/APM_Compass/keywords.txt b/libraries/APM_Compass/keywords.txt index c53c5c878c..068eef86b9 100644 --- a/libraries/APM_Compass/keywords.txt +++ b/libraries/APM_Compass/keywords.txt @@ -1,15 +1,16 @@ Compass KEYWORD1 AP_Compass KEYWORD1 APM_Compass KEYWORD1 -init KEYWORD2 -update KEYWORD2 -calculate KEYWORD2 -heading KEYWORD2 -heading_X KEYWORD2 -heading_Y KEYWORD2 -mag_X KEYWORD2 -mag_Y KEYWORD2 -mag_Z KEYWORD2 +Init KEYWORD2 +Read KEYWORD2 +Calculate KEYWORD2 +SetOrientation KEYWORD2 +Heading KEYWORD2 +Heading_X KEYWORD2 +Heading_Y KEYWORD2 +Mag_X KEYWORD2 +Mag_Y KEYWORD2 +Mag_Z KEYWORD2