src/main/sensors/initialisation.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17 #include <stdbool.h>
  18 #include <stdint.h>
  19 #include <string.h>
  20
  21 #include "platform.h"
  22
  23 #include "build_config.h"
  24
  25 #include "common/axis.h"
  26
  27 #include "drivers/gpio.h"
  28 #include "drivers/system.h"
  29 #include "drivers/exti.h"
  30
  31 #include "drivers/sensor.h"
  32
  33 #include "drivers/accgyro.h"
  34 #include "drivers/accgyro_adxl345.h"
  35 #include "drivers/accgyro_bma280.h"
  36 #include "drivers/accgyro_l3g4200d.h"
  37 #include "drivers/accgyro_mma845x.h"
  38 #include "drivers/accgyro_mpu.h"
  39 #include "drivers/accgyro_mpu3050.h"
  40 #include "drivers/accgyro_mpu6050.h"
  41 #include "drivers/accgyro_mpu6500.h"
  42 #include "drivers/accgyro_l3gd20.h"
  43 #include "drivers/accgyro_lsm303dlhc.h"
  44
  45 #include "drivers/bus_spi.h"
  46 #include "drivers/accgyro_spi_mpu6000.h"
  47 #include "drivers/accgyro_spi_mpu6500.h"
  48 #include "drivers/gyro_sync.h"
  49
  50 #include "drivers/barometer.h"
  51 #include "drivers/barometer_bmp085.h"
  52 #include "drivers/barometer_bmp280.h"
  53 #include "drivers/barometer_ms5611.h"
  54
  55 #include "drivers/compass.h"
  56 #include "drivers/compass_hmc5883l.h"
  57 #include "drivers/compass_ak8975.h"
  58 #include "drivers/compass_mag3110.h"
  59
  60 #include "drivers/sonar_hcsr04.h"
  61 #include "drivers/sonar_srf10.h"
  62
  63 #include "config/runtime_config.h"
  64 #include "config/config.h"
  65
  66 #include "sensors/sensors.h"
  67 #include "sensors/acceleration.h"
  68 #include "sensors/barometer.h"
  69 #include "sensors/gyro.h"
  70 #include "sensors/compass.h"
  71 #include "sensors/rangefinder.h"
  72 #include "sensors/initialisation.h"
  73
  74 #ifdef NAZE
  75 #include "hardware_revision.h"
  76 #endif
  77
  78 #ifdef GPS
  79 extern bool gpsMagDetect(mag_t *mag);
  80 #endif
  81
  82 extern float magneticDeclination;
  83
  84 extern gyro_t gyro;
  85 extern baro_t baro;
  86 extern acc_t acc;
  87
  88 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
  89
  90
  91 const extiConfig_t *selectMPUIntExtiConfig(void)
  92 {
  93 #ifdef NAZE
  94     // MPU_INT output on rev4 PB13
  95     static const extiConfig_t nazeRev4MPUIntExtiConfig = {
  96             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
  97             .gpioPin = Pin_13,
  98             .gpioPort = GPIOB,
  99             .exti_port_source = GPIO_PortSourceGPIOB,
 100             .exti_line = EXTI_Line13,
 101             .exti_pin_source = GPIO_PinSource13,
 102             .exti_irqn = EXTI15_10_IRQn
 103     };
 104     // MPU_INT output on rev5 hardware PC13
 105     static const extiConfig_t nazeRev5MPUIntExtiConfig = {
 106             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
 107             .gpioPin = Pin_13,
 108             .gpioPort = GPIOC,
 109             .exti_port_source = GPIO_PortSourceGPIOC,
 110             .exti_line = EXTI_Line13,
 111             .exti_pin_source = GPIO_PinSource13,
 112             .exti_irqn = EXTI15_10_IRQn
 113     };
 114
 115     if (hardwareRevision < NAZE32_REV5) {
 116         return &nazeRev4MPUIntExtiConfig;
 117     } else {
 118         return &nazeRev5MPUIntExtiConfig;
 119     }
 120 #endif
 121
 122 #if defined(SPRACINGF3)
 123     static const extiConfig_t spRacingF3MPUIntExtiConfig = {
 124             .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
 125             .gpioPort = GPIOC,
 126             .gpioPin = Pin_13,
 127             .exti_port_source = EXTI_PortSourceGPIOC,
 128             .exti_pin_source = EXTI_PinSource13,
 129             .exti_line = EXTI_Line13,
 130             .exti_irqn = EXTI15_10_IRQn
 131     };
 132     return &spRacingF3MPUIntExtiConfig;
 133 #endif
 134
 135 #if defined(CC3D)
 136     static const extiConfig_t cc3dMPUIntExtiConfig = {
 137             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOA,
 138             .gpioPort = GPIOA,
 139             .gpioPin = Pin_3,
 140             .exti_port_source = GPIO_PortSourceGPIOA,
 141             .exti_pin_source = GPIO_PinSource3,
 142             .exti_line = EXTI_Line3,
 143             .exti_irqn = EXTI3_IRQn
 144     };
 145     return &cc3dMPUIntExtiConfig;
 146 #endif
 147
 148 #ifdef MOTOLAB
 149     static const extiConfig_t MotolabF3MPUIntExtiConfig = {
 150             .gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
 151             .gpioPort = GPIOA,
 152             .gpioPin = Pin_15,
 153             .exti_port_source = EXTI_PortSourceGPIOA,
 154             .exti_pin_source = EXTI_PinSource15,
 155             .exti_line = EXTI_Line15,
 156             .exti_irqn = EXTI15_10_IRQn
 157     };
 158     return &MotolabF3MPUIntExtiConfig;
 159 #endif
 160
 161 #if defined(COLIBRI_RACE) || defined(LUX_RACE)
 162     static const extiConfig_t colibriRaceMPUIntExtiConfig = {
 163          .gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
 164          .gpioPort = GPIOA,
 165          .gpioPin = Pin_5,
 166          .exti_port_source = EXTI_PortSourceGPIOA,
 167          .exti_pin_source = EXTI_PinSource5,
 168          .exti_line = EXTI_Line5,
 169          .exti_irqn = EXTI9_5_IRQn
 170     };
 171     return &colibriRaceMPUIntExtiConfig;
 172 #endif
 173
 174     return NULL;
 175 }
 176
 177 #ifdef USE_FAKE_GYRO
 178 static void fakeGyroInit(uint8_t lpf)
 179 {
 180     UNUSED(lpf);
 181 }
 182
 183 static bool fakeGyroRead(int16_t *gyroADC)
 184 {
 185     memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
 186     return true;
 187 }
 188
 189 static bool fakeGyroReadTemp(int16_t *tempData)
 190 {
 191     UNUSED(tempData);
 192     return true;
 193 }
 194
 195 bool fakeGyroDetect(gyro_t *gyro)
 196 {
 197     gyro->init = fakeGyroInit;
 198     gyro->read = fakeGyroRead;
 199     gyro->temperature = fakeGyroReadTemp;
 200     return true;
 201 }
 202 #endif
 203
 204 #ifdef USE_FAKE_ACC
 205 static void fakeAccInit(acc_t *acc) {UNUSED(acc);}
 206 static bool fakeAccRead(int16_t *accData) {
 207     memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
 208     return true;
 209 }
 210
 211 bool fakeAccDetect(acc_t *acc)
 212 {
 213     acc->init = fakeAccInit;
 214     acc->read = fakeAccRead;
 215     acc->revisionCode = 0;
 216     return true;
 217 }
 218 #endif
 219
 220 #ifdef USE_FAKE_BARO
 221 static void fakeBaroStartGet(void)
 222 {
 223 }
 224
 225 static void fakeBaroCalculate(int32_t *pressure, int32_t *temperature)
 226 {
 227     if (pressure)
 228         *pressure = 101325;    // pressure in Pa (0m MSL)
 229     if (temperature)
 230         *temperature = 2500;   // temperature in 0.01 C = 25 deg
 231 }
 232
 233 bool fakeBaroDetect(baro_t *baro)
 234 {
 235     // these are dummy as temperature is measured as part of pressure
 236     baro->ut_delay = 10000;
 237     baro->get_ut = fakeBaroStartGet;
 238     baro->start_ut = fakeBaroStartGet;
 239
 240     // only _up part is executed, and gets both temperature and pressure
 241     baro->up_delay = 10000;
 242     baro->start_up = fakeBaroStartGet;
 243     baro->get_up = fakeBaroStartGet;
 244     baro->calculate = fakeBaroCalculate;
 245
 246     return true;
 247 }
 248 #endif
 249
 250 #ifdef USE_FAKE_MAG
 251 static void fakeMagInit(void)
 252 {
 253 }
 254
 255 static bool fakeMagRead(int16_t *magData)
 256 {
 257     // Always pointint North
 258     magData[X] = 4096;
 259     magData[Y] = 0;
 260     magData[Z] = 0;
 261     return true;
 262 }
 263
 264 static bool fakeMagDetect(mag_t *mag)
 265 {
 266     mag->init = fakeMagInit;
 267     mag->read = fakeMagRead;
 268
 269     return true;
 270 }
 271 #endif
 272
 273 bool detectGyro(void)
 274 {
 275     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 276
 277     gyroAlign = ALIGN_DEFAULT;
 278
 279     switch(gyroHardware) {
 280         case GYRO_DEFAULT:
 281             ; // fallthrough
 282         case GYRO_MPU6050:
 283 #ifdef USE_GYRO_MPU6050
 284             if (mpu6050GyroDetect(&gyro)) {
 285 #ifdef GYRO_MPU6050_ALIGN
 286                 gyroHardware = GYRO_MPU6050;
 287                 gyroAlign = GYRO_MPU6050_ALIGN;
 288 #endif
 289                 break;
 290             }
 291 #endif
 292             ; // fallthrough
 293         case GYRO_L3G4200D:
 294 #ifdef USE_GYRO_L3G4200D
 295             if (l3g4200dDetect(&gyro)) {
 296 #ifdef GYRO_L3G4200D_ALIGN
 297                 gyroHardware = GYRO_L3G4200D;
 298                 gyroAlign = GYRO_L3G4200D_ALIGN;
 299 #endif
 300                 break;
 301             }
 302 #endif
 303             ; // fallthrough
 304
 305         case GYRO_MPU3050:
 306 #ifdef USE_GYRO_MPU3050
 307             if (mpu3050Detect(&gyro)) {
 308 #ifdef GYRO_MPU3050_ALIGN
 309                 gyroHardware = GYRO_MPU3050;
 310                 gyroAlign = GYRO_MPU3050_ALIGN;
 311 #endif
 312                 break;
 313             }
 314 #endif
 315             ; // fallthrough
 316
 317         case GYRO_L3GD20:
 318 #ifdef USE_GYRO_L3GD20
 319             if (l3gd20Detect(&gyro)) {
 320 #ifdef GYRO_L3GD20_ALIGN
 321                 gyroHardware = GYRO_L3GD20;
 322                 gyroAlign = GYRO_L3GD20_ALIGN;
 323 #endif
 324                 break;
 325             }
 326 #endif
 327             ; // fallthrough
 328
 329         case GYRO_MPU6000:
 330 #ifdef USE_GYRO_SPI_MPU6000
 331             if (mpu6000SpiGyroDetect(&gyro)) {
 332 #ifdef GYRO_MPU6000_ALIGN
 333                 gyroHardware = GYRO_MPU6000;
 334                 gyroAlign = GYRO_MPU6000_ALIGN;
 335 #endif
 336                 break;
 337             }
 338 #endif
 339             ; // fallthrough
 340
 341         case GYRO_MPU6500:
 342 #ifdef USE_GYRO_MPU6500
 343 #ifdef USE_GYRO_SPI_MPU6500
 344             if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
 345 #else
 346             if (mpu6500GyroDetect(&gyro))
 347 #endif
 348             {
 349                 gyroHardware = GYRO_MPU6500;
 350 #ifdef GYRO_MPU6500_ALIGN
 351                 gyroAlign = GYRO_MPU6500_ALIGN;
 352 #endif
 353
 354                 break;
 355             }
 356 #endif
 357             ; // fallthrough
 358
 359         case GYRO_FAKE:
 360 #ifdef USE_FAKE_GYRO
 361             if (fakeGyroDetect(&gyro)) {
 362                 gyroHardware = GYRO_FAKE;
 363                 break;
 364             }
 365 #endif
 366             ; // fallthrough
 367         case GYRO_NONE:
 368             gyroHardware = GYRO_NONE;
 369     }
 370
 371     if (gyroHardware == GYRO_NONE) {
 372         return false;
 373     }
 374
 375     detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
 376     sensorsSet(SENSOR_GYRO);
 377
 378     return true;
 379 }
 380
 381 static void detectAcc(accelerationSensor_e accHardwareToUse)
 382 {
 383     accelerationSensor_e accHardware;
 384
 385 #ifdef USE_ACC_ADXL345
 386     drv_adxl345_config_t acc_params;
 387 #endif
 388
 389 retry:
 390     accAlign = ALIGN_DEFAULT;
 391
 392     switch (accHardwareToUse) {
 393         case ACC_DEFAULT:
 394             ; // fallthrough
 395         case ACC_ADXL345: // ADXL345
 396 #ifdef USE_ACC_ADXL345
 397             acc_params.useFifo = false;
 398             acc_params.dataRate = 800; // unused currently
 399 #ifdef NAZE
 400             if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
 401 #else
 402             if (adxl345Detect(&acc_params, &acc)) {
 403 #endif
 404 #ifdef ACC_ADXL345_ALIGN
 405                 accAlign = ACC_ADXL345_ALIGN;
 406 #endif
 407                 accHardware = ACC_ADXL345;
 408                 break;
 409             }
 410 #endif
 411             ; // fallthrough
 412         case ACC_LSM303DLHC:
 413 #ifdef USE_ACC_LSM303DLHC
 414             if (lsm303dlhcAccDetect(&acc)) {
 415 #ifdef ACC_LSM303DLHC_ALIGN
 416                 accAlign = ACC_LSM303DLHC_ALIGN;
 417 #endif
 418                 accHardware = ACC_LSM303DLHC;
 419                 break;
 420             }
 421 #endif
 422             ; // fallthrough
 423         case ACC_MPU6050: // MPU6050
 424 #ifdef USE_ACC_MPU6050
 425             if (mpu6050AccDetect(&acc)) {
 426 #ifdef ACC_MPU6050_ALIGN
 427                 accAlign = ACC_MPU6050_ALIGN;
 428 #endif
 429                 accHardware = ACC_MPU6050;
 430                 break;
 431             }
 432 #endif
 433             ; // fallthrough
 434         case ACC_MMA8452: // MMA8452
 435 #ifdef USE_ACC_MMA8452
 436 #ifdef NAZE
 437             // Not supported with this frequency
 438             if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
 439 #else
 440             if (mma8452Detect(&acc)) {
 441 #endif
 442 #ifdef ACC_MMA8452_ALIGN
 443                 accAlign = ACC_MMA8452_ALIGN;
 444 #endif
 445                 accHardware = ACC_MMA8452;
 446                 break;
 447             }
 448 #endif
 449             ; // fallthrough
 450         case ACC_BMA280: // BMA280
 451 #ifdef USE_ACC_BMA280
 452             if (bma280Detect(&acc)) {
 453 #ifdef ACC_BMA280_ALIGN
 454                 accAlign = ACC_BMA280_ALIGN;
 455 #endif
 456                 accHardware = ACC_BMA280;
 457                 break;
 458             }
 459 #endif
 460             ; // fallthrough
 461         case ACC_MPU6000:
 462 #ifdef USE_ACC_SPI_MPU6000
 463             if (mpu6000SpiAccDetect(&acc)) {
 464 #ifdef ACC_MPU6000_ALIGN
 465                 accAlign = ACC_MPU6000_ALIGN;
 466 #endif
 467                 accHardware = ACC_MPU6000;
 468                 break;
 469             }
 470 #endif
 471             ; // fallthrough
 472         case ACC_MPU6500:
 473 #ifdef USE_ACC_MPU6500
 474 #ifdef USE_ACC_SPI_MPU6500
 475             if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
 476 #else
 477             if (mpu6500AccDetect(&acc))
 478 #endif
 479             {
 480 #ifdef ACC_MPU6500_ALIGN
 481                 accAlign = ACC_MPU6500_ALIGN;
 482 #endif
 483                 accHardware = ACC_MPU6500;
 484                 break;
 485             }
 486 #endif
 487             ; // fallthrough
 488         case ACC_FAKE:
 489 #ifdef USE_FAKE_ACC
 490             if (fakeAccDetect(&acc)) {
 491                 accHardware = ACC_FAKE;
 492                 break;
 493             }
 494 #endif
 495             ; // fallthrough
 496         case ACC_NONE: // disable ACC
 497             accHardware = ACC_NONE;
 498             break;
 499
 500     }
 501
 502     // Found anything? Check if error or ACC is really missing.
 503     if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
 504         // Nothing was found and we have a forced sensor that isn't present.
 505         accHardwareToUse = ACC_DEFAULT;
 506         goto retry;
 507     }
 508
 509
 510     if (accHardware == ACC_NONE) {
 511         return;
 512     }
 513
 514     detectedSensors[SENSOR_INDEX_ACC] = accHardware;
 515     sensorsSet(SENSOR_ACC);
 516 }
 517
 518 static void detectBaro(baroSensor_e baroHardwareToUse)
 519 {
 520 #ifndef BARO
 521     UNUSED(baroHardwareToUse);
 522 #else
 523     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 524
 525     baroSensor_e baroHardware = baroHardwareToUse;
 526
 527 #ifdef USE_BARO_BMP085
 528
 529     const bmp085Config_t *bmp085Config = NULL;
 530
 531 #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
 532     static const bmp085Config_t defaultBMP085Config = {
 533             .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
 534             .xclrGpioPin = BARO_XCLR_PIN,
 535             .xclrGpioPort = BARO_XCLR_GPIO,
 536             .eocGpioPin = BARO_EOC_PIN,
 537             .eocGpioPort = BARO_EOC_GPIO
 538     };
 539     bmp085Config = &defaultBMP085Config;
 540 #endif
 541
 542 #ifdef NAZE
 543     if (hardwareRevision == NAZE32) {
 544         bmp085Disable(bmp085Config);
 545     }
 546 #endif
 547
 548 #endif
 549
 550     switch (baroHardware) {
 551         case BARO_DEFAULT:
 552             ; // fallthough
 553
 554         case BARO_MS5611:
 555 #ifdef USE_BARO_MS5611
 556             if (ms5611Detect(&baro)) {
 557                 baroHardware = BARO_MS5611;
 558                 break;
 559             }
 560 #endif
 561             ; // fallthough
 562         case BARO_BMP085:
 563 #ifdef USE_BARO_BMP085
 564             if (bmp085Detect(bmp085Config, &baro)) {
 565                 baroHardware = BARO_BMP085;
 566                 break;
 567             }
 568 #endif
 569             ; // fallthough
 570         case BARO_BMP280:
 571 #ifdef USE_BARO_BMP280
 572             if (bmp280Detect(&baro)) {
 573                 baroHardware = BARO_BMP280;
 574                 break;
 575             }
 576 #endif
 577             ; // fallthrough
 578         case BARO_FAKE:
 579 #ifdef USE_FAKE_BARO
 580             if (fakeBaroDetect(&baro)) {
 581                 baroHardware = BARO_FAKE;
 582                 break;
 583             }
 584 #endif
 585             ; // fallthrough
 586         case BARO_NONE:
 587             baroHardware = BARO_NONE;
 588             break;
 589     }
 590
 591     if (baroHardware == BARO_NONE) {
 592         return;
 593     }
 594
 595     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
 596     sensorsSet(SENSOR_BARO);
 597 #endif
 598 }
 599
 600 static void detectMag(magSensor_e magHardwareToUse)
 601 {
 602     magSensor_e magHardware;
 603
 604 #ifdef USE_MAG_HMC5883
 605     const hmc5883Config_t *hmc5883Config = 0;
 606
 607 #ifdef NAZE
 608     static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
 609             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
 610             .gpioPin = Pin_12,
 611             .gpioPort = GPIOB,
 612
 613             /* Disabled for v4 needs more work.
 614             .exti_port_source = GPIO_PortSourceGPIOB,
 615             .exti_pin_source = GPIO_PinSource12,
 616             .exti_line = EXTI_Line12,
 617             .exti_irqn = EXTI15_10_IRQn
 618             */
 619     };
 620     static const hmc5883Config_t nazeHmc5883Config_v5 = {
 621             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
 622             .gpioPin = Pin_14,
 623             .gpioPort = GPIOC,
 624             .exti_port_source = GPIO_PortSourceGPIOC,
 625             .exti_line = EXTI_Line14,
 626             .exti_pin_source = GPIO_PinSource14,
 627             .exti_irqn = EXTI15_10_IRQn
 628     };
 629     if (hardwareRevision < NAZE32_REV5) {
 630         hmc5883Config = &nazeHmc5883Config_v1_v4;
 631     } else {
 632         hmc5883Config = &nazeHmc5883Config_v5;
 633     }
 634 #endif
 635
 636 #ifdef SPRACINGF3
 637     static const hmc5883Config_t spRacingF3Hmc5883Config = {
 638         .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
 639         .gpioPin = Pin_14,
 640         .gpioPort = GPIOC,
 641         .exti_port_source = EXTI_PortSourceGPIOC,
 642         .exti_pin_source = EXTI_PinSource14,
 643         .exti_line = EXTI_Line14,
 644         .exti_irqn = EXTI15_10_IRQn
 645     };
 646
 647     hmc5883Config = &spRacingF3Hmc5883Config;
 648 #endif
 649
 650 #endif
 651
 652 retry:
 653
 654     magAlign = ALIGN_DEFAULT;
 655
 656     switch(magHardwareToUse) {
 657         case MAG_DEFAULT:
 658             ; // fallthrough
 659
 660         case MAG_HMC5883:
 661 #ifdef USE_MAG_HMC5883
 662             if (hmc5883lDetect(&mag, hmc5883Config)) {
 663 #ifdef MAG_HMC5883_ALIGN
 664                 magAlign = MAG_HMC5883_ALIGN;
 665 #endif
 666                 magHardware = MAG_HMC5883;
 667                 break;
 668             }
 669 #endif
 670             ; // fallthrough
 671
 672         case MAG_AK8975:
 673 #ifdef USE_MAG_AK8975
 674             if (ak8975detect(&mag)) {
 675 #ifdef MAG_AK8975_ALIGN
 676                 magAlign = MAG_AK8975_ALIGN;
 677 #endif
 678                 magHardware = MAG_AK8975;
 679                 break;
 680             }
 681 #endif
 682             ; // fallthrough
 683
 684         case MAG_GPS:
 685 #ifdef GPS
 686             if (gpsMagDetect(&mag)) {
 687 #ifdef MAG_GPS_ALIGN
 688                 magAlign = MAG_GPS_ALIGN;
 689 #endif
 690                 magHardware = MAG_GPS;
 691                 break;
 692             }
 693 #endif
 694             ; // fallthrough
 695
 696         case MAG_MAG3110:
 697 #ifdef USE_MAG_MAG3110
 698             if (mag3110detect(&mag)) {
 699 #ifdef MAG_MAG3110_ALIGN
 700                 magAlign = MAG_MAG3110_ALIGN;
 701 #endif
 702                 magHardware = MAG_MAG3110;
 703                 break;
 704             }
 705 #endif
 706             ; // fallthrough
 707
 708         case MAG_FAKE:
 709 #ifdef USE_FAKE_MAG
 710             if (fakeMagDetect(&mag)) {
 711                 magHardware = MAG_FAKE;
 712                 break;
 713             }
 714 #endif
 715             ; // fallthrough
 716
 717         case MAG_NONE:
 718             magHardware = MAG_NONE;
 719             break;
 720     }
 721
 722     if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
 723         // Nothing was found and we have a forced sensor that isn't present.
 724         magHardwareToUse = MAG_DEFAULT;
 725         goto retry;
 726     }
 727
 728     if (magHardware == MAG_NONE) {
 729         return;
 730     }
 731
 732     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
 733     sensorsSet(SENSOR_MAG);
 734 }
 735
 736 #ifdef SONAR
 737 /*
 738  * Detect which rangefinder is present
 739  */
 740 static rangefinderType_e detectRangefinder(void)
 741 {
 742     rangefinderType_e rangefinderType = RANGEFINDER_NONE;
 743     if (feature(FEATURE_SONAR)) {
 744         // the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
 745         // since there is no way to detect it
 746         rangefinderType = RANGEFINDER_HCSR04;
 747     }
 748 #ifdef USE_SONAR_SRF10
 749     if (srf10_detect()) {
 750         // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04
 751         rangefinderType = RANGEFINDER_SRF10;
 752     }
 753 #endif
 754     detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType;
 755
 756     if (rangefinderType != RANGEFINDER_NONE) {
 757         sensorsSet(SENSOR_SONAR);
 758     }
 759
 760     return rangefinderType;
 761 }
 762 #endif
 763
 764 static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
 765 {
 766     if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
 767         gyroAlign = sensorAlignmentConfig->gyro_align;
 768     }
 769     if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
 770         accAlign = sensorAlignmentConfig->acc_align;
 771     }
 772     if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
 773         magAlign = sensorAlignmentConfig->mag_align;
 774     }
 775 }
 776
 777 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
 778         int16_t magDeclinationFromConfig) {
 779
 780     memset(&acc, 0, sizeof(acc));
 781     memset(&gyro, 0, sizeof(gyro));
 782
 783 #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)
 784     const extiConfig_t *mpuExtiConfig = selectMPUIntExtiConfig();
 785     detectMpu(mpuExtiConfig);
 786 #endif
 787
 788     if (!detectGyro()) {
 789         return false;
 790     }
 791     detectAcc(accHardwareToUse);
 792     detectBaro(baroHardwareToUse);
 793
 794
 795     // Now time to init things, acc first
 796     if (sensors(SENSOR_ACC)) {
 797         acc.init(&acc);
 798     }
 799
 800     gyro.init(gyroLpf);
 801
 802     detectMag(magHardwareToUse);
 803
 804     reconfigureAlignment(sensorAlignmentConfig);
 805
 806     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
 807     if (sensors(SENSOR_MAG)) {
 808         // calculate magnetic declination
 809         const int deg = magDeclinationFromConfig / 100;
 810         const int min = magDeclinationFromConfig % 100;
 811         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
 812     } else {
 813         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.
 814     }
 815
 816 #ifdef SONAR
 817     const rangefinderType_e rangefinderType = detectRangefinder();
 818     rangefinderInit(rangefinderType);
 819 #endif
 820
 821     return true;
 822 }
 823