From cde9a9517bfd1e36badd18cbc8a53935915487b7 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 3 Mar 2018 23:29:31 +0100 Subject: [PATCH] SPRacingF7DUAL - Dual SIMULTANEOUS gyro support. (#5264) * CF/BF - Set STM32F7 SPI FAST clock to 13.5Mhz - Gyros not stable at 27mhz. * CF/BF - Initial SPRacingF7DUAL commit. Support two simultaneous gyro support (code by Dominic Clifton and Martin Budden) There are new debug modes so you can see the difference between each gyro. Notes: * spi bus instance caching broke spi mpu detection because the detection tries I2C first which overwrites the selected bus instance when using dual gyro. * ALL other dual-gyro boards have one sensor per bus. SPRacingF7DUAL is has two per bus and thus commit has a lot of changes to fix SPI/BUS/GYRO initialisation issues. * CF/BF - Add SPRacingF4EVODG target. This target adds a second gyro to the board using the SPI pads on the back of the board. * CF/BF - Temporarily disable Gyro EXTI pin to allow NEO target to build. --- make/mcu/STM32F7.mk | 2 - make/targets.mk | 3 +- src/main/build/debug.c | 4 + src/main/build/debug.h | 4 + src/main/drivers/accgyro/accgyro.h | 1 + src/main/drivers/accgyro/accgyro_mpu.c | 77 +++--- src/main/drivers/accgyro/accgyro_mpu.h | 3 - .../drivers/accgyro/accgyro_spi_icm20689.c | 7 +- .../drivers/accgyro/accgyro_spi_mpu6500.c | 8 - src/main/drivers/bus_spi.h | 2 +- src/main/drivers/buttons.c | 24 +- src/main/drivers/serial_uart_hal.c | 4 + src/main/fc/fc_init.c | 35 +-- src/main/interface/settings.c | 11 +- src/main/interface/settings.h | 3 + src/main/sensors/acceleration.c | 12 +- src/main/sensors/gyro.c | 229 +++++++++++++++-- src/main/sensors/gyro.h | 4 + src/main/target/KAKUTEF7/target.h | 12 +- src/main/target/OMNIBUSF7/target.h | 18 +- src/main/target/REVO/target.h | 4 + src/main/target/SPRACINGF3NEO/target.h | 8 +- src/main/target/SPRACINGF4EVO/target.h | 54 +++- src/main/target/SPRACINGF4EVO/target.mk | 6 + src/main/target/SPRACINGF7DUAL/config.c | 62 +++++ src/main/target/SPRACINGF7DUAL/target.c | 68 ++++++ src/main/target/SPRACINGF7DUAL/target.h | 231 ++++++++++++++++++ src/main/target/SPRACINGF7DUAL/target.mk | 16 ++ src/main/target/stm32f7xx_hal_conf.h | 2 +- 29 files changed, 797 insertions(+), 117 deletions(-) create mode 100644 src/main/target/SPRACINGF7DUAL/config.c create mode 100644 src/main/target/SPRACINGF7DUAL/target.c create mode 100644 src/main/target/SPRACINGF7DUAL/target.h create mode 100644 src/main/target/SPRACINGF7DUAL/target.mk diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index 16aa520f1f..88fbd95b98 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -18,8 +18,6 @@ EXCLUDES = stm32f7xx_hal_can.c \ stm32f7xx_hal_crc_ex.c \ stm32f7xx_hal_cryp.c \ stm32f7xx_hal_cryp_ex.c \ - stm32f7xx_hal_dac.c \ - stm32f7xx_hal_dac_ex.c \ stm32f7xx_hal_dcmi.c \ stm32f7xx_hal_dcmi_ex.c \ stm32f7xx_hal_dfsdm.c \ diff --git a/make/targets.mk b/make/targets.mk index 800903bbc8..9ec2481fed 100644 --- a/make/targets.mk +++ b/make/targets.mk @@ -1,4 +1,4 @@ -OFFICIAL_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 FURYF4 REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO SPRACINGF3NEO SPRACINGF4EVO STM32F3DISCOVERY +OFFICIAL_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 FURYF4 REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO SPRACINGF3NEO SPRACINGF4EVO SPRACINGF7DUAL STM32F3DISCOVERY SKIP_TARGETS := ALIENWHOOP MOTOLABF4 ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk))))) OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS)) @@ -124,6 +124,7 @@ GROUP_4_TARGETS := \ SPRACINGF3OSD \ SPRACINGF4EVO \ SPRACINGF4NEO \ + SPRACINGF7DUAL \ STM32F3DISCOVERY \ TINYBEEF3 \ TINYFISH \ diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 4a41f19aaa..f7ac241b08 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -48,6 +48,10 @@ const char * const debugModeNames[DEBUG_COUNT] = { "FFT_FREQ", "RX_FRSKY_SPI", "GYRO_RAW", + "DUAL_GYRO", + "DUAL_GYRO_RAW", + "DUAL_GYRO_COMBINE", + "DUAL_GYRO_DIFF", "MAX7456_SIGNAL", "MAX7456_SPICLOCK", "SBUS", diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 813b88fb69..bed323de0f 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -66,6 +66,10 @@ typedef enum { DEBUG_FFT_FREQ, DEBUG_RX_FRSKY_SPI, DEBUG_GYRO_RAW, + DEBUG_DUAL_GYRO, + DEBUG_DUAL_GYRO_RAW, + DEBUG_DUAL_GYRO_COMBINE, + DEBUG_DUAL_GYRO_DIFF, DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SPICLOCK, DEBUG_SBUS, diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index eeb04b6970..08b1aaccb5 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -65,6 +65,7 @@ typedef struct gyroDev_s { float scale; // scalefactor float gyroZero[XYZ_AXIS_COUNT]; float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment + float gyroADCf[XYZ_AXIS_COUNT]; int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t temperature; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 9ec2e64dd7..b83e60a410 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -208,8 +208,10 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) uint8_t sensor = MPU_NONE; UNUSED(sensor); + // note, when USE_DUAL_GYRO is enabled the gyro->bus must already be initialised. + #ifdef USE_GYRO_SPI_MPU6000 -#ifdef MPU6000_SPI_INSTANCE +#ifndef USE_DUAL_GYRO spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE); #endif #ifdef MPU6000_CS_PIN @@ -223,7 +225,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_MPU6500 -#ifdef MPU6500_SPI_INSTANCE +#ifndef USE_DUAL_GYRO spiBusSetInstance(&gyro->bus, MPU6500_SPI_INSTANCE); #endif #ifdef MPU6500_CS_PIN @@ -238,7 +240,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_MPU9250 -#ifdef MPU9250_SPI_INSTANCE +#ifndef USE_DUAL_GYRO spiBusSetInstance(&gyro->bus, MPU9250_SPI_INSTANCE); #endif #ifdef MPU9250_CS_PIN @@ -267,13 +269,14 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_ICM20689 -#ifdef ICM20689_SPI_INSTANCE +#ifndef USE_DUAL_GYRO spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE); #endif #ifdef ICM20689_CS_PIN gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; #endif sensor = icm20689SpiDetect(&gyro->bus); + // icm20689SpiDetect detects ICM20602 and ICM20689 if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; return true; @@ -281,7 +284,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_ACCGYRO_BMI160 -#ifdef BMI160_SPI_INSTANCE +#ifndef USE_DUAL_GYRO spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE); #endif #ifdef BMI160_CS_PIN @@ -304,39 +307,43 @@ void mpuDetect(gyroDev_t *gyro) delay(35); #ifdef USE_I2C - gyro->bus.bustype = BUSTYPE_I2C; - gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE; - gyro->bus.busdev_u.i2c.address = MPU_ADDRESS; - uint8_t sig = 0; - bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1); -#else - bool ack = false; + if (gyro->bus.bustype == BUSTYPE_NONE) { + // if no bustype is selected try I2C first. + gyro->bus.bustype = BUSTYPE_I2C; + } + + if (gyro->bus.bustype == BUSTYPE_I2C) { + gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE; + gyro->bus.busdev_u.i2c.address = MPU_ADDRESS; + + uint8_t sig = 0; + bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1); + + if (ack) { + // If an MPU3050 is connected sig will contain 0. + uint8_t inquiryResult; + ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); + inquiryResult &= MPU_INQUIRY_MASK; + if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_3050; + return; + } + + sig &= MPU_INQUIRY_MASK; + if (sig == MPUx0x0_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_60x0; + mpu6050FindRevision(gyro); + } else if (sig == MPU6500_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; + } + return; + } + } #endif - if (!ack) { #ifdef USE_SPI - detectSPISensorsAndUpdateDetectionResult(gyro); -#endif - return; - } - -#ifdef USE_I2C - // If an MPU3050 is connected sig will contain 0. - uint8_t inquiryResult; - ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); - inquiryResult &= MPU_INQUIRY_MASK; - if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_3050; - return; - } - - sig &= MPU_INQUIRY_MASK; - if (sig == MPUx0x0_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_60x0; - mpu6050FindRevision(gyro); - } else if (sig == MPU6500_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; - } + gyro->bus.bustype = BUSTYPE_SPI; + detectSPISensorsAndUpdateDetectionResult(gyro); #endif } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 6e42fe2a44..7ec59cb542 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -32,7 +32,6 @@ #define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I_LEGACY 0x00 - #define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU3050, 6000 and 6050 #define MPU6000_WHO_AM_I_CONST (0x68) #define MPU6500_WHO_AM_I_CONST (0x70) @@ -44,8 +43,6 @@ #define ICM20649_WHO_AM_I_CONST (0xE1) #define ICM20689_WHO_AM_I_CONST (0x98) - - // RA = Register Address #define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index becca56dc6..6d31a17b25 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -92,7 +92,6 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus) spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); return icmDetected; - } void icm20689AccInit(accDev_t *acc) @@ -102,7 +101,11 @@ void icm20689AccInit(accDev_t *acc) bool icm20689SpiAccDetect(accDev_t *acc) { - if (acc->mpuDetectionResult.sensor != ICM_20689_SPI) { + switch (acc->mpuDetectionResult.sensor) { + case ICM_20602_SPI: + case ICM_20689_SPI: + break; + default: return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 8d2918ddd3..db7b4da3a5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -38,19 +38,11 @@ static void mpu6500SpiInit(const busDevice_t *bus) { - static bool hardwareInitialised = false; - - if (hardwareInitialised) { - return; - } - IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->busdev_u.spi.csnPin); spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_FAST); - - hardwareInitialised = true; } uint8_t mpu6500SpiDetect(const busDevice_t *bus) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 59a13c25df..be4fab6ca1 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -56,7 +56,7 @@ typedef enum { #elif defined(STM32F7) SPI_CLOCK_SLOW = 256, //00.42188 MHz SPI_CLOCK_STANDARD = 16, //06.57500 MHz - SPI_CLOCK_FAST = 4, //27.00000 MHz + SPI_CLOCK_FAST = 8, //13.50000 MHz SPI_CLOCK_ULTRAFAST = 2 //54.00000 MHz #else SPI_CLOCK_SLOW = 128, //00.56250 MHz diff --git a/src/main/drivers/buttons.c b/src/main/drivers/buttons.c index 9235f25a87..bba73668d3 100644 --- a/src/main/drivers/buttons.c +++ b/src/main/drivers/buttons.c @@ -35,32 +35,52 @@ static IO_t buttonAPin = IO_NONE; static IO_t buttonBPin = IO_NONE; #endif +#ifdef BUTTON_A_PIN_INVERTED +#define BUTTON_A_PIN_GPIO_MODE IOCFG_IPD +#else +#define BUTTON_A_PIN_GPIO_MODE IOCFG_IPU +#endif + +#ifdef BUTTON_B_PIN_INVERTED +#define BUTTON_B_PIN_GPIO_MODE IOCFG_IPD +#else +#define BUTTON_B_PIN_GPIO_MODE IOCFG_IPU +#endif + void buttonsInit(void) { #ifdef BUTTON_A_PIN buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN)); IOInit(buttonAPin, OWNER_SYSTEM, 0); - IOConfigGPIO(buttonAPin, IOCFG_IPU); + IOConfigGPIO(buttonAPin, BUTTON_A_PIN_GPIO_MODE); #endif #ifdef BUTTON_B_PIN buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN)); IOInit(buttonBPin, OWNER_SYSTEM, 0); - IOConfigGPIO(buttonBPin, IOCFG_IPU); + IOConfigGPIO(buttonBPin, BUTTON_B_PIN_GPIO_MODE); #endif } #ifdef BUTTON_A_PIN bool buttonAPressed(void) { +#ifdef BUTTON_A_PIN_INVERTED + return IORead(buttonAPin); +#else return !IORead(buttonAPin); +#endif } #endif #ifdef BUTTON_B_PIN bool buttonBPressed(void) { +#ifdef BUTTON_B_PIN_INVERTED + return IORead(buttonBPin); +#else return !IORead(buttonBPin); +#endif } #endif diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 29a1cf2ea6..6034db58e0 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -94,6 +94,10 @@ void uartReconfigure(uartPort_t *uartPort) usartConfigurePinInversion(uartPort); +#ifdef TARGET_USART_CONFIG + usartTargetConfigure(uartPort); +#endif + if (uartPort->port.options & SERIAL_BIDIR) { HAL_HalfDuplex_Init(&uartPort->Handle); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 0558b8561b..0395435e18 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -18,6 +18,7 @@ #include #include #include +#include #include "platform.h" @@ -196,52 +197,58 @@ static IO_t busSwitchResetPin = IO_NONE; void spiPreInit(void) { -#ifdef USE_ACC_SPI_MPU6000 +#ifdef GYRO_1_CS_PIN + spiPreInitCs(IO_TAG(GYRO_1_CS_PIN)); +#endif +#ifdef GYRO_2_CS_PIN + spiPreInitCs(IO_TAG(GYRO_2_CS_PIN)); +#endif +#ifdef MPU6000_CS_PIN spiPreInitCs(IO_TAG(MPU6000_CS_PIN)); #endif -#ifdef USE_ACC_SPI_MPU6500 +#ifdef MPU6500_CS_PIN spiPreInitCs(IO_TAG(MPU6500_CS_PIN)); #endif -#ifdef USE_GYRO_SPI_MPU9250 +#ifdef MPU9250_CS_PIN spiPreInitCs(IO_TAG(MPU9250_CS_PIN)); #endif -#ifdef USE_GYRO_SPI_ICM20649 +#ifdef ICM20649_CS_PIN spiPreInitCs(IO_TAG(ICM20649_CS_PIN)); #endif -#ifdef USE_GYRO_SPI_ICM20689 +#ifdef ICM20689_CS_PIN spiPreInitCs(IO_TAG(ICM20689_CS_PIN)); #endif -#ifdef USE_ACCGYRO_BMI160 +#ifdef BMI160_CS_PIN spiPreInitCs(IO_TAG(BMI160_CS_PIN)); #endif -#ifdef USE_GYRO_L3GD20 +#ifdef L3GD20_CS_PIN spiPreInitCs(IO_TAG(L3GD20_CS_PIN)); #endif -#ifdef USE_MAX7456 +#ifdef MAX7456_SPI_CS_PIN spiPreInitCsOutPU(IO_TAG(MAX7456_SPI_CS_PIN)); // XXX 3.2 workaround for Kakute F4. See comment for spiPreInitCSOutPU. #endif #ifdef USE_SDCARD spiPreInitCs(sdcardConfig()->chipSelectTag); #endif -#ifdef USE_BARO_SPI_BMP280 +#ifdef BMP280_CS_PIN spiPreInitCs(IO_TAG(BMP280_CS_PIN)); #endif -#ifdef USE_BARO_SPI_MS5611 +#ifdef MS5611_CS_PIN spiPreInitCs(IO_TAG(MS5611_CS_PIN)); #endif -#ifdef USE_BARO_SPI_LPS +#ifdef LPS_CS_PIN spiPreInitCs(IO_TAG(LPS_CS_PIN)); #endif -#ifdef USE_MAG_SPI_HMC5883 +#ifdef HMC5883_CS_PIN spiPreInitCs(IO_TAG(HMC5883_CS_PIN)); #endif -#ifdef USE_MAG_SPI_AK8963 +#ifdef AK8963_CS_PIN spiPreInitCs(IO_TAG(AK8963_CS_PIN)); #endif #if defined(RTC6705_CS_PIN) && !defined(USE_VTX_RTC6705_SOFTSPI) // RTC6705 soft SPI initialisation handled elsewhere. spiPreInitCs(IO_TAG(RTC6705_CS_PIN)); #endif -#ifdef USE_FLASH_M25P16 +#ifdef M25P16_CS_PIN spiPreInitCs(IO_TAG(M25P16_CS_PIN)); #endif #if defined(USE_RX_SPI) && !defined(USE_RX_SOFTSPI) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 18548c4a58..7514b899d5 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -149,6 +149,12 @@ static const char * const lookupTableAlignment[] = { "CW270FLIP" }; +#ifdef USE_DUAL_GYRO +static const char * const lookupTableGyro[] = { + "FIRST", "SECOND", "BOTH" +}; +#endif + #ifdef USE_GPS static const char * const lookupTableGPSProvider[] = { "NMEA", "UBLOX" @@ -340,6 +346,9 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_OVERCLOCK { lookupOverclock, sizeof(lookupOverclock) / sizeof(char *) }, #endif +#ifdef USE_DUAL_GYRO + { lookupTableGyro, sizeof(lookupTableGyro) / sizeof(char *) }, +#endif }; const clivalue_t valueTable[] = { @@ -374,7 +383,7 @@ const clivalue_t valueTable[] = { #endif #endif #ifdef USE_DUAL_GYRO - { "gyro_to_use", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, + { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif // PG_ACCELEROMETER_CONFIG diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 0bc91bee23..433e6dcf42 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -76,6 +76,9 @@ typedef enum { TABLE_RATES_TYPE, #ifdef USE_OVERCLOCK TABLE_OVERCLOCK, +#endif +#ifdef USE_DUAL_GYRO + TABLE_GYRO, #endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index b0deb47927..147f49d702 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -133,7 +133,6 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) #endif retry: - dev->accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: @@ -333,6 +332,17 @@ bool accInit(uint32_t gyroSamplingInverval) acc.dev.bus = *gyroSensorBus(); acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr; + +#ifdef USE_DUAL_GYRO + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) { + acc.dev.accAlign = ACC_2_ALIGN; + } else { + acc.dev.accAlign = ACC_1_ALIGN; + } +#else + acc.dev.accAlign = ALIGN_DEFAULT; +#endif + if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8d7134cc30..619b0175ef 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -80,6 +80,8 @@ FAST_RAM gyro_t gyro; static FAST_RAM uint8_t gyroDebugMode; +static uint8_t gyroToUse = 0; + #ifdef USE_GYRO_OVERFLOW_CHECK static FAST_RAM uint8_t overflowAxisMask; #endif @@ -130,6 +132,10 @@ typedef struct gyroSensor_s { } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1; +#ifdef USE_DUAL_GYRO +STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor2; +#endif + #ifdef UNIT_TEST STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1; STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev; @@ -158,6 +164,14 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); +#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT +#ifdef USE_DUAL_GYRO +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH +#else +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 +#endif +#endif + PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = ALIGN_DEFAULT, .gyroMovementCalibrationThreshold = 48, @@ -167,7 +181,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_lpf_hz = 90, .gyro_high_fsr = false, .gyro_use_32khz = false, - .gyro_to_use = 0, + .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, .gyro_soft_notch_hz_1 = 400, .gyro_soft_notch_cutoff_1 = 300, .gyro_soft_notch_hz_2 = 200, @@ -183,25 +197,47 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, const busDevice_t *gyroSensorBus(void) { +#ifdef USE_DUAL_GYRO + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { + return &gyroSensor2.gyroDev.bus; + } else { + return &gyroSensor1.gyroDev.bus; + } +#else return &gyroSensor1.gyroDev.bus; +#endif } const mpuConfiguration_t *gyroMpuConfiguration(void) { +#ifdef USE_DUAL_GYRO + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { + return &gyroSensor2.gyroDev.mpuConfiguration; + } else { + return &gyroSensor1.gyroDev.mpuConfiguration; + } +#else return &gyroSensor1.gyroDev.mpuConfiguration; +#endif } const mpuDetectionResult_t *gyroMpuDetectionResult(void) { +#ifdef USE_DUAL_GYRO + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { + return &gyroSensor2.gyroDev.mpuDetectionResult; + } else { + return &gyroSensor1.gyroDev.mpuDetectionResult; + } +#else return &gyroSensor1.gyroDev.mpuDetectionResult; +#endif } STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { gyroSensor_e gyroHardware = GYRO_DEFAULT; - dev->gyroAlign = ALIGN_DEFAULT; - switch (gyroHardware) { case GYRO_DEFAULT: FALLTHROUGH; @@ -372,27 +408,14 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) static bool gyroInitSensor(gyroSensor_t *gyroSensor) { + gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; + #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_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) -#if defined(MPU_INT_EXTI) - gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); -#elif defined(USE_HARDWARE_REVISION_DETECTION) - gyroSensor->gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); -#else - gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG_NONE; -#endif // MPU_INT_EXTI - -#ifdef USE_DUAL_GYRO - // set cnsPin using GYRO_n_CS_PIN defined in target.h - gyroSensor->gyroDev.bus.busdev_u.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_1_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_2_CS_PIN)); -#else - gyroSensor->gyroDev.bus.busdev_u.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h -#endif // USE_DUAL_GYRO mpuDetect(&gyroSensor->gyroDev); mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif - gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); if (gyroHardware == GYRO_NONE) { @@ -423,6 +446,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) } gyroInitSensorFilters(gyroSensor); + #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyseInit(gyro.targetLooptime); #endif @@ -454,8 +478,80 @@ bool gyroInit(void) break; } firstArmingCalibrationWasStarted = false; + + bool ret = false; memset(&gyro, 0, sizeof(gyro)); - return gyroInitSensor(&gyroSensor1); + gyroToUse = gyroConfig()->gyro_to_use; + +#if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN) + if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_1_CS_PIN)); + IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOHi(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. + IOConfigGPIO(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); + } +#endif + +#if defined(USE_DUAL_GYRO) && defined(GYRO_2_CS_PIN) + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_2_CS_PIN)); + IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, 1); + IOHi(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. + IOConfigGPIO(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); + } +#endif + + gyroSensor1.gyroDev.gyroAlign = ALIGN_DEFAULT; + +#if defined(GYRO_1_EXTI_PIN) + gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG(GYRO_1_EXTI_PIN); +#elif defined(MPU_INT_EXTI) + gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); +#elif defined(USE_HARDWARE_REVISION_DETECTION) + gyroSensor1.gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); +#else + gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG_NONE; +#endif // GYRO_1_EXTI_PIN +#ifdef USE_DUAL_GYRO +#ifdef GYRO_1_ALIGN + gyroSensor1.gyroDev.gyroAlign = GYRO_1_ALIGN; +#endif + gyroSensor1.gyroDev.bus.bustype = BUSTYPE_SPI; + spiBusSetInstance(&gyroSensor1.gyroDev.bus, GYRO_1_SPI_INSTANCE); + if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + ret = gyroInitSensor(&gyroSensor1); + if (!ret) { + return false; // TODO handle failure of first gyro detection better. - Perhaps update the config to use second gyro then indicate a new failure mode and reboot. + } + } +#else + ret = gyroInitSensor(&gyroSensor1); +#endif + +#ifdef USE_DUAL_GYRO + + gyroSensor2.gyroDev.gyroAlign = ALIGN_DEFAULT; + +#if defined(GYRO_2_EXTI_PIN) + gyroSensor2.gyroDev.mpuIntExtiTag = IO_TAG(GYRO_2_EXTI_PIN); +#elif defined(USE_HARDWARE_REVISION_DETECTION) + gyroSensor2.gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); +#else + gyroSensor2.gyroDev.mpuIntExtiTag = IO_TAG_NONE; +#endif // GYRO_2_EXTI_PIN +#ifdef GYRO_2_ALIGN + gyroSensor2.gyroDev.gyroAlign = GYRO_2_ALIGN; +#endif + gyroSensor2.gyroDev.bus.bustype = BUSTYPE_SPI; + spiBusSetInstance(&gyroSensor2.gyroDev.bus, GYRO_2_SPI_INSTANCE); + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + ret = gyroInitSensor(&gyroSensor2); + if (!ret) { + return false; // TODO handle failure of second gyro detection better. - Perhaps update the config to use first gyro then indicate a new failure mode and reboot. + } + } +#endif + return ret; } void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz) @@ -613,6 +709,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) void gyroInitFilters(void) { gyroInitSensorFilters(&gyroSensor1); +#ifdef USE_DUAL_GYRO + gyroInitSensorFilters(&gyroSensor2); +#endif } FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) @@ -622,7 +721,22 @@ FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) FAST_CODE bool isGyroCalibrationComplete(void) { +#ifdef USE_DUAL_GYRO + switch (gyroToUse) { + default: + case GYRO_CONFIG_USE_GYRO_1: { + return isGyroSensorCalibrationComplete(&gyroSensor1); + } + case GYRO_CONFIG_USE_GYRO_2: { + return isGyroSensorCalibrationComplete(&gyroSensor2); + } + case GYRO_CONFIG_USE_GYRO_BOTH: { + return isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2); + } + } +#else return isGyroSensorCalibrationComplete(&gyroSensor1); +#endif } static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) @@ -649,6 +763,9 @@ void gyroStartCalibration(bool isFirstArmingCalibration) { if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) { gyroSetCalibrationCycles(&gyroSensor1); +#ifdef USE_DUAL_GYRO + gyroSetCalibrationCycles(&gyroSensor2); +#endif if (isFirstArmingCalibration) { firstArmingCalibrationWasStarted = true; @@ -793,10 +910,6 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else { performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold); - // Reset gyro values to zero to prevent other code from using uncalibrated data - gyro.gyroADCf[X] = 0.0f; - gyro.gyroADCf[Y] = 0.0f; - gyro.gyroADCf[Z] = 0.0f; // still calibrating, so no need to further process gyro data return; } @@ -829,7 +942,7 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); - gyro.gyroADCf[axis] = gyroADCf; + gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; if (!gyroSensor->overflowDetected) { // integrate using trapezium rule to avoid bias accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; @@ -873,7 +986,7 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); - gyro.gyroADCf[axis] = gyroADCf; + gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; if (!gyroSensor->overflowDetected) { // integrate using trapezium rule to avoid bias accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; @@ -885,7 +998,65 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) { +#ifdef USE_DUAL_GYRO + switch (gyroToUse) { + case GYRO_CONFIG_USE_GYRO_1: + gyroUpdateSensor(&gyroSensor1, currentTimeUs); + if (isGyroSensorCalibrationComplete(&gyroSensor1)) { + gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; + gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; + gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; + } + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y])); + break; + case GYRO_CONFIG_USE_GYRO_2: + gyroUpdateSensor(&gyroSensor2, currentTimeUs); + if (isGyroSensorCalibrationComplete(&gyroSensor2)) { + gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X]; + gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y]; + gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z]; + } + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y])); + break; + case GYRO_CONFIG_USE_GYRO_BOTH: + gyroUpdateSensor(&gyroSensor1, currentTimeUs); + gyroUpdateSensor(&gyroSensor2, currentTimeUs); + if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) { + gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f; + gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f; + gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f; + } + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z])); + break; + } +#else gyroUpdateSensor(&gyroSensor1, currentTimeUs); + gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; + gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; + gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; +#endif } bool gyroGetAccumulationAverage(float *accumulationAverage) @@ -920,7 +1091,15 @@ int16_t gyroGetTemperature(void) int16_t gyroRateDps(int axis) { +#ifdef USE_DUAL_GYRO + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) { + return lrintf(gyro.gyroADCf[axis] / gyroSensor2.gyroDev.scale); + } else { + return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale); + } +#else return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale); +#endif } bool gyroOverflowDetected(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 3c4dcabe12..05d3ab4451 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -55,6 +55,10 @@ typedef enum { GYRO_OVERFLOW_CHECK_ALL_AXES } gyroOverflowCheck_e; +#define GYRO_CONFIG_USE_GYRO_1 0 +#define GYRO_CONFIG_USE_GYRO_2 1 +#define GYRO_CONFIG_USE_GYRO_BOTH 2 + 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. diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index b4c1f0ad02..48ce3e6627 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -43,13 +43,19 @@ #define ACC_MPU6000_ALIGN CW270_DEG //#define MPU_INT_EXTI PB9 -#define MPU6000_CS_PIN SPI3_NSS_PIN -#define MPU6000_SPI_INSTANCE SPI3 #define ICM20689_CS_PIN SPI4_NSS_PIN #define ICM20689_SPI_INSTANCE SPI4 -#define GYRO_2_CS_PIN MPU6000_CS_PIN +#define MPU6000_CS_PIN SPI3_NSS_PIN +#define MPU6000_SPI_INSTANCE SPI3 #define GYRO_1_CS_PIN ICM20689_CS_PIN +#define GYRO_2_CS_PIN MPU6000_CS_PIN +#define GYRO_1_SPI_INSTANCE ICM20689_SPI_INSTANCE +#define GYRO_2_SPI_INSTANCE MPU6000_SPI_INSTANCE +#define ACC_1_ALIGN ACC_ICM20689_ALIGN +#define ACC_2_ALIGN ACC_MPU6000_ALIGN +#define GYRO_1_ALIGN GYRO_ICM20689_ALIGN +#define GYRO_2_ALIGN GYRO_MPU6000_ALIGN //#define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index fbc6accafc..01a04b1636 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -63,12 +63,18 @@ #define MPU6000_SPI_INSTANCE SPI1 #define MPU6500_CS_PIN SPI3_NSS_PIN #define MPU6500_SPI_INSTANCE SPI3 -#define GYRO_2_CS_PIN MPU6000_CS_PIN #define GYRO_1_CS_PIN MPU6500_CS_PIN +#define GYRO_2_CS_PIN MPU6000_CS_PIN #define GYRO_MPU6500_ALIGN CW90_DEG #define ACC_MPU6500_ALIGN CW90_DEG -#define GYRO_MPU6000_ALIGN CW90_DEG -#define ACC_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN ALIGN_DEFAULT +#define ACC_MPU6000_ALIGN ALIGN_DEFAULT +#define ACC_1_ALIGN ACC_MPU6500_ALIGN +#define ACC_2_ALIGN ACC_MPU6000_ALIGN +#define GYRO_1_ALIGN GYRO_MPU6500_ALIGN +#define GYRO_2_ALIGN GYRO_MPU6000_ALIGN +#define GYRO_1_SPI_INSTANCE MPU6500_SPI_INSTANCE +#define GYRO_2_SPI_INSTANCE MPU6000_SPI_INSTANCE #else #define MPU6000_CS_PIN SPI3_NSS_PIN #define MPU6000_SPI_INSTANCE SPI3 @@ -76,6 +82,12 @@ #define MPU6500_SPI_INSTANCE SPI1 #define GYRO_1_CS_PIN MPU6000_CS_PIN #define GYRO_2_CS_PIN MPU6500_CS_PIN +#define ACC_1_ALIGN ALIGN_DEFAULT +#define ACC_2_ALIGN ALIGN_DEFAULT +#define GYRO_1_ALIGN ALIGN_DEFAULT +#define GYRO_2_ALIGN ALIGN_DEFAULT +#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE +#define GYRO_2_SPI_INSTANCE MPU6500_SPI_INSTANCE #endif // TODO: dual gyro support diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 02e8e82ca5..eb85702561 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -98,6 +98,10 @@ #define USE_DUAL_GYRO #define GYRO_1_CS_PIN MPU6000_CS_PIN #define GYRO_2_CS_PIN ICM20601_CS_PIN +#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE +#define GYRO_2_SPI_INSTANCE ICM20601_SPI_INSTANCE +#define ACC_1_ALIGN ALIGN_DEFAULT +#define ACC_2_ALIGN ALIGN_DEFAULT #endif #if defined(SOULF4) diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index d44a505f5b..68733e4fcc 100644 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -36,10 +36,10 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USE_EXTI -#define MPU_INT_EXTI PC13 -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW +//#define USE_EXTI +//#define MPU_INT_EXTI PC13 +//#define USE_MPU_DATA_READY_SIGNAL +//#define ENSURE_MPU_DATA_READY_IS_LOW #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index e62cc3a8be..82bd325efd 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -23,6 +23,9 @@ #ifndef SPRACINGF4EVO_REV #define SPRACINGF4EVO_REV 2 #endif +#ifdef SPRACINGF4EVODG +#define USE_DUAL_GYRO +#endif #define USBD_PRODUCT_STRING "SP Racing F4 EVO" @@ -34,22 +37,42 @@ #define INVERTER_PIN_UART2 PB2 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define GYRO_1_EXTI_PIN PC13 +#ifdef USE_DUAL_GYRO +#define GYRO_2_EXTI_PIN PC5 // GYRO 2 / NC on prototype boards, but if it was it'd be here. +#endif +#define MPU_INT_EXTI +#ifndef SPRACINGF4EVODG #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW +#endif #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH #define USE_GYRO +// actual gyro is ICM20602 which is detected by the MPU6500 code #define USE_GYRO_SPI_MPU6500 #define USE_ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#ifndef USE_DUAL_GYRO +#define ACC_MPU6500_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG +#else +#define ACC_MPU6500_1_ALIGN CW0_DEG +#define GYRO_MPU6500_1_ALIGN CW0_DEG + +#define ACC_MPU6500_2_ALIGN CW0_DEG +#define GYRO_MPU6500_2_ALIGN CW0_DEG + +#define GYRO_1_ALIGN GYRO_MPU6500_1_ALIGN +#define GYRO_2_ALIGN GYRO_MPU6500_2_ALIGN +#define ACC_1_ALIGN ACC_MPU6500_1_ALIGN +#define ACC_2_ALIGN ACC_MPU6500_2_ALIGN +#endif #define USE_BARO #define USE_BARO_BMP280 @@ -111,16 +134,18 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI3_NSS_PIN PA15 // NC -#define SPI3_SCK_PIN PB3 // NC -#define SPI3_MISO_PIN PB4 // NC -#define SPI3_MOSI_PIN PB5 // NC +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 +#if !defined(SPRACINGF4EVODG) #define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // SPI3 on an F4 EVO may be used for RTC6705 VTX control. #define RTC6705_CS_PIN SPI3_NSS_PIN #define RTC6705_SPI_INSTANCE SPI3 +#endif #define USE_SDCARD @@ -134,13 +159,19 @@ #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz - #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 #define SDCARD_DMA_CHANNEL 0 +#ifndef USE_DUAL_GYRO +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_INSTANCE SPI1 +#else +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN SPI3_NSS_PIN +#define GYRO_2_SPI_INSTANCE SPI3 +#endif -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_INSTANCE SPI1 #define USE_ADC // It's possible to use ADC1 or ADC3 on this target, same pins. @@ -155,8 +186,9 @@ #define CURRENT_METER_ADC_PIN PC2 #define RSSI_ADC_PIN PC0 +// PC3 - NC - Free for ADC12_IN13 / VTX Enable // PC4 - NC - Free for ADC12_IN14 / VTX CS -// PC5 - NC - Free for ADC12_IN15 / VTX Enable / OSD VSYNC +// PC5 - NC - Free for ADC12_IN15 / OSD VSYNC / G2 MPU INT #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC diff --git a/src/main/target/SPRACINGF4EVO/target.mk b/src/main/target/SPRACINGF4EVO/target.mk index 9ae5387862..f53196b6b6 100644 --- a/src/main/target/SPRACINGF4EVO/target.mk +++ b/src/main/target/SPRACINGF4EVO/target.mk @@ -10,3 +10,9 @@ TARGET_SRC = \ drivers/compass/compass_hmc5883l.c \ drivers/max7456.c \ drivers/vtx_rtc6705.c + +ifeq ($(TARGET), SPRACINGF4EVODG) +TARGET_SRC += \ + drivers/accgyro/accgyro_spi_icm20689.c +endif + diff --git a/src/main/target/SPRACINGF7DUAL/config.c b/src/main/target/SPRACINGF7DUAL/config.c new file mode 100644 index 0000000000..9acea29d35 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/config.c @@ -0,0 +1,62 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include + +#include "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "telemetry/telemetry.h" + +#include "sensors/sensors.h" +#include "sensors/compass.h" +#include "sensors/barometer.h" + +#include "config/feature.h" + +#include "fc/config.h" + +#ifdef USE_TARGET_CONFIG +void targetConfiguration(void) +{ + barometerConfigMutable()->baro_hardware = BARO_DEFAULT; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; + +#ifdef USE_TELEMETRY + // change telemetry settings + telemetryConfigMutable()->telemetry_inverted = 1; + telemetryConfigMutable()->halfDuplex = 1; +#endif + +} +#endif diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c new file mode 100644 index 0000000000..ca367b2516 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/target.c @@ -0,0 +1,68 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" + +#include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" + + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX + DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), // ESC 1 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // ESC 2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // ESC 3 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // ESC 4 + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // ESC 5 / Conflicts with USART5_RX / SPI3_RX - SPI3_RX can be mapped to DMA1_ST3_CH0 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // ESC 6 / Conflicts with USART3_RX + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // ESC 7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // ESC 8 + + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip + // Additional 2 PWM channels available on UART3 RX/TX pins + // However, when using led strip the timer cannot be used, but no code appears to prevent that right now + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0), // Shared with UART3 TX PIN and SPI3 TX (OSD) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 0, 0), // Shared with UART3 RX PIN + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_TRANSPONDER, 0, 0), // Transponder + // Additional 2 PWM channels available on UART1 RX/TX pins + // However, when using transponder the timer cannot be used, but no code appears to prevent that right now + DEF_TIM(TIM1, CH2, PA9, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 3 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 4 +}; + +#if (SPRACINGF7DUAL_REV <= 1) + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" + +void usartTargetConfigure(uartPort_t *uartPort) +{ + if (uartPort->USARTx == USART3) { + uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT; + uartPort->Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE; + } +} +#endif diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h new file mode 100644 index 0000000000..c394ee504e --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -0,0 +1,231 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SP7D" +#define USE_TARGET_CONFIG + +#ifndef SPRACINGF7DUAL_REV +#define SPRACINGF7DUAL_REV 2 +#endif + +#define USBD_PRODUCT_STRING "SP Racing F7 DUAL" + +#define TEST_SOUND // for factory testing audio output + +#define USE_DUAL_GYRO +//#define DEBUG_MODE DEBUG_DUAL_GYRO_DIFF + +#define LED0_PIN PC4 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_EXTI +#define GYRO_1_EXTI_PIN PC13 +#define GYRO_2_EXTI_PIN PC14 +#define MPU_INT_EXTI + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6500 + +#define USE_ACC +#define USE_ACC_SPI_MPU6500 + +#if (SPRACINGF7DUAL_REV >= 2) +#define ACC_MPU6500_1_ALIGN CW0_DEG +#define GYRO_MPU6500_1_ALIGN CW0_DEG + +#define ACC_MPU6500_2_ALIGN CW270_DEG +#define GYRO_MPU6500_2_ALIGN CW270_DEG +#else +#define ACC_MPU6500_1_ALIGN CW180_DEG +#define GYRO_MPU6500_1_ALIGN CW180_DEG + +#define ACC_MPU6500_2_ALIGN CW270_DEG +#define GYRO_MPU6500_2_ALIGN CW270_DEG +#endif + + +#define GYRO_1_ALIGN GYRO_MPU6500_1_ALIGN +#define GYRO_2_ALIGN GYRO_MPU6500_2_ALIGN +#define ACC_1_ALIGN ACC_MPU6500_1_ALIGN +#define ACC_2_ALIGN ACC_MPU6500_2_ALIGN + +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define DEFAULT_BARO_BMP280 + +#define USE_MAG +#define USE_MAG_HMC5883 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define SERIAL_PORT_COUNT 6 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#if (SPRACINGF7DUAL_REV <= 1) + #define TARGET_USART_CONFIG +#endif + +// TODO +// #define USE_ESCSERIAL +// #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 // 2xMPU +#define SPI1_NSS_PIN PA15 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 // MAX7456 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN + +#define USE_SPI_DEVICE_3 // SDCARD +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SDCARD + +#define SDCARD_SPI_INSTANCE SPI3 +#define SDCARD_SPI_CS_PIN PC3 + + +// SPI3 is on the APB1 bus whose clock runs at 54MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 54Mhz/256 = 210kHz +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 54Mhz/3 = 18Mhz // FIXME currently running slower than 18mhz for testing. + +#define SDCARD_DMA_STREAM_TX_FULL DMA1_Stream7 +#define SDCARD_DMA_TX DMA1 +#define SDCARD_DMA_STREAM_TX 7 +#define SDCARD_DMA_CLK LL_AHB1_GRP1_PERIPH_DMA1 + +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF7_4 +#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 + +#define USE_VTX_RTC6705 +#define USE_VTX_RTC6705_SOFTSPI +#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL + +#define RTC6705_SPI_MOSI_PIN PB0 // Shared with PWM8 +#define RTC6705_CS_PIN PB6 // Shared with PWM5 +#define RTC6705_SPICLK_PIN PB1 // Shared with PWM7 +#define RTC6705_POWER_PIN PB7 // Shared with PWM6 + +#define GYRO_1_CS_PIN SPI1_NSS_PIN +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PB2 +#define GYRO_2_SPI_INSTANCE SPI1 + +#define USE_ADC +// It's possible to use ADC1 or ADC3 on this target, same pins. +//#define ADC_INSTANCE ADC1 +//#define ADC1_DMA_STREAM DMA2_Stream0 + +// Using ADC3 frees up DMA2_Stream0 for SPI1_RX +#define ADC_INSTANCE ADC3 +#define ADC3_DMA_STREAM DMA2_Stream1 + +#define VBAT_ADC_PIN PC1 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC0 + +#define CURRENT_METER_SCALE_DEFAULT 300 + +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define USE_OSD +#define USE_OSD_OVER_MSP_DISPLAYPORT +#define USE_MSP_CURRENT_METER + +#define USE_LED_STRIP + +//TODO Implement transponder on F7 +//#define USE_TRANSPONDER + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +//#define RX_CHANNELS_TAER +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP) + +#define GPS_UART SERIAL_PORT_USART3 + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define TELEMETRY_UART SERIAL_PORT_UART5 +#define TELEMETRY_PROVIDER_DEFAULT FUNCTION_TELEMETRY_SMARTPORT + +#define SPEKTRUM_BIND_PIN UART2_RX_PIN + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_BUTTONS +#define BUTTON_A_PIN UART5_RX_PIN +#define BUTTON_A_PIN_INVERTED + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +//#define USABLE_TIMER_CHANNEL_COUNT 16 // 4xPWM, 8xESC, 2xESC via UART3 RX/TX, 1xLED Strip, 1xIR. +#define USABLE_TIMER_CHANNEL_COUNT 16 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(9)) + diff --git a/src/main/target/SPRACINGF7DUAL/target.mk b/src/main/target/SPRACINGF7DUAL/target.mk new file mode 100644 index 0000000000..27cd2b15a6 --- /dev/null +++ b/src/main/target/SPRACINGF7DUAL/target.mk @@ -0,0 +1,16 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro/accgyro_fake.c \ + drivers/barometer/barometer_fake.c \ + drivers/compass/compass_fake.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/max7456.c \ + drivers/vtx_rtc6705_soft_spi.c + + diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h index d84ddb9eda..7e97a51b9f 100644 --- a/src/main/target/stm32f7xx_hal_conf.h +++ b/src/main/target/stm32f7xx_hal_conf.h @@ -54,7 +54,7 @@ /* #define HAL_CEC_MODULE_ENABLED */ /* #define HAL_CRC_MODULE_ENABLED */ /* #define HAL_CRYP_MODULE_ENABLED */ -/* #define HAL_DAC_MODULE_ENABLED */ +#define HAL_DAC_MODULE_ENABLED /* #define HAL_DCMI_MODULE_ENABLED */ /* #define HAL_DMA2D_MODULE_ENABLED */ /* #define HAL_ETH_MODULE_ENABLED */