/**
* @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 none
*/
void PIOS_HMC5883_ReadMag(struct pios_hmc5883_data * data)
{
uint8_t buffer[6];
uint8_t ctrlB;
int16_t out[3];
pios_hmc5883_data_ready = false;
while (!PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrlB, 1)) ;
while (!PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6)) ;
switch (ctrlB & 0xE0) {
case 0x00:
for (int i = 0; i < 3; i++)
out[i] = ((int16_t) ((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_0_88Ga;
break;
case 0x20:
for (int i = 0; i < 3; i++)
out[i] = ((int16_t) ((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_1_3Ga;
break;
case 0x40:
for (int i = 0; i < 3; i++)
out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_1_9Ga;
break;
case 0x60:
for (int i = 0; i < 3; i++)
out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_2_5Ga;
break;
case 0x80:
for (int i = 0; i < 3; i++)
out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_4_0Ga;
break;
case 0xA0:
for (int i = 0; i < 3; i++)
out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_4_7Ga;
break;
case 0xC0:
for (int i = 0; i < 3; i++)
out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_5_6Ga;
break;
case 0xE0:
for (int i = 0; i < 3; i++)
out[i] = (int16_t) (((uint16_t) buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / PIOS_HMC5883_Sensitivity_8_1Ga;
break;
}
data->x = out[0];
data->y = out[2];
data->z = out[1];
while (!PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE)) ;
}
/**
* @brief Read the identification bytes from the HMC5883 sensor
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
* \return 0 if successful, -1 if not
*/
static uint8_t PIOS_HMC5883_ReadID(uint8_t out[4])
{
uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @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;
}
/**
* @brief Read the identification bytes from the HMC5883 sensor
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
* \return none
*/
void PIOS_HMC5883_ReadID(uint8_t out[4])
{
while (!PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3)) ;
out[3] = '\0';
}
