/**
* @brief Initialize the HMC5883 magnetometer sensor
* \return none
* \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF8000x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF8000x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF8000x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF8000x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF8000x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF8000x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF8000x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF8000x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg)
{
// CRTL_REGA
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, cfg->M_ODR | cfg->Meas_Conf) != 0)
return -1;
// CRTL_REGB
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, cfg->Gain) != 0)
return -1;
// Mode register
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, cfg->Mode) != 0)
return -1;
return 0;
}
/**
* @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 Initialize the HMC5883 magnetometer sensor
* \return none
* \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF8000x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF8000x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF8000x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF8000x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF8000x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF8000x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF8000x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF8000x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static void PIOS_HMC5883_Config(PIOS_HMC5883_ConfigTypeDef * HMC5883_Config_Struct)
{
uint8_t CTRLA = 0x00;
uint8_t CTRLB = 0x00;
uint8_t MODE = 0x00;
CTRLA |= (uint8_t) (HMC5883_Config_Struct->M_ODR | HMC5883_Config_Struct->Meas_Conf);
CTRLB |= (uint8_t) (HMC5883_Config_Struct->Gain);
MODE |= (uint8_t) (HMC5883_Config_Struct->Mode);
// CRTL_REGA
while (!PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA)) ;
// CRTL_REGB
while (!PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB)) ;
// Mode register
while (!PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE)) ;
}
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
CTRLB = 0;
CTRLA |= (uint8_t) (cfg->M_ODR | cfg->Meas_Conf);
CTRLB |= (uint8_t) (cfg->Gain);
MODE |= (uint8_t) (cfg->Mode);
// CRTL_REGA
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0)
return -1;
// CRTL_REGB
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0)
return -1;
// Mode register
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0)
return -1;
return 0;
}
/**
* @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;
}
