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