Пример #1
0
void sensorsInit(void)
{
    zeroSensorAccumulators();

    // TODO allow user to select hardware if there are multiple choices

    if(mpu6050Detect(&gyro, &accel, cfg.mpu6050Scale)) {
        sensorsSet(SENSOR_ACC);
    } else if(!mpu3050Detect(&gyro)) {
        failureMode(3);
    }

    if(adxl345Detect(&accel)) {
        sensorsSet(SENSOR_ACC);
    }

    // At the moment we will do this after mpu6050 and overrride mpu6050 accel if detected
    if(mma8452Detect(&accel)) {
        sensorsSet(SENSOR_ACC);
    }

    gyro.init();

    if(sensorsGet(SENSOR_ACC))
        accel.init();

    if(hmc5883Detect(&mag)) {
        mag.init();
        sensorsSet(SENSOR_MAG);
    }

#ifdef SONAR
    if(feature(FEATURE_PPM);) {
Пример #2
0
bool detectGyro(uint16_t gyroLpf)
{
    gyroAlign = ALIGN_DEFAULT;

#ifdef USE_GYRO_MPU6050
    if (mpu6050GyroDetect(&gyro, gyroLpf)) {
#ifdef NAZE
        gyroAlign = CW0_DEG;
#endif
        return true;
    }
#endif

#ifdef USE_GYRO_L3G4200D
    if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef NAZE
        gyroAlign = CW0_DEG;
#endif
        return true;
    }
#endif

#ifdef USE_GYRO_MPU3050
    if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef NAZE
        gyroAlign = CW0_DEG;
#endif
        return true;
    }
#endif

#ifdef USE_GYRO_L3GD20
    if (l3gd20Detect(&gyro, gyroLpf)) {
        return true;
    }
#endif

#ifdef USE_GYRO_SPI_MPU6000
    if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef CC3D
        gyroAlign = CW270_DEG;
#endif
        return true;
    }
#endif

#ifdef USE_FAKE_GYRO
    if (fakeGyroDetect(&gyro, gyroLpf)) {
        return true;
    }
#endif
    return false;
}
Пример #3
0
STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
{
    gyroHardware_e gyroHardware = GYRO_DEFAULT;

    switch (gyroHardware) {
    case GYRO_DEFAULT:
        FALLTHROUGH;

#ifdef USE_GYRO_MPU6050
    case GYRO_MPU6050:
        if (mpu6050GyroDetect(dev)) {
            gyroHardware = GYRO_MPU6050;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_L3G4200D
    case GYRO_L3G4200D:
        if (l3g4200dDetect(dev)) {
            gyroHardware = GYRO_L3G4200D;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_MPU3050
    case GYRO_MPU3050:
        if (mpu3050Detect(dev)) {
            gyroHardware = GYRO_MPU3050;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_L3GD20
    case GYRO_L3GD20:
        if (l3gd20GyroDetect(dev)) {
            gyroHardware = GYRO_L3GD20;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_MPU6000
    case GYRO_MPU6000:
        if (mpu6000SpiGyroDetect(dev)) {
            gyroHardware = GYRO_MPU6000;
            break;
        }
        FALLTHROUGH;
#endif

#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
    case GYRO_MPU6500:
    case GYRO_ICM20601:
    case GYRO_ICM20602:
    case GYRO_ICM20608G:
#ifdef USE_GYRO_SPI_MPU6500
        if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
#else
        if (mpu6500GyroDetect(dev)) {
#endif
            switch (dev->mpuDetectionResult.sensor) {
            case MPU_9250_SPI:
                gyroHardware = GYRO_MPU9250;
                break;
            case ICM_20601_SPI:
                gyroHardware = GYRO_ICM20601;
                break;
            case ICM_20602_SPI:
                gyroHardware = GYRO_ICM20602;
                break;
            case ICM_20608_SPI:
                gyroHardware = GYRO_ICM20608G;
                break;
            default:
                gyroHardware = GYRO_MPU6500;
            }
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_MPU9250
    case GYRO_MPU9250:
        if (mpu9250SpiGyroDetect(dev)) {
            gyroHardware = GYRO_MPU9250;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_ICM20649
    case GYRO_ICM20649:
        if (icm20649SpiGyroDetect(dev)) {
            gyroHardware = GYRO_ICM20649;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_ICM20689
    case GYRO_ICM20689:
        if (icm20689SpiGyroDetect(dev)) {
            gyroHardware = GYRO_ICM20689;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_ACCGYRO_BMI160
    case GYRO_BMI160:
        if (bmi160SpiGyroDetect(dev)) {
            gyroHardware = GYRO_BMI160;
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_FAKE_GYRO
    case GYRO_FAKE:
        if (fakeGyroDetect(dev)) {
            gyroHardware = GYRO_FAKE;
            break;
        }
        FALLTHROUGH;
#endif

    default:
        gyroHardware = GYRO_NONE;
    }

    if (gyroHardware != GYRO_NONE) {
        sensorsSet(SENSOR_GYRO);
    }


    return gyroHardware;
}

static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
    mpuPreInit(config);
#else
    UNUSED(config);
#endif
}
Пример #4
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;
}
Пример #5
0
bool detectGyro(void)
{
    bool sensorDetected;
    UNUSED(sensorDetected); // avoid unused-variable warning on some targets.

    gyroSensor_e gyroHardware = GYRO_DEFAULT;
    gyroAlign = ALIGN_DEFAULT;


    switch(gyroHardware) {
        case GYRO_DEFAULT:
            ; // fallthrough
        case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
            if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
                gyroHardware = GYRO_MPU6050;
                gyroAlign = GYRO_MPU6050_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough
        case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
            if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
                gyroHardware = GYRO_L3G4200D;
                gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
            if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
                gyroHardware = GYRO_MPU3050;
                gyroAlign = GYRO_MPU3050_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
            if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
                gyroHardware = GYRO_L3GD20;
                gyroAlign = GYRO_L3GD20_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
            if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
                gyroHardware = GYRO_MPU6000;
                gyroAlign = GYRO_MPU6000_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU6500:
#ifdef USE_GYRO_MPU6500
#ifdef USE_GYRO_SPI_MPU6500
            sensorDetected = mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro);
#else
            sensorDetected = mpu6500GyroDetect(&gyro);
#endif
            if (sensorDetected) {
                gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
                gyroAlign = GYRO_MPU6500_ALIGN;
#endif

                break;
            }
#endif
            ; // fallthrough

        case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
            if (fakeGyroDetect(&gyro)) {
                gyroHardware = GYRO_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case GYRO_NONE:
            gyroHardware = GYRO_NONE;
    }

    if (gyroHardware == GYRO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
    sensorsSet(SENSOR_GYRO);

    return true;
}
Пример #6
0
bool detectGyro(void)
{
    gyroSensor_e gyroHardware = GYRO_DEFAULT;

    gyroAlign = ALIGN_DEFAULT;

    switch(gyroHardware) {
        case GYRO_DEFAULT:
            ; // fallthrough
        case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
            if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
                gyroHardware = GYRO_MPU6050;
                gyroAlign = GYRO_MPU6050_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough
        case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
            if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
                gyroHardware = GYRO_L3G4200D;
                gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
            if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
                gyroHardware = GYRO_MPU3050;
                gyroAlign = GYRO_MPU3050_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
            if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
                gyroHardware = GYRO_L3GD20;
                gyroAlign = GYRO_L3GD20_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
            if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
                gyroHardware = GYRO_MPU6000;
                gyroAlign = GYRO_MPU6000_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
            if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
#else
            if (mpu6500GyroDetect(&gyro))
#endif
            {
                gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
                gyroAlign = GYRO_MPU6500_ALIGN;
#endif

                break;
            }
#endif
            ; // fallthrough

    case GYRO_MPU9250:
#ifdef USE_GYRO_SPI_MPU9250

        if (mpu9250SpiGyroDetect(&gyro))
        {
            gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN
            gyroAlign = GYRO_MPU9250_ALIGN;
#endif

            break;
        }
#endif
        ; // fallthrough
        case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
            if (fakeGyroDetect(&gyro)) {
                gyroHardware = GYRO_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case GYRO_NONE:
            gyroHardware = GYRO_NONE;
    }

    if (gyroHardware == GYRO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
    sensorsSet(SENSOR_GYRO);

    return true;
}
Пример #7
0
bool detectGyro(uint16_t gyroLpf)
{
    gyroSensor_e gyroHardware = GYRO_DEFAULT;

    gyroAlign = ALIGN_DEFAULT;

    switch(gyroHardware) {
        case GYRO_DEFAULT:
            ; // fallthrough
        case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
            if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN
                gyroHardware = GYRO_MPU6050;
                gyroAlign = GYRO_MPU6050_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough
        case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
            if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef GYRO_L3G4200D_ALIGN
                gyroHardware = GYRO_L3G4200D;
                gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
            if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU3050_ALIGN
                gyroHardware = GYRO_MPU3050;
                gyroAlign = GYRO_MPU3050_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
            if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_L3GD20_ALIGN
                gyroHardware = GYRO_L3GD20;
                gyroAlign = GYRO_L3GD20_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_SPI_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
            if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
                gyroHardware = GYRO_SPI_MPU6000;
                gyroAlign = GYRO_SPI_MPU6000_ALIGN;
#endif
                break;
            }
#endif
            ; // fallthrough

        case GYRO_SPI_MPU6500:
#ifdef USE_GYRO_SPI_MPU6500
#ifdef USE_HARDWARE_REVISION_DETECTION
		  spiBusInit();
#endif
#ifdef NAZE
            if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
                gyroHardware = GYRO_SPI_MPU6500;
                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
                break;
            }
#else
            if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
                gyroHardware = GYRO_SPI_MPU6500;
                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
                break;
            }
#endif
#endif
            ; // fallthrough

        case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
            if (fakeGyroDetect(&gyro, gyroLpf)) {
                gyroHardware = GYRO_FAKE;
                break;
            }
#endif
            ; // fallthrough
        case GYRO_NONE:
            gyroHardware = GYRO_NONE;
    }

    if (gyroHardware == GYRO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
    sensorsSet(SENSOR_GYRO);

    return true;
}
Пример #8
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
}
Пример #9
0
Файл: gyro.c Проект: oleost/inav
static bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig)
{
    dev->mpuIntExtiConfig =  extiConfig;

    gyroSensor_e gyroHardware = GYRO_AUTODETECT;

    dev->gyroAlign = ALIGN_DEFAULT;

    switch(gyroHardware) {
    case GYRO_AUTODETECT:
        ; // fallthrough
    case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
        if (mpu6050GyroDetect(dev)) {
            gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
            dev->gyroAlign = GYRO_MPU6050_ALIGN;
#endif
            break;
        }
#endif
        ; // fallthrough
    case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
        if (l3g4200dDetect(dev)) {
            gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN
            dev->gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
            break;
        }
#endif
        ; // fallthrough

    case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
        if (mpu3050Detect(dev)) {
            gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN
            dev->gyroAlign = GYRO_MPU3050_ALIGN;
#endif
            break;
        }
#endif
        ; // fallthrough

    case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
        if (l3gd20Detect(dev)) {
            gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN
            dev->gyroAlign = GYRO_L3GD20_ALIGN;
#endif
            break;
        }
#endif
        ; // fallthrough

    case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
        if (mpu6000SpiGyroDetect(dev)) {
            gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN
            dev->gyroAlign = GYRO_MPU6000_ALIGN;
#endif
            break;
        }
#endif
        ; // fallthrough

    case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
        if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
#else
        if (mpu6500GyroDetect(dev)) {
#endif
            gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
            dev->gyroAlign = GYRO_MPU6500_ALIGN;
#endif

            break;
        }
#endif
        ; // fallthrough

    case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
        if (fakeGyroDetect(dev)) {
            gyroHardware = GYRO_FAKE;
            break;
        }
#endif
        ; // fallthrough
    case GYRO_NONE:
        gyroHardware = GYRO_NONE;
    }

    addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0);

    if (gyroHardware == GYRO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
    sensorsSet(SENSOR_GYRO);

    return true;
}

bool gyroInit(const gyroConfig_t *gyroConfigToUse)
{
    gyroConfig = gyroConfigToUse;
    memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
    const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
    mpuDetect(&gyro.dev);
    mpuReset = gyro.dev.mpuConfiguration.reset;
#endif

    if (!gyroDetect(&gyro.dev, extiConfig)) {
        return false;
    }
    // After refactoring this function is always called after gyro sampling rate is known, so
    // no additional condition is required
    // Set gyro sample rate before driver initialisation
    gyro.dev.lpf = gyroConfig->gyro_lpf;
    gyro.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
    // driver initialisation
    gyro.dev.init(&gyro.dev);
    if (gyroConfig->gyro_soft_lpf_hz) {
        for (int axis = 0; axis < 3; axis++) {
        #ifdef ASYNC_GYRO_PROCESSING
            biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, getGyroUpdateRate());
        #else
            biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
        #endif
        }
    }
    return true;
}
Пример #10
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
}