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/build_config.h"
  24
  25 #include "common/axis.h"
  26
  27 #include "drivers/logging.h"
  28
  29 #include "drivers/io.h"
  30 #include "drivers/system.h"
  31 #include "drivers/exti.h"
  32
  33 #include "drivers/sensor.h"
  34
  35 #include "drivers/accgyro.h"
  36 #include "drivers/accgyro_adxl345.h"
  37 #include "drivers/accgyro_bma280.h"
  38 #include "drivers/accgyro_l3g4200d.h"
  39 #include "drivers/accgyro_mma845x.h"
  40 #include "drivers/accgyro_mpu.h"
  41 #include "drivers/accgyro_mpu3050.h"
  42 #include "drivers/accgyro_mpu6050.h"
  43 #include "drivers/accgyro_mpu6500.h"
  44 #include "drivers/accgyro_l3gd20.h"
  45 #include "drivers/accgyro_lsm303dlhc.h"
  46
  47 #include "drivers/bus_spi.h"
  48 #include "drivers/accgyro_spi_mpu6000.h"
  49 #include "drivers/accgyro_spi_mpu6500.h"
  50 #include "drivers/accgyro_spi_mpu9250.h"
  51 #include "drivers/gyro_sync.h"
  52
  53 #include "drivers/barometer.h"
  54 #include "drivers/barometer_bmp085.h"
  55 #include "drivers/barometer_bmp280.h"
  56 #include "drivers/barometer_ms5611.h"
  57
  58 #include "drivers/compass.h"
  59 #include "drivers/compass_hmc5883l.h"
  60 #include "drivers/compass_ak8975.h"
  61 #include "drivers/compass_ak8963.h"
  62 #include "drivers/compass_mag3110.h"
  63
  64 #include "drivers/sonar_hcsr04.h"
  65 #include "drivers/sonar_srf10.h"
  66
  67 #include "fc/runtime_config.h"
  68
  69 #include "config/config.h"
  70
  71 #include "io/gps.h"
  72
  73 #include "sensors/sensors.h"
  74 #include "sensors/acceleration.h"
  75 #include "sensors/barometer.h"
  76 #include "sensors/gyro.h"
  77 #include "sensors/compass.h"
  78 #include "sensors/rangefinder.h"
  79 #include "sensors/initialisation.h"
  80
  81 #ifdef USE_HARDWARE_REVISION_DETECTION
  82 #include "hardware_revision.h"
  83 #endif
  84
  85 extern baro_t baro;
  86 extern mag_t mag;
  87 extern sensor_align_e gyroAlign;
  88
  89 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
  90
  91
  92 const extiConfig_t *selectMPUIntExtiConfig(void)
  93 {
  94 #if defined(MPU_INT_EXTI)
  95     static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) };
  96     return &mpuIntExtiConfig;
  97 #elif defined(USE_HARDWARE_REVISION_DETECTION)
  98     return selectMPUIntExtiConfigByHardwareRevision();
  99 #else
 100     return NULL;
 101 #endif
 102 }
 103
 104 #ifdef USE_FAKE_GYRO
 105 static void fakeGyroInit(uint8_t lpf)
 106 {
 107     UNUSED(lpf);
 108 }
 109
 110 static bool fakeGyroRead(int16_t *gyroADC)
 111 {
 112     memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
 113     return true;
 114 }
 115
 116 static bool fakeGyroReadTemp(int16_t *tempData)
 117 {
 118     UNUSED(tempData);
 119     return true;
 120 }
 121
 122
 123 static bool fakeGyroInitStatus(void) {
 124     return true;
 125 }
 126
 127 bool fakeGyroDetect(gyro_t *gyro)
 128 {
 129     gyro->init = fakeGyroInit;
 130     gyro->intStatus = fakeGyroInitStatus;
 131     gyro->read = fakeGyroRead;
 132     gyro->temperature = fakeGyroReadTemp;
 133     gyro->scale = 1.0f / 16.4f;
 134     return true;
 135 }
 136 #endif
 137
 138 #ifdef USE_FAKE_ACC
 139 static void fakeAccInit(acc_t *acc) {UNUSED(acc);}
 140 static bool fakeAccRead(int16_t *accData) {
 141     memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
 142     return true;
 143 }
 144
 145 bool fakeAccDetect(acc_t *acc)
 146 {
 147     acc->init = fakeAccInit;
 148     acc->read = fakeAccRead;
 149     acc->revisionCode = 0;
 150     return true;
 151 }
 152 #endif
 153
 154 #ifdef USE_FAKE_BARO
 155 static void fakeBaroStartGet(void)
 156 {
 157 }
 158
 159 static void fakeBaroCalculate(int32_t *pressure, int32_t *temperature)
 160 {
 161     if (pressure)
 162         *pressure = 101325;    // pressure in Pa (0m MSL)
 163     if (temperature)
 164         *temperature = 2500;   // temperature in 0.01 C = 25 deg
 165 }
 166
 167 bool fakeBaroDetect(baro_t *baro)
 168 {
 169     // these are dummy as temperature is measured as part of pressure
 170     baro->ut_delay = 10000;
 171     baro->get_ut = fakeBaroStartGet;
 172     baro->start_ut = fakeBaroStartGet;
 173
 174     // only _up part is executed, and gets both temperature and pressure
 175     baro->up_delay = 10000;
 176     baro->start_up = fakeBaroStartGet;
 177     baro->get_up = fakeBaroStartGet;
 178     baro->calculate = fakeBaroCalculate;
 179
 180     return true;
 181 }
 182 #endif
 183
 184 #ifdef USE_FAKE_MAG
 185 static bool fakeMagInit(void)
 186 {
 187     return true;
 188 }
 189
 190 static bool fakeMagRead(int16_t *magData)
 191 {
 192     // Always pointint North
 193     magData[X] = 4096;
 194     magData[Y] = 0;
 195     magData[Z] = 0;
 196     return true;
 197 }
 198
 199 static bool fakeMagDetect(mag_t *mag)
 200 {
 201     mag->init = fakeMagInit;
 202     mag->read = fakeMagRead;
 203
 204     return true;
 205 }
 206 #endif
 207
 208 bool detectGyro(void)
 209 {
 210     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 211
 212     gyroAlign = ALIGN_DEFAULT;
 213
 214     switch(gyroHardware) {
 215         case GYRO_DEFAULT:
 216             ; // fallthrough
 217         case GYRO_MPU6050:
 218 #ifdef USE_GYRO_MPU6050
 219             if (mpu6050GyroDetect(&gyro)) {
 220                 gyroHardware = GYRO_MPU6050;
 221 #ifdef GYRO_MPU6050_ALIGN
 222                 gyroAlign = GYRO_MPU6050_ALIGN;
 223 #endif
 224                 break;
 225             }
 226 #endif
 227             ; // fallthrough
 228         case GYRO_L3G4200D:
 229 #ifdef USE_GYRO_L3G4200D
 230             if (l3g4200dDetect(&gyro)) {
 231                 gyroHardware = GYRO_L3G4200D;
 232 #ifdef GYRO_L3G4200D_ALIGN
 233                 gyroAlign = GYRO_L3G4200D_ALIGN;
 234 #endif
 235                 break;
 236             }
 237 #endif
 238             ; // fallthrough
 239
 240         case GYRO_MPU3050:
 241 #ifdef USE_GYRO_MPU3050
 242             if (mpu3050Detect(&gyro)) {
 243                 gyroHardware = GYRO_MPU3050;
 244 #ifdef GYRO_MPU3050_ALIGN
 245                 gyroAlign = GYRO_MPU3050_ALIGN;
 246 #endif
 247                 break;
 248             }
 249 #endif
 250             ; // fallthrough
 251
 252         case GYRO_L3GD20:
 253 #ifdef USE_GYRO_L3GD20
 254             if (l3gd20Detect(&gyro)) {
 255                 gyroHardware = GYRO_L3GD20;
 256 #ifdef GYRO_L3GD20_ALIGN
 257                 gyroAlign = GYRO_L3GD20_ALIGN;
 258 #endif
 259                 break;
 260             }
 261 #endif
 262             ; // fallthrough
 263
 264         case GYRO_MPU6000:
 265 #ifdef USE_GYRO_SPI_MPU6000
 266             if (mpu6000SpiGyroDetect(&gyro)) {
 267                 gyroHardware = GYRO_MPU6000;
 268 #ifdef GYRO_MPU6000_ALIGN
 269                 gyroAlign = GYRO_MPU6000_ALIGN;
 270 #endif
 271                 break;
 272             }
 273 #endif
 274             ; // fallthrough
 275
 276         case GYRO_MPU6500:
 277 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 278 #ifdef USE_GYRO_SPI_MPU6500
 279             if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
 280 #else
 281             if (mpu6500GyroDetect(&gyro)) {
 282 #endif
 283                 gyroHardware = GYRO_MPU6500;
 284 #ifdef GYRO_MPU6500_ALIGN
 285                 gyroAlign = GYRO_MPU6500_ALIGN;
 286 #endif
 287
 288                 break;
 289             }
 290 #endif
 291             ; // fallthrough
 292
 293     case GYRO_MPU9250:
 294 #ifdef USE_GYRO_SPI_MPU9250
 295         if (mpu9250SpiGyroDetect(&gyro))
 296         {
 297             gyroHardware = GYRO_MPU9250;
 298 #ifdef GYRO_MPU9250_ALIGN
 299             gyroAlign = GYRO_MPU9250_ALIGN;
 300 #endif
 301
 302             break;
 303         }
 304 #endif
 305         ; // fallthrough
 306         case GYRO_FAKE:
 307 #ifdef USE_FAKE_GYRO
 308             if (fakeGyroDetect(&gyro)) {
 309                 gyroHardware = GYRO_FAKE;
 310                 break;
 311             }
 312 #endif
 313             ; // fallthrough
 314         case GYRO_NONE:
 315             gyroHardware = GYRO_NONE;
 316     }
 317
 318     addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0);
 319
 320     if (gyroHardware == GYRO_NONE) {
 321         return false;
 322     }
 323
 324     detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
 325     sensorsSet(SENSOR_GYRO);
 326
 327     return true;
 328 }
 329
 330 static bool detectAcc(accelerationSensor_e accHardwareToUse)
 331 {
 332     accelerationSensor_e accHardware;
 333
 334 #ifdef USE_ACC_ADXL345
 335     drv_adxl345_config_t acc_params;
 336 #endif
 337
 338 retry:
 339     accAlign = ALIGN_DEFAULT;
 340
 341     switch (accHardwareToUse) {
 342         case ACC_DEFAULT:
 343             ; // fallthrough
 344         case ACC_ADXL345: // ADXL345
 345 #ifdef USE_ACC_ADXL345
 346             acc_params.useFifo = false;
 347             acc_params.dataRate = 800; // unused currently
 348 #ifdef NAZE
 349             if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
 350 #else
 351             if (adxl345Detect(&acc_params, &acc)) {
 352 #endif
 353 #ifdef ACC_ADXL345_ALIGN
 354                 accAlign = ACC_ADXL345_ALIGN;
 355 #endif
 356                 accHardware = ACC_ADXL345;
 357                 break;
 358             }
 359 #endif
 360             ; // fallthrough
 361         case ACC_LSM303DLHC:
 362 #ifdef USE_ACC_LSM303DLHC
 363             if (lsm303dlhcAccDetect(&acc)) {
 364 #ifdef ACC_LSM303DLHC_ALIGN
 365                 accAlign = ACC_LSM303DLHC_ALIGN;
 366 #endif
 367                 accHardware = ACC_LSM303DLHC;
 368                 break;
 369             }
 370 #endif
 371             ; // fallthrough
 372         case ACC_MPU6050: // MPU6050
 373 #ifdef USE_ACC_MPU6050
 374             if (mpu6050AccDetect(&acc)) {
 375 #ifdef ACC_MPU6050_ALIGN
 376                 accAlign = ACC_MPU6050_ALIGN;
 377 #endif
 378                 accHardware = ACC_MPU6050;
 379                 break;
 380             }
 381 #endif
 382             ; // fallthrough
 383         case ACC_MMA8452: // MMA8452
 384 #ifdef USE_ACC_MMA8452
 385 #ifdef NAZE
 386             // Not supported with this frequency
 387             if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
 388 #else
 389             if (mma8452Detect(&acc)) {
 390 #endif
 391 #ifdef ACC_MMA8452_ALIGN
 392                 accAlign = ACC_MMA8452_ALIGN;
 393 #endif
 394                 accHardware = ACC_MMA8452;
 395                 break;
 396             }
 397 #endif
 398             ; // fallthrough
 399         case ACC_BMA280: // BMA280
 400 #ifdef USE_ACC_BMA280
 401             if (bma280Detect(&acc)) {
 402 #ifdef ACC_BMA280_ALIGN
 403                 accAlign = ACC_BMA280_ALIGN;
 404 #endif
 405                 accHardware = ACC_BMA280;
 406                 break;
 407             }
 408 #endif
 409             ; // fallthrough
 410         case ACC_MPU6000:
 411 #ifdef USE_ACC_SPI_MPU6000
 412             if (mpu6000SpiAccDetect(&acc)) {
 413 #ifdef ACC_MPU6000_ALIGN
 414                 accAlign = ACC_MPU6000_ALIGN;
 415 #endif
 416                 accHardware = ACC_MPU6000;
 417                 break;
 418             }
 419 #endif
 420             ; // fallthrough
 421         case ACC_MPU6500:
 422 #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
 423 #ifdef USE_ACC_SPI_MPU6500
 424             if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
 425 #else
 426             if (mpu6500AccDetect(&acc)) {
 427 #endif
 428 #ifdef ACC_MPU6500_ALIGN
 429                 accAlign = ACC_MPU6500_ALIGN;
 430 #endif
 431                 accHardware = ACC_MPU6500;
 432                 break;
 433             }
 434 #endif
 435
 436             ; // fallthrough
 437         case ACC_FAKE:
 438 #ifdef USE_FAKE_ACC
 439             if (fakeAccDetect(&acc)) {
 440                 accHardware = ACC_FAKE;
 441                 break;
 442             }
 443 #endif
 444             ; // fallthrough
 445         case ACC_NONE: // disable ACC
 446             accHardware = ACC_NONE;
 447             break;
 448
 449     }
 450
 451     // Found anything? Check if error or ACC is really missing.
 452     if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
 453         // Nothing was found and we have a forced sensor that isn't present.
 454         accHardwareToUse = ACC_DEFAULT;
 455         goto retry;
 456     }
 457
 458     addBootlogEvent6(BOOT_EVENT_ACC_DETECTION, BOOT_EVENT_FLAGS_NONE, accHardware, 0, 0, 0);
 459
 460     if (accHardware == ACC_NONE) {
 461         return false;
 462     }
 463
 464     detectedSensors[SENSOR_INDEX_ACC] = accHardware;
 465     sensorsSet(SENSOR_ACC);
 466     return true;
 467 }
 468
 469 #ifdef BARO
 470 static bool detectBaro(baroSensor_e baroHardwareToUse)
 471 {
 472     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 473
 474     baroSensor_e baroHardware = baroHardwareToUse;
 475
 476 #ifdef USE_BARO_BMP085
 477
 478     const bmp085Config_t *bmp085Config = NULL;
 479
 480 #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
 481     static const bmp085Config_t defaultBMP085Config = {
 482         .xclrIO = IO_TAG(BARO_XCLR_PIN),
 483         .eocIO = IO_TAG(BARO_EOC_PIN),
 484     };
 485     bmp085Config = &defaultBMP085Config;
 486 #endif
 487
 488 #ifdef NAZE
 489     if (hardwareRevision == NAZE32) {
 490         bmp085Disable(bmp085Config);
 491     }
 492 #endif
 493
 494 #endif
 495
 496     switch (baroHardware) {
 497         case BARO_DEFAULT:
 498             ; // fallthough
 499
 500         case BARO_MS5611:
 501 #ifdef USE_BARO_MS5611
 502             if (ms5611Detect(&baro)) {
 503                 baroHardware = BARO_MS5611;
 504                 break;
 505             }
 506 #endif
 507             ; // fallthough
 508         case BARO_BMP085:
 509 #ifdef USE_BARO_BMP085
 510             if (bmp085Detect(bmp085Config, &baro)) {
 511                 baroHardware = BARO_BMP085;
 512                 break;
 513             }
 514 #endif
 515         ; // fallthough
 516         case BARO_BMP280:
 517 #ifdef USE_BARO_BMP280
 518             if (bmp280Detect(&baro)) {
 519                 baroHardware = BARO_BMP280;
 520                 break;
 521             }
 522 #endif
 523             ; // fallthrough
 524         case BARO_FAKE:
 525 #ifdef USE_FAKE_BARO
 526             if (fakeBaroDetect(&baro)) {
 527                 baroHardware = BARO_FAKE;
 528                 break;
 529             }
 530 #endif
 531             ; // fallthrough
 532         case BARO_NONE:
 533             baroHardware = BARO_NONE;
 534             break;
 535     }
 536
 537     addBootlogEvent6(BOOT_EVENT_BARO_DETECTION, BOOT_EVENT_FLAGS_NONE, baroHardware, 0, 0, 0);
 538
 539     if (baroHardware == BARO_NONE) {
 540         return false;
 541     }
 542
 543     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
 544     sensorsSet(SENSOR_BARO);
 545     return true;
 546 }
 547 #endif // BARO
 548
 549 #ifdef MAG
 550 static bool detectMag(magSensor_e magHardwareToUse)
 551 {
 552     magSensor_e magHardware = MAG_NONE;
 553
 554 #ifdef USE_MAG_HMC5883
 555     const hmc5883Config_t *hmc5883Config = 0;
 556
 557 #ifdef NAZE // TODO remove this target specific define
 558     static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
 559             .intTag = IO_TAG(PB12) /* perhaps disabled? */
 560     };
 561     static const hmc5883Config_t nazeHmc5883Config_v5 = {
 562             .intTag = IO_TAG(MAG_INT_EXTI)
 563     };
 564     if (hardwareRevision < NAZE32_REV5) {
 565         hmc5883Config = &nazeHmc5883Config_v1_v4;
 566     } else {
 567         hmc5883Config = &nazeHmc5883Config_v5;
 568     }
 569 #endif
 570
 571 #ifdef MAG_INT_EXTI
 572     static const hmc5883Config_t extiHmc5883Config = {
 573         .intTag = IO_TAG(MAG_INT_EXTI)
 574     };
 575
 576     hmc5883Config = &extiHmc5883Config;
 577 #endif
 578
 579 #endif
 580
 581     magAlign = ALIGN_DEFAULT;
 582
 583     switch(magHardwareToUse) {
 584         case MAG_DEFAULT:
 585             ; // fallthrough
 586
 587         case MAG_HMC5883:
 588 #ifdef USE_MAG_HMC5883
 589             if (hmc5883lDetect(&mag, hmc5883Config)) {
 590 #ifdef MAG_HMC5883_ALIGN
 591                 magAlign = MAG_HMC5883_ALIGN;
 592 #endif
 593                 magHardware = MAG_HMC5883;
 594                 break;
 595             }
 596 #endif
 597             ; // fallthrough
 598
 599         case MAG_AK8975:
 600 #ifdef USE_MAG_AK8975
 601             if (ak8975Detect(&mag)) {
 602 #ifdef MAG_AK8975_ALIGN
 603                 magAlign = MAG_AK8975_ALIGN;
 604 #endif
 605                 magHardware = MAG_AK8975;
 606                 break;
 607             }
 608 #endif
 609             ; // fallthrough
 610
 611         case MAG_AK8963:
 612 #ifdef USE_MAG_AK8963
 613             if (ak8963Detect(&mag)) {
 614 #ifdef MAG_AK8963_ALIGN
 615                 magAlign = MAG_AK8963_ALIGN;
 616 #endif
 617                 magHardware = MAG_AK8963;
 618                 break;
 619             }
 620 #endif
 621             ; // fallthrough
 622
 623         case MAG_GPS:
 624 #ifdef GPS
 625             if (gpsMagDetect(&mag)) {
 626 #ifdef MAG_GPS_ALIGN
 627                 magAlign = MAG_GPS_ALIGN;
 628 #endif
 629                 magHardware = MAG_GPS;
 630                 break;
 631             }
 632 #endif
 633             ; // fallthrough
 634
 635         case MAG_MAG3110:
 636 #ifdef USE_MAG_MAG3110
 637             if (mag3110detect(&mag)) {
 638 #ifdef MAG_MAG3110_ALIGN
 639                 magAlign = MAG_MAG3110_ALIGN;
 640 #endif
 641                 magHardware = MAG_MAG3110;
 642                 break;
 643             }
 644 #endif
 645             ; // fallthrough
 646
 647         case MAG_FAKE:
 648 #ifdef USE_FAKE_MAG
 649             if (fakeMagDetect(&mag)) {
 650                 magHardware = MAG_FAKE;
 651                 break;
 652             }
 653 #endif
 654             ; // fallthrough
 655
 656         case MAG_NONE:
 657             magHardware = MAG_NONE;
 658             break;
 659     }
 660
 661     addBootlogEvent6(BOOT_EVENT_MAG_DETECTION, BOOT_EVENT_FLAGS_NONE, magHardware, 0, 0, 0);
 662
 663     // If not in autodetect mode and detected the wrong chip - disregard the compass even if detected
 664     if ((magHardwareToUse != MAG_DEFAULT && magHardware != magHardwareToUse) || (magHardware == MAG_NONE)) {
 665         return false;
 666     }
 667
 668     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
 669     sensorsSet(SENSOR_MAG);
 670     return true;
 671 }
 672 #endif // MAG
 673
 674 #ifdef SONAR
 675 /*
 676  * Detect which rangefinder is present
 677  */
 678 static rangefinderType_e detectRangefinder(void)
 679 {
 680     rangefinderType_e rangefinderType = RANGEFINDER_NONE;
 681     if (feature(FEATURE_SONAR)) {
 682         // the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
 683         // since there is no way to detect it
 684         rangefinderType = RANGEFINDER_HCSR04;
 685     }
 686 #ifdef USE_SONAR_SRF10
 687     if (srf10_detect()) {
 688         // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04
 689         rangefinderType = RANGEFINDER_SRF10;
 690     }
 691 #endif
 692
 693     addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderType, 0, 0, 0);
 694
 695     detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType;
 696
 697     if (rangefinderType != RANGEFINDER_NONE) {
 698         sensorsSet(SENSOR_SONAR);
 699     }
 700
 701     return rangefinderType;
 702 }
 703 #endif
 704
 705 static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
 706 {
 707     if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
 708         gyroAlign = sensorAlignmentConfig->gyro_align;
 709     }
 710     if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
 711         accAlign = sensorAlignmentConfig->acc_align;
 712     }
 713     if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
 714         magAlign = sensorAlignmentConfig->mag_align;
 715     }
 716 }
 717
 718 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
 719         uint8_t accHardwareToUse,
 720         uint8_t magHardwareToUse,
 721         uint8_t baroHardwareToUse,
 722         int16_t magDeclinationFromConfig,
 723         uint32_t looptime,
 724         uint8_t gyroLpf,
 725         uint8_t gyroSync,
 726         uint8_t gyroSyncDenominator)
 727 {
 728     memset(&acc, 0, sizeof(acc));
 729     memset(&gyro, 0, sizeof(gyro));
 730
 731 #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)
 732     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
 733     detectMpu(extiConfig);
 734 #endif
 735
 736     if (!detectGyro()) {
 737         return false;
 738     }
 739
 740     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
 741     gyro.targetLooptime = gyroSetSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator);    // Set gyro sample rate before initialisation
 742     gyro.init(gyroLpf); // driver initialisation
 743     gyroInit(); // sensor initialisation
 744
 745     if (detectAcc(accHardwareToUse)) {
 746         acc.acc_1G = 256; // set default
 747         acc.init(&acc);
 748         accInit(gyro.targetLooptime); // acc and gyro updated at same frequency in taskMainPidLoop in mw.c
 749     }
 750
 751 #ifdef BARO
 752     detectBaro(baroHardwareToUse);
 753 #else
 754     UNUSED(baroHardwareToUse);
 755 #endif
 756
 757     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
 758     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.
 759 #ifdef MAG
 760     if (detectMag(magHardwareToUse)) {
 761         // calculate magnetic declination
 762         if (!compassInit(magDeclinationFromConfig)) {
 763             addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
 764             sensorsClear(SENSOR_MAG);
 765         }
 766     }
 767 #else
 768     UNUSED(magHardwareToUse);
 769     UNUSED(magDeclinationFromConfig);
 770 #endif
 771
 772 #ifdef SONAR
 773     const rangefinderType_e rangefinderType = detectRangefinder();
 774     rangefinderInit(rangefinderType);
 775 #endif
 776     reconfigureAlignment(sensorAlignmentConfig);
 777
 778     return true;
 779 }
 780