From e7d4273d52e15d8f5d998ed1625325135685a39d Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Tue, 28 Sep 2010 13:21:38 +0000 Subject: [PATCH] ported John's compass calibration code to trunk. also added 45 deg angles to orientation matrices to allow more flexibility in mounting the compass to the APM frame. git-svn-id: https://arducopter.googlecode.com/svn/trunk@565 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APM_Compass/APM_Compass.cpp | 112 ++++++++++++++++---------- libraries/APM_Compass/APM_Compass.h | 23 ++++-- 2 files changed, 85 insertions(+), 50 deletions(-) diff --git a/libraries/APM_Compass/APM_Compass.cpp b/libraries/APM_Compass/APM_Compass.cpp index baac867a86..7ace9fb396 100644 --- a/libraries/APM_Compass/APM_Compass.cpp +++ b/libraries/APM_Compass/APM_Compass.cpp @@ -37,18 +37,34 @@ extern "C" { #include "APM_Compass.h" #include "../AP_Math/AP_Math.h" -#define CompassAddress 0x1E +#define CompassAddress 0x1E +#define ConfigRegA 0x00 +#define ConfigRegB 0x01 +#define MagGain 0x20 +#define PositiveBiasConfig 0x11 +#define NormalOperation 0x10 +#define ModeRegister 0x02 +#define ContinuousConversion 0x00 +#define SingleConversion 0x01 // constant rotation matrices -const Matrix3f rotation[8] = { +const Matrix3f rotation[16] = { Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ), // COMPONENTS_UP_PINS_BACK = no rotation + Matrix3f( 0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1 ), //COMPONENTS_UP_PINS_BACK_LEFT = rotation_yaw_315 Matrix3f( 0, 1, 0, -1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_LEFT = rotation_yaw_270 + Matrix3f( -0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_LEFT = rotation_yaw_225 Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD = rotation_yaw_180 + Matrix3f( -0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_RIGHT = rotation_yaw_135 Matrix3f( 0, -1, 0, 1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_RIGHT = rotation_yaw_90 + Matrix3f( 0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_BACK_RIGHT = rotation_yaw_45 Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK = rotation_roll_180 + Matrix3f( 0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK_LEFT = rotation_roll_180_yaw_315 Matrix3f( 0, -1, 0, -1, 0, 0, 0, 0, -1 ),// COMPONENTS_DOWN_PINS_LEFT = rotation_roll_180_yaw_270 + Matrix3f( -0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD_LEFT = rotation_roll_180_yaw_225 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 + Matrix3f( -0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD_RIGHT = rotation_roll_180_yaw_135 + Matrix3f( 0, 1, 0, 1, 0, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_RIGHT = rotation_roll_180_yaw_90 + Matrix3f( 0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1 ) // COMPONENTS_DOWN_PINS_BACK_RIGHT = rotation_roll_180_yaw_45 }; // Constructors //////////////////////////////////////////////////////////////// @@ -59,11 +75,50 @@ APM_Compass_Class::APM_Compass_Class() : orientation(0) // Public Methods ////////////////////////////////////////////////////////////// void APM_Compass_Class::Init(void) { - Wire.begin(); + Wire.begin(); + + delay(10); + + calibration[0] = 1.0; + calibration[1] = 1.0; + calibration[2] = 1.0; + Wire.beginTransmission(CompassAddress); - Wire.send(0x02); - Wire.send(0x00); // Set continouos mode (default to 10Hz) - Wire.endTransmission(); //end transmission + Wire.send(ConfigRegA); + Wire.send(PositiveBiasConfig); + Wire.endTransmission(); + delay(50); + + Wire.beginTransmission(CompassAddress); + Wire.send(ConfigRegA); + Wire.send(MagGain); + Wire.endTransmission(); + delay(10); + + Wire.beginTransmission(CompassAddress); + Wire.send(ModeRegister); + Wire.send(SingleConversion); + Wire.endTransmission(); + delay(10); + + Read(); + delay(10); + + calibration[0] = abs(715.0 / Mag_X); + calibration[1] = abs(715.0 / Mag_Y); + calibration[2] = abs(715.0 / Mag_Z); + + Wire.beginTransmission(CompassAddress); + Wire.send(ConfigRegA); + Wire.send(NormalOperation); + Wire.endTransmission(); + delay(50); + + Wire.beginTransmission(CompassAddress); + Wire.send(ModeRegister); + Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) + Wire.endTransmission(); // End transmission + } // Read Sensor data @@ -88,9 +143,9 @@ void APM_Compass_Class::Read() 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 + Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis + Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis + Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis } } @@ -110,13 +165,14 @@ void APM_Compass_Class::Calculate(float roll, float pitch) sin_pitch = sin(pitch); // rotate the magnetometer values depending upon orientation - rotMagVec = rotation[orientation]*Vector3f(Mag_X,Mag_Y,Mag_Z); + if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_BACK) + rotMagVec = Vector3f(Mag_X,Mag_Y,Mag_Z); + else + 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 = 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 = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; // Magnetic Heading Heading = atan2(-Head_Y,Head_X); @@ -129,36 +185,6 @@ 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; - }*/ } diff --git a/libraries/APM_Compass/APM_Compass.h b/libraries/APM_Compass/APM_Compass.h index e6cf5d904b..3594324b3c 100644 --- a/libraries/APM_Compass/APM_Compass.h +++ b/libraries/APM_Compass/APM_Compass.h @@ -2,17 +2,26 @@ #define APM_Compass_h #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 +#define APM_COMPONENTS_UP_PINS_BACK_LEFT 1 +#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 2 +#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT 3 +#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 4 +#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT 5 +#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 6 +#define APM_COMPONENTS_UP_PINS_BACK_RIGHT 7 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 8 +#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT 9 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 10 +#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT 11 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 12 +#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT 13 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 14 +#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT 15 class APM_Compass_Class { private: + float calibration[3]; public: int Mag_X; int Mag_Y;