/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
static int32_t PIOS_HMC5883_ReadMag(struct pios_sensor_mag_data *mag_data)
{
if (PIOS_HMC5883_Validate(dev) != 0)
return -1;
/* don't use PIOS_HMC5883_Read and PIOS_HMC5883_Write here because the task could be
* switched out of context in between which would give the sensor less time to capture
* the next sample.
*/
uint8_t addr_read = PIOS_HMC5883_DATAOUT_XMSB_REG;
uint8_t buffer_read[6];
// PIOS_HMC5883_MODE_CONTINUOUS: This should not be necessary but for some reason it is coming out of continuous conversion mode
// PIOS_HMC5883_MODE_SINGLE: This triggers the next measurement
uint8_t buffer_write[2] = {
PIOS_HMC5883_MODE_REG,
dev->cfg->Mode
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_read),
.buf = &addr_read,
},
{
.info = __func__,
/**
* @brief Updates the HMC5883 chip orientation.
* @returns 0 for success or -1 for failure
*/
int32_t PIOS_HMC5883_SetOrientation(enum pios_hmc5883_orientation orientation)
{
if (PIOS_HMC5883_Validate(dev) != 0)
return -1;
dev->orientation = orientation;
return 0;
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5883 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len)
{
if(PIOS_HMC5883_Validate(dev) != 0)
return -1;
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
static int32_t PIOS_HMC5883_ReadMag(struct pios_sensor_mag_data *mag_data)
{
if(PIOS_HMC5883_Validate(dev) != 0)
return -1;
uint8_t buffer[6];
int32_t sensitivity;
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
int16_t mag_x, mag_y, mag_z;
mag_x = ((int16_t) ((uint16_t) buffer[0] << 8) + buffer[1]) * 1000 / sensitivity;
mag_z = ((int16_t) ((uint16_t) buffer[2] << 8) + buffer[3]) * 1000 / sensitivity;
mag_y = ((int16_t) ((uint16_t) buffer[4] << 8) + buffer[5]) * 1000 / sensitivity;
// Define "0" when the fiducial is in the front left of the board
switch (dev->cfg->orientation) {
case PIOS_HMC5883_TOP_0DEG:
mag_data->x = -mag_x;
mag_data->y = mag_y;
mag_data->z = -mag_z;
break;
case PIOS_HMC5883_TOP_90DEG:
mag_data->x = -mag_y;
mag_data->y = -mag_x;
mag_data->z = -mag_z;
break;
case PIOS_HMC5883_TOP_180DEG:
mag_data->x = mag_x;
mag_data->y = -mag_y;
mag_data->z = -mag_z;
break;
case PIOS_HMC5883_TOP_270DEG:
mag_data->x = mag_y;
mag_data->y = mag_x;
mag_data->z = -mag_z;
break;
}
// This should not be necessary but for some reason it is coming out of continuous conversion mode
PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS);
return 0;
}
