RobotHardware-UESTC/Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/Lib/IMU/HMC5883L.cpp

#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];
}