philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Compass/Magnetometer drivers

Browse Source
This commit is contained in:
philsson 2018-08-25 11:36:29 +02:00
parent bd22dd6ab3
commit 3c66623707
2 changed files with 409 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
src/drivers/HMC5883L.cpp Normal file
View File

@ -0,0 +1,150 @@
/*
* @file HMC5883L.cpp
* @author Oskar Lopez de Gamboa
*
* @section LICENSE
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
* and associated documentation files (the "Software"), to deal in the Software without restriction,
* including without limitation the rights to use, copy, modify, merge, publish, distribute,
* sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all copies or
* substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* @section DESCRIPTION
*
* HMC5883L 3-Axis Digital Compas IC
* The library done by Tyler Weaver with:
*
* *Corrected the XZY order instead of the previous XYZ to match the datasheet.
* *Added Declination compensation by a define
*
*/
#include "HMC5883L.h"
HMC5883L::HMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
{
// Placement new to avoid additional heap memory allocation.
new(i2cRaw) I2C(sda, scl);
init();
}
HMC5883L::~HMC5883L()
{
// If the I2C object is initialized in the buffer in this object, call destructor of it.
if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
}
void HMC5883L::init()
{
// init - configure your setup here
setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
setConfigurationB(0x20); // default gain
setMode(CONTINUOUS_MODE); // continuous sample mode
wait(0.006);//wait 6ms as told in the datasheet
}
void HMC5883L::setConfigurationA(char config)
{
char cmd[2];
cmd[0] = CONFIG_A_REG; // register a address
cmd[1] = config;
i2c_.write(I2C_ADDRESS, cmd, 2);
}
void HMC5883L::setConfigurationB(char config)
{
char cmd[2];
cmd[0] = CONFIG_B_REG; // register b address
cmd[1] = config;
i2c_.write(I2C_ADDRESS, cmd, 2);
}
char HMC5883L::getConfigurationA()
{
char cmd[2];
cmd[0] = CONFIG_A_REG; // register a address
i2c_.write(I2C_ADDRESS, cmd, 1, true);
i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
return cmd[1];
}
char HMC5883L::getConfigurationB()
{
char cmd[2];
cmd[0] = CONFIG_A_REG; // register b address
i2c_.write(I2C_ADDRESS, cmd, 1, true);
i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
return cmd[1];
}
void HMC5883L::setMode(char mode = SINGLE_MODE)
{
char cmd[2];
cmd[0] = MODE_REG; // mode register address
cmd[1] = mode;
i2c_.write(I2C_ADDRESS,cmd,2);
}
char HMC5883L::getMode()
{
char cmd[2];
cmd[0] = MODE_REG; // mode register
i2c_.write(I2C_ADDRESS, cmd, 1, true);
i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
return cmd[1];
}
char HMC5883L::getStatus()
{
char cmd[2];
cmd[0] = STATUS_REG; // status register
i2c_.write(I2C_ADDRESS, cmd, 1, true);
i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
return cmd[1];
}
void HMC5883L::getXYZ(int16_t output[3])
{
char cmd[2];
char data[6];
cmd[0] = 0x03; // starting point for reading
i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
i2c_.read(I2C_ADDRESS, data, 6, false);
for(int i = 0; i < 3; i++) // fill the output variables
output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
}
double HMC5883L::getHeadingXY()
{
int16_t raw_data[3];
getXYZ(raw_data);
//The HMC5883L gives X Z Y order
double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X)
heading += DECLINATION_ANGLE;
if(heading < 0.0) // fix sign
heading += PI2;
if(heading > PI2) // fix overflow
heading -= PI2;
return heading;
}

259
src/drivers/HMC5883L.h Normal file
View File

@ -0,0 +1,259 @@
/*
* @file HMC5883L.h
* @author Oskar Lopez de Gamboa
*
* @section LICENSE
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
* and associated documentation files (the "Software"), to deal in the Software without restriction,
* including without limitation the rights to use, copy, modify, merge, publish, distribute,
* sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all copies or
* substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* @section DESCRIPTION
*
* HMC5883L 3-Axis Digital Compas IC
* The library done by Tyler Weaver with:
*
* *Corrected the XZY order instead of the previous XYZ to match the datasheet.
* *Added Declination compensation by a define
*
*
*/
#ifndef HMC5883L_H
#define HMC5883L_H
#include "mbed.h"
/*
* Defines
*/
//-----------
// Registers
//-----------
#define CONFIG_A_REG 0x00
#define CONFIG_B_REG 0x01
#define MODE_REG 0x02
#define OUTPUT_REG 0x03
#define STATUS_REG 0x09
// configuration register a
#define AVG1_SAMPLES 0x00
#define AVG2_SAMPLES 0x20
#define AVG4_SAMPLES 0x80
#define AVG8_SAMPLES 0xC0
#define OUTPUT_RATE_0_75 0x00
#define OUTPUT_RATE_1_5 0x04
#define OUTPUT_RATE_3 0x08
#define OUTPUT_RATE_7_5 0x0C
#define OUTPUT_RATE_15 0x10
#define OUTPUT_RATE_30 0x14
#define OUTPUT_RATE_75 0x18
#define NORMAL_MEASUREMENT 0x00
#define POSITIVE_BIAS 0x01
#define NEGATIVE_BIAS 0x02
// mode register
#define CONTINUOUS_MODE 0x00
#define SINGLE_MODE 0x01
#define IDLE_MODE 0x02
// status register
#define STATUS_LOCK 0x02
#define STATUS_READY 0x01
// Utility
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
#define PI2 (2*M_PI)
#define RAD_TO_DEG (180.0/M_PI)
#define DEG_TO_RAD (M_PI/180.0)
// Once you have your heading, you must then add your 'Declination Angle',
// which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/
// Mine is: -1° 13' WEST which is -1.2167 Degrees, or (which we need)
// 0,021234839232597676519238237683278 radians, I will use 0.02123
// If you cannot find your Declination, put 0, your compass will be slightly off.
#define DECLINATION_ANGLE -0.02123
//#define DECLINATION_ANGLE 0
/**
* The HMC5883L 3-Axis Digital Compass IC
*/
00101 class HMC5883L
{
public:
/**
* The I2C address that can be passed directly to i2c object (it's already shifted 1 bit left).
*/
00109 static const int I2C_ADDRESS = 0x3D;
/**
* Constructor.
*
* Calls init function
*
* @param sda - mbed pin to use for the SDA I2C line.
* @param scl - mbed pin to use for the SCL I2C line.
*/
HMC5883L(PinName sda, PinName scl);
/**
* Constructor that accepts external i2c interface object.
*
* Calls init function
*
* @param i2c The I2C interface object to use.
*/
00128 HMC5883L(I2C &i2c) : i2c_(i2c) {
init();
}
~HMC5883L();
/**
* Initalize function called by all constructors.
*
* Place startup code in here.
*/
void init();
/**
* Function for setting configuration register A
*
* Defined constants should be ored together to create value.
* Defualt is 0x10 - 1 Sample per output, 15Hz Data output rate, normal measurement mode
*
* Refer to datasheet for instructions for setting Configuration Register A.
*
* @param config the value to place in Configuration Register A
*/
void setConfigurationA(char);
/**
* Function for retrieving the contents of configuration register A
*
* @returns Configuration Register A
*/
char getConfigurationA();
/**
* Function for setting configuration register B
*
* Configuration Register B is for setting the device gain.
* Default value is 0x20
*
* Refer to datasheet for instructions for setting Configuration Register B
*
* @param config the value to place in Configuration Register B
*/
void setConfigurationB(char);
/**
* Function for retrieving the contents of configuration register B
*
* @returns Configuration Register B
*/
char getConfigurationB();
/**
* Funciton for setting the mode register
*
* Constants: CONTINUOUS_MODE, SINGLE_MODE, IDLE_MODE
*
* When you send a the Single-Measurement Mode instruction to the mode register
* a single measurement is made, the RDY bit is set in the status register,
* and the mode is placed in idle mode.
*
* When in Continous-Measurement Mode the device continuously performs measurements
* and places the results in teh data register. After being placed in this mode
* it takes two periods at the rate set in the data output rate before the first
* sample is avaliable.
*
* Refer to datasheet for more detailed instructions for setting the mode register.
*
* @param mode the value for setting in the Mode Register
*/
void setMode(char);
/**
* Function for retrieving the contents of mode register
*
* @returns mode register
*/
char getMode();
/**
* Function for retriaval of the raw data
* Caution!! the HMC5883L gives you the data in XZY order
*
* @param output buffer that is at least 3 in length
*/
void getXYZ(int16_t raw[3]);
/**
* Function for retrieving the contents of status register
*
* Bit1: LOCK, Bit0: RDY
*
* @returns status register
*/
char getStatus();
/**
* Function for getting radian heading using 2-dimensional calculation.
*
* Compass must be held flat and away from an magnetic field generating
* devices such as cell phones and speakers.
*
*
*
* @returns heading in radians
*/
double getHeadingXY();
/**
* Function for getting degree heading using 2-dimensional calculation.
*
* Compass must be held flat and away from an magnetic field generating
* devices such as cell phones and speakers.
*
*
*
* @returns heading in degrees
*/
00245 double getHeadingXYDeg() {
return (getHeadingXY() * RAD_TO_DEG);
}
private:
I2C &i2c_;
/**
* The raw buffer for allocating I2C object in its own without heap memory.
*/
char i2cRaw[sizeof(I2C)];
};
#endif // HMC5883L