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