bool mpu6000SpiGyroDetect(const mpu6000Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
{
mpu6000Config = configToUse;
if (!mpu6000SpiDetect()) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
else
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Determine the new sample divider
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
delayMicroseconds(1);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
gyro->intStatus = checkMPU6000DataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}
bool mpu6000SpiAccDetect(acc_t *acc)
{
if (!mpu6000SpiDetect()) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
acc->init = mpu6000SpiAccInit;
acc->read = mpu6000SpiAccRead;
delay(100);
return true;
}
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
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) {
mpuDetectionResult.sensor = MPU_9250_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu9250ReadRegister;
mpuConfiguration.slowread = mpu9250SlowReadRegister;
mpuConfiguration.verifywrite = verifympu9250WriteRegister;
mpuConfiguration.write = mpu9250WriteRegister;
mpuConfiguration.reset = mpu9250ResetGyro;
return true;
}
#endif
return false;
}
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;
}
}
}
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
{
if (!mpu6000SpiDetect()) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
break;
case 188:
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
break;
case 98:
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
break;
default:
case 42:
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
break;
case 20:
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
break;
case 10:
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
break;
case 5:
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
break;
case 0:
mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF;
break;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}