Beispiel #1
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
}
Beispiel #2
0
// AfroFlight32 i2c sensors
void sensorsAutodetect(void)
{
    int16_t deg, min;
    drv_adxl345_config_t acc_params;
    bool haveMpu6k = false;
    bool havel3g4200d = false;

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

    // Accelerometer. F**k it. Let user break shit.
retry:
    switch (cfg.acc_hardware) {
        case 0: // autodetect
        case 1: // ADXL345
            acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
            if (adxl345Detect(&acc_params, &acc))
                accHardware = ACC_ADXL345;
            if (cfg.acc_hardware == ACC_ADXL345)
                break;
            ; // fallthrough
       case 2: // MPU6050
            if (haveMpu6k) {
                mpu6050Detect(&acc, &gyro, cfg.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
                accHardware = ACC_MPU6050;
                if (cfg.acc_hardware == ACC_MPU6050)
                    break;
            }
            ; // fallthrough
        case 3: // MMA8452
            if (mma8452Detect(&acc)) {
                accHardware = ACC_MMA8452;
                if (cfg.acc_hardware == ACC_MMA8452)
                    break;
            }
    }

    // Found anything? Check if user f****d up or ACC is really missing.
    if (accHardware == ACC_DEFAULT) {
        if (cfg.acc_hardware > ACC_DEFAULT) {
            // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
            cfg.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 (!ms5611Detect(&baro)) {
        // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
        if (!bmp085Detect(&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();
    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    gyro.init();

    // todo: this is driver specific :(
    if (havel3g4200d) {
        l3g4200dConfig(cfg.gyro_lpf);
    } else {
        if (!haveMpu6k)
            mpu3050Config(cfg.gyro_lpf);
    }

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

    // 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
}