Fixed failure notices on mpu9250 startup by adding back device probing on I2C interface init.

src/drivers/imu/mpu9250/mpu9250_i2c.cpp

@@ -107,43 +107,30 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
 int
 MPU9250_I2C::probe()
 {
-	// uint8_t whoami = 0;
-	// uint8_t reg_whoami = 0;
-	// uint8_t expected = 0;
-	// uint8_t register_select = REG_BANK(BANK0);  // register bank containing WHOAMI for ICM20948
-
-	// switch (_whoami) {
-	// case MPU_WHOAMI_9250:
-	// 	reg_whoami = MPUREG_WHOAMI;
-	// 	expected = MPU_WHOAMI_9250;
-	// 	break;
-
-	// case MPU_WHOAMI_6500:
-	// 	reg_whoami = MPUREG_WHOAMI;
-	// 	expected = MPU_WHOAMI_6500;
-	// 	break;
-
-	// case ICM_WHOAMI_20948:
-	// 	reg_whoami = ICMREG_20948_WHOAMI;
-	// 	expected = ICM_WHOAMI_20948;
-	// 	/*
-	// 	 * 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, &register_select, 1);
-
-	// 	break;
-	// }
-
-	// return (read(reg_whoami, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
-
-
-	// // Try the mpu9250/6500 first
-	// read(MPUREG_WHOAMI, &whoami, 1);
-	// if (whoami == MPU_WHOAMI_9250)
-
-	// this does not matter
-	return PX4_OK;
+	 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, &register_select, 1);
+	     read(ICMREG_20948_WHOAMI, &whoami, 1);
+
+	     if(whoami == ICM_WHOAMI_20948) {
+	         return PX4_OK;
+	     }
+	 }
+
+	 return -ENODEV;
 }
 
 #endif /* USE_I2C */