Пример #1
0
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;
}
Пример #2
0
bool mpu6000SpiAccDetect(acc_t *acc)
{
    if (!mpu6000SpiDetect()) {
        return false;
    }

    spiResetErrorCounter(MPU6000_SPI_INSTANCE);

    mpu6000AccAndGyroInit();

    acc->init = mpu6000SpiAccInit;
    acc->read = mpu6000SpiAccRead;

    delay(100);
    return true;
}
Пример #3
0
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;
}
Пример #4
0
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;
        }
    }
}
Пример #5
0
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;
}