diff --git a/libraries/APM_Compass/APM_Compass.cpp b/libraries/APM_Compass/APM_Compass.cpp
index 71d72f5936..9db760a8e4 100644
--- a/libraries/APM_Compass/APM_Compass.cpp
+++ b/libraries/APM_Compass/APM_Compass.cpp
@@ -18,12 +18,17 @@
 		Mag_X : Raw X axis magnetometer data
 		Mag_Y : Raw Y axis magnetometer data
 		Mag_Z : Raw Z axis magnetometer data		
+		lastUpdate : the time of the last successful reading		
 	
 	Methods:
 		Init() : Initialization of I2C and sensor
-		Read() : Read Sensor data		
+		Read() : Read Sensor data	
+		Calculate(float roll, float pitch) : Calculate tilt adjusted heading
+		SetOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass
+		SetOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances
+		SetDeclination(float radians) : Set heading adjustment between true north and magnetic north
 
-	To do : Calibration of the sensor, code optimization
+	To do : code optimization
 	Mount position : UPDATED
                 Big capacitor pointing backward, connector forward
 		
@@ -36,7 +41,6 @@ extern "C" {
 
 #include <Wire.h>
 #include "APM_Compass.h"
-#include "../AP_Math/AP_Math.h"
 
 #define CompassAddress       0x1E
 #define ConfigRegA           0x00
@@ -49,26 +53,6 @@ extern "C" {
 #define ContinuousConversion 0x00
 #define SingleConversion     0x01
 
-// constant rotation matrices
-const Matrix3f rotation[16] = { 
-	Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ),   //  COMPONENTS_UP_PINS_FORWARD = no rotation
-	Matrix3f( 0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_RIGHT = rotation_yaw_45
-	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_135
-	Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), //  COMPONENTS_UP_PINS_BACK = rotation_yaw_180
-	Matrix3f( -0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_BACK_LEFT = rotation_yaw_225
-	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_315
-	Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), //  COMPONENTS_DOWN_PINS_FORWARD = rotation_roll_180
-	Matrix3f( 0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD_RIGHT = rotation_roll_180_yaw_45
-	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_135
-	Matrix3f( -1, 0, 0, 0, 1, 0, 0, 0, -1 ), //  COMPONENTS_DOWN_PINS_BACK = rotation_pitch_180
-	Matrix3f( -0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK_LEFT = rotation_roll_180_yaw_225
-	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_315
-};
-
 // Constructors ////////////////////////////////////////////////////////////////
 APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0)
 {
@@ -76,6 +60,9 @@ APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0)
   offset[0] = 0;
   offset[1] = 0;
   offset[2] = 0;
+  
+  // initialise orientation matrix
+  orientationMatrix = ROTATION_NONE;
 }
 
 // Public Methods //////////////////////////////////////////////////////////////
@@ -196,10 +183,10 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
   sin_pitch = sin(pitch);
   
   // rotate the magnetometer values depending upon orientation
-  if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_FORWARD)
+  if( orientation == 0 )
       rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);  
   else
-      rotMagVec = rotation[orientation]*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);  
+      rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); 
   
   // Tilt compensated Magnetic field X component:
   Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
@@ -223,10 +210,13 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
   Heading_Y = sin(Heading);
 }
 
-
-void APM_Compass_Class::SetOrientation(int newOrientation)
+void APM_Compass_Class::SetOrientation(const Matrix3f &rotationMatrix)
 {
-    orientation = newOrientation;
+    orientationMatrix = rotationMatrix;
+	if( orientationMatrix == ROTATION_NONE )
+	    orientation = 0;
+	else
+	    orientation = 1;
 }
 
 void APM_Compass_Class::SetOffsets(int x, int y, int z)
diff --git a/libraries/APM_Compass/APM_Compass.h b/libraries/APM_Compass/APM_Compass.h
index b6b11fde79..525abe3a51 100644
--- a/libraries/APM_Compass/APM_Compass.h
+++ b/libraries/APM_Compass/APM_Compass.h
@@ -1,27 +1,48 @@
 #ifndef APM_Compass_h
 #define APM_Compass_h
 
-#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 0
-#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT 1
-#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 2
-#define APM_COMPONENTS_UP_PINS_BACK_RIGHT 3
-#define APM_COMPASS_COMPONENTS_UP_PINS_BACK 4
-#define APM_COMPONENTS_UP_PINS_BACK_LEFT 5
-#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 6
-#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT 7
-#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 8
-#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT 9
-#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 10
-#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT 11
-#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 12
-#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT 13
-#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 14
-#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT 15
+#include "../AP_Math/AP_Math.h"
+
+// Rotation matrices
+#define ROTATION_NONE				Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
+#define ROTATION_YAW_45				Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
+#define ROTATION_YAW_90				Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
+#define ROTATION_YAW_135			Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
+#define ROTATION_YAW_180			Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
+#define ROTATION_YAW_225			Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
+#define ROTATION_YAW_270			Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
+#define ROTATION_YAW_315			Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
+#define ROTATION_ROLL_180 			Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
+#define ROTATION_ROLL_180_YAW_45	Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
+#define ROTATION_ROLL_180_YAW_90	Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
+#define ROTATION_ROLL_180_YAW_135	Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
+#define ROTATION_PITCH_180			Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
+#define ROTATION_ROLL_180_YAW_225	Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
+#define ROTATION_ROLL_180_YAW_270	Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
+#define ROTATION_ROLL_180_YAW_315	Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
+
+#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
+#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45
+#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90
+#define APM_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135
+#define APM_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180
+#define APM_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225
+#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270
+#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
+#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180
+#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45
+#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90
+#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135
+#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180
+#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225
+#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270
+#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
 
 class APM_Compass_Class
 {
   private:
 	int orientation;
+	Matrix3f orientationMatrix;
 	float calibration[3];
 	int offset[3];
 	float declination;
@@ -38,7 +59,7 @@ class APM_Compass_Class
 	bool Init();
 	void Read();
 	void Calculate(float roll, float pitch);
-	void SetOrientation(int newOrientation);
+	void SetOrientation(const Matrix3f &rotationMatrix);
 	void SetOffsets(int x, int y, int z);
 	void SetDeclination(float radians);
 };