From 19e2876148dba92bb6e5a274a8c4670e894418e8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 12 Dec 2016 11:18:41 +0000 Subject: [PATCH] Moved sensor detection into respective sensor module --- src/main/drivers/barometer_bmp085.h | 2 + src/main/sensors/acceleration.c | 195 ++++++++- src/main/sensors/acceleration.h | 2 +- src/main/sensors/barometer.c | 89 ++++ src/main/sensors/barometer.h | 3 +- src/main/sensors/compass.c | 125 ++++++ src/main/sensors/compass.h | 1 + src/main/sensors/gyro.c | 174 +++++++- src/main/sensors/gyro.h | 2 +- src/main/sensors/initialisation.c | 638 +--------------------------- src/main/sensors/pitotmeter.c | 48 ++- src/main/sensors/pitotmeter.h | 3 +- src/main/sensors/rangefinder.c | 37 +- src/main/sensors/rangefinder.h | 1 + src/main/sensors/sensors.h | 2 + 15 files changed, 673 insertions(+), 649 deletions(-) diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index f6c6dece76..02fc0d8eb3 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -17,6 +17,8 @@ #pragma once +#include "io_types.h" + typedef struct bmp085Config_s { ioTag_t xclrIO; ioTag_t eocIO; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 14b32512a0..5d2bff834d 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -17,6 +17,7 @@ #include #include +#include #include #include "platform.h" @@ -25,17 +26,38 @@ #include "common/maths.h" #include "common/filter.h" -#include "drivers/sensor.h" - -#include "sensors/battery.h" -#include "sensors/sensors.h" -#include "io/beeper.h" -#include "sensors/boardalignment.h" -#include "fc/runtime_config.h" - #include "config/config.h" +#include "drivers/accgyro.h" +#include "drivers/accgyro_adxl345.h" +#include "drivers/accgyro_bma280.h" +#include "drivers/accgyro_fake.h" +#include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_mma845x.h" +#include "drivers/accgyro_mpu.h" +#include "drivers/accgyro_mpu3050.h" +#include "drivers/accgyro_mpu6050.h" +#include "drivers/accgyro_mpu6500.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" +#include "drivers/accgyro_spi_mpu6000.h" +#include "drivers/accgyro_spi_mpu6500.h" +#include "drivers/accgyro_spi_mpu9250.h" +#include "drivers/logging.h" +#include "drivers/sensor.h" + #include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + +#include "fc/runtime_config.h" + +#include "io/beeper.h" + +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif acc_t acc; // acc access functions @@ -48,8 +70,162 @@ static flightDynamicsTrims_t * accGain; static uint8_t accLpfCutHz = 0; static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; -void accInit(uint32_t targetLooptime) +static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { + accelerationSensor_e accHardware; + +#ifdef USE_ACC_ADXL345 + drv_adxl345_config_t acc_params; +#endif + +retry: + dev->accAlign = ALIGN_DEFAULT; + + requestedSensors[SENSOR_INDEX_ACC] = accHardwareToUse; + + switch (accHardwareToUse) { + case ACC_AUTODETECT: + ; // fallthrough + case ACC_ADXL345: // ADXL345 +#ifdef USE_ACC_ADXL345 + acc_params.useFifo = false; + acc_params.dataRate = 800; // unused currently +#ifdef NAZE + if (hardwareRevision < NAZE32_REV5 && adxl345Detect(dev, &acc_params)) { +#else + if (adxl345Detect(dev, &acc_params)) { +#endif +#ifdef ACC_ADXL345_ALIGN + dev->accAlign = ACC_ADXL345_ALIGN; +#endif + accHardware = ACC_ADXL345; + break; + } +#endif + ; // fallthrough + case ACC_LSM303DLHC: +#ifdef USE_ACC_LSM303DLHC + if (lsm303dlhcAccDetect(dev)) { +#ifdef ACC_LSM303DLHC_ALIGN + dev->accAlign = ACC_LSM303DLHC_ALIGN; +#endif + accHardware = ACC_LSM303DLHC; + break; + } +#endif + ; // fallthrough + case ACC_MPU6050: // MPU6050 +#ifdef USE_ACC_MPU6050 + if (mpu6050AccDetect(dev)) { +#ifdef ACC_MPU6050_ALIGN + dev->accAlign = ACC_MPU6050_ALIGN; +#endif + accHardware = ACC_MPU6050; + break; + } +#endif + ; // fallthrough + case ACC_MMA8452: // MMA8452 +#ifdef USE_ACC_MMA8452 +#ifdef NAZE + // Not supported with this frequency + if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) { +#else + if (mma8452Detect(dev)) { +#endif +#ifdef ACC_MMA8452_ALIGN + dev->accAlign = ACC_MMA8452_ALIGN; +#endif + accHardware = ACC_MMA8452; + break; + } +#endif + ; // fallthrough + case ACC_BMA280: // BMA280 +#ifdef USE_ACC_BMA280 + if (bma280Detect(dev)) { +#ifdef ACC_BMA280_ALIGN + dev->accAlign = ACC_BMA280_ALIGN; +#endif + accHardware = ACC_BMA280; + break; + } +#endif + ; // fallthrough + case ACC_MPU6000: +#ifdef USE_ACC_SPI_MPU6000 + if (mpu6000SpiAccDetect(dev)) { +#ifdef ACC_MPU6000_ALIGN + dev->accAlign = ACC_MPU6000_ALIGN; +#endif + accHardware = ACC_MPU6000; + break; + } +#endif + ; // fallthrough + case ACC_MPU6500: +#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) +#ifdef USE_ACC_SPI_MPU6500 + if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) { +#else + if (mpu6500AccDetect(dev)) { +#endif +#ifdef ACC_MPU6500_ALIGN + dev->accAlign = ACC_MPU6500_ALIGN; +#endif + accHardware = ACC_MPU6500; + break; + } +#endif + case ACC_MPU9250: +#ifdef USE_ACC_SPI_MPU9250 + if (mpu9250SpiAccDetect(dev)) { +#ifdef ACC_MPU9250_ALIGN + dev->accAlign = ACC_MPU9250_ALIGN; +#endif + accHardware = ACC_MPU9250; + break; + } +#endif + ; // fallthrough + case ACC_FAKE: +#ifdef USE_FAKE_ACC + if (fakeAccDetect(dev)) { + accHardware = ACC_FAKE; + break; + } +#endif + ; // fallthrough + case ACC_NONE: // disable ACC + accHardware = ACC_NONE; + break; + + } + + // Found anything? Check if error or ACC is really missing. + if (accHardware == ACC_NONE && accHardwareToUse != ACC_AUTODETECT && accHardwareToUse != ACC_NONE) { + // Nothing was found and we have a forced sensor that isn't present. + accHardwareToUse = ACC_AUTODETECT; + goto retry; + } + + addBootlogEvent6(BOOT_EVENT_ACC_DETECTION, BOOT_EVENT_FLAGS_NONE, accHardware, 0, 0, 0); + + if (accHardware == ACC_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_ACC] = accHardware; + sensorsSet(SENSOR_ACC); + return true; +} + +bool accInit(const accelerometerConfig_t *accConfig, uint32_t targetLooptime) +{ + memset(&acc, 0, sizeof(acc)); + if (!accDetect(&acc.dev, accConfig->acc_hardware)) { + return false; + } acc.dev.acc_1G = 256; // set default acc.dev.init(&acc.dev); acc.accTargetLooptime = targetLooptime; @@ -58,6 +234,7 @@ void accInit(uint32_t targetLooptime) biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accTargetLooptime); } } + return true; } void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index c4fcc143b7..bf44b35329 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -50,7 +50,7 @@ typedef struct accelerometerConfig_s { uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device } accelerometerConfig_t; -void accInit(uint32_t accTargetLooptime); +bool accInit(const accelerometerConfig_t *accConfig, uint32_t accTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void updateAccelerationReadings(void); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 6ffc7d1eb4..be9532ae98 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -24,11 +24,23 @@ #include "common/maths.h" #include "drivers/barometer.h" +#include "drivers/barometer_bmp085.h" +#include "drivers/barometer_bmp280.h" +#include "drivers/barometer_fake.h" +#include "drivers/barometer_ms5611.h" +#include "drivers/logging.h" + +#include "fc/runtime_config.h" #include "sensors/barometer.h" +#include "sensors/sensors.h" #include "flight/hil.h" +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif + baro_t baro; // barometer access functions #ifdef BARO @@ -40,6 +52,83 @@ static int32_t baroGroundPressure = 0; static barometerConfig_t *barometerConfig; +bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) +{ + // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function + + baroSensor_e baroHardware = BARO_NONE; + requestedSensors[SENSOR_INDEX_BARO] = baroHardwareToUse; + +#ifdef USE_BARO_BMP085 + + const bmp085Config_t *bmp085Config = NULL; + +#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) + static const bmp085Config_t defaultBMP085Config = { + .xclrIO = IO_TAG(BARO_XCLR_PIN), + .eocIO = IO_TAG(BARO_EOC_PIN), + }; + bmp085Config = &defaultBMP085Config; +#endif + +#ifdef NAZE + if (hardwareRevision == NAZE32) { + bmp085Disable(bmp085Config); + } +#endif + +#endif + + switch (baroHardwareToUse) { + case BARO_BMP085: +#ifdef USE_BARO_BMP085 + if (bmp085Detect(bmp085Config, dev)) { + baroHardware = BARO_BMP085; + } +#endif + break; + + case BARO_MS5611: +#ifdef USE_BARO_MS5611 + if (ms5611Detect(dev)) { + baroHardware = BARO_MS5611; + } +#endif + break; + + case BARO_BMP280: +#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) + if (bmp280Detect(dev)) { + baroHardware = BARO_BMP280; + } +#endif + break; + + case BARO_FAKE: +#ifdef USE_FAKE_BARO + if (fakeBaroDetect(dev)) { + baroHardware = BARO_FAKE; + } +#endif + break; + + case BARO_NONE: + baroHardware = BARO_NONE; + break; + } + + addBootlogEvent6(BOOT_EVENT_BARO_DETECTION, BOOT_EVENT_FLAGS_NONE, baroHardware, 0, 0, 0); + + if (baroHardware == BARO_NONE) { + sensorsClear(SENSOR_BARO); + return false; + } + + detectedSensors[SENSOR_INDEX_BARO] = baroHardware; + sensorsSet(SENSOR_BARO); + return true; +} + void useBarometerConfig(barometerConfig_t *barometerConfigToUse) { barometerConfig = barometerConfigToUse; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index b1df84f174..33d990cdfd 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -41,7 +41,7 @@ typedef struct baro_s { extern baro_t baro; -#ifdef BARO +bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse); void useBarometerConfig(barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); @@ -49,4 +49,3 @@ uint32_t baroUpdate(void); bool isBaroReady(void); int32_t baroCalculateAltitude(void); bool isBarometerHealthy(void); -#endif diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7a10afad7a..b9645201ee 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -28,10 +28,20 @@ #include "config/config.h" #include "drivers/compass.h" +#include "drivers/compass_ak8963.h" +#include "drivers/compass_ak8975.h" +#include "drivers/compass_fake.h" +#include "drivers/compass_hmc5883l.h" +#include "drivers/compass_mag3110.h" +#include "drivers/io.h" #include "drivers/light_led.h" +#include "drivers/logging.h" +#include "drivers/system.h" #include "fc/runtime_config.h" +#include "io/gps.h" + #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/compass.h" @@ -48,6 +58,121 @@ static int16_t magADCRaw[XYZ_AXIS_COUNT]; static uint8_t magInit = 0; static uint8_t magUpdatedAtLeastOnce = 0; +bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) +{ + magSensor_e magHardware = MAG_NONE; + requestedSensors[SENSOR_INDEX_MAG] = magHardwareToUse; + +#ifdef USE_MAG_HMC5883 + const hmc5883Config_t *hmc5883Config = 0; + +#ifdef NAZE // TODO remove this target specific define + static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { + .intTag = IO_TAG(PB12) /* perhaps disabled? */ + }; + static const hmc5883Config_t nazeHmc5883Config_v5 = { + .intTag = IO_TAG(MAG_INT_EXTI) + }; + if (hardwareRevision < NAZE32_REV5) { + hmc5883Config = &nazeHmc5883Config_v1_v4; + } else { + hmc5883Config = &nazeHmc5883Config_v5; + } +#endif + +#ifdef MAG_INT_EXTI + static const hmc5883Config_t extiHmc5883Config = { + .intTag = IO_TAG(MAG_INT_EXTI) + }; + + hmc5883Config = &extiHmc5883Config; +#endif + +#endif + + dev->magAlign = ALIGN_DEFAULT; + + switch(magHardwareToUse) { + case MAG_HMC5883: +#ifdef USE_MAG_HMC5883 + if (hmc5883lDetect(dev, hmc5883Config)) { +#ifdef MAG_HMC5883_ALIGN + dev->magAlign = MAG_HMC5883_ALIGN; +#endif + magHardware = MAG_HMC5883; + } +#endif + break; + + case MAG_AK8975: +#ifdef USE_MAG_AK8975 + if (ak8975Detect(dev)) { +#ifdef MAG_AK8975_ALIGN + dev->magAlign = MAG_AK8975_ALIGN; +#endif + magHardware = MAG_AK8975; + } +#endif + break; + + case MAG_AK8963: +#ifdef USE_MAG_AK8963 + if (ak8963Detect(dev)) { +#ifdef MAG_AK8963_ALIGN + dev->magAlign = MAG_AK8963_ALIGN; +#endif + magHardware = MAG_AK8963; + } +#endif + break; + + case MAG_GPS: +#ifdef GPS + if (gpsMagDetect(dev)) { +#ifdef MAG_GPS_ALIGN + dev->magAlign = MAG_GPS_ALIGN; +#endif + magHardware = MAG_GPS; + } +#endif + break; + + case MAG_MAG3110: +#ifdef USE_MAG_MAG3110 + if (mag3110detect(dev)) { +#ifdef MAG_MAG3110_ALIGN + dev->magAlign = MAG_MAG3110_ALIGN; +#endif + magHardware = MAG_MAG3110; + } +#endif + break; + + case MAG_FAKE: +#ifdef USE_FAKE_MAG + if (fakeMagDetect(dev)) { + magHardware = MAG_FAKE; + } +#endif + break; + + case MAG_NONE: + magHardware = MAG_NONE; + break; + } + + addBootlogEvent6(BOOT_EVENT_MAG_DETECTION, BOOT_EVENT_FLAGS_NONE, magHardware, 0, 0, 0); + + if (magHardware == MAG_NONE) { + sensorsClear(SENSOR_MAG); + return false; + } + + detectedSensors[SENSOR_INDEX_MAG] = magHardware; + sensorsSet(SENSOR_MAG); + return true; +} + bool compassInit(const compassConfig_t *compassConfig) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 2d47362c33..47d9d78f14 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -50,6 +50,7 @@ typedef struct compassConfig_s { uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device } compassConfig_t; +bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); bool compassInit(const compassConfig_t *compassConfig); union flightDynamicsTrims_u; void compassUpdate(timeUs_t currentTimeUs, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 58c3d4d0f0..bcebebdb66 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -17,6 +17,7 @@ #include #include +#include #include #include "platform.h" @@ -25,16 +26,39 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/config.h" + +#include "drivers/accgyro.h" +#include "drivers/accgyro_adxl345.h" +#include "drivers/accgyro_bma280.h" +#include "drivers/accgyro_fake.h" +#include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_mma845x.h" +#include "drivers/accgyro_mpu.h" +#include "drivers/accgyro_mpu3050.h" +#include "drivers/accgyro_mpu6050.h" +#include "drivers/accgyro_mpu6500.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" +#include "drivers/accgyro_spi_mpu6000.h" +#include "drivers/accgyro_spi_mpu6500.h" +#include "drivers/accgyro_spi_mpu9250.h" #include "drivers/gyro_sync.h" +#include "drivers/io.h" +#include "drivers/logging.h" + +#include "fc/runtime_config.h" #include "io/beeper.h" #include "io/statusindicator.h" -#include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/gyro.h" +#include "sensors/sensors.h" -#include "config/config.h" +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif gyro_t gyro; // gyro access functions @@ -45,9 +69,152 @@ static uint16_t calibratingG = 0; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; -void gyroInit(const gyroConfig_t *gyroConfigToUse) +static const extiConfig_t *selectMPUIntExtiConfig(void) +{ +#if defined(MPU_INT_EXTI) + static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) }; + return &mpuIntExtiConfig; +#elif defined(USE_HARDWARE_REVISION_DETECTION) + return selectMPUIntExtiConfigByHardwareRevision(); +#else + return NULL; +#endif +} + +static bool gyroDetect(gyroDev_t *dev) +{ + gyroSensor_e gyroHardware = GYRO_AUTODETECT; + + dev->gyroAlign = ALIGN_DEFAULT; + + switch(gyroHardware) { + case GYRO_AUTODETECT: + ; // fallthrough + case GYRO_MPU6050: +#ifdef USE_GYRO_MPU6050 + if (mpu6050GyroDetect(dev)) { + gyroHardware = GYRO_MPU6050; +#ifdef GYRO_MPU6050_ALIGN + dev->gyroAlign = GYRO_MPU6050_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + case GYRO_L3G4200D: +#ifdef USE_GYRO_L3G4200D + if (l3g4200dDetect(dev)) { + gyroHardware = GYRO_L3G4200D; +#ifdef GYRO_L3G4200D_ALIGN + dev->gyroAlign = GYRO_L3G4200D_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_MPU3050: +#ifdef USE_GYRO_MPU3050 + if (mpu3050Detect(dev)) { + gyroHardware = GYRO_MPU3050; +#ifdef GYRO_MPU3050_ALIGN + dev->gyroAlign = GYRO_MPU3050_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_L3GD20: +#ifdef USE_GYRO_L3GD20 + if (l3gd20Detect(dev)) { + gyroHardware = GYRO_L3GD20; +#ifdef GYRO_L3GD20_ALIGN + dev->gyroAlign = GYRO_L3GD20_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_MPU6000: +#ifdef USE_GYRO_SPI_MPU6000 + if (mpu6000SpiGyroDetect(dev)) { + gyroHardware = GYRO_MPU6000; +#ifdef GYRO_MPU6000_ALIGN + dev->gyroAlign = GYRO_MPU6000_ALIGN; +#endif + break; + } +#endif + ; // fallthrough + + case GYRO_MPU6500: +#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) +#ifdef USE_GYRO_SPI_MPU6500 + if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) { +#else + if (mpu6500GyroDetect(dev)) { +#endif + gyroHardware = GYRO_MPU6500; +#ifdef GYRO_MPU6500_ALIGN + dev->gyroAlign = GYRO_MPU6500_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough + + case GYRO_MPU9250: +#ifdef USE_GYRO_SPI_MPU9250 + if (mpu9250SpiGyroDetect(dev)) + { + gyroHardware = GYRO_MPU9250; +#ifdef GYRO_MPU9250_ALIGN + dev->gyroAlign = GYRO_MPU9250_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough + case GYRO_FAKE: +#ifdef USE_FAKE_GYRO + if (fakeGyroDetect(dev)) { + gyroHardware = GYRO_FAKE; + break; + } +#endif + ; // fallthrough + case GYRO_NONE: + gyroHardware = GYRO_NONE; + } + + addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0); + + if (gyroHardware == GYRO_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + sensorsSet(SENSOR_GYRO); + + return true; +} + +bool gyroInit(const gyroConfig_t *gyroConfigToUse) { gyroConfig = gyroConfigToUse; +#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) + const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); + mpuDetect(extiConfig); +#endif + + memset(&gyro, 0, sizeof(gyro)); + if (!gyroDetect(&gyro.dev)) { + return false; + } // After refactoring this function is always called after gyro sampling rate is known, so // no additional condition is required // Set gyro sample rate before driver initialisation @@ -64,6 +231,7 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse) #endif } } + return true; } void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d7ecbd5fe9..a852384d87 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -52,7 +52,7 @@ typedef struct gyroConfig_s { } gyroConfig_t; -void gyroInit(const gyroConfig_t *gyroConfigToUse); +bool gyroInit(const gyroConfig_t *gyroConfigToUse); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroUpdate(void); bool isGyroCalibrationComplete(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index c091ee59e7..08028d95f9 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -20,63 +20,14 @@ #include "platform.h" -#include "build/build_config.h" +#include "common/utils.h" -#include "common/axis.h" +#include "config/config.h" #include "drivers/logging.h" -#include "drivers/io.h" -#include "drivers/system.h" -#include "drivers/exti.h" - -#include "drivers/sensor.h" - -#include "drivers/accgyro.h" -#include "drivers/accgyro_adxl345.h" -#include "drivers/accgyro_bma280.h" -#include "drivers/accgyro_fake.h" -#include "drivers/accgyro_l3g4200d.h" -#include "drivers/accgyro_mma845x.h" -#include "drivers/accgyro_mpu.h" -#include "drivers/accgyro_mpu3050.h" -#include "drivers/accgyro_mpu6050.h" -#include "drivers/accgyro_mpu6500.h" -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" - -#include "drivers/bus_spi.h" -#include "drivers/accgyro_spi_mpu6000.h" -#include "drivers/accgyro_spi_mpu6500.h" -#include "drivers/accgyro_spi_mpu9250.h" - -#include "drivers/barometer.h" -#include "drivers/barometer_bmp085.h" -#include "drivers/barometer_bmp280.h" -#include "drivers/barometer_fake.h" -#include "drivers/barometer_ms5611.h" - -#include "drivers/pitotmeter.h" -#include "drivers/pitotmeter_ms4525.h" -#include "drivers/pitotmeter_fake.h" - -#include "drivers/compass.h" -#include "drivers/compass_ak8963.h" -#include "drivers/compass_ak8975.h" -#include "drivers/compass_fake.h" -#include "drivers/compass_hmc5883l.h" -#include "drivers/compass_mag3110.h" - -#include "drivers/sonar_hcsr04.h" -#include "drivers/sonar_srf10.h" - #include "fc/runtime_config.h" -#include "config/config.h" -#include "config/feature.h" - -#include "io/gps.h" - #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -86,603 +37,36 @@ #include "sensors/rangefinder.h" #include "sensors/initialisation.h" -#ifdef USE_HARDWARE_REVISION_DETECTION -#include "hardware_revision.h" -#endif - uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE }; uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE }; -const extiConfig_t *selectMPUIntExtiConfig(void) -{ -#if defined(MPU_INT_EXTI) - static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) }; - return &mpuIntExtiConfig; -#elif defined(USE_HARDWARE_REVISION_DETECTION) - return selectMPUIntExtiConfigByHardwareRevision(); -#else - return NULL; -#endif -} - -static bool detectGyro(gyroDev_t *dev) -{ - gyroSensor_e gyroHardware = GYRO_AUTODETECT; - - dev->gyroAlign = ALIGN_DEFAULT; - - switch(gyroHardware) { - case GYRO_AUTODETECT: - ; // fallthrough - case GYRO_MPU6050: -#ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(dev)) { - gyroHardware = GYRO_MPU6050; -#ifdef GYRO_MPU6050_ALIGN - dev->gyroAlign = GYRO_MPU6050_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - case GYRO_L3G4200D: -#ifdef USE_GYRO_L3G4200D - if (l3g4200dDetect(dev)) { - gyroHardware = GYRO_L3G4200D; -#ifdef GYRO_L3G4200D_ALIGN - dev->gyroAlign = GYRO_L3G4200D_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_MPU3050: -#ifdef USE_GYRO_MPU3050 - if (mpu3050Detect(dev)) { - gyroHardware = GYRO_MPU3050; -#ifdef GYRO_MPU3050_ALIGN - dev->gyroAlign = GYRO_MPU3050_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_L3GD20: -#ifdef USE_GYRO_L3GD20 - if (l3gd20Detect(dev)) { - gyroHardware = GYRO_L3GD20; -#ifdef GYRO_L3GD20_ALIGN - dev->gyroAlign = GYRO_L3GD20_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_MPU6000: -#ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiGyroDetect(dev)) { - gyroHardware = GYRO_MPU6000; -#ifdef GYRO_MPU6000_ALIGN - dev->gyroAlign = GYRO_MPU6000_ALIGN; -#endif - break; - } -#endif - ; // fallthrough - - case GYRO_MPU6500: -#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) -#ifdef USE_GYRO_SPI_MPU6500 - if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) { -#else - if (mpu6500GyroDetect(dev)) { -#endif - gyroHardware = GYRO_MPU6500; -#ifdef GYRO_MPU6500_ALIGN - dev->gyroAlign = GYRO_MPU6500_ALIGN; -#endif - - break; - } -#endif - ; // fallthrough - - case GYRO_MPU9250: -#ifdef USE_GYRO_SPI_MPU9250 - if (mpu9250SpiGyroDetect(dev)) - { - gyroHardware = GYRO_MPU9250; -#ifdef GYRO_MPU9250_ALIGN - dev->gyroAlign = GYRO_MPU9250_ALIGN; -#endif - - break; - } -#endif - ; // fallthrough - case GYRO_FAKE: -#ifdef USE_FAKE_GYRO - if (fakeGyroDetect(dev)) { - gyroHardware = GYRO_FAKE; - break; - } -#endif - ; // fallthrough - case GYRO_NONE: - gyroHardware = GYRO_NONE; - } - - addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0); - - if (gyroHardware == GYRO_NONE) { - return false; - } - - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; - sensorsSet(SENSOR_GYRO); - - return true; -} - -static bool detectAcc(accDev_t *dev, accelerationSensor_e accHardwareToUse) -{ - accelerationSensor_e accHardware; - -#ifdef USE_ACC_ADXL345 - drv_adxl345_config_t acc_params; -#endif - -retry: - dev->accAlign = ALIGN_DEFAULT; - - requestedSensors[SENSOR_INDEX_ACC] = accHardwareToUse; - - switch (accHardwareToUse) { - case ACC_AUTODETECT: - ; // fallthrough - case ACC_ADXL345: // ADXL345 -#ifdef USE_ACC_ADXL345 - acc_params.useFifo = false; - acc_params.dataRate = 800; // unused currently -#ifdef NAZE - if (hardwareRevision < NAZE32_REV5 && adxl345Detect(dev, &acc_params)) { -#else - if (adxl345Detect(dev, &acc_params)) { -#endif -#ifdef ACC_ADXL345_ALIGN - dev->accAlign = ACC_ADXL345_ALIGN; -#endif - accHardware = ACC_ADXL345; - break; - } -#endif - ; // fallthrough - case ACC_LSM303DLHC: -#ifdef USE_ACC_LSM303DLHC - if (lsm303dlhcAccDetect(dev)) { -#ifdef ACC_LSM303DLHC_ALIGN - dev->accAlign = ACC_LSM303DLHC_ALIGN; -#endif - accHardware = ACC_LSM303DLHC; - break; - } -#endif - ; // fallthrough - case ACC_MPU6050: // MPU6050 -#ifdef USE_ACC_MPU6050 - if (mpu6050AccDetect(dev)) { -#ifdef ACC_MPU6050_ALIGN - dev->accAlign = ACC_MPU6050_ALIGN; -#endif - accHardware = ACC_MPU6050; - break; - } -#endif - ; // fallthrough - case ACC_MMA8452: // MMA8452 -#ifdef USE_ACC_MMA8452 -#ifdef NAZE - // Not supported with this frequency - if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) { -#else - if (mma8452Detect(dev)) { -#endif -#ifdef ACC_MMA8452_ALIGN - dev->accAlign = ACC_MMA8452_ALIGN; -#endif - accHardware = ACC_MMA8452; - break; - } -#endif - ; // fallthrough - case ACC_BMA280: // BMA280 -#ifdef USE_ACC_BMA280 - if (bma280Detect(dev)) { -#ifdef ACC_BMA280_ALIGN - dev->accAlign = ACC_BMA280_ALIGN; -#endif - accHardware = ACC_BMA280; - break; - } -#endif - ; // fallthrough - case ACC_MPU6000: -#ifdef USE_ACC_SPI_MPU6000 - if (mpu6000SpiAccDetect(dev)) { -#ifdef ACC_MPU6000_ALIGN - dev->accAlign = ACC_MPU6000_ALIGN; -#endif - accHardware = ACC_MPU6000; - break; - } -#endif - ; // fallthrough - case ACC_MPU6500: -#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) -#ifdef USE_ACC_SPI_MPU6500 - if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) { -#else - if (mpu6500AccDetect(dev)) { -#endif -#ifdef ACC_MPU6500_ALIGN - dev->accAlign = ACC_MPU6500_ALIGN; -#endif - accHardware = ACC_MPU6500; - break; - } -#endif - case ACC_MPU9250: -#ifdef USE_ACC_SPI_MPU9250 - if (mpu9250SpiAccDetect(dev)) { -#ifdef ACC_MPU9250_ALIGN - dev->accAlign = ACC_MPU9250_ALIGN; -#endif - accHardware = ACC_MPU9250; - break; - } -#endif - ; // fallthrough - case ACC_FAKE: -#ifdef USE_FAKE_ACC - if (fakeAccDetect(dev)) { - accHardware = ACC_FAKE; - break; - } -#endif - ; // fallthrough - case ACC_NONE: // disable ACC - accHardware = ACC_NONE; - break; - - } - - // Found anything? Check if error or ACC is really missing. - if (accHardware == ACC_NONE && accHardwareToUse != ACC_AUTODETECT && accHardwareToUse != ACC_NONE) { - // Nothing was found and we have a forced sensor that isn't present. - accHardwareToUse = ACC_AUTODETECT; - goto retry; - } - - addBootlogEvent6(BOOT_EVENT_ACC_DETECTION, BOOT_EVENT_FLAGS_NONE, accHardware, 0, 0, 0); - - if (accHardware == ACC_NONE) { - return false; - } - - detectedSensors[SENSOR_INDEX_ACC] = accHardware; - sensorsSet(SENSOR_ACC); - return true; -} - -#ifdef BARO -static bool detectBaro(baroDev_t *dev, baroSensor_e baroHardwareToUse) -{ - // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function - - baroSensor_e baroHardware = BARO_NONE; - requestedSensors[SENSOR_INDEX_BARO] = baroHardwareToUse; - -#ifdef USE_BARO_BMP085 - - const bmp085Config_t *bmp085Config = NULL; - -#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) - static const bmp085Config_t defaultBMP085Config = { - .xclrIO = IO_TAG(BARO_XCLR_PIN), - .eocIO = IO_TAG(BARO_EOC_PIN), - }; - bmp085Config = &defaultBMP085Config; -#endif - -#ifdef NAZE - if (hardwareRevision == NAZE32) { - bmp085Disable(bmp085Config); - } -#endif - -#endif - - switch (baroHardwareToUse) { - case BARO_BMP085: -#ifdef USE_BARO_BMP085 - if (bmp085Detect(bmp085Config, dev)) { - baroHardware = BARO_BMP085; - } -#endif - break; - - case BARO_MS5611: -#ifdef USE_BARO_MS5611 - if (ms5611Detect(dev)) { - baroHardware = BARO_MS5611; - } -#endif - break; - - case BARO_BMP280: -#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) - if (bmp280Detect(dev)) { - baroHardware = BARO_BMP280; - } -#endif - break; - - case BARO_FAKE: -#ifdef USE_FAKE_BARO - if (fakeBaroDetect(dev)) { - baroHardware = BARO_FAKE; - } -#endif - break; - - case BARO_NONE: - baroHardware = BARO_NONE; - break; - } - - addBootlogEvent6(BOOT_EVENT_BARO_DETECTION, BOOT_EVENT_FLAGS_NONE, baroHardware, 0, 0, 0); - - if (baroHardware == BARO_NONE) { - sensorsClear(SENSOR_BARO); - return false; - } - - detectedSensors[SENSOR_INDEX_BARO] = baroHardware; - sensorsSet(SENSOR_BARO); - return true; -} -#endif // BARO - -#ifdef PITOT -static bool detectPitot(pitotDev_t *dev, uint8_t pitotHardwareToUse) -{ - pitotSensor_e pitotHardware = PITOT_NONE; - requestedSensors[SENSOR_INDEX_PITOT] = pitotHardwareToUse; - - switch (pitotHardwareToUse) { - case PITOT_MS4525: -#ifdef USE_PITOT_MS4525 - if (ms4525Detect(dev)) { - pitotHardware = PITOT_MS4525; - } -#endif - break; - - case PITOT_FAKE: -#ifdef USE_PITOT_FAKE - if (fakePitotDetect(&pitot)) { - pitotHardware = PITOT_FAKE; - } -#endif - break; - - case PITOT_NONE: - pitotHardware = PITOT_NONE; - break; - } - - addBootlogEvent6(BOOT_EVENT_PITOT_DETECTION, BOOT_EVENT_FLAGS_NONE, pitotHardware, 0, 0, 0); - - if (pitotHardware == PITOT_NONE) { - sensorsClear(SENSOR_PITOT); - return false; - } - - detectedSensors[SENSOR_INDEX_PITOT] = pitotHardware; - sensorsSet(SENSOR_PITOT); - return true; -} -#endif - -#ifdef MAG -static bool detectMag(magDev_t *dev, magSensor_e magHardwareToUse) -{ - magSensor_e magHardware = MAG_NONE; - requestedSensors[SENSOR_INDEX_MAG] = magHardwareToUse; - -#ifdef USE_MAG_HMC5883 - const hmc5883Config_t *hmc5883Config = 0; - -#ifdef NAZE // TODO remove this target specific define - static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { - .intTag = IO_TAG(PB12) /* perhaps disabled? */ - }; - static const hmc5883Config_t nazeHmc5883Config_v5 = { - .intTag = IO_TAG(MAG_INT_EXTI) - }; - if (hardwareRevision < NAZE32_REV5) { - hmc5883Config = &nazeHmc5883Config_v1_v4; - } else { - hmc5883Config = &nazeHmc5883Config_v5; - } -#endif - -#ifdef MAG_INT_EXTI - static const hmc5883Config_t extiHmc5883Config = { - .intTag = IO_TAG(MAG_INT_EXTI) - }; - - hmc5883Config = &extiHmc5883Config; -#endif - -#endif - - dev->magAlign = ALIGN_DEFAULT; - - switch(magHardwareToUse) { - case MAG_HMC5883: -#ifdef USE_MAG_HMC5883 - if (hmc5883lDetect(dev, hmc5883Config)) { -#ifdef MAG_HMC5883_ALIGN - dev->magAlign = MAG_HMC5883_ALIGN; -#endif - magHardware = MAG_HMC5883; - } -#endif - break; - - case MAG_AK8975: -#ifdef USE_MAG_AK8975 - if (ak8975Detect(dev)) { -#ifdef MAG_AK8975_ALIGN - dev->magAlign = MAG_AK8975_ALIGN; -#endif - magHardware = MAG_AK8975; - } -#endif - break; - - case MAG_AK8963: -#ifdef USE_MAG_AK8963 - if (ak8963Detect(dev)) { -#ifdef MAG_AK8963_ALIGN - dev->magAlign = MAG_AK8963_ALIGN; -#endif - magHardware = MAG_AK8963; - } -#endif - break; - - case MAG_GPS: -#ifdef GPS - if (gpsMagDetect(dev)) { -#ifdef MAG_GPS_ALIGN - dev->magAlign = MAG_GPS_ALIGN; -#endif - magHardware = MAG_GPS; - } -#endif - break; - - case MAG_MAG3110: -#ifdef USE_MAG_MAG3110 - if (mag3110detect(dev)) { -#ifdef MAG_MAG3110_ALIGN - dev->magAlign = MAG_MAG3110_ALIGN; -#endif - magHardware = MAG_MAG3110; - } -#endif - break; - - case MAG_FAKE: -#ifdef USE_FAKE_MAG - if (fakeMagDetect(dev)) { - magHardware = MAG_FAKE; - } -#endif - break; - - case MAG_NONE: - magHardware = MAG_NONE; - break; - } - - addBootlogEvent6(BOOT_EVENT_MAG_DETECTION, BOOT_EVENT_FLAGS_NONE, magHardware, 0, 0, 0); - - if (magHardware == MAG_NONE) { - sensorsClear(SENSOR_MAG); - return false; - } - - detectedSensors[SENSOR_INDEX_MAG] = magHardware; - sensorsSet(SENSOR_MAG); - return true; -} -#endif // MAG - -#ifdef SONAR -/* - * Detect which rangefinder is present - */ -static rangefinderType_e detectRangefinder(void) -{ - rangefinderType_e rangefinderType = RANGEFINDER_NONE; - if (feature(FEATURE_SONAR)) { - // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, - // since there is no way to detect it - rangefinderType = RANGEFINDER_HCSR04; - } -#ifdef USE_SONAR_SRF10 - if (srf10_detect()) { - // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04 - rangefinderType = RANGEFINDER_SRF10; - } -#endif - - addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderType, 0, 0, 0); - - requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; // FIXME: Make rangefinder type selectable from CLI - detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; - - if (rangefinderType != RANGEFINDER_NONE) { - sensorsSet(SENSOR_SONAR); - } - - return rangefinderType; -} -#endif - bool sensorsAutodetect(const gyroConfig_t *gyroConfig, const accelerometerConfig_t *accConfig, const compassConfig_t *compassConfig, const barometerConfig_t *baroConfig, const pitotmeterConfig_t *pitotConfig) { -#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) - const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); - mpuDetect(extiConfig); -#endif - - memset(&gyro, 0, sizeof(gyro)); - if (!detectGyro(&gyro.dev)) { + if (!gyroInit(gyroConfig)) { return false; } - gyroInit(gyroConfig); - memset(&acc, 0, sizeof(acc)); - if (detectAcc(&acc.dev, accConfig->acc_hardware)) { #ifdef ASYNC_GYRO_PROCESSING - // ACC will be updated at its own rate - accInit(getAccUpdateRate()); + // ACC will be updated at its own rate + accInit(accConfig, getAccUpdateRate()); #else - // acc updated at same frequency in taskMainPidLoop in mw.c - accInit(gyro.targetLooptime); + // acc updated at same frequency in taskMainPidLoop in mw.c + accInit(accConfig, gyro.targetLooptime); #endif - } #ifdef BARO - detectBaro(&baro.dev, baroConfig->baro_hardware); + baroDetect(&baro.dev, baroConfig->baro_hardware); #else UNUSED(baroConfig); #endif #ifdef PITOT - detectPitot(&pitot.dev, pitotConfig->pitot_hardware); + pitotDetect(&pitot.dev, pitotConfig->pitot_hardware); #else UNUSED(pitotConfig); #endif @@ -690,7 +74,7 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c mag.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. #ifdef MAG - if (detectMag(&mag.dev, compassConfig->mag_hardware)) { + if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { // calculate magnetic declination if (!compassInit(compassConfig)) { addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR); @@ -702,7 +86,7 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, #endif #ifdef SONAR - const rangefinderType_e rangefinderType = detectRangefinder(); + const rangefinderType_e rangefinderType = rangefinderDetect(); rangefinderInit(rangefinderType); #endif if (gyroConfig->gyro_align != ALIGN_DEFAULT) { diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 303fd7014e..174e790856 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -22,12 +22,19 @@ #include "platform.h" #include "build/debug.h" + #include "common/maths.h" -#include "drivers/pitotmeter.h" #include "config/config.h" +#include "drivers/logging.h" +#include "drivers/pitotmeter.h" +#include "drivers/pitotmeter_ms4525.h" + +#include "fc/runtime_config.h" + #include "sensors/pitotmeter.h" +#include "sensors/sensors.h" pitot_t pitot; @@ -41,6 +48,45 @@ static float CalibratedAirspeed = 0; pitotmeterConfig_t *pitotmeterConfig; +bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) +{ + pitotSensor_e pitotHardware = PITOT_NONE; + requestedSensors[SENSOR_INDEX_PITOT] = pitotHardwareToUse; + + switch (pitotHardwareToUse) { + case PITOT_MS4525: +#ifdef USE_PITOT_MS4525 + if (ms4525Detect(dev)) { + pitotHardware = PITOT_MS4525; + } +#endif + break; + + case PITOT_FAKE: +#ifdef USE_PITOT_FAKE + if (fakePitotDetect(&pitot)) { + pitotHardware = PITOT_FAKE; + } +#endif + break; + + case PITOT_NONE: + pitotHardware = PITOT_NONE; + break; + } + + addBootlogEvent6(BOOT_EVENT_PITOT_DETECTION, BOOT_EVENT_FLAGS_NONE, pitotHardware, 0, 0, 0); + + if (pitotHardware == PITOT_NONE) { + sensorsClear(SENSOR_PITOT); + return false; + } + + detectedSensors[SENSOR_INDEX_PITOT] = pitotHardware; + sensorsSet(SENSOR_PITOT); + return true; +} + void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse) { pitotmeterConfig = pitotmeterConfigToUse; diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 4813a5a178..6e4dc81b5e 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -42,7 +42,7 @@ typedef struct pito_s { extern pitot_t pitot; -#ifdef PITOT +bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse); void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse); bool isPitotCalibrationComplete(void); void pitotSetCalibrationCycles(uint16_t calibrationCyclesRequired); @@ -50,4 +50,3 @@ uint32_t pitotUpdate(void); bool isPitotReady(void); int32_t pitotCalculateAirSpeed(void); bool isPitotmeterHealthy(void); -#endif diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 81833b53c7..0c09251922 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -26,18 +26,19 @@ #include "build/build_config.h" - #include "common/maths.h" #include "config/config.h" -#include "fc/runtime_config.h" - +#include "config/feature.h" #include "drivers/io.h" +#include "drivers/logging.h" #include "drivers/sonar_hcsr04.h" #include "drivers/sonar_srf10.h" #include "drivers/rangefinder.h" +#include "fc/runtime_config.h" + #include "sensors/sensors.h" #include "sensors/rangefinder.h" #include "sensors/battery.h" @@ -56,6 +57,36 @@ static float rangefinderMaxTiltCos; static int32_t calculatedAltitude; +/* + * Detect which rangefinder is present + */ +rangefinderType_e rangefinderDetect(void) +{ + rangefinderType_e rangefinderType = RANGEFINDER_NONE; + if (feature(FEATURE_SONAR)) { + // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, + // since there is no way to detect it + rangefinderType = RANGEFINDER_HCSR04; + } +#ifdef USE_SONAR_SRF10 + if (srf10_detect()) { + // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04 + rangefinderType = RANGEFINDER_SRF10; + } +#endif + + addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderType, 0, 0, 0); + + requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; // FIXME: Make rangefinder type selectable from CLI + detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; + + if (rangefinderType != RANGEFINDER_NONE) { + sensorsSet(SENSOR_SONAR); + } + + return rangefinderType; +} + static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(currentSensor_e currentSensor) { #if defined(SONAR_PWM_TRIGGER_PIN) diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 2bbe557e48..f9212d6960 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -36,6 +36,7 @@ typedef struct rangefinderFunctionPointers_s { rangefinderReadFunctionPtr read; } rangefinderFunctionPointers_t; +rangefinderType_e rangefinderDetect(void); int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle); int32_t rangefinderGetLatestAltitude(void); rangefinderType_e rangefinderDetect(void); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 9237be57d5..a346cb3b30 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -61,3 +61,5 @@ typedef struct sensorTrims_s { flightDynamicsTrims_t magZero; // Compass offset } sensorTrims_t; +extern uint8_t requestedSensors[SENSOR_INDEX_COUNT]; +extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];