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

@@ -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;
-	}*/
 }
 
 

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;