forked from logzhan/RobotHardware-UESTC
261 lines
7.5 KiB
C++
261 lines
7.5 KiB
C++
#include "HMC5883L.h"
|
|
#include "board.h"
|
|
|
|
/** Default constructor, uses default I2C address.
|
|
* @see HMC5883L_DEFAULT_ADDRESS
|
|
*/
|
|
HMC5883L::HMC5883L()
|
|
{
|
|
devAddr = HMC5883L_DEFAULT_ADDRESS;
|
|
}
|
|
|
|
/** Specific address constructor.
|
|
* @param address I2C address
|
|
* @see HMC5883L_DEFAULT_ADDRESS
|
|
* @see HMC5883L_ADDRESS
|
|
*/
|
|
HMC5883L::HMC5883L(unsigned char address)
|
|
{
|
|
devAddr = address;
|
|
}
|
|
|
|
/** Power on and prepare for general usage.
|
|
*/
|
|
void HMC5883L::initialize()
|
|
{
|
|
// write CONFIG_A register
|
|
Board::get()->i2c_write_byte(devAddr, HMC5883L_RA_CONFIG_A,
|
|
(HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
|
|
(HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
|
|
(HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
|
|
|
|
// write CONFIG_B register
|
|
setGain(HMC5883L_GAIN_1090);
|
|
|
|
// write MODE register
|
|
setMode(HMC5883L_MODE_SINGLE);
|
|
}
|
|
|
|
/** Verify the I2C connection.
|
|
* Make sure the device is connected and responds as expected.
|
|
* @return True if connection is valid, false otherwise
|
|
*/
|
|
bool HMC5883L::testConnection()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_ID_A, buffer, 3);
|
|
|
|
return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
|
|
}
|
|
|
|
// CONFIG_A register
|
|
/** Set number of samples averaged per measurement.
|
|
* @param averaging New samples averaged per measurement setting(0-3 for 1/2/4/8 respectively)
|
|
* @see HMC5883L_RA_CONFIG_A
|
|
* @see HMC5883L_CRA_AVERAGE_BIT
|
|
* @see HMC5883L_CRA_AVERAGE_LENGTH
|
|
*/
|
|
void HMC5883L::setSampleAveraging(unsigned char averaging)
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
|
|
}
|
|
|
|
/** Get data output rate value.
|
|
*
|
|
* Value | Typical Data Output Rate (Hz)
|
|
* ------+------------------------------
|
|
* 0 | 0.75
|
|
* 1 | 1.5
|
|
* 2 | 3
|
|
* 3 | 7.5
|
|
* 4 | 15 (Default)
|
|
* 5 | 30
|
|
* 6 | 75
|
|
* 7 | Not used
|
|
*
|
|
* @return Current rate of data output to registers
|
|
* @see HMC5883L_RATE_15
|
|
* @see HMC5883L_RA_CONFIG_A
|
|
* @see HMC5883L_CRA_RATE_BIT
|
|
* @see HMC5883L_CRA_RATE_LENGTH
|
|
*/
|
|
unsigned char HMC5883L::getDataRate()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_CONFIG_A, buffer, 1);
|
|
|
|
return (buffer[0] >> (HMC5883L_CRA_RATE_BIT+1-HMC5883L_CRA_RATE_LENGTH)) & (1<<HMC5883L_CRA_RATE_LENGTH-1);
|
|
}
|
|
/** Set data output rate value.
|
|
* @param rate Rate of data output to registers
|
|
* @see getDataRate()
|
|
* @see HMC5883L_RATE_15
|
|
* @see HMC5883L_RA_CONFIG_A
|
|
* @see HMC5883L_CRA_RATE_BIT
|
|
* @see HMC5883L_CRA_RATE_LENGTH
|
|
*/
|
|
void HMC5883L::setDataRate(unsigned char rate)
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
|
|
}
|
|
|
|
/** Get measurement bias value.
|
|
* @return Current bias value (0-2 for normal/positive/negative respectively)
|
|
* @see HMC5883L_BIAS_NORMAL
|
|
* @see HMC5883L_RA_CONFIG_A
|
|
* @see HMC5883L_CRA_BIAS_BIT
|
|
* @see HMC5883L_CRA_BIAS_LENGTH
|
|
*/
|
|
unsigned char HMC5883L::getMeasurementBias()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_CONFIG_A, buffer, 1);
|
|
|
|
return (buffer[0] >> (HMC5883L_CRA_BIAS_BIT+1-HMC5883L_CRA_BIAS_LENGTH)) & (1<<HMC5883L_CRA_BIAS_LENGTH-1);
|
|
}
|
|
|
|
/** Set measurement bias value.
|
|
* @param bias New bias value (0-2 for normal/positive/negative respectively)
|
|
* @see HMC5883L_BIAS_NORMAL
|
|
* @see HMC5883L_RA_CONFIG_A
|
|
* @see HMC5883L_CRA_BIAS_BIT
|
|
* @see HMC5883L_CRA_BIAS_LENGTH
|
|
*/
|
|
void HMC5883L::setMeasurementBias(unsigned char bias)
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
|
|
}
|
|
|
|
// CONFIG_B register
|
|
|
|
/** Get magnetic field gain value.
|
|
* The data output range for all settings is 0xF800-0x07FF (-2048 - 2047).
|
|
*
|
|
* Value | Field Range | Gain (LSB/Gauss)
|
|
* ------+-------------+-----------------
|
|
* 0 | +/- 0.88 Ga | 1370
|
|
* 1 | +/- 1.3 Ga | 1090 (Default)
|
|
* 2 | +/- 1.9 Ga | 820
|
|
* 3 | +/- 2.5 Ga | 660
|
|
* 4 | +/- 4.0 Ga | 440
|
|
* 5 | +/- 4.7 Ga | 390
|
|
* 6 | +/- 5.6 Ga | 330
|
|
* 7 | +/- 8.1 Ga | 230
|
|
*
|
|
* @return Current magnetic field gain value
|
|
* @see HMC5883L_GAIN_1090
|
|
* @see HMC5883L_RA_CONFIG_B
|
|
* @see HMC5883L_CRB_GAIN_BIT
|
|
* @see HMC5883L_CRB_GAIN_LENGTH
|
|
*/
|
|
unsigned char HMC5883L::getGain()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_CONFIG_B, buffer, 1);
|
|
|
|
return (buffer[0] >> (HMC5883L_CRB_GAIN_BIT+1-HMC5883L_CRB_GAIN_LENGTH)) & (1<<HMC5883L_CRB_GAIN_LENGTH-1);
|
|
}
|
|
|
|
/** Set magnetic field gain value.
|
|
* @param gain New magnetic field gain value
|
|
* @see getGain()
|
|
* @see HMC5883L_RA_CONFIG_B
|
|
* @see HMC5883L_CRB_GAIN_BIT
|
|
* @see HMC5883L_CRB_GAIN_LENGTH
|
|
*/
|
|
void HMC5883L::setGain(unsigned char gain)
|
|
{
|
|
Board::get()->i2c_write_byte(devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
|
|
}
|
|
|
|
// MODE register
|
|
|
|
/** Get measurement mode.
|
|
*
|
|
* @return Current measurement mode
|
|
* @see HMC5883L_MODE_CONTINUOUS
|
|
* @see HMC5883L_MODE_SINGLE
|
|
* @see HMC5883L_MODE_IDLE
|
|
* @see HMC5883L_RA_MODE
|
|
* @see HMC5883L_MODEREG_BIT
|
|
* @see HMC5883L_MODEREG_LENGTH
|
|
*/
|
|
unsigned char HMC5883L::getMode()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_MODE, buffer, 1);
|
|
|
|
return (buffer[0] >> (HMC5883L_MODEREG_BIT+1-HMC5883L_MODEREG_LENGTH)) & (1<<HMC5883L_MODEREG_LENGTH-1);
|
|
}
|
|
|
|
/** Set measurement mode.
|
|
* @param newMode New measurement mode
|
|
* @see getMode()
|
|
* @see HMC5883L_MODE_CONTINUOUS
|
|
* @see HMC5883L_MODE_SINGLE
|
|
* @see HMC5883L_MODE_IDLE
|
|
* @see HMC5883L_RA_MODE
|
|
* @see HMC5883L_MODEREG_BIT
|
|
* @see HMC5883L_MODEREG_LENGTH
|
|
*/
|
|
void HMC5883L::setMode(unsigned char newMode)
|
|
{
|
|
Board::get()->i2c_write_byte(devAddr, HMC5883L_RA_MODE, newMode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
|
|
mode = newMode; // track to tell if we have to clear bit 7 after a read
|
|
}
|
|
|
|
// DATA* registers
|
|
|
|
/** Get 3-axis heading measurements.
|
|
*
|
|
* @param x 16-bit signed integer container for X-axis heading
|
|
* @param y 16-bit signed integer container for Y-axis heading
|
|
* @param z 16-bit signed integer container for Z-axis heading
|
|
* @see HMC5883L_RA_DATAX_H
|
|
*/
|
|
void HMC5883L::getHeading(short *x, short *y, short *z)
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_DATAX_H, buffer, 6);
|
|
if (mode == HMC5883L_MODE_SINGLE)
|
|
Board::get()->i2c_write_byte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
|
|
|
|
*x = (((short)buffer[0]) << 8) | buffer[1];
|
|
*y = (((short)buffer[4]) << 8) | buffer[5];
|
|
*z = (((short)buffer[2]) << 8) | buffer[3];
|
|
}
|
|
|
|
// STATUS register
|
|
/** Get data ready status.
|
|
* @return Data ready status
|
|
* @see HMC5883L_RA_STATUS
|
|
* @see HMC5883L_STATUS_READY_BIT
|
|
*/
|
|
bool HMC5883L::getReadyStatus()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_STATUS, buffer, 1);
|
|
|
|
return (buffer[0] >> (HMC5883L_STATUS_READY_BIT)) & (1<<1-1);
|
|
}
|
|
|
|
// ID_* registers
|
|
|
|
/** Get identification byte A
|
|
* @return ID_A byte (should be 01001000, ASCII value 'H')
|
|
*/
|
|
unsigned char HMC5883L::getIDA()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_ID_A, buffer, 1);
|
|
return buffer[0];
|
|
}
|
|
/** Get identification byte B
|
|
* @return ID_A byte (should be 00110100, ASCII value '4')
|
|
*/
|
|
unsigned char HMC5883L::getIDB()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_ID_B, buffer, 1);
|
|
return buffer[0];
|
|
}
|
|
/** Get identification byte C
|
|
* @return ID_A byte (should be 00110011, ASCII value '3')
|
|
*/
|
|
unsigned char HMC5883L::getIDC()
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, HMC5883L_RA_ID_C, buffer, 1);
|
|
return buffer[0];
|
|
}
|