void sensorsInit(void)
{
zeroSensorAccumulators();
// TODO allow user to select hardware if there are multiple choices
if(mpu6050Detect(&gyro, &accel, cfg.mpu6050Scale)) {
sensorsSet(SENSOR_ACC);
} else if(!mpu3050Detect(&gyro)) {
failureMode(3);
}
if(adxl345Detect(&accel)) {
sensorsSet(SENSOR_ACC);
}
// At the moment we will do this after mpu6050 and overrride mpu6050 accel if detected
if(mma8452Detect(&accel)) {
sensorsSet(SENSOR_ACC);
}
gyro.init();
if(sensorsGet(SENSOR_ACC))
accel.init();
if(hmc5883Detect(&mag)) {
mag.init();
sensorsSet(SENSOR_MAG);
}
#ifdef SONAR
if(feature(FEATURE_PPM);) {
static void detectAcc(uint8_t accHardwareToUse)
{
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
retry:
accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) {
#ifdef USE_FAKE_ACC
default:
if (fakeAccDetect(&acc)) {
accHardware = ACC_FAKE;
if (accHardwareToUse == ACC_FAKE)
break;
}
#endif
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
#ifdef USE_ACC_ADXL345
case ACC_ADXL345: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc)) {
accHardware = ACC_ADXL345;
#ifdef NAZE
accAlign = CW270_DEG;
#endif
}
if (accHardwareToUse == ACC_ADXL345)
break;
; // fallthrough
#endif
#ifdef USE_ACC_MPU6050
case ACC_MPU6050: // MPU6050
if (mpu6050AccDetect(&acc)) {
accHardware = ACC_MPU6050;
#ifdef NAZE
accAlign = CW0_DEG;
#endif
if (accHardwareToUse == ACC_MPU6050)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_MMA8452
case ACC_MMA8452: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
#ifdef NAZE
accAlign = CW90_DEG;
#endif
if (accHardwareToUse == ACC_MMA8452)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_BMA280
case ACC_BMA280: // BMA280
if (bma280Detect(&acc)) {
accHardware = ACC_BMA280;
#ifdef NAZE
accAlign = CW0_DEG;
#endif
if (accHardwareToUse == ACC_BMA280)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_LSM303DLHC
case ACC_LSM303DLHC:
if (lsm303dlhcAccDetect(&acc)) {
accHardware = ACC_LSM303DLHC;
if (accHardwareToUse == ACC_LSM303DLHC)
break;
}
; // fallthrough
#endif
#ifdef USE_GYRO_SPI_MPU6000
case ACC_SPI_MPU6000:
if (mpu6000SpiAccDetect(&acc)) {
accHardware = ACC_SPI_MPU6000;
#ifdef CC3D
accAlign = CW270_DEG;
#endif
if (accHardwareToUse == ACC_SPI_MPU6000)
break;
}
; // fallthrough
#endif
; // prevent compiler error
}
// Found anything? Check if user f****d up or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (accHardwareToUse > ACC_DEFAULT) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
} else {
// No ACC was detected
sensorsClear(SENSOR_ACC);
}
}
}
bool sensorsAutodetect(void)
{
int16_t deg, min;
drv_adxl345_config_t acc_params;
bool haveMpu6k = false;
// Autodetect gyro hardware. We have MPU3050 or MPU6050.
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
// this filled up acc.* struct with init values
haveMpu6k = true;
} else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
// well, we found our gyro
;
} else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
return false;
}
// Accelerometer. F**k it. Let user break shit.
retry:
switch (mcfg.acc_hardware) {
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
case ACC_ADXL345: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc))
accHardware = ACC_ADXL345;
if (mcfg.acc_hardware == ACC_ADXL345)
break;
; // fallthrough
case ACC_MPU6050: // MPU6050
if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050)
break;
}
; // fallthrough
#ifndef OLIMEXINO
case ACC_MMA8452: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452)
break;
}
; // fallthrough
case ACC_BMA280: // BMA280
if (bma280Detect(&acc)) {
accHardware = ACC_BMA280;
if (mcfg.acc_hardware == ACC_BMA280)
break;
}
#endif
}
// Found anything? Check if user f****d up or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (mcfg.acc_hardware > ACC_DEFAULT) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
mcfg.acc_hardware = ACC_DEFAULT;
goto retry;
} else {
// We're really screwed
sensorsClear(SENSOR_ACC);
}
}
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
if (!bmp085Detect(&baro)) {
// ms5611 disables BMP085, and tries to initialize + check PROM crc.
// moved 5611 init here because there have been some reports that calibration data in BMP180
// has been "passing" ms5611 PROM crc check
if (!ms5611Detect(&baro)) {
// if both failed, we don't have anything
sensorsClear(SENSOR_BARO);
}
}
#endif
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init(mcfg.acc_align);
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(mcfg.gyro_align);
#ifdef MAG
if (!hmc5883lDetect(mcfg.mag_align))
sensorsClear(SENSOR_MAG);
#endif
// calculate magnetic declination
deg = cfg.mag_declination / 100;
min = cfg.mag_declination % 100;
if (sensors(SENSOR_MAG))
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
else
magneticDeclination = 0.0f;
return true;
}
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
bool sensorDetected;
UNUSED(sensorDetected); // avoid unused-variable warning on some targets.
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
sensorDetected = (hardwareRevision < NAZE32_REV5) && adxl345Detect(&acc_params, &acc);
#else
sensorDetected = adxl345Detect(&acc_params, &acc);
#endif
if (sensorDetected) {
#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
sensorDetected = (hardwareRevision < NAZE32_REV5) && mma8452Detect(&acc);
#else
sensorDetected = mma8452Detect(&acc);
#endif
if (sensorDetected) {
#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:
#ifdef USE_ACC_MPU6500
#ifdef USE_ACC_SPI_MPU6500
sensorDetected = mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc);
#else
sensorDetected = mpu6500AccDetect(&acc);
#endif
if (sensorDetected) {
#ifdef ACC_MPU6500_ALIGN
accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_MPU6500;
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 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 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(selectMPU6050Config(), &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_SPI_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_SPI_MPU6000_ALIGN
accAlign = ACC_SPI_MPU6000_ALIGN;
#endif
accHardware = ACC_SPI_MPU6000;
break;
}
#endif
; // fallthrough
case ACC_SPI_MPU6500:
#ifdef USE_ACC_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
#else
if (mpu6500SpiAccDetect(&acc)) {
#endif
#ifdef ACC_SPI_MPU6500_ALIGN
accAlign = ACC_SPI_MPU6500_ALIGN;
#endif
accHardware = ACC_SPI_MPU6500;
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)
{
#ifdef BARO
// 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 = {
.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
#endif
switch (baroHardware) {
case BARO_DEFAULT:
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) {
baroHardware = BARO_BMP085;
break;
}
#endif
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
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_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);
}
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;
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
{
int16_t deg, min;
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
if (!detectGyro(gyroLpf)) {
return false;
}
detectAcc(accHardwareToUse);
detectBaro(baroHardwareToUse);
// 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();
detectMag(magHardwareToUse);
reconfigureAlignment(sensorAlignmentConfig);
// 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;
}
void SensorDetectAndINI(void) // "enabledSensors" is "0" in config.c so all sensors disabled per default
{
int16_t deg, min;
uint8_t sig = 0;
bool ack = false;
bool haveMpu6k = false;
GyroScale16 = (16.0f / 16.4f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
if (mpu6050Detect(&acc, &gyro)) // Autodetect gyro hardware. We have MPU3050 or MPU6050.
{
haveMpu6k = true; // this filled up acc.* struct with init values
}
else if (l3g4200dDetect(&gyro))
{
havel3g4200d = true;
GyroScale16 = (16.0f / 14.2857f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
}
else if (!mpu3050Detect(&gyro))
{
failureMode(3); // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
}
sensorsSet(SENSOR_ACC); // ACC part. Will be cleared if not available
retry:
switch (cfg.acc_hdw)
{
case 0: // autodetect
case 1: // ADXL345
if (adxl345Detect(&acc)) accHardware = ACC_ADXL345;
if (cfg.acc_hdw == ACC_ADXL345) break;
case 2: // MPU6050
if (haveMpu6k)
{
mpu6050Detect(&acc, &gyro); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (cfg.acc_hdw == ACC_MPU6050) break;
}
case 3: // MMA8452
if (mma8452Detect(&acc))
{
accHardware = ACC_MMA8452;
if (cfg.acc_hdw == ACC_MMA8452) break;
}
}
if (accHardware == ACC_DEFAULT) // Found anything? Check if user f****d up or ACC is really missing.
{
if (cfg.acc_hdw > ACC_DEFAULT)
{
cfg.acc_hdw = ACC_DEFAULT; // Nothing was found and we have a forced sensor type. User probably chose a sensor that isn't present.
goto retry;
}
else sensorsClear(SENSOR_ACC); // We're really screwed
}
if (sensors(SENSOR_ACC)) acc.init();
if (haveMpu6k && accHardware == ACC_MPU6050) MpuSpecial = true;
else MpuSpecial = false;
if (feature(FEATURE_PASS)) return; // Stop here we need just ACC for Vibrationmonitoring if present
if (feature(FEATURE_GPS) && !SerialRCRX) gpsInit(cfg.gps_baudrate);// SerialRX and GPS can not coexist.
gyro.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
if (havel3g4200d) l3g4200dConfig();
else if (!haveMpu6k) mpu3050Config();
Gyro_Calibrate(); // Do Gyrocalibration here (is blocking), provides nice Warmuptime for the following rest!
#ifdef MAG
if (hmc5883lDetect())
{
sensorsSet(SENSOR_MAG);
hmc5883lInit(magCal); // Crashpilot: Calculate Gains / Scale
deg = cfg.mag_dec / 100; // calculate magnetic declination
min = cfg.mag_dec % 100;
magneticDeclination = ((float)deg + ((float)min / 60.0f));// heading is in decimaldeg units NO 0.1 deg shit here
}
#endif
#ifdef BARO // No delay necessary since Gyrocal blocked a lot already
ack = i2cRead(0x77, 0x77, 1, &sig); // Check Baroadr.(MS & BMP) BMP will say hello here, MS not
if ( ack) ack = bmp085Detect(&baro); // Are we really dealing with BMP?
if (!ack) ack = ms5611Detect(&baro); // No, Check for MS Baro
if (ack) sensorsSet(SENSOR_BARO);
if(cfg.esc_nfly) ESCnoFlyThrottle = constrain_int(cfg.esc_nfly, cfg.esc_min, cfg.esc_max); // Set the ESC PWM signal threshold for not flyable RPM
else ESCnoFlyThrottle = cfg.esc_min + (((cfg.esc_max - cfg.esc_min) * 5) / 100); // If not configured, take 5% above esc_min
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) Sonar_init(); // Initialize Sonars here depending on Rc configuration.
SonarLandWanted = cfg.snr_land; // Variable may be overwritten by failsave
#endif
MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
}
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:
#ifdef USE_ACC_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_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 = {
.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
#endif
switch (baroHardware) {
case BARO_DEFAULT:
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) {
baroHardware = BARO_BMP085;
break;
}
#endif
; // fallthough
case BARO_BMP280:
#ifdef USE_BARO_BMP280
if (bmp280Detect(&baro)) {
baroHardware = BARO_BMP280;
break;
}
#endif
; // fallthrough
case BARO_FAKE:
#ifdef USE_FAKE_BARO
if (fakeBaroDetect(&baro)) {
baroHardware = BARO_FAKE;
break;
}
#endif
; // fallthrough
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
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_GPS:
#ifdef GPS
if (gpsMagDetect(&mag)) {
#ifdef MAG_GPS_ALIGN
magAlign = MAG_GPS_ALIGN;
#endif
magHardware = MAG_GPS;
break;
}
#endif
; // fallthrough
case MAG_MAG3110:
#ifdef USE_MAG_MAG3110
if (mag3110detect(&mag)) {
#ifdef MAG_MAG3110_ALIGN
magAlign = MAG_MAG3110_ALIGN;
#endif
magHardware = MAG_MAG3110;
break;
}
#endif
; // fallthrough
case MAG_FAKE:
#ifdef USE_FAKE_MAG
if (fakeMagDetect(&mag)) {
magHardware = MAG_FAKE;
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 sensorsAutodetect(void)
{
int16_t deg, min;
mpu_params_t mpu_config;
bool haveMpu = false;
#ifndef CJMCU
drv_adxl345_config_t acc_params;
#endif
// mpu driver parameters
mpu_config.lpf = mcfg.gyro_lpf;
// Autodetect Invensense acc/gyro hardware
haveMpu = mpuDetect(&acc, &gyro, &mpu_config);
// MPU6500 on I2C bus
if (hse_value == 12000000 && mpu_config.deviceType == MPU_65xx_I2C)
hw_revision = NAZE32_REV6;
#ifndef CJMCU
if (!haveMpu) {
// Try some other gyros or bail out if fail
if (!l3g4200dDetect(&gyro, mcfg.gyro_lpf))
return false;
}
#endif
// Accelerometer. F**k it. Let user break shit.
retry:
switch (mcfg.acc_hardware) {
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
#ifndef CJMCU
case ACC_ADXL345: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc))
accHardware = ACC_ADXL345;
if (mcfg.acc_hardware == ACC_ADXL345)
break;
; // fallthrough
#endif
case ACC_MPU6050: // MPU6050
if (haveMpu && mpu_config.deviceType == MPU_60x0) {
accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050)
break;
}
; // fallthrough
#ifdef NAZE
case ACC_MPU6500: // MPU6500
if (haveMpu && (mpu_config.deviceType >= MPU_65xx_I2C)) {
accHardware = ACC_MPU6500;
if (mcfg.acc_hardware == ACC_MPU6500)
break;
}
; // fallthrough
case ACC_MMA8452: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452)
break;
}
; // fallthrough
case ACC_BMA280: // BMA280
if (bma280Detect(&acc)) {
accHardware = ACC_BMA280;
if (mcfg.acc_hardware == ACC_BMA280)
break;
}
#endif
}
// Found anything? Check if user f****d up or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (mcfg.acc_hardware > ACC_DEFAULT && mcfg.acc_hardware < ACC_NONE) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
mcfg.acc_hardware = ACC_DEFAULT;
goto retry;
} else {
// We're really screwed
sensorsClear(SENSOR_ACC);
}
}
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
if (!bmp280Detect(&baro)) {
if (!bmp085Detect(&baro)) {
// ms5611 disables BMP085, and tries to initialize + check PROM crc.
// moved 5611 init here because there have been some reports that calibration data in BMP180
// has been "passing" ms5611 PROM crc check
if (!ms5611Detect(&baro)) {
// if both failed, we don't have anything
sensorsClear(SENSOR_BARO);
}
}
}
#endif
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init(mcfg.acc_align);
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(mcfg.gyro_align);
#ifdef MAG
retryMag:
switch (mcfg.mag_hardware) {
case MAG_NONE: // disable MAG
sensorsClear(SENSOR_MAG);
break;
case MAG_DEFAULT: // autodetect
case MAG_HMC5883L:
if (hmc5883lDetect(&mag)) {
magHardware = MAG_HMC5883L;
if (mcfg.mag_hardware == MAG_HMC5883L)
break;
}
; // fallthrough
#ifdef NAZE
case MAG_AK8975:
if (ak8975detect(&mag)) {
magHardware = MAG_AK8975;
if (mcfg.mag_hardware == MAG_AK8975)
break;
}
#endif
}
// Found anything? Check if user f****d up or mag is really missing.
if (magHardware == MAG_DEFAULT) {
if (mcfg.mag_hardware > MAG_DEFAULT && mcfg.mag_hardware < MAG_NONE) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
mcfg.mag_hardware = MAG_DEFAULT;
goto retryMag;
} else {
// No mag present
sensorsClear(SENSOR_MAG);
}
}
#endif
// calculate magnetic declination
deg = cfg.mag_declination / 100;
min = cfg.mag_declination % 100;
if (sensors(SENSOR_MAG))
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
else
magneticDeclination = 0.0f;
return true;
}
// AfroFlight32 i2c sensors
void nazeDriversInit( uint8_t accHardware ) {
// int16_t deg, min;
drv_adxl345_config_t acc_params;
bool haveMpu6k = false;
uint16_t gyro_lpf = 28;
uint8_t gyro_scale = 1;
//Assume we always have a gyro
sensorMask |= BOARD_SENSOR_GYRO;
// Autodetect gyro hardware. We have MPU3050 or MPU6050.
if (mpu6050Detect(&acc, &gyro, gyro_lpf, &gyro_scale)) {
// this filled up acc.* struct with init values
haveMpu6k = true;
} else if (l3g4200dDetect(&gyro, gyro_lpf)) {
// well, we found our gyro
;
} else if (!mpu3050Detect(&gyro, gyro_lpf)) {
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
boardFault( BOARD_FAULT_FATAL );
}
// Accelerometer. F**k it. Let user break shit.
retryAcc:
switch (accHardware) {
case 0: // autodetect
case 1: // MPU6050
if (haveMpu6k) {
//acc struct already filled from previous gyro detection
// PB13 - MPU_INT output on rev4 hardware
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
goto validAcc;
}
case 2: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc) )
goto validAcc;
//Fallthrough to the next one
case 3: // MMA8452
if (mma8452Detect(&acc)) {
//Some gpio magic to trigger an init
GPIO_InitTypeDef GPIO_InitStructure;
// PA5 - ACC_INT2 output on rev3/4 hardware
// OLIMEXINO - The PA5 pin is wired up to LED1, if you need to use an mma8452 on an Olimexino use a different pin and provide support in code.
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
goto validAcc;
}
default:
//nothing found, seems there's no ACC
goto skipAcc;
}
accHardware++;
goto retryAcc;
validAcc:
sensorMask |= BOARD_SENSOR_ACC;
//Found a valid acc, init it
acc.init();
skipAcc:
#ifdef BARO
GPIO_InitTypeDef GPIO_InitStructure;
// PC13 (BMP085's XCLR reset input, which we use to disable it)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
BMP085_OFF;
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
// ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
if ( ms5611Detect(&baro) || bmp085Detect(&baro) ) {
sensorMask |= BOARD_SENSOR_BARO;
}
#endif
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
if ( hmc5883lDetect() ) {
sensorMask |= BOARD_SENSOR_MAG;
}
// calculate magnetic declination
// deg = cfg.mag_declination / 100;
// min = cfg.mag_declination % 100;
// magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
}
