bool sensorsAutodetect(void) { memset(&acc, 0, sizeof(acc)); 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(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { return false; } detectAcc(sensorSelectionConfig()->acc_hardware); detectBaro(sensorSelectionConfig()->baro_hardware); // 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(gyroConfig()->gyro_lpf); #ifdef MAG detectMag(sensorSelectionConfig()->mag_hardware); #endif reconfigureAlignment(sensorAlignmentConfig()); return true; }
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime, uint8_t gyroSync, uint8_t gyroSyncDenominator) { int16_t deg, min; #ifndef MAG UNUSED(magHardwareToUse); #endif memset(&acc, 0, sizeof(acc)); 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(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { 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. gyroUpdateSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator); // Set gyro sampling rate divider before initialization gyro.init(gyroLpf); #ifdef MAG detectMag(magHardwareToUse); #endif 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; }
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_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 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(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { return false; } detectAcc(accHardwareToUse); detectBaro(baroHardwareToUse); // Now time to init things, acc first if (sensors(SENSOR_ACC)) acc.init(&acc); gyro.init(gyroLpf); 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; }
bool sensorsAutodetect(void) { memset(&acc, 0, sizeof(acc)); 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(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { return false; } gyro.sampleFrequencyHz = gyroConfig()->gyro_sample_hz; // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(&gyro, gyroConfig()->gyro_lpf); gyroInit(); // the combination of LPF and GYRO_SAMPLE_HZ may be invalid for the gyro, update the configuration to use the sample frequency that was determined for the desired LPF. gyroConfig()->gyro_sample_hz = gyro.sampleFrequencyHz; if (detectAcc(sensorSelectionConfig()->acc_hardware)) { acc.acc_1G = 256; // set default acc.init(&acc); } #ifdef BARO detectBaro(sensorSelectionConfig()->baro_hardware); #endif #ifdef MAG if (detectMag(sensorSelectionConfig()->mag_hardware)) { if (!compassInit()) { sensorsClear(SENSOR_MAG); } } #endif reconfigureAlignment(sensorAlignmentConfig()); return true; }
bool sensorsAutodetect(void) { memset(&acc, 0, sizeof(acc)); 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(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { return false; } // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(gyroConfig()->gyro_lpf); if (detectAcc(sensorSelectionConfig()->acc_hardware)) { acc.acc_1G = 256; // set default acc.init(&acc); } #ifdef BARO detectBaro(sensorSelectionConfig()->baro_hardware); #endif #ifdef MAG if (detectMag(sensorSelectionConfig()->mag_hardware)) { if (!compassInit()) { sensorsClear(SENSOR_MAG); } } #endif reconfigureAlignment(sensorAlignmentConfig()); 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; }