Exemplo n.º 1
0
void mpu6000SpiGyroInit(uint8_t lpf)
{
	debug[3]++;
    mpuIntExtiInit();

    mpu6000AccAndGyroInit(lpf);

    spiResetErrorCounter(MPU6000_SPI_INSTANCE);

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_FAST_CLOCK); //high speed now that we don't need to write to the slow registers

    int16_t data[3];
    mpuGyroRead(data);

    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}
Exemplo n.º 2
0
void mpu6000SpiGyroInit(uint8_t lpf)
{
    mpuIntExtiInit();

    mpu6000AccAndGyroInit();

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

    // Accel and Gyro DLPF Setting
    mpu6000WriteRegister(MPU6000_CONFIG, lpf);
    delayMicroseconds(1);

    int16_t data[3];
    mpuGyroRead(data);

    if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}
Exemplo n.º 3
0
void mpu6000SpiGyroInit(uint8_t lpf)
{
    mpuIntExtiInit();

    mpu6000AccAndGyroInit();

    spiResetErrorCounter(MPU6000_SPI_INSTANCE);

    spiSetDivisor(MPU6000_SPI_INSTANCE, LOW_SPEED_SPI);

    // Accel and Gyro DLPF Setting
    mpu6000WriteRegister(MPU6000_CONFIG, lpf);
    delayMicroseconds(1);

    int16_t data[3];
    mpuGyroRead(data);

    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}
Exemplo n.º 4
0
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{
    busDevice_t * busDev = gyro->busDev;
    const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs);
    gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;

    gyroIntExtiInit(gyro);

    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);

    // Device Reset
    busWrite(busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
    delay(150);

    busWrite(busDev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
    delay(150);

    // Clock Source PPL with Z axis gyro reference
    busWrite(busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
    delayMicroseconds(15);

    // Disable Primary I2C Interface
    busWrite(busDev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
    delayMicroseconds(15);

    busWrite(busDev, MPU_RA_PWR_MGMT_2, 0x00);
    delayMicroseconds(15);

    // Accel Sample Rate 1kHz
    // Gyroscope Output Rate =  1kHz when the DLPF is enabled
    busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]);
    delayMicroseconds(15);

    // Gyro +/- 1000 DPS Full Scale
    busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
    delayMicroseconds(15);

    // Accel +/- 8 G Full Scale
    busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
    delayMicroseconds(15);

    busWrite(busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
    delayMicroseconds(15);

#ifdef USE_MPU_DATA_READY_SIGNAL
    busWrite(busDev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
    delayMicroseconds(15);
#endif

    // Accel and Gyro DLPF Setting
    busWrite(busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]);
    delayMicroseconds(1);

    busSetSpeed(busDev, BUS_SPEED_FAST);

    mpuGyroRead(gyro);

    if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}