void
MPU9250::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
if (_whoami == ICM_WHOAMI_20948) {
select_register_bank(REG_BANK(reg));
_interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1);
} else {
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
}
}
void
MPU9250::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
}
uint16_t
MPU9250::read_reg16(unsigned reg)
{
uint8_t buf[2];
// general register transfer at low clock speed
if (_whoami == ICM_WHOAMI_20948) {
select_register_bank(REG_BANK(reg));
_interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf));
} else {
_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
}
return (uint16_t)(buf[0] << 8) | buf[1];
}
int
MPU9250::select_register_bank(uint8_t bank)
{
uint8_t ret;
uint8_t buf;
uint8_t retries = 3;
if (_selected_bank != bank) {
ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);
if (ret != OK) {
return ret;
}
}
/*
* Making sure the right register bank is selected (even if it should be). Observed some
* unexpected changes to this, don't risk writing to the wrong register bank.
*/
_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
while (bank != buf && retries > 0) {
//PX4_WARN("user bank: expected %d got %d",bank,buf);
ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);
if (ret != OK) {
return ret;
}
retries--;
//PX4_WARN("BANK retries: %d", 4-retries);
_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
}
_selected_bank = bank;
if (bank != buf) {
PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf);
return PX4_ERROR;
} else {
return PX4_OK;
}
}
uint16_t
MPU9250::read_reg16(unsigned reg)
{
uint8_t buf[2];
// general register transfer at low clock speed
_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
return (uint16_t)(buf[0] << 8) | buf[1];
}
int MPU9250::reset_mpu()
{
uint8_t retries;
switch (_whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
write_checked_reg(MPUREG_PWR_MGMT_2, 0);
usleep(1000);
break;
case ICM_WHOAMI_20948:
write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET);
usleep(1000);
write_checked_reg(ICMREG_20948_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
usleep(200);
write_checked_reg(ICMREG_20948_PWR_MGMT_2, 0);
break;
}
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), is_i2c() ? 0 : BIT_I2C_IF_DIS);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
// Gyro scale 2000 deg/s ()
switch (_whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
break;
case ICM_WHOAMI_20948:
modify_checked_reg(ICMREG_20948_GYRO_CONFIG_1, ICM_BITS_GYRO_FS_SEL_MASK, ICM_BITS_GYRO_FS_SEL_2000DPS);
break;
}
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
set_accel_range(ACCEL_RANGE_G);
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPU_OR_ICM(MPUREG_INT_ENABLE, ICMREG_20948_INT_ENABLE_1),
BIT_RAW_RDY_EN); // INT: Raw data ready
#ifdef USE_I2C
bool bypass = !_mag->is_passthrough();
#else
bool bypass = false;
#endif
/* INT: Clear on any read.
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
* that it will use to access the either: a) the internal mag device on the internal I2C bus
* or b) it could be used to access a downstream I2C devices connected to the chip on
* it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
* controller that chip provides as a SPI to I2C bridge.
* so bypass is true if the mag has an i2c non null interfaces.
*/
write_checked_reg(MPU_OR_ICM(MPUREG_INT_PIN_CFG, ICMREG_20948_INT_PIN_CFG),
BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
write_checked_reg(MPU_OR_ICM(MPUREG_ACCEL_CONFIG2, ICMREG_20948_ACCEL_CONFIG_2),
MPU_OR_ICM(BITS_ACCEL_CONFIG2_41HZ, ICM_BITS_DEC3_CFG_32));
retries = 3;
bool all_ok = false;
while (!all_ok && retries--) {
// Assume all checked values are as expected
all_ok = true;
uint8_t reg;
uint8_t bankcheck = 0;
for (uint8_t i = 0; i < _num_checked_registers; i++) {
if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
if (_whoami == ICM_WHOAMI_20948) {
_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1);
}
write_reg(_checked_registers[i], _checked_values[i]);
PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries,
REG_BANK(_checked_registers[i]), bankcheck);
all_ok = false;
}
}
}
return all_ok ? OK : -EIO;
}
