added magnetometer orientation functionality to APM_Compass

git-svn-id: https://arducopter.googlecode.com/svn/trunk@560 f9c3cf11-9bcb-44bc-f272-b75c42450872

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;

libraries/APM_Compass/APM_Compass.h

@@ -1,6 +1,17 @@
 #ifndef APM_Compass_h
 #define APM_Compass_h
 
+#include <AP_Math.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
+
 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;

libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde

@@ -5,6 +5,7 @@
 
 #include <Wire.h>
 #include <APM_Compass.h> // Compass Library
+#include <AP_Math.h> // Math library
 
 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 
@@ -38,4 +39,4 @@ void loop()
     Serial.print(APM_Compass.Mag_Z);
     Serial.println(" )");
     }
-}
+}

libraries/APM_Compass/keywords.txt

@@ -1,15 +1,16 @@
 Compass	KEYWORD1
 AP_Compass	KEYWORD1
 APM_Compass	KEYWORD1
-init	KEYWORD2
+Init	KEYWORD2
-update	KEYWORD2
+Read	KEYWORD2
-calculate	KEYWORD2
+Calculate	KEYWORD2
-heading	KEYWORD2
+SetOrientation	KEYWORD2
-heading_X	KEYWORD2
+Heading	KEYWORD2
-heading_Y	KEYWORD2
+Heading_X	KEYWORD2
-mag_X	KEYWORD2
+Heading_Y	KEYWORD2
-mag_Y	KEYWORD2
+Mag_X	KEYWORD2
-mag_Z	KEYWORD2
+Mag_Y	KEYWORD2
+Mag_Z	KEYWORD2