mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse) { memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult)); memset(&mpuConfiguration, 0, sizeof(mpuConfiguration)); mpuIntExtiConfig = configToUse; bool ack; uint8_t sig; uint8_t inquiryResult; // MPU datasheet specifies 30ms. delay(35); #ifndef USE_I2C ack = false; sig = 0; #else ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); #endif if (ack) { mpuConfiguration.read = mpuReadRegisterI2C; mpuConfiguration.write = mpuWriteRegisterI2C; } else { #ifdef USE_SPI bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(); UNUSED(detectedSpiSensor); #endif return &mpuDetectionResult; } mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; // If an MPU3050 is connected sig will contain 0. ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { mpuDetectionResult.sensor = MPU_3050; mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; return &mpuDetectionResult; } sig &= MPU_INQUIRY_MASK; if (sig == MPUx0x0_WHO_AM_I_CONST) { mpuDetectionResult.sensor = MPU_60x0; mpu6050FindRevision(); } else if (sig == MPU6500_WHO_AM_I_CONST) { mpuDetectionResult.sensor = MPU_65xx_I2C; } return &mpuDetectionResult; }
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse) { memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult)); memset(&mpuConfiguration, 0, sizeof(mpuConfiguration)); mpuIntExtiConfig = configToUse; bool ack; uint8_t sig; uint8_t inquiryResult; // MPU datasheet specifies 30ms. delay(35); #ifdef USE_I2C ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); if (ack) { mpuConfiguration.read = mpuReadRegisterI2C; mpuConfiguration.write = mpuWriteRegisterI2C; } else { #else { #endif #ifdef USE_SPI bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(); UNUSED(detectedSpiSensor); #endif return &mpuDetectionResult; } mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; // If an MPU3050 is connected sig will contain 0. ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { mpuDetectionResult.sensor = MPU_3050; mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; return &mpuDetectionResult; } sig &= MPU_INQUIRY_MASK; if (sig == MPUx0x0_WHO_AM_I_CONST) { mpuDetectionResult.sensor = MPU_60x0; mpu6050FindRevision(); } else if (sig == MPU6500_WHO_AM_I_CONST) { mpuDetectionResult.sensor = MPU_65xx_I2C; } return &mpuDetectionResult; } #ifdef USE_SPI static bool detectSPISensorsAndUpdateDetectionResult(void) { #ifdef USE_GYRO_SPI_MPU6500 if (mpu6500SpiDetect()) { mpuDetectionResult.sensor = MPU_65xx_SPI; mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; mpuConfiguration.read = mpu6500ReadRegister; mpuConfiguration.write = mpu6500WriteRegister; return true; } #endif #ifdef USE_GYRO_SPI_MPU6000 if (mpu6000SpiDetect()) { mpuDetectionResult.sensor = MPU_60x0_SPI; mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; mpuConfiguration.read = mpu6000ReadRegister; mpuConfiguration.write = mpu6000WriteRegister; return true; } #endif return false; } #endif static void mpu6050FindRevision(void) { bool ack; UNUSED(ack); uint8_t readBuffer[6]; uint8_t revision; uint8_t productId; // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // determine product ID and accel revision ack = mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); if (revision) { /* Congrats, these parts are better. */ if (revision == 1) { mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; } else if (revision == 2) { mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; } else if ((revision == 3) || (revision == 7)) { mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; } else { failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { ack = mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); revision = productId & 0x0F; if (!revision) { failureMode(FAILURE_ACC_INCOMPATIBLE); } else if (revision == 4) { mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; } else { mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; } } }