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