Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
rmackay9@yahoo.com 2011-06-28 16:30:42 +00:00
parent 203bc4643d
commit da7a13128a
7 changed files with 79 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -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);

65
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -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);
}

4
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -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

2
libraries/AP_Compass/AP_Compass_HMC5883L.h
View File

@ -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();
};

3
libraries/AP_Compass/Compass.cpp
View File

@ -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.

7
libraries/AP_Compass/Compass.h
View File

@ -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

4
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
View File

@ -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