From da7a13128a36fa8834a7b8cab8af910fe8635dcf Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Tue, 28 Jun 2011 16:30:42 +0000 Subject: [PATCH] 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 | 2 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 65 +++++++++++++++++-- libraries/AP_Compass/AP_Compass_HMC5843.h | 4 ++ libraries/AP_Compass/AP_Compass_HMC5883L.h | 2 +- libraries/AP_Compass/Compass.cpp | 3 +- libraries/AP_Compass/Compass.h | 9 ++- .../AP_Compass_test/AP_Compass_test.pde | 4 +- 7 files changed, 79 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index d0aa29a59f..3949fc9750 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/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); diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index dc04a06401..34d7dde9fc 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/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); +} \ No newline at end of file diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 08b7fadab7..79b56c72b6 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/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 diff --git a/libraries/AP_Compass/AP_Compass_HMC5883L.h b/libraries/AP_Compass/AP_Compass_HMC5883L.h index fee59c7f2b..dff9984b3a 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5883L.h +++ b/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(); }; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 54906ecd58..49ea187547 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/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. diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 9a5557211b..b16614fc01 100644 --- a/libraries/AP_Compass/Compass.h +++ b/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; 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 e1852ad629..27d27322f6 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 @@ -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