From ed7e0d166094067f937cc9b4808667200384526c Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Wed, 1 Jun 2011 13:16:18 +0000 Subject: [PATCH] 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 | 1 + libraries/AP_Compass/AP_Compass_HMC5883L.cpp | 173 ++++++++++++++++++ libraries/AP_Compass/AP_Compass_HMC5883L.h | 36 ++++ .../AP_Compass_test/AP_Compass_test.pde | 1 + 4 files changed, 211 insertions(+) create mode 100644 libraries/AP_Compass/AP_Compass_HMC5883L.cpp create mode 100644 libraries/AP_Compass/AP_Compass_HMC5883L.h diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 01405a3076..ef319a3897 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/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" diff --git a/libraries/AP_Compass/AP_Compass_HMC5883L.cpp b/libraries/AP_Compass/AP_Compass_HMC5883L.cpp new file mode 100644 index 0000000000..eb6c81e08c --- /dev/null +++ b/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 +#include "WConstants.h" + +#include +#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; + } +} + diff --git a/libraries/AP_Compass/AP_Compass_HMC5883L.h b/libraries/AP_Compass/AP_Compass_HMC5883L.h new file mode 100644 index 0000000000..fee59c7f2b --- /dev/null +++ b/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 diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index 9731934051..1533074110 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/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;