|
|
|
@ -107,30 +107,30 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
@@ -107,30 +107,30 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
|
|
|
|
|
int |
|
|
|
|
MPU9250_I2C::probe() |
|
|
|
|
{ |
|
|
|
|
uint8_t whoami = 0; |
|
|
|
|
uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948
|
|
|
|
|
|
|
|
|
|
// Try first for mpu9250/6500
|
|
|
|
|
read(MPUREG_WHOAMI, &whoami, 1); |
|
|
|
|
|
|
|
|
|
if(whoami == MPU_WHOAMI_9250 || whoami == MPU_WHOAMI_6500) { |
|
|
|
|
return PX4_OK; |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
/*
|
|
|
|
|
* If it's not an MPU it must be an ICM |
|
|
|
|
* Make sure register bank 0 is selected - whoami is only present on bank 0, and that is |
|
|
|
|
* not sure e.g. if the device has rebooted without repowering the sensor |
|
|
|
|
*/ |
|
|
|
|
write(ICMREG_20948_BANK_SEL, ®ister_select, 1); |
|
|
|
|
read(ICMREG_20948_WHOAMI, &whoami, 1); |
|
|
|
|
|
|
|
|
|
if(whoami == ICM_WHOAMI_20948) { |
|
|
|
|
return PX4_OK; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return -ENODEV; |
|
|
|
|
uint8_t whoami = 0; |
|
|
|
|
uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948
|
|
|
|
|
|
|
|
|
|
// Try first for mpu9250/6500
|
|
|
|
|
read(MPUREG_WHOAMI, &whoami, 1); |
|
|
|
|
|
|
|
|
|
if (whoami == MPU_WHOAMI_9250 || whoami == MPU_WHOAMI_6500) { |
|
|
|
|
return PX4_OK; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
/*
|
|
|
|
|
* If it's not an MPU it must be an ICM |
|
|
|
|
* Make sure register bank 0 is selected - whoami is only present on bank 0, and that is |
|
|
|
|
* not sure e.g. if the device has rebooted without repowering the sensor |
|
|
|
|
*/ |
|
|
|
|
write(ICMREG_20948_BANK_SEL, ®ister_select, 1); |
|
|
|
|
read(ICMREG_20948_WHOAMI, &whoami, 1); |
|
|
|
|
|
|
|
|
|
if (whoami == ICM_WHOAMI_20948) { |
|
|
|
|
return PX4_OK; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return -ENODEV; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#endif /* USE_I2C */ |
|
|
|
|