initialisation.c 16.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "platform.h"

#include "build_config.h"

#include "common/axis.h"

27
#include "drivers/sensor.h"
28

29
#include "drivers/accgyro.h"
30
31
32
33
34
35
36
37
#include "drivers/accgyro_adxl345.h"
#include "drivers/accgyro_bma280.h"
#include "drivers/accgyro_l3g4200d.h"
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
38

39
#include "drivers/accgyro_spi_mpu6000.h"
40
#include "drivers/accgyro_spi_mpu6500.h"
41
42
43
44

#include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h"
#include "drivers/barometer_ms5611.h"
45
46

#include "drivers/compass.h"
47
#include "drivers/compass_hmc5883l.h"
48
49
#include "drivers/compass_ak8975.h"

50
#include "drivers/sonar_hcsr04.h"
51

52
#include "drivers/gpio.h"
53
54
55
56
57
58
59
60
61
62
#include "drivers/system.h"

#include "config/runtime_config.h"

#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/sonar.h"
63
#include "sensors/initialisation.h"
64

65
66
67
68
#ifdef NAZE
#include "hardware_revision.h"
#endif

69
70
71
72
73
74
extern float magneticDeclination;

extern gyro_t gyro;
extern baro_t baro;
extern acc_t acc;

75
76
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };

77
const mpu6050Config_t *selectMPU6050Config(void)
78
79
{
#ifdef NAZE
80
    // MPU_INT output on rev4 PB13
81
82
    static const mpu6050Config_t nazeRev4MPU6050Config = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
83
            .gpioPin = Pin_13,
84
            .gpioPort = GPIOB,
85
86
            .exti_port_source = GPIO_PortSourceGPIOB,
            .exti_line = EXTI_Line13,
87
            .exti_pin_source = GPIO_PinSource13,
88
            .exti_irqn = EXTI15_10_IRQn
89
    };
90
    // MPU_INT output on rev5 hardware PC13
91
92
    static const mpu6050Config_t nazeRev5MPU6050Config = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
93
            .gpioPin = Pin_13,
94
            .gpioPort = GPIOC,
95
96
            .exti_port_source = GPIO_PortSourceGPIOC,
            .exti_line = EXTI_Line13,
97
            .exti_pin_source = GPIO_PinSource13,
98
            .exti_irqn = EXTI15_10_IRQn
99
    };
100
101

    if (hardwareRevision < NAZE32_REV5) {
102
        return &nazeRev4MPU6050Config;
103
    } else {
104
        return &nazeRev5MPU6050Config;
105
106
    }
#endif
107
108
109
110
111

#ifdef SPRACINGF3
    static const mpu6050Config_t spRacingF3MPU6050Config = {
            .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
            .gpioPort = GPIOC,
112
113
114
115
116
            .gpioPin = Pin_13,
            .exti_port_source = EXTI_PortSourceGPIOC,
            .exti_pin_source = EXTI_PinSource13,
            .exti_line = EXTI_Line13,
            .exti_irqn = EXTI15_10_IRQn
117
118
119
    };
    return &spRacingF3MPU6050Config;
#endif
120

121
122
123
    return NULL;
}

124
125
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {}
126
127
static void fakeGyroRead(int16_t *gyroADC) {
    memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
}
static void fakeGyroReadTemp(int16_t *tempData) {
    UNUSED(tempData);
}

bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
{
    UNUSED(lpf);
    gyro->init = fakeGyroInit;
    gyro->read = fakeGyroRead;
    gyro->temperature = fakeGyroReadTemp;
    return true;
}
#endif

#ifdef USE_FAKE_ACC
static void fakeAccInit(void) {}
static void fakeAccRead(int16_t *accData) {
146
    memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
147
148
149
150
151
152
153
154
155
156
157
158
159
}

bool fakeAccDetect(acc_t *acc)
{
    acc->init = fakeAccInit;
    acc->read = fakeAccRead;
    acc->revisionCode = 0;
    return true;
}
#endif

bool detectGyro(uint16_t gyroLpf)
{
160
161
    gyroSensor_e gyroHardware = GYRO_DEFAULT;

162
163
    gyroAlign = ALIGN_DEFAULT;

164
165
166
167
    switch(gyroHardware) {
        case GYRO_DEFAULT:
            ; // fallthrough
        case GYRO_MPU6050:
168
#ifdef USE_GYRO_MPU6050
169
            if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
170
#ifdef GYRO_MPU6050_ALIGN
171
172
                gyroHardware = GYRO_MPU6050;
                gyroAlign = GYRO_MPU6050_ALIGN;
173
#endif
174
175
                break;
            }
176
#endif
177
178
            ; // fallthrough
        case GYRO_L3G4200D:
179
#ifdef USE_GYRO_L3G4200D
180
            if (l3g4200dDetect(&gyro, gyroLpf)) {
181
#ifdef GYRO_L3G4200D_ALIGN
182
183
                gyroHardware = GYRO_L3G4200D;
                gyroAlign = GYRO_L3G4200D_ALIGN;
184
#endif
185
186
                break;
            }
187
#endif
188
            ; // fallthrough
189

190
        case GYRO_MPU3050:
191
#ifdef USE_GYRO_MPU3050
192
            if (mpu3050Detect(&gyro, gyroLpf)) {
193
#ifdef GYRO_MPU3050_ALIGN
194
195
                gyroHardware = GYRO_MPU3050;
                gyroAlign = GYRO_MPU3050_ALIGN;
196
#endif
197
198
                break;
            }
199
#endif
200
            ; // fallthrough
201

202
        case GYRO_L3GD20:
203
#ifdef USE_GYRO_L3GD20
204
            if (l3gd20Detect(&gyro, gyroLpf)) {
205
#ifdef GYRO_L3GD20_ALIGN
206
207
                gyroHardware = GYRO_L3GD20;
                gyroAlign = GYRO_L3GD20_ALIGN;
208
#endif
209
210
                break;
            }
211
#endif
212
            ; // fallthrough
213

214
        case GYRO_SPI_MPU6000:
215
#ifdef USE_GYRO_SPI_MPU6000
216
            if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
217
#ifdef GYRO_SPI_MPU6000_ALIGN
218
219
                gyroHardware = GYRO_SPI_MPU6000;
                gyroAlign = GYRO_SPI_MPU6000_ALIGN;
220
#endif
221
222
                break;
            }
223
#endif
224
            ; // fallthrough
225

226
        case GYRO_SPI_MPU6500:
227
228
#ifdef USE_GYRO_SPI_MPU6500
#ifdef NAZE
229
            if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
230
#ifdef GYRO_SPI_MPU6500_ALIGN
231
232
                gyroHardware = GYRO_SPI_MPU6500;
                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
233
#endif
234
235
                break;
            }
236
#else
237
            if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
238
#ifdef GYRO_SPI_MPU6500_ALIGN
239
240
                gyroHardware = GYRO_SPI_MPU6500;
                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
241
#endif
242
243
                break;
            }
244
245
#endif
#endif
246
            ; // fallthrough
247

248
        case GYRO_FAKE:
249
#ifdef USE_FAKE_GYRO
250
251
252
253
            if (fakeGyroDetect(&gyro, gyroLpf)) {
                gyroHardware = GYRO_FAKE;
                break;
            }
254
#endif
255
256
257
258
259
260
261
262
263
264
265
266
267
            ; // fallthrough
        case GYRO_NONE:
            gyroHardware = GYRO_NONE;
    }

    if (gyroHardware == GYRO_NONE) {
        return false;
    }

    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
    sensorsSet(SENSOR_GYRO);

    return true;
268
269
}

270
static void detectAcc(accelerationSensor_e accHardwareToUse)
271
{
272
273
274
    accelerationSensor_e accHardware;

    #ifdef USE_ACC_ADXL345
275
276
277
278
279
280
281
    drv_adxl345_config_t acc_params;
#endif

retry:
    accAlign = ALIGN_DEFAULT;

    switch (accHardwareToUse) {
282
283
        case ACC_DEFAULT:
            ; // fallthrough
284
        case ACC_ADXL345: // ADXL345
285
#ifdef USE_ACC_ADXL345
286
287
288
            acc_params.useFifo = false;
            acc_params.dataRate = 800; // unused currently
#ifdef NAZE
289
290
291
            if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
#else
            if (adxl345Detect(&acc_params, &acc)) {
292
#endif
293
294
295
#ifdef ACC_ADXL345_ALIGN
                accAlign = ACC_ADXL345_ALIGN;
#endif
296
                accHardware = ACC_ADXL345;
297
                break;
298
            }
299
#endif
300
            ; // fallthrough
301
        case ACC_LSM303DLHC:
302
#ifdef USE_ACC_LSM303DLHC
303
304
305
306
307
            if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
                accAlign = ACC_LSM303DLHC_ALIGN;
#endif
                accHardware = ACC_LSM303DLHC;
308
                break;
309
310
            }
#endif
311
            ; // fallthrough
312
        case ACC_MPU6050: // MPU6050
313
#ifdef USE_ACC_MPU6050
314
            if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
315
316
#ifdef ACC_MPU6050_ALIGN
                accAlign = ACC_MPU6050_ALIGN;
317
#endif
318
                accHardware = ACC_MPU6050;
319
                break;
Dominic Clifton's avatar
Dominic Clifton committed
320
321
            }
#endif
322
            ; // fallthrough
323
        case ACC_MMA8452: // MMA8452
324
#ifdef USE_ACC_MMA8452
325
#ifdef NAZE
326
327
328
329
            // Not supported with this frequency
            if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
#else
            if (mma8452Detect(&acc)) {
330
331
332
#endif
#ifdef ACC_MMA8452_ALIGN
                accAlign = ACC_MMA8452_ALIGN;
333
#endif
334
                accHardware = ACC_MMA8452;
335
                break;
336
337
            }
#endif
338
            ; // fallthrough
339
        case ACC_BMA280: // BMA280
340
#ifdef USE_ACC_BMA280
341
            if (bma280Detect(&acc)) {
342
343
#ifdef ACC_BMA280_ALIGN
                accAlign = ACC_BMA280_ALIGN;
344
#endif
345
                accHardware = ACC_BMA280;
346
                break;
347
348
            }
#endif
349
            ; // fallthrough
350
        case ACC_SPI_MPU6000:
351
#ifdef USE_ACC_SPI_MPU6000
352
            if (mpu6000SpiAccDetect(&acc)) {
353
354
#ifdef ACC_SPI_MPU6000_ALIGN
                accAlign = ACC_SPI_MPU6000_ALIGN;
355
#endif
356
                accHardware = ACC_SPI_MPU6000;
357
                break;
358
            }
359
#endif
360
            ; // fallthrough
361
        case ACC_SPI_MPU6500:
362
#ifdef USE_ACC_SPI_MPU6500
363
#ifdef NAZE
364
            if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
365
366
367
#else
            if (mpu6500SpiAccDetect(&acc)) {
#endif
368
369
#ifdef ACC_SPI_MPU6500_ALIGN
                accAlign = ACC_SPI_MPU6500_ALIGN;
370
#endif
371
                accHardware = ACC_SPI_MPU6500;
372
                break;
373
            }
374
375
376
377
378
379
380
381
#endif
            ; // fallthrough
        case ACC_FAKE:
#ifdef USE_FAKE_ACC
            if (fakeAccDetect(&acc)) {
                accHardware = ACC_FAKE;
                break;
            }
382
#endif
383
            ; // fallthrough
384
385
386
387
        case ACC_NONE: // disable ACC
            accHardware = ACC_NONE;
            break;

388
389
    }

390
    // Found anything? Check if error or ACC is really missing.
391
    if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
392
393
394
        // Nothing was found and we have a forced sensor that isn't present.
        accHardwareToUse = ACC_DEFAULT;
        goto retry;
395
    }
396

397
398
399

    if (accHardware == ACC_NONE) {
        return;
400
    }
401
402
403

    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
    sensorsSet(SENSOR_ACC);
404
405
406
407
}

static void detectBaro()
{
408
#ifdef BARO
409
410
    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function

411
    baroSensor_e baroHardware = BARO_DEFAULT;
412

413
#ifdef USE_BARO_BMP085
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433

    const bmp085Config_t *bmp085Config = NULL;

#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
    static const bmp085Config_t defaultBMP085Config = {
            .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS,
            .xclrGpioPin = BARO_XCLR_PIN,
            .xclrGpioPort = BARO_XCLR_GPIO,
            .eocGpioPin = BARO_EOC_PIN,
            .eocGpioPort = BARO_EOC_GPIO
    };
    bmp085Config = &defaultBMP085Config;
#endif

#ifdef NAZE
    if (hardwareRevision == NAZE32) {
        bmp085Disable(bmp085Config);
    }
#endif

434
#endif
435

436
437
438
439
440
    switch (baroHardware) {
        case BARO_DEFAULT:
            ; // fallthough

        case BARO_MS5611:
441
#ifdef USE_BARO_MS5611
442
443
444
445
            if (ms5611Detect(&baro)) {
                baroHardware = BARO_MS5611;
                break;
            }
446
#endif
447
448
            ; // fallthough
        case BARO_BMP085:
449
#ifdef USE_BARO_BMP085
450
451
452
453
454
455
            if (bmp085Detect(bmp085Config, &baro)) {
                baroHardware = BARO_BMP085;
                break;
            }
#endif
        case BARO_NONE:
456
            baroHardware = BARO_NONE;
457
458
459
            break;
    }

460
461
    if (baroHardware == BARO_NONE) {
        return;
462
    }
463
464
465
466

    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
    sensorsSet(SENSOR_BARO);
#endif
467
468
}

469
static void detectMag(magSensor_e magHardwareToUse)
470
{
471
472
    magSensor_e magHardware;

473
#ifdef USE_MAG_HMC5883
474
    const hmc5883Config_t *hmc5883Config = 0;
475
476

#ifdef NAZE
477
478
479
    static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
            .gpioPin = Pin_12,
480
481
482
483
484
485
486
487
            .gpioPort = GPIOB,

            /* Disabled for v4 needs more work.
            .exti_port_source = GPIO_PortSourceGPIOB,
            .exti_pin_source = GPIO_PinSource12,
            .exti_line = EXTI_Line12,
            .exti_irqn = EXTI15_10_IRQn
            */
488
489
490
491
    };
    static const hmc5883Config_t nazeHmc5883Config_v5 = {
            .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
            .gpioPin = Pin_14,
492
493
494
            .gpioPort = GPIOC,
            .exti_port_source = GPIO_PortSourceGPIOC,
            .exti_line = EXTI_Line14,
495
            .exti_pin_source = GPIO_PinSource14,
496
            .exti_irqn = EXTI15_10_IRQn
497
    };
498
    if (hardwareRevision < NAZE32_REV5) {
499
        hmc5883Config = &nazeHmc5883Config_v1_v4;
500
    } else {
501
        hmc5883Config = &nazeHmc5883Config_v5;
502
503
    }
#endif
504
505

#ifdef SPRACINGF3
506
    static const hmc5883Config_t spRacingF3Hmc5883Config = {
507
508
        .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
        .gpioPin = Pin_14,
509
510
511
512
513
        .gpioPort = GPIOC,
        .exti_port_source = EXTI_PortSourceGPIOC,
        .exti_pin_source = EXTI_PinSource14,
        .exti_line = EXTI_Line14,
        .exti_irqn = EXTI15_10_IRQn
514
515
516
517
518
    };

    hmc5883Config = &spRacingF3Hmc5883Config;
#endif

519
520
521
522
523
524
525
#endif

retry:

    magAlign = ALIGN_DEFAULT;

    switch(magHardwareToUse) {
526
527
        case MAG_DEFAULT:
            ; // fallthrough
528
529

        case MAG_HMC5883:
530
#ifdef USE_MAG_HMC5883
531
            if (hmc5883lDetect(&mag, hmc5883Config)) {
532
533
#ifdef MAG_HMC5883_ALIGN
                magAlign = MAG_HMC5883_ALIGN;
534
535
#endif
                magHardware = MAG_HMC5883;
536
                break;
537
538
            }
#endif
539
            ; // fallthrough
540
541

        case MAG_AK8975:
542
#ifdef USE_MAG_AK8975
543
            if (ak8975detect(&mag)) {
544
545
546
#ifdef MAG_AK8975_ALIGN
                magAlign = MAG_AK8975_ALIGN;
#endif
547
                magHardware = MAG_AK8975;
548
                break;
549
550
            }
#endif
551
            ; // fallthrough
552
553
554
555

        case MAG_NONE:
            magHardware = MAG_NONE;
            break;
556
557
    }

558
    if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
559
560
561
        // Nothing was found and we have a forced sensor that isn't present.
        magHardwareToUse = MAG_DEFAULT;
        goto retry;
562
563
    }

564
565
    if (magHardware == MAG_NONE) {
        return;
566
567
    }

568
569
    detectedSensors[SENSOR_INDEX_MAG] = magHardware;
    sensorsSet(SENSOR_MAG);
570
571
}

572
573
574
575
576
577
578
579
580
581
582
583
584
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
{
    if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
        gyroAlign = sensorAlignmentConfig->gyro_align;
    }
    if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
        accAlign = sensorAlignmentConfig->acc_align;
    }
    if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
        magAlign = sensorAlignmentConfig->mag_align;
    }
}

585
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig)
586
587
{
    int16_t deg, min;
588

589
590
591
592
593
594
595
596
    memset(&acc, sizeof(acc), 0);
    memset(&gyro, sizeof(gyro), 0);

    if (!detectGyro(gyroLpf)) {
        return false;
    }
    detectAcc(accHardwareToUse);
    detectBaro();
597

598
599
600
601
602
603
604

    // 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();

Dominic Clifton's avatar
Dominic Clifton committed
605
606
607
608
    detectMag(magHardwareToUse);

    reconfigureAlignment(sensorAlignmentConfig);

609
610
611
612
613
614
615
616
617
618
619
620
621
622
    // 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;
}