static void terminal_read_reg(int argc, const char **argv) {
if (argc == 3) {
int bank = -1;
int reg = -1;
sscanf(argv[1], "%d", &bank);
sscanf(argv[2], "%d", ®);
if (reg >= 0 && (bank == 0 || bank == 1 || bank == 2)) {
write_single_reg(m_terminal_state, ICM20948_BANK_SEL, bank << 4);
unsigned int res = read_single_reg(m_terminal_state, reg);
char bl[9];
write_single_reg(m_terminal_state, ICM20948_BANK_SEL, 0 << 4);
utils_byte_to_binary(res & 0xFF, bl);
commands_printf("Reg 0x%02x: %s (0x%02x)\n", reg, bl, res);
} else {
commands_printf("Invalid argument(s).\n");
}
} else {
commands_printf("This command requires one argument.\n");
}
}
static int reset_init_mpu(void) {
msg_t res = MSG_OK;
palSetPadMode(SCL_GPIO, SCL_PAD,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1);
for(int i = 0;i < 16;i++) {
palClearPad(SCL_GPIO, SCL_PAD);
delay_short();
palSetPad(SCL_GPIO, SCL_PAD);
delay_short();
}
palSetPadMode(SCL_GPIO, SCL_PAD,
PAL_MODE_ALTERNATE(GPIO_AF_I2C1) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1);
I2C_DEV.state = I2C_STOP;
i2cStart(&I2C_DEV, &i2cfg);
chThdSleepMicroseconds(1000);
// Set clock source to gyro x
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_PWR_MGMT_1;
tx_buf[1] = 0x01;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
// Try the other address
if (res != MSG_OK) {
if (mpu_addr == MPU_ADDR1) {
mpu_addr = MPU_ADDR2;
} else {
mpu_addr = MPU_ADDR1;
}
// Set clock source to gyro x
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_PWR_MGMT_1;
tx_buf[1] = 0x01;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
}
// Set accelerometer full-scale range to +/- 16g
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_ACCEL_CONFIG;
tx_buf[1] = MPU9150_ACCEL_FS_16 << MPU9150_ACONFIG_AFS_SEL_BIT;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
// Set gyroscope full-scale range to +/- 2000 deg/s
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_GYRO_CONFIG;
tx_buf[1] = MPU9150_GYRO_FS_2000 << MPU9150_GCONFIG_FS_SEL_BIT;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
// Set low pass filter to 256Hz (1ms delay)
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_CONFIG;
tx_buf[1] = MPU9150_DLPF_BW_256;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
#if USE_MAGNETOMETER
// Set the i2c bypass enable pin to true to access the magnetometer
i2cAcquireBus(&I2C_DEV);
tx_buf[0] = MPU9150_INT_PIN_CFG;
tx_buf[1] = 0x02;
res = i2cMasterTransmitTimeout(&I2C_DEV, mpu_addr, tx_buf, 2, rx_buf, 0, MPU_I2C_TIMEOUT);
i2cReleaseBus(&I2C_DEV);
if (res != MSG_OK) {
return 0;
}
#endif
is_mpu9250 = read_single_reg(MPU9150_WHO_AM_I) == 0x71;
return 1;
}