Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +03:00
Code Issues Wiki Activity

Moved sensor detection into respective sensor modules

Browse source
This commit is contained in:
Martin Budden 2016-12-05 15:26:14 +00:00
parent 4bee6193e8
commit 4bb6820c42
10 changed files with 535 additions and 508 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -22,6 +22,12 @@
#include "common/axis.h"
#include "drivers/compass.h"
#include "drivers/compass_ak8975.h"
#include "drivers/compass_ak8963.h"
#include "drivers/compass_fake.h"
#include "drivers/compass_hmc5883l.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
#include "fc/config.h"
@ -42,9 +48,108 @@ mag_t mag; // mag access functions
static int16_t magADCRaw[XYZ_AXIS_COUNT];
static uint8_t magInit = 0;
void compassInit(void)
bool compassDetect(magDev_t *dev, 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:
mag.dev.magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_DEFAULT:
; // fallthrough
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(dev, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
mag.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
mag.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
mag.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;
}
void compassInit(const compassConfig_t *compassConfig)
{
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
// calculate magnetic declination
const int16_t deg = compassConfig->mag_declination / 100;
const int16_t min = compassConfig->mag_declination % 100;
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
LED1_ON;
mag.dev.init();
LED1_OFF;