// Read a single compass register
int mpu9250_read_compass_reg(struct mpu9250_s *device, uint8_t reg, uint8_t *val)
{
int res;
// Set I2C to read address
res = mpu9250_write_reg(device, MPU9250_REG_I2C_SLV0_ADDR, MPU9250_COMPASS_ADDR | MPU9250_COMPASS_READ_FLAG);
if (res < 0) {
return res;
}
// Set register address
res = mpu9250_write_reg(device, MPU9250_REG_I2C_SLV0_REG, reg);
if (res < 0) {
return res;
}
// Trigger read
res = mpu9250_write_reg(device, MPU9250_REG_I2C_SLV0_CTRL, MPU9250_I2C_SLV_CTRL_EN | (1 & MPU9250_I2C_SLV_CTRL_LENG_MASK));
if (res < 0) {
return res;
}
// Wait for response
//TODO: should be able to await interrupt on pin here
PLATFORM_SLEEP_MS(1);
res = mpu9250_read_reg(device, MPU9250_REG_EXT_SENS_DATA_00, val);
return res;
}
static uint8_t i2c_Mag_read(uint8_t reg)
{
u16 j = 5000;
mpu9250_write_reg(I2C_SLV0_ADDR, MPU9250_AK8963_ADDR|0x80); //设置磁力计地址，mode：read
mpu9250_write_reg(I2C_SLV0_REG, reg); // set reg addr
mpu9250_write_reg(I2C_SLV0_DO, 0xff); //read
while(j--); //此处因为MPU内部I2C读取速度较慢，必须延时等待内部读取完毕
return mpu9250_read_reg(EXT_SENS_DATA_00);
}
// Update a single register on the device
// Mask should be the mask for the information to be changed
int mpu9250_update_reg(struct mpu9250_s *device, uint8_t reg, uint8_t val, uint8_t mask)
{
uint8_t data = 0;
int res;
// Read existing config
res = mpu9250_read_reg(device, reg, &data);
if (res < 0) {
return res;
}
// Update
data &= ~mask;
data |= mask & val;
// Write back
return mpu9250_write_reg(device, reg, data);
}
/**********************Init MAG **********************************/
i2c_Mag_write(AK8963_CNTL2_REG,AK8963_CNTL2_SRST); // Reset AK8963
i2c_Mag_write(AK8963_CNTL1_REG, 0x12); //use i2c to set AK8963 working on Continuous measurement mode1 & 16-bit output
mpu9250_mag_init();
}
//************************加速度读取**************************/
void mpu9250_read_accel(void)//
{
BUF[0] = mpu9250_read_reg(ACCEL_XOUT_L);
BUF[1] = mpu9250_read_reg(ACCEL_XOUT_H);
mpu_value.Accel[0] = (BUF[1]<<8) | BUF[0];
mpu_value.Accel[0] /= 164; //读取计算X轴数据
BUF[2] = mpu9250_read_reg(ACCEL_YOUT_L);
BUF[3] = mpu9250_read_reg(ACCEL_YOUT_H);
mpu_value.Accel[1] = (BUF[3]<<8) | BUF[2];
mpu_value.Accel[1]/=164; //读取计算Y轴数据
BUF[4] = mpu9250_read_reg(ACCEL_ZOUT_L);
BUF[5] = mpu9250_read_reg(ACCEL_ZOUT_H);
mpu_value.Accel[2] = (BUF[5]<<8) | BUF[4];
mpu_value.Accel[2] /= 164; //读取计算Z轴数据
}
/**********************陀螺仪读取*****************************/
void mpu9250_read_gyro(void)
{
BUF[0] = mpu9250_read_reg(GYRO_XOUT_L);
BUF[1] = mpu9250_read_reg(GYRO_XOUT_H);
mpu_value.Gyro[0] = (BUF[1]<<8) | BUF[0];
mpu_value.Gyro[0] /= 16.4; //读取计算X轴数据
BUF[2] = mpu9250_read_reg(GYRO_YOUT_L);
BUF[3] = mpu9250_read_reg(GYRO_YOUT_H);
mpu_value.Gyro[1] = (BUF[3]<<8) | BUF[2];
mpu_value.Gyro[1]/=16.4; //读取计算Y轴数据
BUF[4] = mpu9250_read_reg(GYRO_ZOUT_L);
BUF[5] = mpu9250_read_reg(GYRO_ZOUT_H);
mpu_value.Gyro[2] = (BUF[5]<<8) | BUF[4];
mpu_value.Gyro[2] /= 16.4; //读取计算Z轴数据
}
int8_t mpu9250_init(struct mpu9250_s *device, struct mpu9250_driver_s *driver, void* driver_ctx)
{
int res;
// Check driver functions exist
if (driver->spi_transfer == NULL) {
return MPU9250_DRIVER_INVALID;
}
// Save driver pointers
device->driver = driver;
device->driver_ctx = driver_ctx;
// Initialize device
// Hard reset chip (nb. only works if SPI working)
res = mpu9250_write_reg(device, MPU9250_REG_PWR_MGMT_1, MPU9250_PWR_MGMT_1_HRESET);
if (res < 0) {
MPU9250_DEBUG_PRINT("RESET write error: %d\r\n", res);
return MPU9250_DRIVER_ERROR;
}
// Give device time to reset
PLATFORM_SLEEP_MS(10);
MPU9250_DEBUG_PRINT("RESET complete\r\n");
// Check communication
uint8_t who;
res = mpu9250_read_reg(device, MPU9250_REG_WHO_AM_I, &who);
if (res < 0) {
MPU9250_DEBUG_PRINT("WHOAMI read error: %d\r\n", res);
return MPU9250_DRIVER_ERROR;
}
if (who != 0x71) {
MPU9250_DEBUG_PRINT("Unexpected whoami response: %.2x\r\n", who);
return MPU9250_COMMS_ERROR;
}
MPU9250_DEBUG_PRINT("Device identified\r\n");
// Set default scales
res = mpu9250_set_gyro_scale(device, MPU9250_GYRO_SCALE_2000DPS);
if (res < 0) {
MPU9250_DEBUG_PRINT("Error %d setting gyro scale\r\n", res);
return MPU9250_DRIVER_ERROR;
}
res = mpu9250_set_accel_scale(device, MPU9250_ACCEL_SCALE_16G);
if (res < 0) {
MPU9250_DEBUG_PRINT("Error %d setting accel scale\r\n", res);
return MPU9250_DRIVER_ERROR;
}
// TODO: Set default sampling rates & filters
// Enable compass
// Enable master mode
res = mpu9250_write_reg(device, MPU9250_REG_USER_CTRL, MPU9250_USER_CTRL_I2C_MST_EN);
// Set freq to 400kHz
res = mpu9250_write_reg(device, MPU9250_REG_I2C_MST_CTRL, MPU9250_I2C_MST_CLK_400_KHZ);
// Set compass device address
res = mpu9250_write_reg(device, MPU9250_REG_I2C_SLV0_ADDR, MPU9250_COMPASS_ADDR);
// Reset compass
res = mpu9250_write_compass_reg(device, MPU9250_COMPASS_REG_CNTL2, MPU9250_COMPASS_CTL2_RESET);
// Check for compass whoami response
res = mpu9250_read_compass_reg(device, MPU9250_COMPASS_REG_WIA, &who);
if (res < 0) {
MPU9250_DEBUG_PRINT("WHOAMI compass read error: %d\r\n", res);
return MPU9250_DRIVER_ERROR;
}
if (who != 0x48) {
MPU9250_DEBUG_PRINT("Unexpected compass whoami response: %.2x\r\n", who);
return MPU9250_COMMS_ERROR;
}
MPU9250_DEBUG_PRINT("Compass identified\r\n");
// Enable continuous measurement mode at 16-bit
res = mpu9250_write_compass_reg(device, MPU9250_COMPASS_REG_CNTL1, MPU9250_COMPASS_CTL1_OUTPUT_16BIT
| (MPU9250_COMPASS_CTL1_MODE_MASK & MPU9250_COMPASS_MODE_CONTINUOUS1));
if (res < 0) {
MPU9250_DEBUG_PRINT("Error %d setting compass mode\r\n", res);
return MPU9250_DRIVER_ERROR;
}
//TODO: set compass auto trigger from MPU to regs
return 0;
}
