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;
}
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);
    }
}
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);
    }
}
Exemple #4
0
bool hmc5983DetectSpi(sensor_t *mag)
{
    int16_t data[3];
    int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.

    spiResetErrorCounter();

    uint8_t hmc5983Status = 0;
    uint8_t i, calibrationSteps = 2;
    setSPIdivisor( 8);  // 4.5 MHz SPI clock

    ENABLE_HMC5983;
    spiTransferByte(HMC5983_CONFIG_REG_A);
    spiTransferByte(TS | SENSOR_CONFIG | POSITIVE_BIAS_CONFIGURATION);
    DISABLE_HMC5983;

    delay(50);

    ENABLE_HMC5983;
    spiTransferByte(HMC5983_CONFIG_REG_B);
    spiTransferByte(SENSOR_INIT_GAIN);
    DISABLE_HMC5983;

    delay(20);

    readHMC5983(data);

    for (i = 0; i < 10; i++) {
        ENABLE_HMC5983;
        spiTransferByte(HMC5983_MODE_REG);
        spiTransferByte(OP_MODE_SINGLE);
        DISABLE_HMC5983;

        delay(20);

        while ((hmc5983Status && STATUS_RDY) == 0x00) {
            ENABLE_HMC5983;
            spiTransferByte(HMC5983_STATUS_REG + 0x80);
            hmc5983Status = spiTransferByte(0x00);
            DISABLE_HMC5983;
        }
        if (!readHMC5983(data)) // check for valid data
            break;          // breaks out of the for loop if sensor saturated.
        xyz_total[X] += data[X];
        xyz_total[Y] += data[Y];
        xyz_total[Z] += data[Z];
    }
    if (i == 10) // loop completed
        calibrationSteps--;

    ENABLE_HMC5983;
    spiTransferByte(HMC5983_CONFIG_REG_A);
    spiTransferByte(SENSOR_CONFIG | NEGATIVE_BIAS_CONFIGURATION);
    DISABLE_HMC5983;

    delay(20);
    readHMC5983(data);

    for (i = 0; i < 10; i++) {
        ENABLE_HMC5983;
        spiTransferByte(HMC5983_MODE_REG);
        spiTransferByte(OP_MODE_SINGLE);
        DISABLE_HMC5983;

        delay(20);

        while ((hmc5983Status && STATUS_RDY) == 0x00) {
            ENABLE_HMC5983;
            spiTransferByte(HMC5983_STATUS_REG + 0x80);
            hmc5983Status = spiTransferByte(0x00);
            DISABLE_HMC5983;
        }
        if (!readHMC5983(data))
            break;
        xyz_total[X] += data[X];
        xyz_total[Y] += data[Y];
        xyz_total[Z] += data[Z];
    }
    if (i == 10) // loop completed
        calibrationSteps--;

    ENABLE_HMC5983;
    spiTransferByte(HMC5983_CONFIG_REG_A);
    spiTransferByte(TS | SENSOR_CONFIG | NORMAL_MEASUREMENT_CONFIGURATION);
    DISABLE_HMC5983;

    ENABLE_HMC5983;
    spiTransferByte(HMC5983_CONFIG_REG_B);
    spiTransferByte(SENSOR_GAIN);
    DISABLE_HMC5983;

    delay(50);

    ENABLE_HMC5983;
    spiTransferByte(HMC5983_MODE_REG);
    spiTransferByte(OP_MODE_CONTINUOUS);
    DISABLE_HMC5983;

    delay(20);

    readHMC5983(data);

    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter() != 0) {
        spiResetErrorCounter();
        return false;
    }
//    magGain[X] = fabsf(660.0f * HMC5983_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
//    magGain[Y] = fabsf(660.0f * HMC5983_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
//    magGain[Z] = fabsf(660.0f * HMC5983_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);

    mag->init = hmc5983Init;
    mag->read = readHMC5983;

    return (calibrationSteps == 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;
}