Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

Rebased (size PR is now in)

Browse source
This commit is contained in:
jflyper 2016-12-10 20:48:02 +09:00
commit 6b81e70f41
23 changed files with 705 additions and 546 deletions
Download patch file Download diff file Expand all files Collapse all files

132
Makefile
View file

@ -595,6 +595,98 @@ HIGHEND_SRC = \
telemetry/mavlink.c \
telemetry/esc_telemetry.c \
SPEED_OPTIMISED_SRC = \
common/encoding.c \
common/filter.c \
common/maths.c \
common/typeconversion.c \
drivers/adc.c \
drivers/buf_writer.c \
drivers/bus_i2c_soft.c \
drivers/bus_spi.c \
drivers/bus_spi_soft.c \
drivers/exti.c \
drivers/gyro_sync.c \
drivers/io.c \
drivers/light_led.c \
drivers/resource.c \
drivers/rx_nrf24l01.c \
drivers/rx_spi.c \
drivers/rx_xn297.c \
drivers/pwm_output.c \
drivers/pwm_rx.c \
drivers/rcc.c \
drivers/serial.c \
drivers/serial_uart.c \
drivers/sound_beeper.c \
drivers/stack_check.c \
drivers/system.c \
drivers/timer.c \
fc/fc_tasks.c \
fc/mw.c \
fc/rc_controls.c \
fc/rc_curves.c \
fc/runtime_config.c \
flight/altitudehold.c \
flight/failsafe.c \
flight/imu.c \
flight/mixer.c \
flight/pid.c \
flight/servos.c \
io/beeper.c \
io/serial.c \
io/statusindicator.c \
rx/ibus.c \
rx/jetiexbus.c \
rx/msp.c \
rx/nrf24_cx10.c \
rx/nrf24_inav.c \
rx/nrf24_h8_3d.c \
rx/nrf24_syma.c \
rx/nrf24_v202.c \
rx/pwm.c \
rx/rx.c \
rx/rx_spi.c \
rx/crsf.c \
rx/sbus.c \
rx/spektrum.c \
rx/sumd.c \
rx/sumh.c \
rx/xbus.c \
scheduler/scheduler.c \
sensors/acceleration.c \
sensors/boardalignment.c \
sensors/gyro.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/serial_softserial.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \
io/displayport_oled.c \
io/ledstrip.c \
io/osd.c \
telemetry/telemetry.c \
telemetry/crsf.c \
telemetry/frsky.c \
telemetry/hott.c \
telemetry/smartport.c \
telemetry/ltm.c \
telemetry/mavlink.c \
telemetry/esc_telemetry.c \
SIZE_OPTIMISED_SRC = \
drivers/serial_escserial.c \
io/serial_cli.c \
io/serial_4way.c \
io/serial_4way_avrootloader.c \
io/serial_4way_stk500v2.c \
msp/msp_serial.c \
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
VCP_SRC = \
vcpf4/stm32f4xx_it.c \
@ -754,22 +846,35 @@ SIZE := $(ARM_SDK_PREFIX)size
#
ifeq ($(DEBUG),GDB)
OPTIMIZE = -O0
LTO_FLAGS = $(OPTIMIZE)
OPTIMISE = -O0
CC_SPEED_OPTIMISATION = $(OPTIMISE)
CC_OPTIMISATION = $(OPTIMISE)
CC_SIZE_OPTIMISATION = $(OPTIMISE)
LTO_FLAGS = $(OPTIMISE)
else
ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
OPTIMIZE = -Os
OPTIMISE_SPEED = -Os
OPTIMISE = -Os
OPTIMISE_SIZE = -Os
else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
OPTIMISE_SPEED = -Ofast
OPTIMISE = -O2
OPTIMISE_SIZE = -Os
else
#OPTIMIZE = -Ofast
OPTIMIZE = -O2
OPTIMISE_SPEED = -Ofast
OPTIMISE = -Ofast
OPTIMISE_SIZE = -Ofast
endif
LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE)
OPTIMISATION_BASE = -flto -fuse-linker-plugin -ffast-math
CC_SPEED_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
CC_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE)
CC_SIZE_OPTIMISATION = $(OPTIMISATION_BASE) $(OPTIMISE_SIZE)
LTO_FLAGS = $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
endif
DEBUG_FLAGS = -ggdb3 -DDEBUG
CFLAGS += $(ARCH_FLAGS) \
$(LTO_FLAGS) \
$(addprefix -D,$(OPTIONS)) \
$(addprefix -I,$(INCLUDE_DIRS)) \
$(DEBUG_FLAGS) \
@ -832,6 +937,9 @@ CLEAN_ARTIFACTS := $(TARGET_BIN)
CLEAN_ARTIFACTS += $(TARGET_HEX)
CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
# Make sure build date and revision is updated on every incremental build
$(OBJECT_DIR)/$(TARGET)/build/version.o : $(TARGET_SRC)
# List of buildable ELF files and their object dependencies.
# It would be nice to compute these lists, but that seems to be just beyond make.
@ -849,8 +957,14 @@ $(TARGET_ELF): $(TARGET_OBJS)
# Compile
$(OBJECT_DIR)/$(TARGET)/%.o: %.c
$(V1) mkdir -p $(dir $@)
$(V1) echo "%% $(notdir $<)" "$(STDOUT)"
$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $<
$(V1) $(if $(findstring $(subst ./src/main/,,$<), $(SPEED_OPTIMISED_SRC)), \
echo "%% (speed optimised) $(notdir $<)" "$(STDOUT)" && \
$(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_SPEED_OPTIMISATION) $<, \
$(if $(findstring $(subst ./src/main/,,$<), $(SIZE_OPTIMISED_SRC)), \
echo "%% (size optimised) $(notdir $<)" "$(STDOUT)" && \
$(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_SIZE_OPTIMISATION) $<, \
echo "%% $(notdir $<)" "$(STDOUT)" && \
$(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_OPTIMISATION) $<))
# Assemble
$(OBJECT_DIR)/$(TARGET)/%.o: %.s

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;

3
src/main/drivers/compass_ak8963.c
View file

@ -42,6 +42,9 @@
#include "accgyro_spi_mpu9250.h"
#include "compass_ak8963.h"
void ak8963Init(void);
bool ak8963Read(int16_t *magData);
// This sensor is available in MPU-9250.
// AK8963, mag sensor address

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

@ -18,5 +18,3 @@
#pragma once
bool ak8963Detect(magDev_t *mag);
void ak8963Init(void);
bool ak8963Read(int16_t *magData);

3
src/main/drivers/compass_ak8975.c
View file

@ -37,6 +37,9 @@
#include "compass_ak8975.h"
void ak8975Init(void);
bool ak8975Read(int16_t *magData);
// This sensor is available in MPU-9150.
// AK8975, mag sensor address

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

@ -18,5 +18,3 @@
#pragma once
bool ak8975Detect(magDev_t *mag);
void ak8975Init(void);
bool ak8975Read(int16_t *magData);

3
src/main/drivers/compass_fake.c
View file

@ -32,13 +32,12 @@
static int16_t fakeMagData[XYZ_AXIS_COUNT];
static bool fakeMagInit(void)
static void fakeMagInit(void)
{
// initially point north
fakeMagData[X] = 4096;
fakeMagData[Y] = 0;
fakeMagData[Z] = 0;
return true;
}
void fakeMagSet(int16_t x, int16_t y, int16_t z)

3
src/main/drivers/compass_hmc5883l.c
View file

@ -41,6 +41,9 @@
#include "compass_hmc5883l.h"
void hmc5883lInit(void);
bool hmc5883lRead(int16_t *magData);
//#define DEBUG_MAG_DATA_READY_INTERRUPT
// HMC5883L, default address 0x1E

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

@ -24,5 +24,3 @@ typedef struct hmc5883Config_s {
} hmc5883Config_t;
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
void hmc5883lInit(void);
bool hmc5883lRead(int16_t *magData);

8
src/main/drivers/pwm_rx.c
View file

@ -103,7 +103,6 @@ typedef struct ppmDevice_s {
ppmDevice_t ppmDev;
#define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds
#define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
#define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
@ -111,7 +110,6 @@ ppmDevice_t ppmDev;
#define PPM_IN_MIN_NUM_CHANNELS 4
#define PPM_IN_MAX_NUM_CHANNELS PWM_PORTS_OR_PPM_CAPTURE_COUNT
bool isPPMDataBeingReceived(void)
{
return (ppmFrameCount != lastPPMFrameCount);
@ -364,17 +362,15 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
TIM_ICInit(tim, &TIM_ICInitStructure);
}
#endif
void pwmRxInit(const pwmConfig_t *pwmConfig)
{
for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) {
pwmInputPort_t *port = &pwmInputPorts[channel];
const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM);
const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_ANY);
if (!timer) {
/* TODO: maybe fail here if not enough channels? */
@ -444,7 +440,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT];
const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_PPM);
const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_ANY);
if (!timer) {
/* TODO: fail here? */
return;

2
src/main/drivers/system_stm32f7xx.c
View file

@ -155,7 +155,7 @@ void systemInit(void)
//SystemClock_Config();
// Configure NVIC preempt/priority groups
//NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING);
// cache RCC->CSR value to use it in isMPUSoftreset() and others
cachedRccCsrValue = RCC->CSR;

21
src/main/io/serial_cli.c
View file

@ -2252,18 +2252,35 @@ static void cliFlashInfo(char *cmdline)
layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset());
}
static void cliFlashErase(char *cmdline)
{
UNUSED(cmdline);
cliPrintf("Erasing...\r\n");
#ifndef CLI_MINIMAL_VERBOSITY
uint32_t i;
cliPrintf("Erasing, please wait ... \r\n");
#else
cliPrintf("Erasing,\r\n");
#endif
bufWriterFlush(cliWriter);
flashfsEraseCompletely();
while (!flashfsIsReady()) {
#ifndef CLI_MINIMAL_VERBOSITY
cliPrintf(".");
if (i++ > 120) {
i=0;
cliPrintf("\r\n");
}
bufWriterFlush(cliWriter);
#endif
delay(100);
}
cliPrintf("Done.\r\n");
cliPrintf("\r\nDone.\r\n");
}
#ifdef USE_FLASH_TOOLS

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

@ -26,9 +26,27 @@
#include "common/axis.h"
#include "common/filter.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_icm20689.h"
#include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/accgyro_spi_mpu9250.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"
@ -38,6 +56,10 @@
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
acc_t acc; // acc access functions
@ -53,8 +75,161 @@ static flightDynamicsTrims_t *accelerationTrims;
static uint16_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
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;
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, dev)) {
#else
if (adxl345Detect(&acc_params, dev)) {
#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
; // fallthrough
case ACC_ICM20689:
#ifdef USE_ACC_SPI_ICM20689
if (icm20689SpiAccDetect(dev))
{
#ifdef ACC_ICM20689_ALIGN
dev->accAlign = ACC_ICM20689_ALIGN;
#endif
accHardware = ACC_ICM20689;
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_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 false;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
return true;
}
void accInit(uint32_t gyroSamplingInverval)
{
acc.dev.acc_1G = 256; // set default
acc.dev.init(&acc.dev); // driver initialisation
// set the acc sampling interval according to the gyro sampling interval
switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future
case 500:
@ -205,7 +380,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
DEBUG_SET(DEBUG_ACCELEROMETER, axis, accADCRaw[axis]);
acc.accSmooth[axis] = accADCRaw[axis];
}

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

@ -61,6 +61,7 @@ typedef struct accelerometerConfig_s {
flightDynamicsTrims_t accZero;
} accelerometerConfig_t;
bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse);
void accInit(uint32_t gyroTargetLooptime);
bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);

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

@ -24,9 +24,20 @@
#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/system.h"
#include "fc/runtime_config.h"
#include "sensors/barometer.h"
#include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
baro_t baro; // barometer access functions
@ -40,9 +51,75 @@ static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
static uint32_t baroPressureSum = 0;
static barometerConfig_t *barometerConfig;
static const barometerConfig_t *barometerConfig;
void useBarometerConfig(barometerConfig_t *barometerConfigToUse)
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 = 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, dev)) {
baroHardware = BARO_BMP085;
break;
}
#endif
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(dev)) {
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(dev)) {
baroHardware = BARO_BMP280;
break;
}
#endif
; // fallthough
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
if (baroHardware == BARO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
return true;
}
void useBarometerConfig(const barometerConfig_t *barometerConfigToUse)
{
barometerConfig = barometerConfigToUse;
}

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

@ -45,7 +45,8 @@ typedef struct baro_s {
extern baro_t baro;
void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse);
void useBarometerConfig(const barometerConfig_t *barometerConfigToUse);
bool isBaroCalibrationComplete(void);
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
uint32_t baroUpdate(void);

118
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:
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
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;
}
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;
@ -56,15 +161,16 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
static uint32_t tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
uint32_t axis;
mag.dev.read(magADCRaw);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[axis];
}
alignSensors(mag.magADC, mag.dev.magAlign);
if (STATE(CALIBRATE_MAG)) {
tCal = currentTime;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = mag.magADC[axis];
magZeroTempMax.raw[axis] = mag.magADC[axis];
@ -81,7 +187,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
if (tCal != 0) {
if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
if (mag.magADC[axis] < magZeroTempMin.raw[axis])
magZeroTempMin.raw[axis] = mag.magADC[axis];
if (mag.magADC[axis] > magZeroTempMax.raw[axis])
@ -89,7 +195,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
}
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
}

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

@ -47,7 +47,8 @@ typedef struct compassConfig_s {
flightDynamicsTrims_t magZero;
} compassConfig_t;
void compassInit(void);
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse);
void compassInit(const compassConfig_t *compassConfig);
union flightDynamicsTrims_u;
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);

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

@ -27,8 +27,28 @@
#include "common/maths.h"
#include "common/filter.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_icm20689.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/system.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
@ -51,6 +71,143 @@ static void *notchFilter1[3];
static filterApplyFnPtr notchFilter2ApplyFn;
static void *notchFilter2[3];
bool gyroDetect(gyroDev_t *dev)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
dev->gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // 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_ICM20689:
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiGyroDetect(dev))
{
gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN
dev->gyroAlign = GYRO_ICM20689_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;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
void gyroInit(const gyroConfig_t *gyroConfigToUse)
{
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
@ -60,6 +217,9 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse)
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
gyroConfig = gyroConfigToUse;
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev);
softLpfFilterApplyFn = nullFilterApply;
notchFilter1ApplyFn = nullFilterApply;

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

@ -46,15 +46,16 @@ typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_type;
uint8_t gyro_soft_lpf_hz;
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
} gyroConfig_t;
bool gyroDetect(gyroDev_t *dev);
void gyroSetCalibrationCycles(void);
void gyroInit(const gyroConfig_t *gyroConfigToUse);
void gyroUpdate(void);

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

@ -26,44 +26,14 @@
#include "config/feature.h"
#include "drivers/accgyro_mpu.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_icm20689.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/barometer.h"
#include "drivers/barometer_bmp085.h"
#include "drivers/barometer_bmp280.h"
#include "drivers/barometer_fake.h"
#include "drivers/barometer_ms5611.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/sonar_hcsr04.h"
#include "fc/config.h"
@ -97,459 +67,6 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
#endif
}
bool gyroDetect(gyroDev_t *dev)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyro.dev.gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
gyro.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
gyro.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
gyro.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
gyro.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
gyro.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
gyro.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
gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_ICM20689:
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiGyroDetect(dev))
{
gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN
gyro.dev.gyroAlign = GYRO_ICM20689_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;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
retry:
acc.dev.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, dev)) {
#else
if (adxl345Detect(&acc_params, dev)) {
#endif
#ifdef ACC_ADXL345_ALIGN
acc.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
acc.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
acc.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
acc.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
acc.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
acc.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
acc.dev.accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_MPU6500;
break;
}
#endif
; // fallthrough
case ACC_ICM20689:
#ifdef USE_ACC_SPI_ICM20689
if (icm20689SpiAccDetect(dev))
{
#ifdef ACC_ICM20689_ALIGN
acc.dev.accAlign = ACC_ICM20689_ALIGN;
#endif
accHardware = ACC_ICM20689;
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_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 false;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
return true;
}
#ifdef BARO
static 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 = 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, dev)) {
baroHardware = BARO_BMP085;
break;
}
#endif
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(dev)) {
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(dev)) {
baroHardware = BARO_BMP280;
break;
}
#endif
; // fallthough
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
if (baroHardware == BARO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
return true;
}
#endif
#ifdef MAG
static 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;
}
#endif
#ifdef SONAR
static bool sonarDetect(void)
{
@ -569,8 +86,6 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig,
const barometerConfig_t *barometerConfig,
const sonarConfig_t *sonarConfig)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#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) || defined(USE_GYRO_SPI_ICM20689)
@ -580,33 +95,22 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig,
UNUSED(mpuDetectionResult);
#endif
memset(&gyro, 0, sizeof(gyro));
if (!gyroDetect(&gyro.dev)) {
return false;
}
// gyro must be initialised before accelerometer
gyroInit(gyroConfig);
// Now time to init things
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
memset(&acc, 0, sizeof(acc));
if (accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
acc.dev.acc_1G = 256; // set default
acc.dev.init(&acc.dev); // driver initialisation
accInit(gyro.targetLooptime); // sensor initialisation
accInit(gyro.targetLooptime);
}
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
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (compassDetect(&mag.dev, compassConfig->mag_hardware)) {
// 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
compassInit();
compassInit(compassConfig);
}
#else
UNUSED(compassConfig);

4
src/main/target/SPRACINGF3EVO/target.h
View file

@ -49,8 +49,8 @@
#define BARO
#define USE_BARO_BMP280
//#define MAG
//#define USE_MAG_AK8963
#define MAG
#define USE_MAG_AK8963
//#define USE_MAG_HMC5883 // External
#define MAG_AK8963_ALIGN CW90_DEG_FLIP

8
src/main/target/link/stm32_flash_f745.ld
View file

@ -15,12 +15,14 @@ ENTRY(Reset_Handler)
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1008K
FLASH_CONFIG (r) : ORIGIN = 0x080FC000, LENGTH = 16K
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 768K
FLASH_CONFIG (r) : ORIGIN = 0x080C0000, LENGTH = 256K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 320K
TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K
RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
/* note CCM could be used for stack */
REGION_ALIAS("STACKRAM", TCM)
INCLUDE "stm32_flash.ld"