static void detectAcc(accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
retry:
accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) {
case ACC_DEFAULT:
; // 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(&acc_params, &acc)) {
#else
if (adxl345Detect(&acc_params, &acc)) {
#endif
#ifdef ACC_ADXL345_ALIGN
accAlign = ACC_ADXL345_ALIGN;
#endif
accHardware = ACC_ADXL345;
break;
}
#endif
; // fallthrough
case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
accAlign = ACC_LSM303DLHC_ALIGN;
#endif
accHardware = ACC_LSM303DLHC;
break;
}
#endif
; // fallthrough
case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(&acc)) {
#ifdef ACC_MPU6050_ALIGN
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(&acc)) {
#else
if (mma8452Detect(&acc)) {
#endif
#ifdef ACC_MMA8452_ALIGN
accAlign = ACC_MMA8452_ALIGN;
#endif
accHardware = ACC_MMA8452;
break;
}
#endif
; // fallthrough
case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
if (bma280Detect(&acc)) {
#ifdef ACC_BMA280_ALIGN
accAlign = ACC_BMA280_ALIGN;
#endif
accHardware = ACC_BMA280;
break;
}
#endif
; // fallthrough
case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_MPU6000_ALIGN
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(&acc) || mpu6500SpiAccDetect(&acc))
#else
if (mpu6500AccDetect(&acc))
#endif
{
#ifdef ACC_MPU6500_ALIGN
accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_MPU6500;
break;
}
#endif
; // fallthrough
case ACC_MPU9250:
#ifdef USE_ACC_SPI_MPU9250
if (mpu9250SpiAccDetect(&acc))
{
accHardware = ACC_MPU9250;
#ifdef ACC_MPU9250_ALIGN
accAlign = ACC_MPU9250_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
if (fakeAccDetect(&acc)) {
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_DEFAULT && accHardwareToUse != ACC_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
}
if (accHardware == ACC_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
}
static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifndef BARO
UNUSED(baroHardwareToUse);
#else
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = 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 (baroHardware) {
case BARO_DEFAULT:
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) {
baroHardware = BARO_BMP085;
break;
}
#endif
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP280:
#ifdef USE_BARO_BMP280
if (bmp280Detect(&baro)) {
baroHardware = BARO_BMP280;
break;
}
#endif
; // fallthough
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
if (baroHardware == BARO_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
#endif
}
static void detectMag(magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
#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
retry:
magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_DEFAULT:
; // fallthrough
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
}
#endif
; // fallthrough
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975Detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
}
#endif
; // fallthrough
case MAG_AK8963:
#ifdef USE_MAG_AK8963
if (ak8963Detect(&mag)) {
#ifdef MAG_AK8963_ALIGN
magAlign = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
}
#endif
; // fallthrough
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
}
if (magHardware == MAG_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
}
static void detectMag(magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
#ifdef USE_MAG_HMC5883
const hmc5883Config_t *hmc5883Config = 0;
#ifdef NAZE
static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
.gpioPin = Pin_12,
.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
*/
};
static const hmc5883Config_t nazeHmc5883Config_v5 = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
.gpioPin = Pin_14,
.gpioPort = GPIOC,
.exti_port_source = GPIO_PortSourceGPIOC,
.exti_line = EXTI_Line14,
.exti_pin_source = GPIO_PinSource14,
.exti_irqn = EXTI15_10_IRQn
};
if (hardwareRevision < NAZE32_REV5) {
hmc5883Config = &nazeHmc5883Config_v1_v4;
} else {
hmc5883Config = &nazeHmc5883Config_v5;
}
#endif
#ifdef SPRACINGF3
static const hmc5883Config_t spRacingF3Hmc5883Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPin = Pin_14,
.gpioPort = GPIOC,
.exti_port_source = EXTI_PortSourceGPIOC,
.exti_pin_source = EXTI_PinSource14,
.exti_line = EXTI_Line14,
.exti_irqn = EXTI15_10_IRQn
};
hmc5883Config = &spRacingF3Hmc5883Config;
#endif
#endif
retry:
magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_DEFAULT:
; // fallthrough
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
}
#endif
; // fallthrough
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975Detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
}
#endif
; // fallthrough
case MAG_AK8963:
#ifdef USE_MAG_AK8963
if (ak8963Detect(&mag)) {
#ifdef MAG_AK8963_ALIGN
magAlign = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
}
#endif
; // fallthrough
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
}
if (magHardware == MAG_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
}
bool compassDetect(magDev_t *dev)
{
magSensor_e magHardware = MAG_NONE;
busDevice_t *busdev = &dev->busdev;
#ifdef USE_MAG_DATA_READY_SIGNAL
dev->magIntExtiTag = compassConfig()->interruptTag;
#endif
switch (compassConfig()->mag_bustype) {
#ifdef USE_I2C
case BUSTYPE_I2C:
busdev->bustype = BUSTYPE_I2C;
busdev->busdev_u.i2c.device = I2C_CFG_TO_DEV(compassConfig()->mag_i2c_device);
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
#endif
break;
#ifdef USE_SPI
case BUSTYPE_SPI:
busdev->bustype = BUSTYPE_SPI;
spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(compassConfig()->mag_spi_device)));
busdev->busdev_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn);
#endif
break;
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_MPU_SLAVE:
{
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
busdev->bustype = BUSTYPE_MPU_SLAVE;
busdev->busdev_u.mpuSlave.master = gyroSensorBus();
busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
} else {
return false;
}
}
#endif
break;
default:
return false;
}
dev->magAlign = ALIGN_DEFAULT;
switch (compassConfig()->mag_hardware) {
case MAG_DEFAULT:
FALLTHROUGH;
case MAG_HMC5883:
#if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (hmc5883lDetect(dev)) {
#ifdef MAG_HMC5883_ALIGN
dev->magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
}
#endif
FALLTHROUGH;
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (ak8975Detect(dev)) {
#ifdef MAG_AK8975_ALIGN
dev->magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
}
#endif
FALLTHROUGH;
case MAG_AK8963:
#if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963)
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
dev->busdev.bustype = BUSTYPE_MPU_SLAVE;
busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus();
}
if (ak8963Detect(dev)) {
#ifdef MAG_AK8963_ALIGN
dev->magAlign = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
}
#endif
FALLTHROUGH;
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
return true;
}
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
retry:
dev->magAlign = ALIGN_DEFAULT;
switch (magHardwareToUse) {
case MAG_DEFAULT:
; // fallthrough
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(dev, compassConfig()->interruptTag)) {
#ifdef MAG_HMC5883_ALIGN
dev->magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
}
#endif
; // fallthrough
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975Detect(dev)) {
#ifdef MAG_AK8975_ALIGN
dev->magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
}
#endif
; // fallthrough
case MAG_AK8963:
#ifdef USE_MAG_AK8963
if (ak8963Detect(dev)) {
#ifdef MAG_AK8963_ALIGN
dev->magAlign = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
}
#endif
; // fallthrough
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
}
if (magHardware == MAG_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
return true;
}
