AP_Compass - added support for HMC5883L

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

libraries/AP_Compass/AP_Compass.h

@@ -4,4 +4,5 @@
 /// @brief	Catch-all header that defines all supported compass classes.
 
 #include "AP_Compass_HMC5843.h"
+#include "AP_Compass_HMC5883L.h"
 #include "AP_Compass_HIL.h"

libraries/AP_Compass/AP_Compass_HMC5883L.cpp

@@ -0,0 +1,173 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
+/*
+	AP_Compass_HMC5883L.cpp - Arduino Library for HMC5883L I2C magnetometer
+	Code by Jordi Muñoz and Jose Julio. DIYDrones.com
+
+	This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+	Sensor is conected to I2C port
+	Sensor is initialized in Continuos mode (10Hz)
+
+*/
+
+// AVR LibC Includes
+#include <math.h>
+#include "WConstants.h"
+
+#include <Wire.h>
+#include "AP_Compass_HMC5883L.h"
+
+#define COMPASS_ADDRESS      0x1E
+#define ConfigRegA           0x00
+#define ConfigRegB           0x01
+#define ModeRegister         0x02
+#define DataOutputXMSB       0x03
+#define DataOutputXLSB       0x04
+#define DataOutputZMSB       0x05
+#define DataOutputZLSB       0x06
+#define DataOutputYMSB       0x07
+#define DataOutputYLSB       0x08
+#define StatusRegister       0x09
+#define IDRegisterA          0x0A
+#define IDRegisterB          0x0B
+#define IDRegisterC          0x0C
+
+// default gain value
+#define magGain              0x20
+
+// ModeRegister valid modes
+#define ContinuousConversion 0x00
+#define SingleConversion     0x01
+
+// ConfigRegA valid sample averaging
+#define SampleAveraging_1    0x00
+#define SampleAveraging_2    0x01
+#define SampleAveraging_4    0x02
+#define SampleAveraging_8    0x03
+
+// ConfigRegA valid data output rates
+#define DataOutputRate_0_75HZ 0x00
+#define DataOutputRate_1_5HZ  0x01
+#define DataOutputRate_3HZ    0x02
+#define DataOutputRate_7_5HZ  0x03
+#define DataOutputRate_15HZ   0x04
+#define DataOutputRate_30HZ   0x05
+#define DataOutputRate_75HZ   0x06
+
+// ConfigRegA valid measurement configuration bits
+#define NormalOperation      0x10
+#define PositiveBiasConfig   0x11
+#define NegativeBiasConfig   0x12
+
+
+// Public Methods //////////////////////////////////////////////////////////////
+bool AP_Compass_HMC5883L::init()
+{
+  int numAttempts = 0;
+  int success = 0;
+
+  delay(10);
+
+  // calibration initialisation
+  calibration[0] = 1.0;
+  calibration[1] = 1.0;
+  calibration[2] = 1.0;
+
+  while( success == 0 && numAttempts < 5 )
+  {
+      // record number of attempts at initialisation
+	  numAttempts++;
+
+	  // force positiveBias (compass should return 715 for all channels)
+	  Wire.beginTransmission(COMPASS_ADDRESS);
+	  Wire.send(ConfigRegA);
+	  Wire.send(SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | PositiveBiasConfig);
+	  if (0 != Wire.endTransmission())
+		 continue;			// compass not responding on the bus
+	  delay(50);
+
+	  // set gains
+	  Wire.beginTransmission(COMPASS_ADDRESS);
+	  Wire.send(ConfigRegB);
+	  Wire.send(magGain);
+	  Wire.endTransmission();
+	  delay(10);
+
+	  // put compass into single-measurement mode
+	  Wire.beginTransmission(COMPASS_ADDRESS);
+	  Wire.send(ModeRegister);
+	  Wire.send(SingleConversion);
+	  Wire.endTransmission();
+	  delay(10);
+
+	  // read values from the compass
+	  read();
+	  delay(10);
+
+	  // calibrate
+	  if( abs(mag_x) > 500 && abs(mag_x) < 1000 && abs(mag_y) > 500 && abs(mag_y) < 1000 && abs(mag_z) > 500 && abs(mag_z) < 1000)
+	  {
+		  calibration[0] = fabs(715.0 / mag_x);
+		  calibration[1] = fabs(715.0 / mag_y);
+		  calibration[2] = fabs(715.0 / mag_z);
+
+		  // mark success
+		  success = 1;
+	  }
+
+	  // leave test mode
+	  Wire.beginTransmission(COMPASS_ADDRESS);
+	  Wire.send(ConfigRegA);
+	  Wire.send(SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation);
+	  Wire.endTransmission();
+	  delay(50);
+
+	  Wire.beginTransmission(COMPASS_ADDRESS);
+	  Wire.send(ModeRegister);
+	  Wire.send(ContinuousConversion);        // Set continuous mode (default to 10Hz)
+	  Wire.endTransmission();                 // End transmission
+	  delay(50);
+  }
+  return(success);
+}
+
+// Read Sensor data
+void AP_Compass_HMC5883L::read()
+{
+  int i = 0;
+  byte buff[6];
+  Vector3f rot_mag;
+
+  Wire.beginTransmission(COMPASS_ADDRESS);
+  Wire.send(0x03);        //sends address to read from
+  Wire.endTransmission(); //end transmission
+
+  //Wire.beginTransmission(COMPASS_ADDRESS);
+  Wire.requestFrom(COMPASS_ADDRESS, 6);    // request 6 bytes from device
+  while(Wire.available())
+  {
+    buff[i] = Wire.receive();  // receive one byte
+    i++;
+  }
+  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]) * calibration[0];    // X axis
+    mag_y = ((((int)buff[4]) << 8) | buff[5]) * calibration[1];    // Y axis
+    mag_z = -((((int)buff[2]) << 8) | buff[3]) * 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...
+	rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
+	rot_mag = rot_mag + _offset.get();
+	mag_x = rot_mag.x;
+	mag_y = rot_mag.y;
+	mag_z = rot_mag.z;
+  }
+}
+

libraries/AP_Compass/AP_Compass_HMC5883L.h

@@ -0,0 +1,36 @@
+#ifndef AP_Compass_HMC5883L_H
+#define AP_Compass_HMC5883L_H
+
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Math/AP_Math.h"
+
+#include "Compass.h"
+
+// orientations for DIYDrones magnetometer
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_90
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_135
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_180
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_225
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_BACK ROTATION_YAW_270
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_315
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_LEFT ROTATION_NONE
+#define AP_COMPASS_HMC5883L_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_45
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_270
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_315
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_45
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_90
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_135
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_LEFT ROTATION_PITCH_180
+#define AP_COMPASS_HMC5883L_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_225
+
+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) {}
+	virtual bool init();
+	virtual void read();
+};
+#endif

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -15,6 +15,7 @@ FastSerialPort0(Serial);
 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 
 AP_Compass_HMC5843 compass;
+//AP_Compass_HMC5883L compass;
 
 
 unsigned long timer;