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/sensor.h"
  28
  29 #include "drivers/accgyro.h"
  30 #include "drivers/accgyro_adxl345.h"
  31 #include "drivers/accgyro_bma280.h"
  32 #include "drivers/accgyro_l3g4200d.h"
  33 #include "drivers/accgyro_mma845x.h"
  34 #include "drivers/accgyro_mpu3050.h"
  35 #include "drivers/accgyro_mpu6050.h"
  36 #include "drivers/accgyro_l3gd20.h"
  37 #include "drivers/accgyro_lsm303dlhc.h"
  38
  39 #include "drivers/bus_spi.h"
  40 #include "drivers/accgyro_spi_mpu6000.h"
  41 #include "drivers/accgyro_spi_mpu6500.h"
  42 #include "drivers/gyro_sync.h"
  43
  44 #include "drivers/barometer.h"
  45 #include "drivers/barometer_bmp085.h"
  46 #include "drivers/barometer_bmp280.h"
  47 #include "drivers/barometer_ms5611.h"
  48
  49 #include "drivers/compass.h"
  50 #include "drivers/compass_hmc5883l.h"
  51 #include "drivers/compass_ak8975.h"
  52
  53 #include "drivers/sonar_hcsr04.h"
  54
  55 #include "drivers/gpio.h"
  56 #include "drivers/system.h"
  57
  58 #include "config/runtime_config.h"
  59
  60 #include "sensors/sensors.h"
  61 #include "sensors/acceleration.h"
  62 #include "sensors/barometer.h"
  63 #include "sensors/gyro.h"
  64 #include "sensors/compass.h"
  65 #include "sensors/sonar.h"
  66 #include "sensors/initialisation.h"
  67
  68 #ifdef NAZE
  69 #include "hardware_revision.h"
  70 #endif
  71
  72 extern float magneticDeclination;
  73
  74 extern gyro_t gyro;
  75 extern baro_t baro;
  76 extern acc_t acc;
  77
  78 uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
  79
  80
  81 const mpu6050Config_t *selectMPU6050Config(void)
  82 {
  83 #ifdef NAZE
  84     // MPU_INT output on rev4 PB13
  85     static const mpu6050Config_t nazeRev4MPU6050Config = {
  86             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
  87             .gpioPin = Pin_13,
  88             .gpioPort = GPIOB,
  89             .exti_port_source = GPIO_PortSourceGPIOB,
  90             .exti_line = EXTI_Line13,
  91             .exti_pin_source = GPIO_PinSource13,
  92             .exti_irqn = EXTI15_10_IRQn
  93     };
  94     // MPU_INT output on rev5 hardware PC13
  95     static const mpu6050Config_t nazeRev5MPU6050Config = {
  96             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
  97             .gpioPin = Pin_13,
  98             .gpioPort = GPIOC,
  99             .exti_port_source = GPIO_PortSourceGPIOC,
 100             .exti_line = EXTI_Line13,
 101             .exti_pin_source = GPIO_PinSource13,
 102             .exti_irqn = EXTI15_10_IRQn
 103     };
 104
 105     if (hardwareRevision < NAZE32_REV5) {
 106         return &nazeRev4MPU6050Config;
 107     } else {
 108         return &nazeRev5MPU6050Config;
 109     }
 110 #endif
 111
 112 #ifdef SPRACINGF3
 113     static const mpu6050Config_t spRacingF3MPU6050Config = {
 114             .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
 115             .gpioPort = GPIOC,
 116             .gpioPin = Pin_13,
 117             .exti_port_source = EXTI_PortSourceGPIOC,
 118             .exti_pin_source = EXTI_PinSource13,
 119             .exti_line = EXTI_Line13,
 120             .exti_irqn = EXTI15_10_IRQn
 121     };
 122     return &spRacingF3MPU6050Config;
 123 #endif
 124
 125     return NULL;
 126 }
 127
 128 #ifdef USE_FAKE_GYRO
 129 static void fakeGyroInit(void) {}
 130 static void fakeGyroRead(int16_t *gyroADC) {
 131     memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
 132 }
 133 static void fakeGyroReadTemp(int16_t *tempData) {
 134     UNUSED(tempData);
 135 }
 136
 137 bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
 138 {
 139     UNUSED(lpf);
 140     gyro->init = fakeGyroInit;
 141     gyro->read = fakeGyroRead;
 142     gyro->temperature = fakeGyroReadTemp;
 143     return true;
 144 }
 145 #endif
 146
 147 #ifdef USE_FAKE_ACC
 148 static void fakeAccInit(void) {}
 149 static void fakeAccRead(int16_t *accData) {
 150     memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
 151 }
 152
 153 bool fakeAccDetect(acc_t *acc)
 154 {
 155     acc->init = fakeAccInit;
 156     acc->read = fakeAccRead;
 157     acc->revisionCode = 0;
 158     return true;
 159 }
 160 #endif
 161
 162 bool detectGyro(uint16_t gyroLpf)
 163 {
 164     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 165
 166     gyroAlign = ALIGN_DEFAULT;
 167
 168     switch(gyroHardware) {
 169         case GYRO_DEFAULT:
 170             ; // fallthrough
 171         case GYRO_MPU6050:
 172 #ifdef USE_GYRO_MPU6050
 173             if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
 174 #ifdef GYRO_MPU6050_ALIGN
 175                 gyroHardware = GYRO_MPU6050;
 176                 gyroAlign = GYRO_MPU6050_ALIGN;
 177 #endif
 178                 break;
 179             }
 180 #endif
 181             ; // fallthrough
 182         case GYRO_L3G4200D:
 183 #ifdef USE_GYRO_L3G4200D
 184             if (l3g4200dDetect(&gyro, gyroLpf)) {
 185 #ifdef GYRO_L3G4200D_ALIGN
 186                 gyroHardware = GYRO_L3G4200D;
 187                 gyroAlign = GYRO_L3G4200D_ALIGN;
 188 #endif
 189                 break;
 190             }
 191 #endif
 192             ; // fallthrough
 193
 194         case GYRO_MPU3050:
 195 #ifdef USE_GYRO_MPU3050
 196             if (mpu3050Detect(&gyro, gyroLpf)) {
 197 #ifdef GYRO_MPU3050_ALIGN
 198                 gyroHardware = GYRO_MPU3050;
 199                 gyroAlign = GYRO_MPU3050_ALIGN;
 200 #endif
 201                 break;
 202             }
 203 #endif
 204             ; // fallthrough
 205
 206         case GYRO_L3GD20:
 207 #ifdef USE_GYRO_L3GD20
 208             if (l3gd20Detect(&gyro, gyroLpf)) {
 209 #ifdef GYRO_L3GD20_ALIGN
 210                 gyroHardware = GYRO_L3GD20;
 211                 gyroAlign = GYRO_L3GD20_ALIGN;
 212 #endif
 213                 break;
 214             }
 215 #endif
 216             ; // fallthrough
 217
 218         case GYRO_SPI_MPU6000:
 219 #ifdef USE_GYRO_SPI_MPU6000
 220             if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
 221 #ifdef GYRO_SPI_MPU6000_ALIGN
 222                 gyroHardware = GYRO_SPI_MPU6000;
 223                 gyroAlign = GYRO_SPI_MPU6000_ALIGN;
 224 #endif
 225                 break;
 226             }
 227 #endif
 228             ; // fallthrough
 229
 230         case GYRO_SPI_MPU6500:
 231 #ifdef USE_GYRO_SPI_MPU6500
 232 #ifdef USE_HARDWARE_REVISION_DETECTION
 233                   spiBusInit();
 234 #endif
 235 #ifdef NAZE
 236             if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
 237 #ifdef GYRO_SPI_MPU6500_ALIGN
 238                 gyroHardware = GYRO_SPI_MPU6500;
 239                 gyroAlign = GYRO_SPI_MPU6500_ALIGN;
 240 #endif
 241                 break;
 242             }
 243 #else
 244             if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
 245 #ifdef GYRO_SPI_MPU6500_ALIGN
 246                 gyroHardware = GYRO_SPI_MPU6500;
 247                 gyroAlign = GYRO_SPI_MPU6500_ALIGN;
 248 #endif
 249                 break;
 250             }
 251 #endif
 252 #endif
 253             ; // fallthrough
 254
 255         case GYRO_FAKE:
 256 #ifdef USE_FAKE_GYRO
 257             if (fakeGyroDetect(&gyro, gyroLpf)) {
 258                 gyroHardware = GYRO_FAKE;
 259                 break;
 260             }
 261 #endif
 262             ; // fallthrough
 263         case GYRO_NONE:
 264             gyroHardware = GYRO_NONE;
 265     }
 266
 267     if (gyroHardware == GYRO_NONE) {
 268         return false;
 269     }
 270
 271     detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
 272     sensorsSet(SENSOR_GYRO);
 273
 274     return true;
 275 }
 276
 277 static void detectAcc(accelerationSensor_e accHardwareToUse)
 278 {
 279     accelerationSensor_e accHardware;
 280
 281     #ifdef USE_ACC_ADXL345
 282     drv_adxl345_config_t acc_params;
 283 #endif
 284
 285 retry:
 286     accAlign = ALIGN_DEFAULT;
 287
 288     switch (accHardwareToUse) {
 289         case ACC_DEFAULT:
 290             ; // fallthrough
 291         case ACC_ADXL345: // ADXL345
 292 #ifdef USE_ACC_ADXL345
 293             acc_params.useFifo = false;
 294             acc_params.dataRate = 800; // unused currently
 295 #ifdef NAZE
 296             if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
 297 #else
 298             if (adxl345Detect(&acc_params, &acc)) {
 299 #endif
 300 #ifdef ACC_ADXL345_ALIGN
 301                 accAlign = ACC_ADXL345_ALIGN;
 302 #endif
 303                 accHardware = ACC_ADXL345;
 304                 break;
 305             }
 306 #endif
 307             ; // fallthrough
 308         case ACC_LSM303DLHC:
 309 #ifdef USE_ACC_LSM303DLHC
 310             if (lsm303dlhcAccDetect(&acc)) {
 311 #ifdef ACC_LSM303DLHC_ALIGN
 312                 accAlign = ACC_LSM303DLHC_ALIGN;
 313 #endif
 314                 accHardware = ACC_LSM303DLHC;
 315                 break;
 316             }
 317 #endif
 318             ; // fallthrough
 319         case ACC_MPU6050: // MPU6050
 320 #ifdef USE_ACC_MPU6050
 321             if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
 322 #ifdef ACC_MPU6050_ALIGN
 323                 accAlign = ACC_MPU6050_ALIGN;
 324 #endif
 325                 accHardware = ACC_MPU6050;
 326                 break;
 327             }
 328 #endif
 329             ; // fallthrough
 330         case ACC_MMA8452: // MMA8452
 331 #ifdef USE_ACC_MMA8452
 332 #ifdef NAZE
 333             // Not supported with this frequency
 334             if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
 335 #else
 336             if (mma8452Detect(&acc)) {
 337 #endif
 338 #ifdef ACC_MMA8452_ALIGN
 339                 accAlign = ACC_MMA8452_ALIGN;
 340 #endif
 341                 accHardware = ACC_MMA8452;
 342                 break;
 343             }
 344 #endif
 345             ; // fallthrough
 346         case ACC_BMA280: // BMA280
 347 #ifdef USE_ACC_BMA280
 348             if (bma280Detect(&acc)) {
 349 #ifdef ACC_BMA280_ALIGN
 350                 accAlign = ACC_BMA280_ALIGN;
 351 #endif
 352                 accHardware = ACC_BMA280;
 353                 break;
 354             }
 355 #endif
 356             ; // fallthrough
 357         case ACC_SPI_MPU6000:
 358 #ifdef USE_ACC_SPI_MPU6000
 359             if (mpu6000SpiAccDetect(&acc)) {
 360 #ifdef ACC_SPI_MPU6000_ALIGN
 361                 accAlign = ACC_SPI_MPU6000_ALIGN;
 362 #endif
 363                 accHardware = ACC_SPI_MPU6000;
 364                 break;
 365             }
 366 #endif
 367             ; // fallthrough
 368         case ACC_SPI_MPU6500:
 369 #ifdef USE_ACC_SPI_MPU6500
 370 #ifdef NAZE
 371             if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
 372 #else
 373             if (mpu6500SpiAccDetect(&acc)) {
 374 #endif
 375 #ifdef ACC_SPI_MPU6500_ALIGN
 376                 accAlign = ACC_SPI_MPU6500_ALIGN;
 377 #endif
 378                 accHardware = ACC_SPI_MPU6500;
 379                 break;
 380             }
 381 #endif
 382             ; // fallthrough
 383         case ACC_FAKE:
 384 #ifdef USE_FAKE_ACC
 385             if (fakeAccDetect(&acc)) {
 386                 accHardware = ACC_FAKE;
 387                 break;
 388             }
 389 #endif
 390             ; // fallthrough
 391         case ACC_NONE: // disable ACC
 392             accHardware = ACC_NONE;
 393             break;
 394
 395     }
 396
 397     // Found anything? Check if error or ACC is really missing.
 398     if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
 399         // Nothing was found and we have a forced sensor that isn't present.
 400         accHardwareToUse = ACC_DEFAULT;
 401         goto retry;
 402     }
 403
 404
 405     if (accHardware == ACC_NONE) {
 406         return;
 407     }
 408
 409     detectedSensors[SENSOR_INDEX_ACC] = accHardware;
 410     sensorsSet(SENSOR_ACC);
 411 }
 412
 413 static void detectBaro(baroSensor_e baroHardwareToUse)
 414 {
 415 #ifdef BARO
 416     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 417
 418     baroSensor_e baroHardware = baroHardwareToUse;
 419
 420 #ifdef USE_BARO_BMP085
 421
 422     const bmp085Config_t *bmp085Config = NULL;
 423
 424 #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
 425     static const bmp085Config_t defaultBMP085Config = {
 426             .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
 427             .xclrGpioPin = BARO_XCLR_PIN,
 428             .xclrGpioPort = BARO_XCLR_GPIO,
 429             .eocGpioPin = BARO_EOC_PIN,
 430             .eocGpioPort = BARO_EOC_GPIO
 431     };
 432     bmp085Config = &defaultBMP085Config;
 433 #endif
 434
 435 #ifdef NAZE
 436     if (hardwareRevision == NAZE32) {
 437         bmp085Disable(bmp085Config);
 438     }
 439 #endif
 440
 441 #endif
 442
 443     switch (baroHardware) {
 444         case BARO_DEFAULT:
 445             ; // fallthough
 446
 447         case BARO_MS5611:
 448 #ifdef USE_BARO_MS5611
 449             if (ms5611Detect(&baro)) {
 450                 baroHardware = BARO_MS5611;
 451                 break;
 452             }
 453 #endif
 454             ; // fallthough
 455         case BARO_BMP085:
 456 #ifdef USE_BARO_BMP085
 457             if (bmp085Detect(bmp085Config, &baro)) {
 458                 baroHardware = BARO_BMP085;
 459                 break;
 460             }
 461 #endif
 462             ; // fallthough
 463         case BARO_BMP280:
 464 #ifdef USE_BARO_BMP280
 465             if (bmp280Detect(&baro)) {
 466                 baroHardware = BARO_BMP280;
 467                 break;
 468             }
 469 #endif
 470         case BARO_NONE:
 471             baroHardware = BARO_NONE;
 472             break;
 473     }
 474
 475     if (baroHardware == BARO_NONE) {
 476         return;
 477     }
 478
 479     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
 480     sensorsSet(SENSOR_BARO);
 481 #endif
 482 }
 483
 484 static void detectMag(magSensor_e magHardwareToUse)
 485 {
 486     magSensor_e magHardware;
 487
 488 #ifdef USE_MAG_HMC5883
 489     const hmc5883Config_t *hmc5883Config = 0;
 490
 491 #ifdef NAZE
 492     static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
 493             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
 494             .gpioPin = Pin_12,
 495             .gpioPort = GPIOB,
 496
 497             /* Disabled for v4 needs more work.
 498             .exti_port_source = GPIO_PortSourceGPIOB,
 499             .exti_pin_source = GPIO_PinSource12,
 500             .exti_line = EXTI_Line12,
 501             .exti_irqn = EXTI15_10_IRQn
 502             */
 503     };
 504     static const hmc5883Config_t nazeHmc5883Config_v5 = {
 505             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
 506             .gpioPin = Pin_14,
 507             .gpioPort = GPIOC,
 508             .exti_port_source = GPIO_PortSourceGPIOC,
 509             .exti_line = EXTI_Line14,
 510             .exti_pin_source = GPIO_PinSource14,
 511             .exti_irqn = EXTI15_10_IRQn
 512     };
 513     if (hardwareRevision < NAZE32_REV5) {
 514         hmc5883Config = &nazeHmc5883Config_v1_v4;
 515     } else {
 516         hmc5883Config = &nazeHmc5883Config_v5;
 517     }
 518 #endif
 519
 520 #ifdef SPRACINGF3
 521     static const hmc5883Config_t spRacingF3Hmc5883Config = {
 522         .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
 523         .gpioPin = Pin_14,
 524         .gpioPort = GPIOC,
 525         .exti_port_source = EXTI_PortSourceGPIOC,
 526         .exti_pin_source = EXTI_PinSource14,
 527         .exti_line = EXTI_Line14,
 528         .exti_irqn = EXTI15_10_IRQn
 529     };
 530
 531     hmc5883Config = &spRacingF3Hmc5883Config;
 532 #endif
 533
 534 #endif
 535
 536 retry:
 537
 538     magAlign = ALIGN_DEFAULT;
 539
 540     switch(magHardwareToUse) {
 541         case MAG_DEFAULT:
 542             ; // fallthrough
 543
 544         case MAG_HMC5883:
 545 #ifdef USE_MAG_HMC5883
 546             if (hmc5883lDetect(&mag, hmc5883Config)) {
 547 #ifdef MAG_HMC5883_ALIGN
 548                 magAlign = MAG_HMC5883_ALIGN;
 549 #endif
 550                 magHardware = MAG_HMC5883;
 551                 break;
 552             }
 553 #endif
 554             ; // fallthrough
 555
 556         case MAG_AK8975:
 557 #ifdef USE_MAG_AK8975
 558             if (ak8975detect(&mag)) {
 559 #ifdef MAG_AK8975_ALIGN
 560                 magAlign = MAG_AK8975_ALIGN;
 561 #endif
 562                 magHardware = MAG_AK8975;
 563                 break;
 564             }
 565 #endif
 566             ; // fallthrough
 567
 568         case MAG_NONE:
 569             magHardware = MAG_NONE;
 570             break;
 571     }
 572
 573     if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
 574         // Nothing was found and we have a forced sensor that isn't present.
 575         magHardwareToUse = MAG_DEFAULT;
 576         goto retry;
 577     }
 578
 579     if (magHardware == MAG_NONE) {
 580         return;
 581     }
 582
 583     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
 584     sensorsSet(SENSOR_MAG);
 585 }
 586
 587 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
 588 {
 589     if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
 590         gyroAlign = sensorAlignmentConfig->gyro_align;
 591     }
 592     if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
 593         accAlign = sensorAlignmentConfig->acc_align;
 594     }
 595     if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
 596         magAlign = sensorAlignmentConfig->mag_align;
 597     }
 598 }
 599
 600 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime, uint8_t syncGyroToLoop)
 601 {
 602     int16_t deg, min;
 603
 604     memset(&acc, 0, sizeof(acc));
 605     memset(&gyro, 0, sizeof(gyro));
 606
 607     if (!detectGyro(gyroLpf)) {
 608         return false;
 609     }
 610     detectAcc(accHardwareToUse);
 611     detectBaro(baroHardwareToUse);
 612
 613
 614     // Now time to init things, acc first
 615     if (sensors(SENSOR_ACC))
 616         acc.init();
 617     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
 618     gyroUpdateSampleRate(looptime, gyroLpf, syncGyroToLoop);  // Set gyro refresh rate before initialisation
 619     gyro.init();
 620
 621     detectMag(magHardwareToUse);
 622
 623     reconfigureAlignment(sensorAlignmentConfig);
 624
 625     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
 626     if (sensors(SENSOR_MAG)) {
 627         // calculate magnetic declination
 628         deg = magDeclinationFromConfig / 100;
 629         min = magDeclinationFromConfig % 100;
 630
 631         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
 632     } else {
 633         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.
 634     }
 635
 636     return true;
 637 }
 638