/** * @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'; }