Esempio n. 1
0
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#ifdef USE_BARO_BMP085
    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
        .xclrIO = IO_TAG(BARO_XCLR_PIN),
        .eocIO = IO_TAG(BARO_EOC_PIN),
    };
    bmp085Config = &defaultBMP085Config;
#endif

#endif

    switch (baroHardware) {
    case BARO_DEFAULT:
        ; // fallthough
    case BARO_BMP085:
#ifdef USE_BARO_BMP085
        if (bmp085Detect(bmp085Config, dev)) {
            baroHardware = BARO_BMP085;
            break;
        }
#endif
        ; // fallthough
    case BARO_MS5611:
#ifdef USE_BARO_MS5611
        if (ms5611Detect(dev)) {
            baroHardware = BARO_MS5611;
            break;
        }
#endif
        ; // fallthough
    case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
        if (bmp280Detect(dev)) {
            baroHardware = BARO_BMP280;
            break;
        }
#endif
        ; // fallthough
    case BARO_NONE:
        baroHardware = BARO_NONE;
        break;
    }

    if (baroHardware == BARO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
    return true;
}
Esempio n. 2
0
static void detectBaro()
{
#ifdef BARO
#ifdef USE_BARO_MS5611
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
    if (ms5611Detect(&baro)) {
        return;
    }
#endif

#ifdef USE_BARO_BMP085
    // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
    if (bmp085Detect(&baro)) {
        return;
    }
#endif
    sensorsClear(SENSOR_BARO);
#endif
}
Esempio n. 3
0
bool sensorsAutodetect(void)
{
    int16_t deg, min;
    drv_adxl345_config_t acc_params;
    bool haveMpu6k = false;

    // Autodetect gyro hardware. We have MPU3050 or MPU6050.
    if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
        // this filled up  acc.* struct with init values
        haveMpu6k = true;
    } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
        // well, we found our gyro
        ;
    } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
        // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
        return false;
    }

    // Accelerometer. F**k it. Let user break shit.
retry:
    switch (mcfg.acc_hardware) {
        case ACC_NONE: // disable ACC
            sensorsClear(SENSOR_ACC);
            break;
        case ACC_DEFAULT: // autodetect
        case ACC_ADXL345: // ADXL345
            acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
            if (adxl345Detect(&acc_params, &acc))
                accHardware = ACC_ADXL345;
            if (mcfg.acc_hardware == ACC_ADXL345)
                break;
            ; // fallthrough
        case ACC_MPU6050: // MPU6050
            if (haveMpu6k) {
                mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
                accHardware = ACC_MPU6050;
                if (mcfg.acc_hardware == ACC_MPU6050)
                    break;
            }
            ; // fallthrough
#ifndef OLIMEXINO
        case ACC_MMA8452: // MMA8452
            if (mma8452Detect(&acc)) {
                accHardware = ACC_MMA8452;
                if (mcfg.acc_hardware == ACC_MMA8452)
                    break;
            }
            ; // fallthrough
        case ACC_BMA280: // BMA280
            if (bma280Detect(&acc)) {
                accHardware = ACC_BMA280;
                if (mcfg.acc_hardware == ACC_BMA280)
                    break;
            }
#endif
    }

    // Found anything? Check if user f****d up or ACC is really missing.
    if (accHardware == ACC_DEFAULT) {
        if (mcfg.acc_hardware > ACC_DEFAULT) {
            // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
            mcfg.acc_hardware = ACC_DEFAULT;
            goto retry;
        } else {
            // We're really screwed
            sensorsClear(SENSOR_ACC);
        }
    }

#ifdef BARO
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
    if (!bmp085Detect(&baro)) {
        // ms5611 disables BMP085, and tries to initialize + check PROM crc. 
        // moved 5611 init here because there have been some reports that calibration data in BMP180
        // has been "passing" ms5611 PROM crc check
        if (!ms5611Detect(&baro)) {
            // if both failed, we don't have anything
            sensorsClear(SENSOR_BARO);
        }
    }
#endif

    // Now time to init things, acc first
    if (sensors(SENSOR_ACC))
        acc.init(mcfg.acc_align);
    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    gyro.init(mcfg.gyro_align);

#ifdef MAG
    if (!hmc5883lDetect(mcfg.mag_align))
        sensorsClear(SENSOR_MAG);
#endif

    // calculate magnetic declination
    deg = cfg.mag_declination / 100;
    min = cfg.mag_declination % 100;
    if (sensors(SENSOR_MAG))
        magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
    else
        magneticDeclination = 0.0f;

    return true;
}
Esempio n. 4
0
static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifndef BARO
    UNUSED(baroHardwareToUse);
#else
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#ifdef USE_BARO_BMP085

    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
            .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
            .xclrGpioPin = BARO_XCLR_PIN,
            .xclrGpioPort = BARO_XCLR_GPIO,
            .eocGpioPin = BARO_EOC_PIN,
            .eocGpioPort = BARO_EOC_GPIO
    };
    bmp085Config = &defaultBMP085Config;
#endif

#ifdef NAZE
    if (hardwareRevision == NAZE32) {
        bmp085Disable(bmp085Config);
    }
#endif

#endif

    switch (baroHardware) {
        case BARO_DEFAULT:
            ; // fallthough

        case BARO_MS5611:
#ifdef USE_BARO_MS5611
            if (ms5611Detect(&baro)) {
                baroHardware = BARO_MS5611;
                break;
            }
#endif
            ; // fallthough
        case BARO_BMP085:
#ifdef USE_BARO_BMP085
            if (bmp085Detect(bmp085Config, &baro)) {
                baroHardware = BARO_BMP085;
                break;
            }
#endif
	    ; // fallthough
        case BARO_BMP280:
#ifdef USE_BARO_BMP280
            if (bmp280Detect(&baro)) {
                baroHardware = BARO_BMP280;
                break;
            }
#endif
        case BARO_NONE:
            baroHardware = BARO_NONE;
            break;
    }

    if (baroHardware == BARO_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
#endif
}
Esempio n. 5
0
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
    accelerationSensor_e accHardware;

#ifdef USE_ACC_ADXL345
    drv_adxl345_config_t acc_params;
#endif

retry:
    accAlign = ALIGN_DEFAULT;

    switch (accHardwareToUse) {
        case ACC_DEFAULT:
            ; // fallthrough
        case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345
            acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
#ifdef NAZE
            if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
#else
            if (adxl345Detect(&acc_params, &acc)) {
#endif
#ifdef ACC_ADXL345_ALIGN
                accAlign = ACC_ADXL345_ALIGN;
#endif
                accHardware = ACC_ADXL345;
                break;
            }
#endif
            ; // fallthrough
        case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
            if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
                accAlign = ACC_LSM303DLHC_ALIGN;
#endif
                accHardware = ACC_LSM303DLHC;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
            if (mpu6050AccDetect(&acc)) {
#ifdef ACC_MPU6050_ALIGN
                accAlign = ACC_MPU6050_ALIGN;
#endif
                accHardware = ACC_MPU6050;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MMA8452: // MMA8452
#ifdef USE_ACC_MMA8452
#ifdef NAZE
            // Not supported with this frequency
            if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
#else
            if (mma8452Detect(&acc)) {
#endif
#ifdef ACC_MMA8452_ALIGN
                accAlign = ACC_MMA8452_ALIGN;
#endif
                accHardware = ACC_MMA8452;
                break;
            }
#endif
            ; // fallthrough
        case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
            if (bma280Detect(&acc)) {
#ifdef ACC_BMA280_ALIGN
                accAlign = ACC_BMA280_ALIGN;
#endif
                accHardware = ACC_BMA280;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
            if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_MPU6000_ALIGN
                accAlign = ACC_MPU6000_ALIGN;
#endif
                accHardware = ACC_MPU6000;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6500:
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
#ifdef USE_ACC_SPI_MPU6500
            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
#else
            if (mpu6500AccDetect(&acc))
#endif
            {
#ifdef ACC_MPU6500_ALIGN
                accAlign = ACC_MPU6500_ALIGN;
#endif
                accHardware = ACC_MPU6500;
                break;
            }
#endif
            ; // fallthrough
	case ACC_MPU9250:
#ifdef USE_ACC_SPI_MPU9250

        if (mpu9250SpiAccDetect(&acc))
        {
            accHardware = ACC_MPU9250;
#ifdef ACC_MPU9250_ALIGN
            accAlign = ACC_MPU9250_ALIGN;
#endif

            break;
        }
#endif
        ; // fallthrough
        case ACC_FAKE:
#ifdef USE_FAKE_ACC
            if (fakeAccDetect(&acc)) {
                accHardware = ACC_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case ACC_NONE: // disable ACC
            accHardware = ACC_NONE;
            break;

    }

    // Found anything? Check if error or ACC is really missing.
    if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
        // Nothing was found and we have a forced sensor that isn't present.
        accHardwareToUse = ACC_DEFAULT;
        goto retry;
    }


    if (accHardware == ACC_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
    sensorsSet(SENSOR_ACC);
}

static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifndef BARO
    UNUSED(baroHardwareToUse);
#else
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#ifdef USE_BARO_BMP085

    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
        .xclrIO = IO_TAG(BARO_XCLR_PIN),
        .eocIO = IO_TAG(BARO_EOC_PIN),
    };
    bmp085Config = &defaultBMP085Config;
#endif

#ifdef NAZE
    if (hardwareRevision == NAZE32) {
        bmp085Disable(bmp085Config);
    }
#endif

#endif

    switch (baroHardware) {
        case BARO_DEFAULT:
            ; // fallthough
        case BARO_BMP085:
#ifdef USE_BARO_BMP085
            if (bmp085Detect(bmp085Config, &baro)) {
                baroHardware = BARO_BMP085;
                break;
            }
#endif
            ; // fallthough
        case BARO_MS5611:
#ifdef USE_BARO_MS5611
            if (ms5611Detect(&baro)) {
                baroHardware = BARO_MS5611;
                break;
            }
#endif
            ; // fallthough
        case BARO_BMP280:
#ifdef USE_BARO_BMP280
            if (bmp280Detect(&baro)) {
                baroHardware = BARO_BMP280;
                break;
            }
#endif
            ; // fallthough
        case BARO_NONE:
            baroHardware = BARO_NONE;
            break;
    }

    if (baroHardware == BARO_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
#endif
}

static void detectMag(magSensor_e magHardwareToUse)
{
    magSensor_e magHardware;

#ifdef USE_MAG_HMC5883
    const hmc5883Config_t *hmc5883Config = 0;

#ifdef NAZE // TODO remove this target specific define
    static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
            .intTag = IO_TAG(PB12) /* perhaps disabled? */
    };
    static const hmc5883Config_t nazeHmc5883Config_v5 = {
            .intTag = IO_TAG(MAG_INT_EXTI)
    };
    if (hardwareRevision < NAZE32_REV5) {
        hmc5883Config = &nazeHmc5883Config_v1_v4;
    } else {
        hmc5883Config = &nazeHmc5883Config_v5;
    }
#endif

#ifdef MAG_INT_EXTI
    static const hmc5883Config_t extiHmc5883Config = {
        .intTag = IO_TAG(MAG_INT_EXTI)
    };

    hmc5883Config = &extiHmc5883Config;
#endif

#endif

retry:

    magAlign = ALIGN_DEFAULT;

    switch(magHardwareToUse) {
        case MAG_DEFAULT:
            ; // fallthrough

        case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
            if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
                magAlign = MAG_HMC5883_ALIGN;
#endif
                magHardware = MAG_HMC5883;
                break;
            }
#endif
            ; // fallthrough

        case MAG_AK8975:
#ifdef USE_MAG_AK8975
            if (ak8975Detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
                magAlign = MAG_AK8975_ALIGN;
#endif
                magHardware = MAG_AK8975;
                break;
            }
#endif
            ; // fallthrough

        case MAG_AK8963:
#ifdef USE_MAG_AK8963
            if (ak8963Detect(&mag)) {
#ifdef MAG_AK8963_ALIGN
                magAlign = MAG_AK8963_ALIGN;
#endif
                magHardware = MAG_AK8963;
                break;
            }
#endif
            ; // fallthrough

        case MAG_NONE:
            magHardware = MAG_NONE;
            break;
    }

    if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
        // Nothing was found and we have a forced sensor that isn't present.
        magHardwareToUse = MAG_DEFAULT;
        goto retry;
    }

    if (magHardware == MAG_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_MAG] = magHardware;
    sensorsSet(SENSOR_MAG);
}
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
    accelerationSensor_e accHardware;

    #ifdef USE_ACC_ADXL345
    drv_adxl345_config_t acc_params;
#endif

retry:
    accAlign = ALIGN_DEFAULT;

    switch (accHardwareToUse) {
        case ACC_DEFAULT:
            ; // fallthrough
        case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345
            acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
#ifdef NAZE
            if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
#else
            if (adxl345Detect(&acc_params, &acc)) {
#endif
#ifdef ACC_ADXL345_ALIGN
                accAlign = ACC_ADXL345_ALIGN;
#endif
                accHardware = ACC_ADXL345;
                break;
            }
#endif
            ; // fallthrough
        case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
            if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
                accAlign = ACC_LSM303DLHC_ALIGN;
#endif
                accHardware = ACC_LSM303DLHC;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
            if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
#ifdef ACC_MPU6050_ALIGN
                accAlign = ACC_MPU6050_ALIGN;
#endif
                accHardware = ACC_MPU6050;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MMA8452: // MMA8452
#ifdef USE_ACC_MMA8452
#ifdef NAZE
            // Not supported with this frequency
            if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
#else
            if (mma8452Detect(&acc)) {
#endif
#ifdef ACC_MMA8452_ALIGN
                accAlign = ACC_MMA8452_ALIGN;
#endif
                accHardware = ACC_MMA8452;
                break;
            }
#endif
            ; // fallthrough
        case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
            if (bma280Detect(&acc)) {
#ifdef ACC_BMA280_ALIGN
                accAlign = ACC_BMA280_ALIGN;
#endif
                accHardware = ACC_BMA280;
                break;
            }
#endif
            ; // fallthrough
        case ACC_SPI_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
            if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_SPI_MPU6000_ALIGN
                accAlign = ACC_SPI_MPU6000_ALIGN;
#endif
                accHardware = ACC_SPI_MPU6000;
                break;
            }
#endif
            ; // fallthrough
        case ACC_SPI_MPU6500:
#ifdef USE_ACC_SPI_MPU6500
#ifdef NAZE
            if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
#else
            if (mpu6500SpiAccDetect(&acc)) {
#endif
#ifdef ACC_SPI_MPU6500_ALIGN
                accAlign = ACC_SPI_MPU6500_ALIGN;
#endif
                accHardware = ACC_SPI_MPU6500;
                break;
            }
#endif
            ; // fallthrough
        case ACC_FAKE:
#ifdef USE_FAKE_ACC
            if (fakeAccDetect(&acc)) {
                accHardware = ACC_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case ACC_NONE: // disable ACC
            accHardware = ACC_NONE;
            break;

    }

    // Found anything? Check if error or ACC is really missing.
    if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
        // Nothing was found and we have a forced sensor that isn't present.
        accHardwareToUse = ACC_DEFAULT;
        goto retry;
    }


    if (accHardware == ACC_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
    sensorsSet(SENSOR_ACC);
}

static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifdef BARO
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#ifdef USE_BARO_BMP085

    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
            .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
            .xclrGpioPin = BARO_XCLR_PIN,
            .xclrGpioPort = BARO_XCLR_GPIO,
            .eocGpioPin = BARO_EOC_PIN,
            .eocGpioPort = BARO_EOC_GPIO
    };
    bmp085Config = &defaultBMP085Config;
#endif

#ifdef NAZE
    if (hardwareRevision == NAZE32) {
        bmp085Disable(bmp085Config);
    }
#endif

#endif

    switch (baroHardware) {
        case BARO_DEFAULT:
            ; // fallthough

        case BARO_MS5611:
#ifdef USE_BARO_MS5611
            if (ms5611Detect(&baro)) {
                baroHardware = BARO_MS5611;
                break;
            }
#endif
            ; // fallthough
        case BARO_BMP085:
#ifdef USE_BARO_BMP085
            if (bmp085Detect(bmp085Config, &baro)) {
                baroHardware = BARO_BMP085;
                break;
            }
#endif
        case BARO_NONE:
            baroHardware = BARO_NONE;
            break;
    }

    if (baroHardware == BARO_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
#endif
}

static void detectMag(magSensor_e magHardwareToUse)
{
    magSensor_e magHardware;

#ifdef USE_MAG_HMC5883
    const hmc5883Config_t *hmc5883Config = 0;

#ifdef NAZE
    static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
            .gpioPin = Pin_12,
            .gpioPort = GPIOB,

            /* Disabled for v4 needs more work.
            .exti_port_source = GPIO_PortSourceGPIOB,
            .exti_pin_source = GPIO_PinSource12,
            .exti_line = EXTI_Line12,
            .exti_irqn = EXTI15_10_IRQn
            */
    };
    static const hmc5883Config_t nazeHmc5883Config_v5 = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
            .gpioPin = Pin_14,
            .gpioPort = GPIOC,
            .exti_port_source = GPIO_PortSourceGPIOC,
            .exti_line = EXTI_Line14,
            .exti_pin_source = GPIO_PinSource14,
            .exti_irqn = EXTI15_10_IRQn
    };
    if (hardwareRevision < NAZE32_REV5) {
        hmc5883Config = &nazeHmc5883Config_v1_v4;
    } else {
        hmc5883Config = &nazeHmc5883Config_v5;
    }
#endif

#ifdef SPRACINGF3
    static const hmc5883Config_t spRacingF3Hmc5883Config = {
        .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
        .gpioPin = Pin_14,
        .gpioPort = GPIOC,
        .exti_port_source = EXTI_PortSourceGPIOC,
        .exti_pin_source = EXTI_PinSource14,
        .exti_line = EXTI_Line14,
        .exti_irqn = EXTI15_10_IRQn
    };

    hmc5883Config = &spRacingF3Hmc5883Config;
#endif

#endif

retry:

    magAlign = ALIGN_DEFAULT;

    switch(magHardwareToUse) {
        case MAG_DEFAULT:
            ; // fallthrough

        case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
            if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
                magAlign = MAG_HMC5883_ALIGN;
#endif
                magHardware = MAG_HMC5883;
                break;
            }
#endif
            ; // fallthrough

        case MAG_AK8975:
#ifdef USE_MAG_AK8975
            if (ak8975detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
                magAlign = MAG_AK8975_ALIGN;
#endif
                magHardware = MAG_AK8975;
                break;
            }
#endif
            ; // fallthrough

        case MAG_NONE:
            magHardware = MAG_NONE;
            break;
    }

    if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
        // Nothing was found and we have a forced sensor that isn't present.
        magHardwareToUse = MAG_DEFAULT;
        goto retry;
    }

    if (magHardware == MAG_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_MAG] = magHardware;
    sensorsSet(SENSOR_MAG);
}

void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
{
    if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
        gyroAlign = sensorAlignmentConfig->gyro_align;
    }
    if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
        accAlign = sensorAlignmentConfig->acc_align;
    }
    if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
        magAlign = sensorAlignmentConfig->mag_align;
    }
}

bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
{
    int16_t deg, min;

    memset(&acc, 0, sizeof(acc));
    memset(&gyro, 0, sizeof(gyro));

    if (!detectGyro(gyroLpf)) {
        return false;
    }
    detectAcc(accHardwareToUse);
    detectBaro(baroHardwareToUse);


    // Now time to init things, acc first
    if (sensors(SENSOR_ACC))
        acc.init();
    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    gyro.init();

    detectMag(magHardwareToUse);

    reconfigureAlignment(sensorAlignmentConfig);

    // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
    if (sensors(SENSOR_MAG)) {
        // calculate magnetic declination
        deg = magDeclinationFromConfig / 100;
        min = magDeclinationFromConfig % 100;

        magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
    } else {
        magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
    }

    return true;
}
Esempio n. 7
0
void SensorDetectAndINI(void)                                     // "enabledSensors" is "0" in config.c so all sensors disabled per default
{
    int16_t deg, min;
    uint8_t sig          = 0;
    bool    ack          = false;
    bool    haveMpu6k    = false;

    GyroScale16 = (16.0f / 16.4f) * RADX;                         // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
    if (mpu6050Detect(&acc, &gyro))                               // Autodetect gyro hardware. We have MPU3050 or MPU6050.
    {
        haveMpu6k = true;                                         // this filled up  acc.* struct with init values
    }
    else if (l3g4200dDetect(&gyro))
    {
        havel3g4200d = true;
        GyroScale16 = (16.0f / 14.2857f) * RADX;                  // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
    }
    else if (!mpu3050Detect(&gyro))
    {
        failureMode(3);                                           // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
    }

    sensorsSet(SENSOR_ACC);                                       // ACC part. Will be cleared if not available
retry:
    switch (cfg.acc_hdw)
    {
    case 0:                                                       // autodetect
    case 1:                                                       // ADXL345
        if (adxl345Detect(&acc)) accHardware = ACC_ADXL345;
        if (cfg.acc_hdw == ACC_ADXL345) break;
    case 2:                                                       // MPU6050
        if (haveMpu6k)
        {
            mpu6050Detect(&acc, &gyro);                           // yes, i'm rerunning it again.  re-fill acc struct
            accHardware = ACC_MPU6050;
            if (cfg.acc_hdw == ACC_MPU6050) break;
        }
    case 3:                                                       // MMA8452
        if (mma8452Detect(&acc))
        {
            accHardware = ACC_MMA8452;
            if (cfg.acc_hdw == ACC_MMA8452) break;
        }
    }

    if (accHardware == ACC_DEFAULT)                               // Found anything? Check if user f****d up or ACC is really missing.
    {
        if (cfg.acc_hdw > ACC_DEFAULT)
        {
            cfg.acc_hdw = ACC_DEFAULT;                            // Nothing was found and we have a forced sensor type. User probably chose a sensor that isn't present.
            goto retry;
        }
        else sensorsClear(SENSOR_ACC);                            // We're really screwed
    }

    if (sensors(SENSOR_ACC)) acc.init();
    if (haveMpu6k && accHardware == ACC_MPU6050) MpuSpecial = true;
    else MpuSpecial = false;

    if (feature(FEATURE_PASS)) return;                            // Stop here we need just ACC for Vibrationmonitoring if present
    if (feature(FEATURE_GPS) && !SerialRCRX) gpsInit(cfg.gps_baudrate);// SerialRX and GPS can not coexist.
    gyro.init();                                                  // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    if (havel3g4200d) l3g4200dConfig();
    else if (!haveMpu6k) mpu3050Config();
    Gyro_Calibrate();                                             // Do Gyrocalibration here (is blocking), provides nice Warmuptime for the following rest!
#ifdef MAG
    if (hmc5883lDetect())
    {
        sensorsSet(SENSOR_MAG);
        hmc5883lInit(magCal);                                     // Crashpilot: Calculate Gains / Scale
        deg = cfg.mag_dec / 100;                                  // calculate magnetic declination
        min = cfg.mag_dec % 100;
        magneticDeclination = ((float)deg + ((float)min / 60.0f));// heading is in decimaldeg units NO 0.1 deg shit here
    }
#endif
#ifdef BARO                                                       // No delay necessary since Gyrocal blocked a lot already
    ack = i2cRead(0x77, 0x77, 1, &sig);                           // Check Baroadr.(MS & BMP) BMP will say hello here, MS not
    if ( ack) ack = bmp085Detect(&baro);                          // Are we really dealing with BMP?
    if (!ack) ack = ms5611Detect(&baro);                          // No, Check for MS Baro
    if (ack) sensorsSet(SENSOR_BARO);
    if(cfg.esc_nfly) ESCnoFlyThrottle = constrain_int(cfg.esc_nfly, cfg.esc_min, cfg.esc_max); // Set the ESC PWM signal threshold for not flyable RPM
    else ESCnoFlyThrottle = cfg.esc_min + (((cfg.esc_max - cfg.esc_min) * 5) / 100); // If not configured, take 5% above esc_min
#endif
#ifdef SONAR
    if (feature(FEATURE_SONAR)) Sonar_init();                     // Initialize Sonars here depending on Rc configuration.
    SonarLandWanted = cfg.snr_land;                               // Variable may be overwritten by failsave
#endif
    MainDptCut = RCconstPI / (float)cfg.maincuthz;                // Initialize Cut off frequencies for mainpid D
}
Esempio n. 8
0
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
    accelerationSensor_e accHardware;

#ifdef USE_ACC_ADXL345
    drv_adxl345_config_t acc_params;
#endif

retry:
    accAlign = ALIGN_DEFAULT;

    switch (accHardwareToUse) {
        case ACC_DEFAULT:
            ; // fallthrough
        case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345
            acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
#ifdef NAZE
            if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
#else
            if (adxl345Detect(&acc_params, &acc)) {
#endif
#ifdef ACC_ADXL345_ALIGN
                accAlign = ACC_ADXL345_ALIGN;
#endif
                accHardware = ACC_ADXL345;
                break;
            }
#endif
            ; // fallthrough
        case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
            if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
                accAlign = ACC_LSM303DLHC_ALIGN;
#endif
                accHardware = ACC_LSM303DLHC;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
            if (mpu6050AccDetect(&acc)) {
#ifdef ACC_MPU6050_ALIGN
                accAlign = ACC_MPU6050_ALIGN;
#endif
                accHardware = ACC_MPU6050;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MMA8452: // MMA8452
#ifdef USE_ACC_MMA8452
#ifdef NAZE
            // Not supported with this frequency
            if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
#else
            if (mma8452Detect(&acc)) {
#endif
#ifdef ACC_MMA8452_ALIGN
                accAlign = ACC_MMA8452_ALIGN;
#endif
                accHardware = ACC_MMA8452;
                break;
            }
#endif
            ; // fallthrough
        case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
            if (bma280Detect(&acc)) {
#ifdef ACC_BMA280_ALIGN
                accAlign = ACC_BMA280_ALIGN;
#endif
                accHardware = ACC_BMA280;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
            if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_MPU6000_ALIGN
                accAlign = ACC_MPU6000_ALIGN;
#endif
                accHardware = ACC_MPU6000;
                break;
            }
#endif
            ; // fallthrough
        case ACC_MPU6500:
#ifdef USE_ACC_MPU6500
#ifdef USE_ACC_SPI_MPU6500
            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
#else
            if (mpu6500AccDetect(&acc))
#endif
            {
#ifdef ACC_MPU6500_ALIGN
                accAlign = ACC_MPU6500_ALIGN;
#endif
                accHardware = ACC_MPU6500;
                break;
            }
#endif
            ; // fallthrough
        case ACC_FAKE:
#ifdef USE_FAKE_ACC
            if (fakeAccDetect(&acc)) {
                accHardware = ACC_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case ACC_NONE: // disable ACC
            accHardware = ACC_NONE;
            break;

    }

    // Found anything? Check if error or ACC is really missing.
    if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
        // Nothing was found and we have a forced sensor that isn't present.
        accHardwareToUse = ACC_DEFAULT;
        goto retry;
    }


    if (accHardware == ACC_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
    sensorsSet(SENSOR_ACC);
}

static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifndef BARO
    UNUSED(baroHardwareToUse);
#else
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#ifdef USE_BARO_BMP085

    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
            .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
            .xclrGpioPin = BARO_XCLR_PIN,
            .xclrGpioPort = BARO_XCLR_GPIO,
            .eocGpioPin = BARO_EOC_PIN,
            .eocGpioPort = BARO_EOC_GPIO
    };
    bmp085Config = &defaultBMP085Config;
#endif

#ifdef NAZE
    if (hardwareRevision == NAZE32) {
        bmp085Disable(bmp085Config);
    }
#endif

#endif

    switch (baroHardware) {
        case BARO_DEFAULT:
            ; // fallthough

        case BARO_MS5611:
#ifdef USE_BARO_MS5611
            if (ms5611Detect(&baro)) {
                baroHardware = BARO_MS5611;
                break;
            }
#endif
            ; // fallthough
        case BARO_BMP085:
#ifdef USE_BARO_BMP085
            if (bmp085Detect(bmp085Config, &baro)) {
                baroHardware = BARO_BMP085;
                break;
            }
#endif
	    ; // fallthough
        case BARO_BMP280:
#ifdef USE_BARO_BMP280
            if (bmp280Detect(&baro)) {
                baroHardware = BARO_BMP280;
                break;
            }
#endif
            ; // fallthrough
        case BARO_FAKE:
#ifdef USE_FAKE_BARO
            if (fakeBaroDetect(&baro)) {
                baroHardware = BARO_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case BARO_NONE:
            baroHardware = BARO_NONE;
            break;
    }

    if (baroHardware == BARO_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
#endif
}

static void detectMag(magSensor_e magHardwareToUse)
{
    magSensor_e magHardware;

#ifdef USE_MAG_HMC5883
    const hmc5883Config_t *hmc5883Config = 0;

#ifdef NAZE
    static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
            .gpioPin = Pin_12,
            .gpioPort = GPIOB,

            /* Disabled for v4 needs more work.
            .exti_port_source = GPIO_PortSourceGPIOB,
            .exti_pin_source = GPIO_PinSource12,
            .exti_line = EXTI_Line12,
            .exti_irqn = EXTI15_10_IRQn
            */
    };
    static const hmc5883Config_t nazeHmc5883Config_v5 = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
            .gpioPin = Pin_14,
            .gpioPort = GPIOC,
            .exti_port_source = GPIO_PortSourceGPIOC,
            .exti_line = EXTI_Line14,
            .exti_pin_source = GPIO_PinSource14,
            .exti_irqn = EXTI15_10_IRQn
    };
    if (hardwareRevision < NAZE32_REV5) {
        hmc5883Config = &nazeHmc5883Config_v1_v4;
    } else {
        hmc5883Config = &nazeHmc5883Config_v5;
    }
#endif

#ifdef SPRACINGF3
    static const hmc5883Config_t spRacingF3Hmc5883Config = {
        .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
        .gpioPin = Pin_14,
        .gpioPort = GPIOC,
        .exti_port_source = EXTI_PortSourceGPIOC,
        .exti_pin_source = EXTI_PinSource14,
        .exti_line = EXTI_Line14,
        .exti_irqn = EXTI15_10_IRQn
    };

    hmc5883Config = &spRacingF3Hmc5883Config;
#endif

#endif

retry:

    magAlign = ALIGN_DEFAULT;

    switch(magHardwareToUse) {
        case MAG_DEFAULT:
            ; // fallthrough

        case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
            if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
                magAlign = MAG_HMC5883_ALIGN;
#endif
                magHardware = MAG_HMC5883;
                break;
            }
#endif
            ; // fallthrough

        case MAG_AK8975:
#ifdef USE_MAG_AK8975
            if (ak8975detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
                magAlign = MAG_AK8975_ALIGN;
#endif
                magHardware = MAG_AK8975;
                break;
            }
#endif
            ; // fallthrough

        case MAG_GPS:
#ifdef GPS
            if (gpsMagDetect(&mag)) {
#ifdef MAG_GPS_ALIGN
                magAlign = MAG_GPS_ALIGN;
#endif
                magHardware = MAG_GPS;
                break;
            }
#endif
            ; // fallthrough

        case MAG_MAG3110:
#ifdef USE_MAG_MAG3110
            if (mag3110detect(&mag)) {
#ifdef MAG_MAG3110_ALIGN
                magAlign = MAG_MAG3110_ALIGN;
#endif
                magHardware = MAG_MAG3110;
                break;
            }
#endif
            ; // fallthrough

        case MAG_FAKE:
#ifdef USE_FAKE_MAG
            if (fakeMagDetect(&mag)) {
                magHardware = MAG_FAKE;
                break;
            }
#endif
            ; // fallthrough

        case MAG_NONE:
            magHardware = MAG_NONE;
            break;
    }

    if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
        // Nothing was found and we have a forced sensor that isn't present.
        magHardwareToUse = MAG_DEFAULT;
        goto retry;
    }

    if (magHardware == MAG_NONE) {
        return;
    }

    detectedSensors[SENSOR_INDEX_MAG] = magHardware;
    sensorsSet(SENSOR_MAG);
}
Esempio n. 9
0
bool sensorsAutodetect(void)
{
    int16_t deg, min;
    mpu_params_t mpu_config;
    bool haveMpu = false;
#ifndef CJMCU
    drv_adxl345_config_t acc_params;
#endif

    // mpu driver parameters
    mpu_config.lpf = mcfg.gyro_lpf;
    // Autodetect Invensense acc/gyro hardware
    haveMpu = mpuDetect(&acc, &gyro, &mpu_config);

    // MPU6500 on I2C bus
    if (hse_value == 12000000 && mpu_config.deviceType == MPU_65xx_I2C)
        hw_revision = NAZE32_REV6;

#ifndef CJMCU
    if (!haveMpu) {
        // Try some other gyros or bail out if fail
        if (!l3g4200dDetect(&gyro, mcfg.gyro_lpf))
            return false;
    }
#endif

    // Accelerometer. F**k it. Let user break shit.
retry:
    switch (mcfg.acc_hardware) {
    case ACC_NONE: // disable ACC
        sensorsClear(SENSOR_ACC);
        break;
    case ACC_DEFAULT: // autodetect
#ifndef CJMCU
    case ACC_ADXL345: // ADXL345
        acc_params.useFifo = false;
        acc_params.dataRate = 800; // unused currently
        if (adxl345Detect(&acc_params, &acc))
            accHardware = ACC_ADXL345;
        if (mcfg.acc_hardware == ACC_ADXL345)
            break;
        ; // fallthrough
#endif
    case ACC_MPU6050: // MPU6050
        if (haveMpu && mpu_config.deviceType == MPU_60x0) {
            accHardware = ACC_MPU6050;
            if (mcfg.acc_hardware == ACC_MPU6050)
                break;
        }
        ; // fallthrough
#ifdef NAZE
    case ACC_MPU6500: // MPU6500
        if (haveMpu && (mpu_config.deviceType >= MPU_65xx_I2C)) {
            accHardware = ACC_MPU6500;
            if (mcfg.acc_hardware == ACC_MPU6500)
                break;
        }
        ; // fallthrough
    case ACC_MMA8452: // MMA8452
        if (mma8452Detect(&acc)) {
            accHardware = ACC_MMA8452;
            if (mcfg.acc_hardware == ACC_MMA8452)
                break;
        }
        ; // fallthrough
    case ACC_BMA280: // BMA280
        if (bma280Detect(&acc)) {
            accHardware = ACC_BMA280;
            if (mcfg.acc_hardware == ACC_BMA280)
                break;
        }
#endif
    }

    // Found anything? Check if user f****d up or ACC is really missing.
    if (accHardware == ACC_DEFAULT) {
        if (mcfg.acc_hardware > ACC_DEFAULT && mcfg.acc_hardware < ACC_NONE) {
            // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
            mcfg.acc_hardware = ACC_DEFAULT;
            goto retry;
        } else {
            // We're really screwed
            sensorsClear(SENSOR_ACC);
        }
    }

#ifdef BARO
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
    if (!bmp280Detect(&baro)) {
        if (!bmp085Detect(&baro)) {
            // ms5611 disables BMP085, and tries to initialize + check PROM crc.
            // moved 5611 init here because there have been some reports that calibration data in BMP180
            // has been "passing" ms5611 PROM crc check
            if (!ms5611Detect(&baro)) {
                // if both failed, we don't have anything
                sensorsClear(SENSOR_BARO);
            }
        }
    }
#endif

    // Now time to init things, acc first
    if (sensors(SENSOR_ACC))
        acc.init(mcfg.acc_align);
    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    gyro.init(mcfg.gyro_align);

#ifdef MAG
retryMag:
    switch (mcfg.mag_hardware) {
    case MAG_NONE: // disable MAG
        sensorsClear(SENSOR_MAG);
        break;
    case MAG_DEFAULT: // autodetect

    case MAG_HMC5883L:
        if (hmc5883lDetect(&mag)) {
            magHardware = MAG_HMC5883L;
            if (mcfg.mag_hardware == MAG_HMC5883L)
                break;
        }
        ; // fallthrough

#ifdef NAZE
    case MAG_AK8975:
        if (ak8975detect(&mag)) {
            magHardware = MAG_AK8975;
            if (mcfg.mag_hardware == MAG_AK8975)
                break;
        }
#endif
    }

    // Found anything? Check if user f****d up or mag is really missing.
    if (magHardware == MAG_DEFAULT) {
        if (mcfg.mag_hardware > MAG_DEFAULT && mcfg.mag_hardware < MAG_NONE) {
            // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
            mcfg.mag_hardware = MAG_DEFAULT;
            goto retryMag;
        } else {
            // No mag present
            sensorsClear(SENSOR_MAG);
        }
    }
#endif

    // calculate magnetic declination
    deg = cfg.mag_declination / 100;
    min = cfg.mag_declination % 100;
    if (sensors(SENSOR_MAG))
        magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
    else
        magneticDeclination = 0.0f;

    return true;
}
Esempio n. 10
0
static bool detectBaro(baroSensor_e baroHardwareToUse)
{
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#ifdef USE_BARO_BMP085

    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
            .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
            .xclrGpioPin = BARO_XCLR_PIN,
            .xclrGpioPort = BARO_XCLR_GPIO,
            .eocGpioPin = BARO_EOC_PIN,
            .eocGpioPort = BARO_EOC_GPIO
    };
    bmp085Config = &defaultBMP085Config;

#endif

    bool skipBMP085 = false;
#ifdef NAZE
    if (hardwareRevision == NAZE32) {
        bmp085Disable(bmp085Config);
    }
    if (hardwareRevision > NAZE32) {
        bmp085Config = NULL; // pins used for different purposes on the NAZE32_REV5 and later.
        skipBMP085 = true;
    }
#endif

#endif

    switch (baroHardware) {
        case BARO_DEFAULT:
            ; // fallthrough

        case BARO_BMP085: // Always test before MS5611 as some BMP180's can pass MS5611 CRC test
#ifdef USE_BARO_BMP085
            if (!skipBMP085 && bmp085Detect(bmp085Config, &baro)) {
                baroHardware = BARO_BMP085;
                break;
            }
#endif
            ; // fallthrough

        case BARO_MS5611:
#ifdef USE_BARO_MS5611
            if (ms5611Detect(&baro)) {
                baroHardware = BARO_MS5611;
                break;
            }
#endif
            ; // fallthrough

        case BARO_BMP280:
#ifdef USE_BARO_BMP280
            if (bmp280Detect(&baro)) {
                baroHardware = BARO_BMP280;
                break;
            }
#endif
            ; // fallthrough

        case BARO_NONE:
            baroHardware = BARO_NONE;
            break;
    }

    if (baroHardware == BARO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
    return true;
}
Esempio n. 11
0
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

    baroSensor_e baroHardware = baroHardwareToUse;

#if !defined(USE_BARO_BMP085) && !defined(USE_BARO_MS5611) && !defined(USE_BARO_SPI_MS5611) && !defined(USE_BARO_BMP280) && !defined(USE_BARO_SPI_BMP280)&& !defined(USE_BARO_QMP6988) && !defined(USE_BARO_SPI_QMP6988)
    UNUSED(dev);
#endif

    switch (barometerConfig()->baro_bustype) {
#ifdef USE_I2C
    case BUSTYPE_I2C:
        dev->busdev.bustype = BUSTYPE_I2C;
        dev->busdev.busdev_u.i2c.device = I2C_CFG_TO_DEV(barometerConfig()->baro_i2c_device);
        dev->busdev.busdev_u.i2c.address = barometerConfig()->baro_i2c_address;
        break;
#endif

#ifdef USE_SPI
    case BUSTYPE_SPI:
        {
            SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(barometerConfig()->baro_spi_device));
            if (!instance) {
                return false;
            }

            dev->busdev.bustype = BUSTYPE_SPI;
            spiBusSetInstance(&dev->busdev, instance);
            dev->busdev.busdev_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
        }
        break;
#endif

    default:
        return false;
    }

    switch (baroHardware) {
    case BARO_DEFAULT:
        FALLTHROUGH;

    case BARO_BMP085:
#ifdef USE_BARO_BMP085
        {
            const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
            static const bmp085Config_t defaultBMP085Config = {
                .xclrIO = IO_TAG(BARO_XCLR_PIN),
                .eocIO = IO_TAG(BARO_EOC_PIN),
            };
            bmp085Config = &defaultBMP085Config;
#endif

            if (bmp085Detect(bmp085Config, dev)) {
                baroHardware = BARO_BMP085;
                break;
            }
        }
#endif
        FALLTHROUGH;

    case BARO_MS5611:
#if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
        if (ms5611Detect(dev)) {
            baroHardware = BARO_MS5611;
            break;
        }
#endif
        FALLTHROUGH;

    case BARO_LPS:
#if defined(USE_BARO_SPI_LPS)
        if (lpsDetect(dev)) {
            baroHardware = BARO_LPS;
            break;
        }
#endif
        FALLTHROUGH;

    case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
        if (bmp280Detect(dev)) {
            baroHardware = BARO_BMP280;
            break;
        }
#endif
        FALLTHROUGH;
	
	 case BARO_QMP6988:
#if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
        if (qmp6988Detect(dev)) {
            baroHardware = BARO_QMP6988;
            break;
        }
#endif
		FALLTHROUGH;
    case BARO_NONE:
        baroHardware = BARO_NONE;
        break;
    }

    if (baroHardware == BARO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
    return true;
}
Esempio n. 12
0
// AfroFlight32 i2c sensors
void nazeDriversInit( uint8_t accHardware ) {
//    int16_t deg, min;
    drv_adxl345_config_t acc_params;
    bool haveMpu6k = false;

	uint16_t gyro_lpf = 28;
	uint8_t gyro_scale = 1;

    //Assume we always have a gyro
    sensorMask |= BOARD_SENSOR_GYRO;
    // Autodetect gyro hardware. We have MPU3050 or MPU6050.
    if (mpu6050Detect(&acc, &gyro, gyro_lpf, &gyro_scale)) {
        // this filled up  acc.* struct with init values
        haveMpu6k = true;
    } else if (l3g4200dDetect(&gyro, gyro_lpf)) {
        // well, we found our gyro
        ;
    } else if (!mpu3050Detect(&gyro, gyro_lpf)) {
        // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
    	boardFault( BOARD_FAULT_FATAL );
    }

    // Accelerometer. F**k it. Let user break shit.
retryAcc:
    switch (accHardware) {
        case 0: // autodetect
        case 1: // MPU6050
            if (haveMpu6k) {
            	//acc struct already filled from previous gyro detection

                // PB13 - MPU_INT output on rev4 hardware
                GPIO_InitTypeDef GPIO_InitStructure;
                GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
                GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
                GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
                GPIO_Init(GPIOB, &GPIO_InitStructure);

                goto validAcc;
            }
        case 2: // ADXL345
        	acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
            if (adxl345Detect(&acc_params, &acc) )
                goto validAcc;
            //Fallthrough to the next one
        case 3: // MMA8452
            if (mma8452Detect(&acc)) {
            	//Some gpio magic to trigger an init

                GPIO_InitTypeDef GPIO_InitStructure;

                // PA5 - ACC_INT2 output on rev3/4 hardware
                // OLIMEXINO - The PA5 pin is wired up to LED1, if you need to use an mma8452 on an Olimexino use a different pin and provide support in code.
                GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
                GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
                GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
                GPIO_Init(GPIOA, &GPIO_InitStructure);

                goto validAcc;
            }
        default:
        	//nothing found, seems there's no ACC
        	goto skipAcc;
    }
    accHardware++;
    goto retryAcc;
validAcc:
	sensorMask |= BOARD_SENSOR_ACC;
	//Found a valid acc, init it
	acc.init();
skipAcc:

#ifdef BARO

	 GPIO_InitTypeDef GPIO_InitStructure;

	 // PC13 (BMP085's XCLR reset input, which we use to disable it)
	 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
	 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
	 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	 GPIO_Init(GPIOC, &GPIO_InitStructure);
	 BMP085_OFF;

    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
	// ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
	if ( ms5611Detect(&baro) || bmp085Detect(&baro) ) {


		sensorMask |= BOARD_SENSOR_BARO;
    }
#endif
    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    gyro.init();

    if ( hmc5883lDetect() ) {
    	sensorMask |= BOARD_SENSOR_MAG;
    }

    // calculate magnetic declination
//    deg = cfg.mag_declination / 100;
//    min = cfg.mag_declination % 100;
//    magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
}