AP_Compass - added auto detect of 5843 vs 5883L to AP_Compass_HMC5843 class

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

libraries/AP_Compass/AP_Compass_HIL.h

@@ -6,7 +6,7 @@
 class AP_Compass_HIL : public Compass
 {
   public:
-	AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
+	AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; }
 
 	void 			read();
     void 	    	setHIL(float Mag_X, float Mag_Y, float Mag_Z);

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -32,13 +32,25 @@
 #define SingleConversion     0x01
 
 // Public Methods //////////////////////////////////////////////////////////////
-bool AP_Compass_HMC5843::init()
+bool 
+AP_Compass_HMC5843::init()
 {
   int numAttempts = 0;
   int success = 0;
+  byte temp_value, new_value;  // used to test compass type
 
   delay(10);
 
+  // determine if we are using 5843 or 5883L
+  temp_value = read_register(ConfigRegA);  // read config register A
+  new_value = temp_value | 0x60;           // turn on sample averaging turned on (only avaiable in 5883L)
+  write_register(ConfigRegA, new_value);   // write config register A
+  temp_value = read_register(ConfigRegA);  // re-read config register A
+  if( temp_value == new_value )            // if we've successfully updated it then it's a 5883L
+      product_id = AP_COMPASS_TYPE_HMC5883L;
+  else
+      product_id = AP_COMPASS_TYPE_HMC5843;
+  
   // calibration initialisation
   calibration[0] = 1.0;
   calibration[1] = 1.0;
@@ -102,7 +114,8 @@ bool AP_Compass_HMC5843::init()
 }
 
 // Read Sensor data
-void AP_Compass_HMC5843::read()
+void 
+AP_Compass_HMC5843::read()
 {
   int i = 0;
   byte buff[6];
@@ -125,8 +138,13 @@ void AP_Compass_HMC5843::read()
   {
     // MSB byte first, then LSB, X,Y,Z
     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
+	if( product_id == AP_COMPASS_TYPE_HMC5883L ) {
+		mag_y = ((((int)buff[4]) << 8) | buff[5]) * calibration[1];    // Y axis
+		mag_z = -((((int)buff[2]) << 8) | buff[3]) * calibration[2];    // Z axis
+	}else{
+		mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1];    // Y axis
+		mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2];    // Z axis
+	}	
     last_update = millis();  // record time of update
 	// rotate and offset the magnetometer values
     // XXX this could well be done in common code...
@@ -138,3 +156,42 @@ void AP_Compass_HMC5843::read()
   }
 }
 
+// set orientation
+void
+AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix)
+{
+    if( product_id == AP_COMPASS_TYPE_HMC5883L ) {
+        _orientation_matrix.set_and_save(rotation_matrix * Matrix3f(ROTATION_YAW_90));
+	}else{
+	    _orientation_matrix.set_and_save(rotation_matrix);
+	}
+}
+
+// read_register - read a register value
+byte 
+AP_Compass_HMC5843::read_register(int address)
+{
+  byte result;
+  byte buff[1];
+  Wire.beginTransmission(COMPASS_ADDRESS);
+  Wire.send(address);     //sends address to read from
+  Wire.endTransmission(); //end transmission
+
+  Wire.requestFrom(COMPASS_ADDRESS, 1);    // request 1 byte from device
+  if( Wire.available() )
+      result = Wire.receive();  // receive one byte
+  Wire.endTransmission(); //end transmission
+  
+  return result;
+}
+
+// write_register - update a register value
+void 
+AP_Compass_HMC5843::write_register(int address, byte value)
+{
+  Wire.beginTransmission(COMPASS_ADDRESS);
+  Wire.send(address);
+  Wire.send(value);
+  Wire.endTransmission();
+  delay(10);
+}

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -50,5 +50,9 @@ class AP_Compass_HMC5843 : public Compass
 	AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
 	virtual bool init();
 	virtual void read();
+	virtual void set_orientation(const Matrix3f &rotation_matrix);
+	virtual byte read_register(int address);
+	virtual void write_register(int address, byte value);
+
 };
 #endif

libraries/AP_Compass/AP_Compass_HMC5883L.h

@@ -29,7 +29,7 @@ class AP_Compass_HMC5883L : public Compass
   private:
 	float calibration[3];
   public:
-	AP_Compass_HMC5883L(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
+	AP_Compass_HMC5883L(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HMC5883L; }
 	virtual bool init();
 	virtual void read();
 };

libraries/AP_Compass/Compass.cpp

@@ -9,7 +9,8 @@ Compass::Compass(AP_Var::Key key) :
     _orientation_matrix	(&_group, 0),
     _offset				(&_group, 1),
     _declination		(&_group, 2, 0.0, PSTR("DEC")),
-    _null_init_done(false)
+    _null_init_done(false),
+	product_id(AP_COMPASS_TYPE_UNKNOWN)
 {
     // Default the orientation matrix to none - will be overridden at group load time
     // if an orientation has previously been saved.

libraries/AP_Compass/Compass.h

@@ -5,6 +5,12 @@
 #include "../AP_Common/AP_Common.h"
 #include "../AP_Math/AP_Math.h"
 
+// compass product id
+#define AP_COMPASS_TYPE_UNKNOWN  0x00
+#define AP_COMPASS_TYPE_HIL      0x01
+#define AP_COMPASS_TYPE_HMC5843  0x02
+#define AP_COMPASS_TYPE_HMC5883L 0x03
+
 // standard 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)
@@ -26,6 +32,7 @@
 class Compass
 {
 public:
+	int				product_id;     /// product id
 	int             mag_x;          ///< magnetic field strength along the X axis
 	int             mag_y;          ///< magnetic field strength along the Y axis
 	int             mag_z;          ///< magnetic field strength along the Z axis
@@ -46,7 +53,7 @@ public:
 	///             found or is not functioning.
 	///
 	virtual bool init();
-
+	
 	/// Read the compass and update the mag_ variables.
 	///
 	virtual void read() = 0;

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -22,8 +22,8 @@ unsigned long timer;
 
 void setup()
 {
-  Serial.begin(38400);
-  Serial.println("Compass library test (HMC5843)");
+  Serial.begin(115200);
+  Serial.println("Compass library test (HMC5843 and HMC5883L)");
   Wire.begin();
   compass.init();	 // Initialization