/** * @brief Get a sample of compass data from the device. * @param data * the buffer to store the compass raw data for * X, Y, and Z axes. * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_get_compass_data(long *data) { inv_error_t result; int ii; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); unsigned char *tmp = inv_obj.mag->raw_data; if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len > sizeof(inv_obj.mag->raw_data)) { LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); return INV_ERROR_INVALID_CONFIGURATION; } if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus == EXT_SLAVE_BUS_PRIMARY || !(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) { /*--- read the compass sensor data. The compass read function may return an INV_ERROR_COMPASS_* errors when the data is not ready (read/refresh frequency mismatch) or the internal data sampling timing of the device was not respected. Returning the error code will make the sensor fusion supervisor ignore this compass data sample. ---*/ result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), tmp); if (result) { if (COMPASS_DEBUG) { MPL_LOGV("inv_mpu_read_compass returned %d\n", result); } return result; } for (ii = 0; ii < 3; ii++) { if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian) data[ii] = ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; else data[ii] = ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; } } else { /* compass on the 2nd bus or DMP is off */ result = inv_get_external_sensor_data(data, 3); if (result) { LOG_RESULT_LOCATION(result); return result; } } data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]); data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]); data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]); return INV_SUCCESS; }
/** * @brief Get a sample of compass data from the device. * @param data * the buffer to store the compass raw data for * X, Y, and Z axes. * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_get_compass_data(long *data) { inv_error_t result; int ii; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); unsigned char *tmp = inv_obj.mag->raw_data; if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len > sizeof(inv_obj.mag->raw_data)) { LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); return INV_ERROR_INVALID_CONFIGURATION; } if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus == EXT_SLAVE_BUS_PRIMARY || !(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) { /*--- read the compass sensor data. The compass read function may return an INV_ERROR_COMPASS_* errors when the data is not ready (read/refresh frequency mismatch) or the internal data sampling timing of the device was not respected. Returning the error code will make the sensor fusion supervisor ignore this compass data sample. ---*/ result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), tmp); if (result) { if (COMPASS_DEBUG) { MPL_LOGV("inv_mpu_read_compass returned %d\n", result); } return result; } for (ii = 0; ii < 3; ii++) { if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian) data[ii] = ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; else data[ii] = ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; } } else { #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 result = inv_get_external_sensor_data(data, 3); if (result) { LOG_RESULT_LOCATION(result); return result; } #if defined CONFIG_MPU_SENSORS_MPU6050A2 { static unsigned char first = true; // one-off write to AKM if (first) { unsigned char regs[] = { // beginning Mantis register for one-off slave R/W MPUREG_I2C_SLV4_ADDR, // the slave to write to mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->address, // the register to write to /*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->reg */ 0x0A, // the value to write /*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->value */ 0x01, // enable the write 0xC0 }; result = inv_serial_write(inv_get_serial_handle(), mldl_cfg->mpu_chip_info->addr, ARRAY_SIZE(regs), regs); first = false; } else { unsigned char regs[] = { MPUREG_I2C_SLV4_CTRL, 0xC0 }; result = inv_serial_write(inv_get_serial_handle(), mldl_cfg->mpu_chip_info->addr, ARRAY_SIZE(regs), regs); } } #endif #else return INV_ERROR_INVALID_CONFIGURATION; #endif // CONFIG_MPU_SENSORS_xxxx } data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]); data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]); data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]); return INV_SUCCESS; }