static bool gyroDetectSensor(gyroSensor_t *gyroSensor, 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) || defined(USE_GYRO_L3GD20) if (!mpuDetect(&gyroSensor->gyroDev, config)) { return false; } #else UNUSED(config); #endif const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); gyroSensor->gyroDev.gyroHardware = gyroHardware; return gyroHardware != GYRO_NONE; }
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; }
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; }