Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Moved sensor detection into respective sensor module

Browse source
This commit is contained in:
Martin Budden 2016-12-12 11:18:41 +00:00
parent daba9d8e56
commit 19e2876148
15 changed files with 673 additions and 649 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/barometer_bmp085.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "io_types.h"
typedef struct bmp085Config_s {
ioTag_t xclrIO;
ioTag_t eocIO;

195
src/main/sensors/acceleration.c
View file

@ -17,6 +17,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#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)

2
src/main/sensors/acceleration.h
View file

@ -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);

89
src/main/sensors/barometer.c
View file

@ -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;

3
src/main/sensors/barometer.h
View file

@ -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

125
src/main/sensors/compass.c
View file

@ -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)

1
src/main/sensors/compass.h
View file

@ -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);

174
src/main/sensors/gyro.c
View file

@ -17,6 +17,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#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)

2
src/main/sensors/gyro.h
View file

@ -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);

638
src/main/sensors/initialisation.c
View file

@ -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) {

48
src/main/sensors/pitotmeter.c
View file

@ -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;

3
src/main/sensors/pitotmeter.h
View file

@ -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

37
src/main/sensors/rangefinder.c
View file

@ -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)

1
src/main/sensors/rangefinder.h
View file

@ -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);

2
src/main/sensors/sensors.h
View file

@ -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];