diff --git a/.gitignore b/.gitignore
index b984b1e5dc..cca56cd746 100644
--- a/.gitignore
+++ b/.gitignore
@@ -24,3 +24,6 @@ README.pdf
/build
# local changes only
make/local.mk
+
+mcu.mak
+mcu.mak.old
diff --git a/.travis.yml b/.travis.yml
index 4920dd94b8..4e7a7b408b 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -10,6 +10,7 @@ env:
# - TARGET=ALIENFLIGHTF1
- TARGET=ALIENFLIGHTF3
- TARGET=ALIENFLIGHTF4
+ - TARGET=ANYFCF7
- TARGET=BETAFLIGHTF3
- TARGET=BLUEJAYF4
- TARGET=CC3D
@@ -77,7 +78,7 @@ compiler: clang
install:
- make arm_sdk_install
-before_script: tools/gcc-arm-none-eabi-5_4-2016q2/bin/arm-none-eabi-gcc --version
+before_script: tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version
script: ./.travis.sh
cache: apt
diff --git a/Makefile b/Makefile
index c57a84548e..db350ff96d 100644
--- a/Makefile
+++ b/Makefile
@@ -37,9 +37,9 @@ SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found)
# Flash size (KB). Some low-end chips actually have more flash than advertised, use this to override.
FLASH_SIZE ?=
-## Set verbosity level based on the V= parameter
-## V=0 Low
-## v=1 High
+## V : Set verbosity level based on the V= parameter
+## V=0 Low
+## V=1 High
export AT := @
ifndef V
@@ -67,7 +67,7 @@ INCLUDE_DIRS = $(SRC_DIR) \
$(ROOT)/src/main/target
LINKER_DIR = $(ROOT)/src/main/target/link
-## Build tools, so we all share the same versions
+# Build tools, so we all share the same versions
# import macros common to all supported build systems
include $(ROOT)/make/system-id.mk
# developer preferences, edit these at will, they'll be gitignored
@@ -92,6 +92,7 @@ HSE_VALUE = 8000000
# used for turning on features like VCP and SDCARD
FEATURES =
+SAMPLE_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 CC3D FURYF4 NAZE REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY
ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk)))))
OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS))
@@ -116,23 +117,27 @@ endif
-include $(ROOT)/src/main/target/$(BASE_TARGET)/target.mk
F4_TARGETS = $(F405_TARGETS) $(F411_TARGETS)
+F7_TARGETS = $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS)
ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?)
endif
-ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS)),)
-$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, or F411. Have you prepared a valid target.mk?)
+ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),)
+$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411 or F7x5. Have you prepared a valid target.mk?)
endif
128K_TARGETS = $(F1_TARGETS)
256K_TARGETS = $(F3_TARGETS)
-512K_TARGETS = $(F411_TARGETS)
-1024K_TARGETS = $(F405_TARGETS)
+512K_TARGETS = $(F411_TARGETS) $(F7X5XE_TARGETS)
+1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS)
+2048K_TARGETS = $(F7X5XI_TARGETS)
# Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already.
ifeq ($(FLASH_SIZE),)
-ifeq ($(TARGET),$(filter $(TARGET),$(1024K_TARGETS)))
+ifeq ($(TARGET),$(filter $(TARGET),$(2048K_TARGETS)))
+FLASH_SIZE = 2048
+else ifeq ($(TARGET),$(filter $(TARGET),$(1024K_TARGETS)))
FLASH_SIZE = 1024
else ifeq ($(TARGET),$(filter $(TARGET),$(512K_TARGETS)))
FLASH_SIZE = 512
@@ -307,6 +312,69 @@ DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE)
# End F4 targets
#
+# Start F7 targets
+else ifeq ($(TARGET),$(filter $(TARGET), $(F7_TARGETS)))
+
+#STDPERIPH
+STDPERIPH_DIR = $(ROOT)/lib/main/STM32F7xx_HAL_Driver
+STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/Src/*.c))
+EXCLUDES = stm32f7xx_hal_timebase_rtc_wakeup_template.c \
+ stm32f7xx_hal_timebase_rtc_alarm_template.c \
+ stm32f7xx_hal_timebase_tim_template.c
+
+STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
+
+#USB
+USBCORE_DIR = $(ROOT)/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core
+USBCORE_SRC = $(notdir $(wildcard $(USBCORE_DIR)/Src/*.c))
+EXCLUDES = usbd_conf_template.c
+USBCORE_SRC := $(filter-out ${EXCLUDES}, $(USBCORE_SRC))
+
+USBCDC_DIR = $(ROOT)/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC
+USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/Src/*.c))
+EXCLUDES = usbd_cdc_if_template.c
+USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC))
+
+VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src
+
+DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \
+ $(USBCORE_SRC) \
+ $(USBCDC_SRC)
+
+#CMSIS
+VPATH := $(VPATH):$(CMSIS_DIR)/CM7/Include:$(CMSIS_DIR)/CM7/Device/ST/STM32F7xx
+VPATH := $(VPATH):$(STDPERIPH_DIR)/Src
+CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM7/Include/*.c \
+ $(CMSIS_DIR)/CM7/Device/ST/STM32F7xx/*.c))
+INCLUDE_DIRS := $(INCLUDE_DIRS) \
+ $(STDPERIPH_DIR)/Inc \
+ $(USBCORE_DIR)/Inc \
+ $(USBCDC_DIR)/Inc \
+ $(CMSIS_DIR)/CM7/Include \
+ $(CMSIS_DIR)/CM7/Device/ST/STM32F7xx/Include \
+ $(ROOT)/src/main/vcp_hal
+
+ifneq ($(filter SDCARD,$(FEATURES)),)
+INCLUDE_DIRS := $(INCLUDE_DIRS) \
+ $(FATFS_DIR)
+VPATH := $(VPATH):$(FATFS_DIR)
+endif
+
+#Flags
+ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant -Wdouble-promotion
+
+ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS)))
+DEVICE_FLAGS = -DSTM32F745xx -DUSE_HAL_DRIVER -D__FPU_PRESENT -DDEBUG_HARDFAULTS
+LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f745.ld
+else
+$(error Unknown MCU for F7 target)
+endif
+DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE)
+
+TARGET_FLAGS = -D$(TARGET)
+
+# End F7 targets
+#
# Start F1 targets
else
@@ -505,6 +573,12 @@ VCP_SRC = \
vcpf4/usbd_usr.c \
vcpf4/usbd_cdc_vcp.c \
drivers/serial_usb_vcp.c
+else ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
+VCP_SRC = \
+ vcp_hal/usbd_desc.c \
+ vcp_hal/usbd_conf.c \
+ vcp_hal/usbd_cdc_interface.c \
+ drivers/serial_usb_vcp_hal.c
else
VCP_SRC = \
vcp/hw_config.c \
@@ -558,9 +632,35 @@ STM32F4xx_COMMON_SRC = \
drivers/system_stm32f4xx.c \
drivers/timer_stm32f4xx.c
+STM32F7xx_COMMON_SRC = \
+ startup_stm32f745xx.s \
+ target/system_stm32f7xx.c \
+ drivers/accgyro_mpu.c \
+ drivers/adc_stm32f7xx.c \
+ drivers/bus_i2c_hal.c \
+ drivers/dma_stm32f7xx.c \
+ drivers/gpio_stm32f7xx.c \
+ drivers/inverter.c \
+ drivers/bus_spi_hal.c \
+ drivers/pwm_output_stm32f7xx.c \
+ drivers/timer_hal.c \
+ drivers/timer_stm32f7xx.c \
+ drivers/pwm_output_hal.c \
+ drivers/system_stm32f7xx.c \
+ drivers/serial_uart_stm32f7xx.c \
+ drivers/serial_uart_hal.c
+
+F7EXCLUDES = drivers/bus_spi.c \
+ drivers/bus_i2c.c \
+ drivers/timer.c \
+ drivers/pwm_output.c \
+ drivers/serial_uart.c
+
# check if target.mk supplied
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
TARGET_SRC := $(STM32F4xx_COMMON_SRC) $(TARGET_SRC)
+else ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
+TARGET_SRC := $(STM32F7xx_COMMON_SRC) $(TARGET_SRC)
else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
TARGET_SRC := $(STM32F30x_COMMON_SRC) $(TARGET_SRC)
else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
@@ -573,13 +673,17 @@ TARGET_SRC += \
io/flashfs.c
endif
-ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS) $(F3_TARGETS)))
+ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS) $(F4_TARGETS) $(F3_TARGETS)))
TARGET_SRC += $(HIGHEND_SRC)
else ifneq ($(filter HIGHEND,$(FEATURES)),)
TARGET_SRC += $(HIGHEND_SRC)
endif
TARGET_SRC += $(COMMON_SRC)
+#excludes
+ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
+TARGET_SRC := $(filter-out ${F7EXCLUDES}, $(TARGET_SRC))
+endif
ifneq ($(filter SDCARD,$(FEATURES)),)
TARGET_SRC += \
@@ -724,6 +828,8 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S
$(V1) echo %% $(notdir $<)
$(V1) $(CC) -c -o $@ $(ASFLAGS) $<
+## sample : Build all sample (travis) targets
+sample: $(SAMPLE_TARGETS)
## all : Build all valid targets
all: $(VALID_TARGETS)
@@ -799,7 +905,7 @@ cppcheck: $(CSOURCES)
cppcheck-result.xml: $(CSOURCES)
$(V0) $(CPPCHECK) --xml-version=2 2> cppcheck-result.xml
-## mkdirs
+# mkdirs
$(DL_DIR):
mkdir -p $@
@@ -810,7 +916,7 @@ $(BUILD_DIR):
mkdir -p $@
## help : print this help message and exit
-help: Makefile
+help: Makefile make/tools.mk
$(V0) @echo ""
$(V0) @echo "Makefile for the $(FORKNAME) firmware"
$(V0) @echo ""
@@ -821,7 +927,7 @@ help: Makefile
$(V0) @echo ""
$(V0) @echo "Valid TARGET values are: $(VALID_TARGETS)"
$(V0) @echo ""
- $(V0) @sed -n 's/^## //p' $<
+ $(V0) @sed -n 's/^## //p' $?
## targets : print a list of all valid target platforms (for consumption by scripts)
targets:
diff --git a/make/tools.mk b/make/tools.mk
index a12dc50a3c..8d0b24d145 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -14,23 +14,26 @@
##############################
# Set up ARM (STM32) SDK
-ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q2
+ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3
# Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
GCC_REQUIRED_VERSION := 5.4.1
+## arm_sdk_install : Install Arm SDK
.PHONY: arm_sdk_install
+ARM_SDK_URL_BASE := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926
+
# source: https://launchpad.net/gcc-arm-embedded/5.0/
ifdef LINUX
- arm_sdk_install: ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2
+ arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2
endif
ifdef MACOSX
- arm_sdk_install: ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-mac.tar.bz2
+ arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-mac.tar.bz2
endif
ifdef WINDOWS
- arm_sdk_install: ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-win32.zip
+ arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip
endif
arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
@@ -48,6 +51,7 @@ else
$(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)"
endif
+## arm_sdk_clean : Uninstall Arm SDK
.PHONY: arm_sdk_clean
arm_sdk_clean:
$(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR)
diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index b2aea26282..4603091ed4 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -31,7 +31,7 @@ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint3
__ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
}
-#ifndef STM32F4 /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
+#if !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri)
{
__ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" );
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index e291ae25c9..420418e0d3 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -270,7 +270,7 @@ int16_t firFilterInt16Get(const firFilter_t *filter, int index)
}
void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) {
- filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_DENOISE_WINDOW_SIZE);
+ filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_WINDOW_SIZE);
}
/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 91057e3aa4..4db6999e1c 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -15,7 +15,11 @@
* along with Cleanflight. If not, see .
*/
-#define MAX_DENOISE_WINDOW_SIZE 120
+#ifdef STM32F10X
+#define MAX_FIR_WINDOW_SIZE 60
+#else
+#define MAX_FIR_WINDOW_SIZE 120
+#endif
typedef struct pt1Filter_s {
float state;
@@ -34,7 +38,7 @@ typedef struct firFilterState_s {
int targetCount;
int index;
float movingSum;
- float state[MAX_DENOISE_WINDOW_SIZE];
+ float state[MAX_FIR_WINDOW_SIZE];
} firFilterState_t;
typedef enum {
diff --git a/src/main/common/utils.h b/src/main/common/utils.h
index 6958902b3e..23088d076d 100644
--- a/src/main/common/utils.h
+++ b/src/main/common/utils.h
@@ -32,7 +32,9 @@
#define EXPAND_I(x) x
#define EXPAND(x) EXPAND_I(x)
+#if !defined(USE_HAL_DRIVER)
#define UNUSED(x) (void)(x)
+#endif
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
#define BIT(x) (1 << (x))
@@ -43,6 +45,7 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
#define BITCOUNT(x) (((BX_(x)+(BX_(x)>>4)) & 0x0F0F0F0F) % 255)
#define BX_(x) ((x) - (((x)>>1)&0x77777777) - (((x)>>2)&0x33333333) - (((x)>>3)&0x11111111))
+
/*
* https://groups.google.com/forum/?hl=en#!msg/comp.lang.c/attFnqwhvGk/sGBKXvIkY3AJ
* Return (v ? floor(log2(v)) : 0) when 0 <= v < 1<<[8, 16, 32, 64].
diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index fae6604778..d9ceba8828 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -48,6 +48,10 @@
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
#endif
+ #if defined(STM32F745xx)
+ #define FLASH_PAGE_SIZE ((uint32_t)0x40000)
+ #endif
+
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
@@ -73,6 +77,8 @@
#define FLASH_PAGE_COUNT 4 // just to make calculations work
#elif defined (STM32F411xE)
#define FLASH_PAGE_COUNT 4 // just to make calculations work
+#elif defined (STM32F745xx)
+#define FLASH_PAGE_COUNT 4 // just to make calculations work
#else
#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
#endif
@@ -140,6 +146,70 @@ bool isEEPROMContentValid(void)
return true;
}
+#if defined(STM32F7)
+
+// FIXME: HAL for now this will only work for F4/F7 as flash layout is different
+void writeEEPROM(void)
+{
+ // Generate compile time error if the config does not fit in the reserved area of flash.
+ BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
+
+ HAL_StatusTypeDef status;
+ uint32_t wordOffset;
+ int8_t attemptsRemaining = 3;
+
+ suspendRxSignal();
+
+ // prepare checksum/version constants
+ masterConfig.version = EEPROM_CONF_VERSION;
+ masterConfig.size = sizeof(master_t);
+ masterConfig.magic_be = 0xBE;
+ masterConfig.magic_ef = 0xEF;
+ masterConfig.chk = 0; // erase checksum before recalculating
+ masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
+
+ // write it
+ /* Unlock the Flash to enable the flash control register access *************/
+ HAL_FLASH_Unlock();
+ while (attemptsRemaining--)
+ {
+ /* Fill EraseInit structure*/
+ FLASH_EraseInitTypeDef EraseInitStruct = {0};
+ EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS;
+ EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
+ EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1);
+ EraseInitStruct.NbSectors = 1;
+ uint32_t SECTORError;
+ status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+ if (status != HAL_OK)
+ {
+ continue;
+ }
+ else
+ {
+ for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4)
+ {
+ status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset));
+ if(status != HAL_OK)
+ {
+ break;
+ }
+ }
+ }
+ if (status == HAL_OK) {
+ break;
+ }
+ }
+ HAL_FLASH_Lock();
+
+ // Flash write failed - just die now
+ if (status != HAL_OK || !isEEPROMContentValid()) {
+ failureMode(FAILURE_FLASH_WRITE_FAILED);
+ }
+
+ resumeRxSignal();
+}
+#else
void writeEEPROM(void)
{
// Generate compile time error if the config does not fit in the reserved area of flash.
@@ -202,6 +272,7 @@ void writeEEPROM(void)
resumeRxSignal();
}
+#endif
void readEEPROM(void)
{
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index 6bf8b3f1dc..f5cd4a2d0f 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -256,6 +256,12 @@ void mpuIntExtiInit(void)
}
#endif
+#if defined (STM32F7)
+ IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
+ EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
+ EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
+#else
+
IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
@@ -263,7 +269,8 @@ void mpuIntExtiInit(void)
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
#endif
-
+#endif
+
mpuExtiInitDone = true;
}
diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h
index c89dc1bf53..66e2f887c9 100644
--- a/src/main/drivers/adc_impl.h
+++ b/src/main/drivers/adc_impl.h
@@ -20,7 +20,7 @@
#include "io_types.h"
#include "rcc_types.h"
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(STM32F7)
#define ADC_TAG_MAP_COUNT 16
#elif defined(STM32F3)
#define ADC_TAG_MAP_COUNT 39
@@ -34,7 +34,7 @@ typedef enum ADCDevice {
#if defined(STM32F3)
ADCDEV_2,
ADCDEV_MAX = ADCDEV_2,
-#elif defined(STM32F4)
+#elif defined(STM32F4) || defined(STM32F7)
ADCDEV_2,
ADCDEV_3,
ADCDEV_MAX = ADCDEV_3,
@@ -52,7 +52,7 @@ typedef struct adcDevice_s {
ADC_TypeDef* ADCx;
rccPeriphTag_t rccADC;
rccPeriphTag_t rccDMA;
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(STM32F7)
DMA_Stream_TypeDef* DMAy_Streamx;
uint32_t channel;
#else
diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c
new file mode 100644
index 0000000000..7bdba67f52
--- /dev/null
+++ b/src/main/drivers/adc_stm32f7xx.c
@@ -0,0 +1,209 @@
+/*
+ * 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 "platform.h"
+#include "system.h"
+
+#include "io.h"
+#include "io_impl.h"
+#include "rcc.h"
+
+#include "sensor.h"
+#include "accgyro.h"
+
+#include "adc.h"
+#include "adc_impl.h"
+
+#ifndef ADC_INSTANCE
+#define ADC_INSTANCE ADC1
+#endif
+
+const adcDevice_t adcHardware[] = {
+ { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0 },
+ //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
+};
+
+/* note these could be packed up for saving space */
+const adcTagMap_t adcTagMap[] = {
+/*
+ { DEFIO_TAG_E__PF3, ADC_Channel_9 },
+ { DEFIO_TAG_E__PF4, ADC_Channel_14 },
+ { DEFIO_TAG_E__PF5, ADC_Channel_15 },
+ { DEFIO_TAG_E__PF6, ADC_Channel_4 },
+ { DEFIO_TAG_E__PF7, ADC_Channel_5 },
+ { DEFIO_TAG_E__PF8, ADC_Channel_6 },
+ { DEFIO_TAG_E__PF9, ADC_Channel_7 },
+ { DEFIO_TAG_E__PF10, ADC_Channel_8 },
+*/
+ { DEFIO_TAG_E__PC0, ADC_CHANNEL_10 },
+ { DEFIO_TAG_E__PC1, ADC_CHANNEL_11 },
+ { DEFIO_TAG_E__PC2, ADC_CHANNEL_12 },
+ { DEFIO_TAG_E__PC3, ADC_CHANNEL_13 },
+ { DEFIO_TAG_E__PC4, ADC_CHANNEL_14 },
+ { DEFIO_TAG_E__PC5, ADC_CHANNEL_15 },
+ { DEFIO_TAG_E__PB0, ADC_CHANNEL_8 },
+ { DEFIO_TAG_E__PB1, ADC_CHANNEL_9 },
+ { DEFIO_TAG_E__PA0, ADC_CHANNEL_0 },
+ { DEFIO_TAG_E__PA1, ADC_CHANNEL_1 },
+ { DEFIO_TAG_E__PA2, ADC_CHANNEL_2 },
+ { DEFIO_TAG_E__PA3, ADC_CHANNEL_3 },
+ { DEFIO_TAG_E__PA4, ADC_CHANNEL_4 },
+ { DEFIO_TAG_E__PA5, ADC_CHANNEL_5 },
+ { DEFIO_TAG_E__PA6, ADC_CHANNEL_6 },
+ { DEFIO_TAG_E__PA7, ADC_CHANNEL_7 },
+};
+
+ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
+{
+ if (instance == ADC1)
+ return ADCDEV_1;
+/*
+ if (instance == ADC2) // TODO add ADC2 and 3
+ return ADCDEV_2;
+*/
+ return ADCINVALID;
+}
+
+void adcInit(drv_adc_config_t *init)
+{
+ DMA_HandleTypeDef DmaHandle;
+ ADC_HandleTypeDef ADCHandle;
+
+ uint8_t i;
+ uint8_t configuredAdcChannels = 0;
+
+ memset(&adcConfig, 0, sizeof(adcConfig));
+
+#if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN)
+ UNUSED(init);
+#endif
+
+#ifdef VBAT_ADC_PIN
+ if (init->enableVBat) {
+ adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); //VBAT_ADC_CHANNEL;
+ }
+#endif
+
+#ifdef RSSI_ADC_PIN
+ if (init->enableRSSI) {
+ adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); //RSSI_ADC_CHANNEL;
+ }
+#endif
+
+#ifdef EXTERNAL1_ADC_PIN
+ if (init->enableExternal1) {
+ adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); //EXTERNAL1_ADC_CHANNEL;
+ }
+#endif
+
+#ifdef CURRENT_METER_ADC_PIN
+ if (init->enableCurrentMeter) {
+ adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); //CURRENT_METER_ADC_CHANNEL;
+ }
+#endif
+
+ ADCDevice device = adcDeviceByInstance(ADC_INSTANCE);
+ if (device == ADCINVALID)
+ return;
+
+ adcDevice_t adc = adcHardware[device];
+
+ for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
+ if (!adcConfig[i].tag)
+ continue;
+
+ IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_BATTERY + i, 0);
+ IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_NOPULL));
+ adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
+ adcConfig[i].dmaIndex = configuredAdcChannels++;
+ adcConfig[i].sampleTime = ADC_SAMPLETIME_480CYCLES;
+ adcConfig[i].enabled = true;
+ }
+
+ RCC_ClockCmd(adc.rccDMA, ENABLE);
+ RCC_ClockCmd(adc.rccADC, ENABLE);
+
+ ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8;
+ ADCHandle.Init.ContinuousConvMode = ENABLE;
+ ADCHandle.Init.Resolution = ADC_RESOLUTION_12B;
+ ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
+ ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+ ADCHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+ ADCHandle.Init.NbrOfConversion = configuredAdcChannels;
+ ADCHandle.Init.ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
+ ADCHandle.Init.DiscontinuousConvMode = DISABLE;
+ ADCHandle.Init.NbrOfDiscConversion = 0;
+ ADCHandle.Init.DMAContinuousRequests = ENABLE;
+ ADCHandle.Init.EOCSelection = DISABLE;
+ ADCHandle.Instance = adc.ADCx;
+
+ /*##-1- Configure the ADC peripheral #######################################*/
+ if (HAL_ADC_Init(&ADCHandle) != HAL_OK)
+ {
+ /* Initialization Error */
+ }
+
+ DmaHandle.Init.Channel = adc.channel;
+ DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+ DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ DmaHandle.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
+ DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+ DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+ DmaHandle.Init.Mode = DMA_CIRCULAR;
+ DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
+ DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+ DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+ DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+ DmaHandle.Instance = adc.DMAy_Streamx;
+
+ /*##-2- Initialize the DMA stream ##########################################*/
+ if (HAL_DMA_Init(&DmaHandle) != HAL_OK)
+ {
+ /* Initialization Error */
+ }
+
+ __HAL_LINKDMA(&ADCHandle, DMA_Handle, DmaHandle);
+
+ uint8_t rank = 1;
+ for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
+ if (!adcConfig[i].enabled) {
+ continue;
+ }
+ ADC_ChannelConfTypeDef sConfig;
+ sConfig.Channel = adcConfig[i].adcChannel;
+ sConfig.Rank = rank++;
+ sConfig.SamplingTime = adcConfig[i].sampleTime;
+ sConfig.Offset = 0;
+
+ /*##-3- Configure ADC regular channel ######################################*/
+ if (HAL_ADC_ConfigChannel(&ADCHandle, &sConfig) != HAL_OK)
+ {
+ /* Channel Configuration Error */
+ }
+ }
+
+ /*##-4- Start the conversion process #######################################*/
+ if(HAL_ADC_Start_DMA(&ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK)
+ {
+ /* Start Conversation Error */
+ }
+}
diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h
index 26cce3792f..da2da2ff7c 100644
--- a/src/main/drivers/bus_i2c.h
+++ b/src/main/drivers/bus_i2c.h
@@ -33,7 +33,8 @@ typedef enum I2CDevice {
I2CDEV_1 = 0,
I2CDEV_2,
I2CDEV_3,
- I2CDEV_MAX = I2CDEV_3,
+ I2CDEV_4,
+ I2CDEV_MAX = I2CDEV_4,
} I2CDevice;
typedef struct i2cDevice_s {
@@ -46,6 +47,9 @@ typedef struct i2cDevice_s {
uint8_t ev_irq;
uint8_t er_irq;
#endif
+#if defined(STM32F7)
+ uint8_t af;
+#endif
} i2cDevice_t;
typedef struct i2cState_s {
diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c
new file mode 100644
index 0000000000..88238ed077
--- /dev/null
+++ b/src/main/drivers/bus_i2c_hal.c
@@ -0,0 +1,294 @@
+/*
+ * 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 "io.h"
+#include "system.h"
+
+#include "bus_i2c.h"
+#include "nvic.h"
+#include "io_impl.h"
+#include "rcc.h"
+
+#ifndef SOFT_I2C
+
+#define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default)
+
+static void i2cUnstick(IO_t scl, IO_t sda);
+
+#if defined(USE_I2C_PULLUP)
+#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#else
+#define IOCFG_I2C IOCFG_AF_OD
+#endif
+
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+
+#ifndef I2C3_SCL
+#define I2C3_SCL PA8
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA PB4
+#endif
+
+#ifndef I2C4_SCL
+#define I2C4_SCL PD12
+#endif
+#ifndef I2C4_SDA
+#define I2C4_SDA PD13
+#endif
+
+static i2cDevice_t i2cHardwareMap[] = {
+ { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn, .af = GPIO_AF4_I2C1 },
+ { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 },
+ { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 },
+ { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1(I2C4), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
+};
+
+
+typedef struct{
+ I2C_HandleTypeDef Handle;
+}i2cHandle_t;
+static i2cHandle_t i2cHandle[I2CDEV_MAX+1];
+
+void I2C1_ER_IRQHandler(void)
+{
+ HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_1].Handle);
+}
+
+void I2C1_EV_IRQHandler(void)
+{
+ HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_1].Handle);
+}
+
+void I2C2_ER_IRQHandler(void)
+{
+ HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_2].Handle);
+}
+
+void I2C2_EV_IRQHandler(void)
+{
+ HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_2].Handle);
+}
+
+void I2C3_ER_IRQHandler(void)
+{
+ HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_3].Handle);
+}
+
+void I2C3_EV_IRQHandler(void)
+{
+ HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_3].Handle);
+}
+
+void I2C4_ER_IRQHandler(void)
+{
+ HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_4].Handle);
+}
+
+void I2C4_EV_IRQHandler(void)
+{
+ HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_4].Handle);
+}
+
+static volatile uint16_t i2cErrorCount = 0;
+
+static bool i2cOverClock;
+
+void i2cSetOverclock(uint8_t OverClock) {
+ i2cOverClock = (OverClock) ? true : false;
+}
+
+static bool i2cHandleHardwareFailure(I2CDevice device)
+{
+ (void)device;
+ i2cErrorCount++;
+ // reinit peripheral + clock out garbage
+ //i2cInit(device);
+ return false;
+}
+
+bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
+{
+ HAL_StatusTypeDef status;
+
+ if(reg_ == 0xFF)
+ status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT);
+ else
+ status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT);
+
+ if(status != HAL_OK)
+ return i2cHandleHardwareFailure(device);
+
+ return true;
+}
+
+bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
+{
+ return i2cWriteBuffer(device, addr_, reg_, 1, &data);
+}
+
+bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
+{
+ HAL_StatusTypeDef status;
+
+ if(reg_ == 0xFF)
+ status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT);
+ else
+ status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT);
+
+ if(status != HAL_OK)
+ return i2cHandleHardwareFailure(device);
+
+ return true;
+}
+
+void i2cInit(I2CDevice device)
+{
+ /*## Configure the I2C clock source. The clock is derived from the SYSCLK #*/
+// RCC_PeriphCLKInitTypeDef RCC_PeriphCLKInitStruct;
+// RCC_PeriphCLKInitStruct.PeriphClockSelection = i2cHardwareMap[device].clk;
+// RCC_PeriphCLKInitStruct.I2c1ClockSelection = i2cHardwareMap[device].clk_src;
+// HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphCLKInitStruct);
+
+ switch (device) {
+ case I2CDEV_1:
+ __HAL_RCC_I2C1_CLK_ENABLE();
+ break;
+ case I2CDEV_2:
+ __HAL_RCC_I2C2_CLK_ENABLE();
+ break;
+ case I2CDEV_3:
+ __HAL_RCC_I2C3_CLK_ENABLE();
+ break;
+ case I2CDEV_4:
+ __HAL_RCC_I2C4_CLK_ENABLE();
+ break;
+ default:
+ break;
+ }
+ if (device == I2CINVALID)
+ return;
+
+ i2cDevice_t *i2c;
+ i2c = &(i2cHardwareMap[device]);
+
+ //I2C_InitTypeDef i2cInit;
+
+ IO_t scl = IOGetByTag(i2c->scl);
+ IO_t sda = IOGetByTag(i2c->sda);
+
+ IOInit(scl, OWNER_I2C, RESOURCE_I2C_SCL, RESOURCE_INDEX(device));
+ IOInit(sda, OWNER_I2C, RESOURCE_I2C_SDA, RESOURCE_INDEX(device));
+
+ // Enable RCC
+ RCC_ClockCmd(i2c->rcc, ENABLE);
+
+
+ i2cUnstick(scl, sda);
+
+ // Init pins
+ #ifdef STM32F7
+ IOConfigGPIOAF(scl, IOCFG_I2C, i2c->af);
+ IOConfigGPIOAF(sda, IOCFG_I2C, i2c->af);
+ #else
+ IOConfigGPIO(scl, IOCFG_AF_OD);
+ IOConfigGPIO(sda, IOCFG_AF_OD);
+ #endif
+ // Init I2C peripheral
+ HAL_I2C_DeInit(&i2cHandle[device].Handle);
+
+ i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev;
+ /// TODO: HAL check if I2C timing is correct
+ i2cHandle[device].Handle.Init.Timing = 0x00B01B59;
+ //i2cHandle[device].Handle.Init.Timing = 0x00D00E28; /* (Rise time = 120ns, Fall time = 25ns) */
+ i2cHandle[device].Handle.Init.OwnAddress1 = 0x0;
+ i2cHandle[device].Handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+ i2cHandle[device].Handle.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+ i2cHandle[device].Handle.Init.OwnAddress2 = 0x0;
+ i2cHandle[device].Handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+ i2cHandle[device].Handle.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+
+
+ HAL_I2C_Init(&i2cHandle[device].Handle);
+ /* Enable the Analog I2C Filter */
+ HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
+
+ HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+ HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
+ HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+ HAL_NVIC_EnableIRQ(i2cHardwareMap[device].ev_irq);
+}
+
+uint16_t i2cGetErrorCounter(void)
+{
+ return i2cErrorCount;
+}
+
+static void i2cUnstick(IO_t scl, IO_t sda)
+{
+ int i;
+ int timeout = 100;
+
+ IOHi(scl);
+ IOHi(sda);
+
+ IOConfigGPIO(scl, IOCFG_OUT_OD);
+ IOConfigGPIO(sda, IOCFG_OUT_OD);
+
+ for (i = 0; i < 8; i++) {
+ // Wait for any clock stretching to finish
+ while (!IORead(scl) && timeout) {
+ delayMicroseconds(10);
+ timeout--;
+ }
+
+ // Pull low
+ IOLo(scl); // Set bus low
+ delayMicroseconds(10);
+ IOHi(scl); // Set bus high
+ delayMicroseconds(10);
+ }
+
+ // Generate a start then stop condition
+ IOLo(sda); // Set bus data low
+ delayMicroseconds(10);
+ IOLo(scl); // Set bus scl low
+ delayMicroseconds(10);
+ IOHi(scl); // Set bus scl high
+ delayMicroseconds(10);
+ IOHi(sda); // Set bus sda high
+}
+
+#endif
diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c
index aeff433585..8e62e09957 100644
--- a/src/main/drivers/bus_spi.c
+++ b/src/main/drivers/bus_spi.c
@@ -199,6 +199,13 @@ bool spiInit(SPIDevice device)
return true;
#else
break;
+#endif
+ case SPIDEV_4:
+#if defined(USE_SPI_DEVICE_4)
+ spiInitDevice(device);
+ return true;
+#else
+ break;
#endif
}
return false;
diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h
index 3925cdce81..a3eb8754fc 100644
--- a/src/main/drivers/bus_spi.h
+++ b/src/main/drivers/bus_spi.h
@@ -25,6 +25,11 @@
#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN)
#define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP)
#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
+#elif defined(STM32F7)
+#define SPI_IO_AF_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN)
+#define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#define SPI_IO_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
#elif defined(STM32F1)
#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_50MHz)
#define SPI_IO_AF_MOSI_CFG IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_50MHz)
@@ -42,6 +47,11 @@ typedef enum {
SPI_CLOCK_STANDARD = 8, //10.50000 MHz
SPI_CLOCK_FAST = 4, //21.00000 MHz
SPI_CLOCK_ULTRAFAST = 2, //42.00000 MHz
+#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_ULTRAFAST = 2, //54.00000 MHz
#else
SPI_CLOCK_SLOW = 128, //00.56250 MHz
SPI_CLOCK_STANDARD = 4, //09.00000 MHz
@@ -55,7 +65,8 @@ typedef enum SPIDevice {
SPIDEV_1 = 0,
SPIDEV_2,
SPIDEV_3,
- SPIDEV_MAX = SPIDEV_3,
+ SPIDEV_4,
+ SPIDEV_MAX = SPIDEV_4,
} SPIDevice;
typedef struct SPIDevice_s {
@@ -81,3 +92,7 @@ uint16_t spiGetErrorCounter(SPI_TypeDef *instance);
void spiResetErrorCounter(SPI_TypeDef *instance);
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance);
+#if defined(USE_HAL_DRIVER)
+SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance);
+DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size);
+#endif
diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c
new file mode 100644
index 0000000000..4da26ffb6a
--- /dev/null
+++ b/src/main/drivers/bus_spi_hal.c
@@ -0,0 +1,437 @@
+/*
+ * 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 "bus_spi.h"
+#include "dma.h"
+#include "io.h"
+#include "io_impl.h"
+#include "nvic.h"
+#include "rcc.h"
+
+#ifndef SPI1_SCK_PIN
+#define SPI1_NSS_PIN PA4
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+#endif
+
+#ifndef SPI2_SCK_PIN
+#define SPI2_NSS_PIN PB12
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+#endif
+
+#ifndef SPI3_SCK_PIN
+#define SPI3_NSS_PIN PA15
+#define SPI3_SCK_PIN PB3
+#define SPI3_MISO_PIN PB4
+#define SPI3_MOSI_PIN PB5
+#endif
+
+#ifndef SPI4_SCK_PIN
+#define SPI4_NSS_PIN PA15
+#define SPI4_SCK_PIN PB3
+#define SPI4_MISO_PIN PB4
+#define SPI4_MOSI_PIN PB5
+#endif
+
+#ifndef SPI1_NSS_PIN
+#define SPI1_NSS_PIN NONE
+#endif
+#ifndef SPI2_NSS_PIN
+#define SPI2_NSS_PIN NONE
+#endif
+#ifndef SPI3_NSS_PIN
+#define SPI3_NSS_PIN NONE
+#endif
+#ifndef SPI4_NSS_PIN
+#define SPI4_NSS_PIN NONE
+#endif
+
+
+static spiDevice_t spiHardwareMap[] = {
+ { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, false },
+ { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, false },
+ { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF5_SPI3, false },
+ { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, false }
+};
+
+typedef struct{
+ SPI_HandleTypeDef Handle;
+}spiHandle_t;
+static spiHandle_t spiHandle[SPIDEV_MAX+1];
+
+typedef struct{
+ DMA_HandleTypeDef Handle;
+}dmaHandle_t;
+static dmaHandle_t dmaHandle[SPIDEV_MAX+1];
+
+SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
+{
+ if (instance == SPI1)
+ return SPIDEV_1;
+
+ if (instance == SPI2)
+ return SPIDEV_2;
+
+ if (instance == SPI3)
+ return SPIDEV_3;
+
+ if (instance == SPI4)
+ return SPIDEV_4;
+
+ return SPIINVALID;
+}
+
+SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance)
+{
+ return &spiHandle[spiDeviceByInstance(instance)].Handle;
+}
+
+DMA_HandleTypeDef* dmaHandleByInstance(SPI_TypeDef *instance)
+{
+ return &dmaHandle[spiDeviceByInstance(instance)].Handle;
+}
+
+void SPI1_IRQHandler(void)
+{
+ HAL_SPI_IRQHandler(&spiHandle[SPIDEV_1].Handle);
+}
+
+void SPI2_IRQHandler(void)
+{
+ HAL_SPI_IRQHandler(&spiHandle[SPIDEV_2].Handle);
+}
+
+void SPI3_IRQHandler(void)
+{
+ HAL_SPI_IRQHandler(&spiHandle[SPIDEV_3].Handle);
+}
+
+void SPI4_IRQHandler(void)
+{
+ HAL_SPI_IRQHandler(&spiHandle[SPIDEV_4].Handle);
+}
+
+
+void spiInitDevice(SPIDevice device)
+{
+ static SPI_InitTypeDef spiInit;
+ spiDevice_t *spi = &(spiHardwareMap[device]);
+
+
+#ifdef SDCARD_SPI_INSTANCE
+ if (spi->dev == SDCARD_SPI_INSTANCE) {
+ spi->leadingEdge = true;
+ }
+#endif
+#ifdef RX_SPI_INSTANCE
+ if (spi->dev == RX_SPI_INSTANCE) {
+ spi->leadingEdge = true;
+ }
+#endif
+
+ // Enable SPI clock
+ RCC_ClockCmd(spi->rcc, ENABLE);
+ RCC_ResetCmd(spi->rcc, ENABLE);
+
+ IOInit(IOGetByTag(spi->sck), OWNER_SPI, RESOURCE_SPI_SCK, device + 1);
+ IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1);
+ IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1);
+
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
+ IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af);
+ IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af);
+ IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
+
+ if (spi->nss) {
+ IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af);
+ }
+#endif
+#if defined(STM32F10X)
+ IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
+ IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
+ IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
+
+ if (spi->nss) {
+ IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
+ }
+#endif
+ SPI_HandleTypeDef Handle;
+ Handle.Instance = spi->dev;
+ // Init SPI hardware
+ HAL_SPI_DeInit(&Handle);
+
+ spiInit.Mode = SPI_MODE_MASTER;
+ spiInit.Direction = SPI_DIRECTION_2LINES;
+ spiInit.DataSize = SPI_DATASIZE_8BIT;
+ spiInit.NSS = SPI_NSS_SOFT;
+ spiInit.FirstBit = SPI_FIRSTBIT_MSB;
+ spiInit.CRCPolynomial = 7;
+ spiInit.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
+ spiInit.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+ spiInit.TIMode = SPI_TIMODE_DISABLED;
+
+ if (spi->leadingEdge) {
+ spiInit.CLKPolarity = SPI_POLARITY_LOW;
+ spiInit.CLKPhase = SPI_PHASE_1EDGE;
+ }
+ else {
+ spiInit.CLKPolarity = SPI_POLARITY_HIGH;
+ spiInit.CLKPhase = SPI_PHASE_2EDGE;
+ }
+
+ Handle.Init = spiInit;
+#ifdef STM32F303xC
+ // Configure for 8-bit reads.
+ SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF);
+#endif
+
+ if (HAL_SPI_Init(&Handle) == HAL_OK)
+ {
+ spiHandle[device].Handle = Handle;
+ if (spi->nss) {
+ IOHi(IOGetByTag(spi->nss));
+ }
+ }
+}
+
+bool spiInit(SPIDevice device)
+{
+ switch (device)
+ {
+ case SPIINVALID:
+ return false;
+ case SPIDEV_1:
+#if defined(USE_SPI_DEVICE_1)
+ spiInitDevice(device);
+ return true;
+#else
+ break;
+#endif
+ case SPIDEV_2:
+#if defined(USE_SPI_DEVICE_2)
+ spiInitDevice(device);
+ return true;
+#else
+ break;
+#endif
+ case SPIDEV_3:
+#if defined(USE_SPI_DEVICE_3)
+ spiInitDevice(device);
+ return true;
+#else
+ break;
+#endif
+ case SPIDEV_4:
+#if defined(USE_SPI_DEVICE_4)
+ spiInitDevice(device);
+ return true;
+#else
+ break;
+#endif
+ }
+ return false;
+}
+
+uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)
+{
+ SPIDevice device = spiDeviceByInstance(instance);
+ if (device == SPIINVALID)
+ return -1;
+ spiHardwareMap[device].errorCount++;
+ return spiHardwareMap[device].errorCount;
+}
+
+// return uint8_t value or -1 when failure
+uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in)
+{
+ spiTransfer(instance, &in, &in, 1);
+ return in;
+}
+
+/**
+ * Return true if the bus is currently in the middle of a transmission.
+ */
+bool spiIsBusBusy(SPI_TypeDef *instance)
+{
+ if(spiHandle[spiDeviceByInstance(instance)].Handle.State == HAL_SPI_STATE_BUSY)
+ return true;
+ else
+ return false;
+}
+
+bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len)
+{
+ HAL_StatusTypeDef status;
+
+#define SPI_DEFAULT_TIMEOUT 10
+
+ if(!out) // Tx only
+ {
+ status = HAL_SPI_Transmit(&spiHandle[spiDeviceByInstance(instance)].Handle, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
+ }
+ else if(!in) // Rx only
+ {
+ status = HAL_SPI_Receive(&spiHandle[spiDeviceByInstance(instance)].Handle, out, len, SPI_DEFAULT_TIMEOUT);
+ }
+ else // Tx and Rx
+ {
+ status = HAL_SPI_TransmitReceive(&spiHandle[spiDeviceByInstance(instance)].Handle, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT);
+ }
+
+ if( status != HAL_OK)
+ spiTimeoutUserCallback(instance);
+
+ return true;
+}
+
+
+void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
+{
+ SPI_HandleTypeDef *Handle = &spiHandle[spiDeviceByInstance(instance)].Handle;
+ if (HAL_SPI_DeInit(Handle) == HAL_OK)
+ {
+ }
+
+ switch (divisor) {
+ case 2:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
+ break;
+
+ case 4:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
+ break;
+
+ case 8:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
+ break;
+
+ case 16:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
+ break;
+
+ case 32:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
+ break;
+
+ case 64:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64;
+ break;
+
+ case 128:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128;
+ break;
+
+ case 256:
+ Handle->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
+ break;
+ }
+
+ if (HAL_SPI_Init(Handle) == HAL_OK)
+ {
+ }
+}
+
+uint16_t spiGetErrorCounter(SPI_TypeDef *instance)
+{
+ SPIDevice device = spiDeviceByInstance(instance);
+ if (device == SPIINVALID)
+ return 0;
+ return spiHardwareMap[device].errorCount;
+}
+
+void spiResetErrorCounter(SPI_TypeDef *instance)
+{
+ SPIDevice device = spiDeviceByInstance(instance);
+ if (device != SPIINVALID)
+ spiHardwareMap[device].errorCount = 0;
+}
+
+void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
+{
+ DMA_HandleTypeDef * hdma = &dmaHandle[(descriptor->userParam)].Handle;
+
+ HAL_DMA_IRQHandler(hdma);
+
+ //SCB_InvalidateDCache_by_Addr();
+
+ /*if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF);
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_FEIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_FEIF);
+ }
+ }
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF);
+ }
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_DMEIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_DMEIF);
+ }*/
+}
+
+
+DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size)
+{
+ SPI_HandleTypeDef* hspi = &spiHandle[spiDeviceByInstance(Instance)].Handle;
+ DMA_HandleTypeDef* hdma = &dmaHandle[spiDeviceByInstance(Instance)].Handle;
+
+ hdma->Instance = Stream;
+ hdma->Init.Channel = Channel;
+ hdma->Init.Direction = DMA_MEMORY_TO_PERIPH;
+ hdma->Init.PeriphInc = DMA_PINC_DISABLE;
+ hdma->Init.MemInc = DMA_MINC_ENABLE;
+ hdma->Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ hdma->Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ hdma->Init.Mode = DMA_NORMAL;
+ hdma->Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ hdma->Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
+ hdma->Init.PeriphBurst = DMA_PBURST_SINGLE;
+ hdma->Init.MemBurst = DMA_MBURST_SINGLE;
+ hdma->Init.Priority = DMA_PRIORITY_LOW;
+
+
+ HAL_DMA_DeInit(hdma);
+ HAL_DMA_Init(hdma);
+
+ __HAL_DMA_ENABLE(hdma);
+ __HAL_SPI_ENABLE(hspi);
+ /* Associate the initialized DMA handle to the spi handle */
+ __HAL_LINKDMA(hspi, hdmatx, (*hdma));
+
+ // DMA TX Interrupt
+ dmaSetHandler(DMA2_ST1_HANDLER, dmaSPIIRQHandler, NVIC_BUILD_PRIORITY(3, 0), (uint32_t)spiDeviceByInstance(Instance));
+
+ // SCB_CleanDCache_by_Addr((uint32_t) pData, Size);
+
+ HAL_SPI_Transmit_DMA(hspi, pData, Size);
+
+ //HAL_DMA_Start(&hdma, (uint32_t) pData, (uint32_t) &(Instance->DR), Size);
+
+ return hdma;
+}
diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h
index 726a5a1807..ea2c3484f5 100644
--- a/src/main/drivers/dma.h
+++ b/src/main/drivers/dma.h
@@ -19,7 +19,9 @@
struct dmaChannelDescriptor_s;
typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channelDescriptor);
-#ifdef STM32F4
+#if defined(STM32F4) || defined(STM32F7)
+
+uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream);
typedef enum {
DMA1_ST0_HANDLER = 0,
diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c
index 8372ad0bcb..fe3b7f4f66 100644
--- a/src/main/drivers/dma_stm32f4xx.c
+++ b/src/main/drivers/dma_stm32f4xx.c
@@ -87,3 +87,16 @@ void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr
NVIC_Init(&NVIC_InitStructure);
}
+#define RETURN_TCIF_FLAG(s, n) if (s == DMA1_Stream ## n || s == DMA2_Stream ## n) return DMA_IT_TCIF ## n
+
+uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream)
+{
+ RETURN_TCIF_FLAG(stream, 0);
+ RETURN_TCIF_FLAG(stream, 1);
+ RETURN_TCIF_FLAG(stream, 2);
+ RETURN_TCIF_FLAG(stream, 3);
+ RETURN_TCIF_FLAG(stream, 4);
+ RETURN_TCIF_FLAG(stream, 5);
+ RETURN_TCIF_FLAG(stream, 6);
+ RETURN_TCIF_FLAG(stream, 7);
+}
\ No newline at end of file
diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c
new file mode 100644
index 0000000000..f13c3a0619
--- /dev/null
+++ b/src/main/drivers/dma_stm32f7xx.c
@@ -0,0 +1,97 @@
+/*
+ * 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
+
+#include "drivers/nvic.h"
+#include "drivers/dma.h"
+
+/*
+ * DMA descriptors.
+ */
+static dmaChannelDescriptor_t dmaDescriptors[] = {
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 6, DMA1_Stream1_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 16, DMA1_Stream2_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream3, 22, DMA1_Stream3_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream4, 32, DMA1_Stream4_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream5, 38, DMA1_Stream5_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream6, 48, DMA1_Stream6_IRQn, RCC_AHB1ENR_DMA1EN),
+ DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream7, 54, DMA1_Stream7_IRQn, RCC_AHB1ENR_DMA1EN),
+
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream0, 0, DMA2_Stream0_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream1, 6, DMA2_Stream1_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream2, 16, DMA2_Stream2_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream3, 22, DMA2_Stream3_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream4, 32, DMA2_Stream4_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 38, DMA2_Stream5_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 48, DMA2_Stream6_IRQn, RCC_AHB1ENR_DMA2EN),
+ DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 54, DMA2_Stream7_IRQn, RCC_AHB1ENR_DMA2EN),
+
+};
+
+/*
+ * DMA IRQ Handlers
+ */
+DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER)
+
+
+void dmaInit(void)
+{
+ // TODO: Do we need this?
+}
+
+void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
+{
+ //clock
+ //RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE);
+
+ do {
+ __IO uint32_t tmpreg;
+ SET_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc);
+ /* Delay after an RCC peripheral clock enabling */
+ tmpreg = READ_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc);
+ UNUSED(tmpreg);
+ } while(0);
+
+ dmaDescriptors[identifier].irqHandlerCallback = callback;
+ dmaDescriptors[identifier].userParam = userParam;
+
+
+ HAL_NVIC_SetPriority(dmaDescriptors[identifier].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
+ HAL_NVIC_EnableIRQ(dmaDescriptors[identifier].irqN);
+}
+
diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c
index ce665bb077..b4d4cfaa49 100644
--- a/src/main/drivers/exti.c
+++ b/src/main/drivers/exti.c
@@ -22,7 +22,7 @@ extiChannelRec_t extiChannelRecs[16];
static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 };
static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS];
-#if defined(STM32F1) || defined(STM32F4)
+#if defined(STM32F1) || defined(STM32F4) || defined(STM32F7)
static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
EXTI0_IRQn,
EXTI1_IRQn,
@@ -66,6 +66,38 @@ void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn)
self->fn = fn;
}
+#if defined(STM32F7)
+void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config)
+{
+ (void)config;
+ int chIdx;
+ chIdx = IO_GPIOPinIdx(io);
+ if(chIdx < 0)
+ return;
+ extiChannelRec_t *rec = &extiChannelRecs[chIdx];
+ int group = extiGroups[chIdx];
+
+ GPIO_InitTypeDef init = {
+ .Pin = IO_Pin(io),
+ .Mode = GPIO_MODE_IT_RISING,
+ .Speed = GPIO_SPEED_FREQ_LOW,
+ .Pull = GPIO_NOPULL,
+ };
+ HAL_GPIO_Init(IO_GPIO(io), &init);
+
+ rec->handler = cb;
+ //uint32_t extiLine = IO_EXTI_Line(io);
+
+ //EXTI_ClearITPendingBit(extiLine);
+
+ if(extiGroupPriority[group] > irqPriority) {
+ extiGroupPriority[group] = irqPriority;
+ HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+ HAL_NVIC_EnableIRQ(extiGroupIRQn[group]);
+ }
+}
+#else
+
void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_TypeDef trigger)
{
int chIdx;
@@ -107,6 +139,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
NVIC_Init(&NVIC_InitStructure);
}
}
+#endif
void EXTIRelease(IO_t io)
{
@@ -123,7 +156,7 @@ void EXTIRelease(IO_t io)
void EXTIEnable(IO_t io, bool enable)
{
-#if defined(STM32F1) || defined(STM32F4)
+#if defined(STM32F1) || defined(STM32F4) || defined(STM32F7)
uint32_t extiLine = IO_EXTI_Line(io);
if(!extiLine)
return;
@@ -168,7 +201,7 @@ void EXTI_IRQHandler(void)
_EXTI_IRQ_HANDLER(EXTI0_IRQHandler);
_EXTI_IRQ_HANDLER(EXTI1_IRQHandler);
-#if defined(STM32F1)
+#if defined(STM32F1) || defined(STM32F7)
_EXTI_IRQ_HANDLER(EXTI2_IRQHandler);
#elif defined(STM32F3) || defined(STM32F4)
_EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler);
diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h
index adc95f13ab..205dc6d5f8 100644
--- a/src/main/drivers/exti.h
+++ b/src/main/drivers/exti.h
@@ -35,6 +35,10 @@ struct extiCallbackRec_s {
void EXTIInit(void);
void EXTIHandlerInit(extiCallbackRec_t *cb, extiHandlerCallback *fn);
+#if defined(STM32F7)
+void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config);
+#else
void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_TypeDef trigger);
+#endif
void EXTIRelease(IO_t io);
void EXTIEnable(IO_t io, bool enable);
diff --git a/src/main/drivers/gpio.h b/src/main/drivers/gpio.h
index dfa8d3761f..a3ad5fffce 100644
--- a/src/main/drivers/gpio.h
+++ b/src/main/drivers/gpio.h
@@ -65,6 +65,22 @@ typedef enum
} GPIO_Mode;
#endif
+#ifdef STM32F7
+typedef enum
+{
+ Mode_AIN = (GPIO_NOPULL << 5) | GPIO_MODE_ANALOG,
+ Mode_IN_FLOATING = (GPIO_NOPULL << 5) | GPIO_MODE_INPUT,
+ Mode_IPD = (GPIO_PULLDOWN << 5) | GPIO_MODE_INPUT,
+ Mode_IPU = (GPIO_PULLUP << 5) | GPIO_MODE_INPUT,
+ Mode_Out_OD = GPIO_MODE_OUTPUT_OD,
+ Mode_Out_PP = GPIO_MODE_OUTPUT_PP,
+ Mode_AF_OD = GPIO_MODE_AF_OD,
+ Mode_AF_PP = GPIO_MODE_AF_PP,
+ Mode_AF_PP_PD = (GPIO_PULLDOWN << 5) | GPIO_MODE_AF_PP,
+ Mode_AF_PP_PU = (GPIO_PULLUP << 5) | GPIO_MODE_AF_PP
+} GPIO_Mode;
+#endif
+
typedef enum
{
Speed_10MHz = 1,
@@ -101,7 +117,13 @@ typedef struct
} gpio_config_t;
#ifndef UNIT_TEST
-#ifdef STM32F4
+#if defined(USE_HAL_DRIVER)
+static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { HAL_GPIO_WritePin(p,i,GPIO_PIN_SET); }
+static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { HAL_GPIO_WritePin(p,i,GPIO_PIN_RESET); }
+static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { HAL_GPIO_TogglePin(p,i); }
+static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) { return HAL_GPIO_ReadPin(p,i); }
+#else
+#if defined(STM32F4)
static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { p->BSRRL = i; }
static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BSRRH = i; }
#else
@@ -111,6 +133,7 @@ static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BRR = i; }
static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; }
static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) { return p->IDR & i; }
#endif
+#endif
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config);
diff --git a/src/main/drivers/gpio_stm32f7xx.c b/src/main/drivers/gpio_stm32f7xx.c
new file mode 100644
index 0000000000..e2ad183671
--- /dev/null
+++ b/src/main/drivers/gpio_stm32f7xx.c
@@ -0,0 +1,69 @@
+/*
+ * 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 "platform.h"
+
+#include "gpio.h"
+
+#define MODE_OFFSET 0
+#define PUPD_OFFSET 5
+
+#define MODE_MASK ((1|2|16))
+#define PUPD_MASK ((1|2))
+
+void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ uint32_t pinIndex;
+ for (pinIndex = 0; pinIndex < 16; pinIndex++) {
+ // are we doing this pin?
+ uint32_t pinMask = (0x1 << pinIndex);
+ if (config->pin & pinMask) {
+
+ GPIO_InitStructure.Pin = pinMask;
+ GPIO_InitStructure.Mode = (config->mode >> MODE_OFFSET) & MODE_MASK;
+
+ uint32_t speed = GPIO_SPEED_FREQ_MEDIUM;
+ switch (config->speed) {
+ case Speed_10MHz:
+ speed = GPIO_SPEED_FREQ_MEDIUM;
+ break;
+ case Speed_2MHz:
+ speed = GPIO_SPEED_FREQ_LOW;
+ break;
+ case Speed_50MHz:
+ speed = GPIO_SPEED_FREQ_HIGH;
+ break;
+ }
+
+ GPIO_InitStructure.Speed = speed;
+ GPIO_InitStructure.Pull = (config->mode >> PUPD_OFFSET) & PUPD_MASK;
+ HAL_GPIO_Init(gpio, &GPIO_InitStructure);
+ }
+ }
+}
+
+void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc)
+{
+ (void)portsrc;
+ (void)pinsrc;
+ //SYSCFG_EXTILineConfig(portsrc, pinsrc);
+}
diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c
index 25ceb82ed6..5104e5cf72 100644
--- a/src/main/drivers/io.c
+++ b/src/main/drivers/io.c
@@ -51,6 +51,15 @@ const struct ioPortDef_s ioPortDefs[] = {
{ RCC_AHB1(GPIOE) },
{ RCC_AHB1(GPIOF) },
};
+#elif defined(STM32F7)
+const struct ioPortDef_s ioPortDefs[] = {
+ { RCC_AHB1(GPIOA) },
+ { RCC_AHB1(GPIOB) },
+ { RCC_AHB1(GPIOC) },
+ { RCC_AHB1(GPIOD) },
+ { RCC_AHB1(GPIOE) },
+ { RCC_AHB1(GPIOF) },
+};
# endif
const char * const ownerNames[OWNER_TOTAL_COUNT] = {
@@ -129,6 +138,8 @@ uint32_t IO_EXTI_Line(IO_t io)
return IO_GPIOPinIdx(io);
#elif defined(STM32F4)
return 1 << IO_GPIOPinIdx(io);
+#elif defined(STM32F7)
+ return 1 << IO_GPIOPinIdx(io);
#else
# error "Unknown target type"
#endif
@@ -138,14 +149,25 @@ bool IORead(IO_t io)
{
if (!io)
return false;
+#if defined(USE_HAL_DRIVER)
+ return !! HAL_GPIO_ReadPin(IO_GPIO(io),IO_Pin(io));
+#else
return !! (IO_GPIO(io)->IDR & IO_Pin(io));
+#endif
}
void IOWrite(IO_t io, bool hi)
{
if (!io)
return;
-#ifdef STM32F4
+#if defined(USE_HAL_DRIVER)
+ if (hi) {
+ HAL_GPIO_WritePin(IO_GPIO(io),IO_Pin(io),GPIO_PIN_SET);
+ }
+ else {
+ HAL_GPIO_WritePin(IO_GPIO(io),IO_Pin(io),GPIO_PIN_RESET);
+ }
+#elif defined(STM32F4)
if (hi) {
IO_GPIO(io)->BSRRL = IO_Pin(io);
}
@@ -161,7 +183,9 @@ void IOHi(IO_t io)
{
if (!io)
return;
-#ifdef STM32F4
+#if defined(USE_HAL_DRIVER)
+ HAL_GPIO_WritePin(IO_GPIO(io),IO_Pin(io),GPIO_PIN_SET);
+#elif defined(STM32F4)
IO_GPIO(io)->BSRRL = IO_Pin(io);
#else
IO_GPIO(io)->BSRR = IO_Pin(io);
@@ -172,7 +196,9 @@ void IOLo(IO_t io)
{
if (!io)
return;
-#ifdef STM32F4
+#if defined(USE_HAL_DRIVER)
+ HAL_GPIO_WritePin(IO_GPIO(io),IO_Pin(io),GPIO_PIN_RESET);
+#elif defined(STM32F4)
IO_GPIO(io)->BSRRH = IO_Pin(io);
#else
IO_GPIO(io)->BRR = IO_Pin(io);
@@ -187,7 +213,10 @@ void IOToggle(IO_t io)
// Read pin state from ODR but write to BSRR because it only changes the pins
// high in the mask value rather than all pins. XORing ODR directly risks
// setting other pins incorrectly because it change all pins' state.
-#ifdef STM32F4
+#if defined(USE_HAL_DRIVER)
+ (void)mask;
+ HAL_GPIO_TogglePin(IO_GPIO(io),IO_Pin(io));
+#elif defined(STM32F4)
if (IO_GPIO(io)->ODR & mask) {
IO_GPIO(io)->BSRRH = mask;
} else {
@@ -245,6 +274,40 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg)
GPIO_Init(IO_GPIO(io), &init);
}
+#elif defined(STM32F7)
+
+void IOConfigGPIO(IO_t io, ioConfig_t cfg)
+{
+ if (!io)
+ return;
+ rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+ RCC_ClockCmd(rcc, ENABLE);
+
+ GPIO_InitTypeDef init = {
+ .Pin = IO_Pin(io),
+ .Mode = (cfg >> 0) & 0x13,
+ .Speed = (cfg >> 2) & 0x03,
+ .Pull = (cfg >> 5) & 0x03,
+ };
+ HAL_GPIO_Init(IO_GPIO(io), &init);
+}
+
+void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
+{
+ if (!io)
+ return;
+ rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+ RCC_ClockCmd(rcc, ENABLE);
+
+ GPIO_InitTypeDef init = {
+ .Pin = IO_Pin(io),
+ .Mode = (cfg >> 0) & 0x13,
+ .Speed = (cfg >> 2) & 0x03,
+ .Pull = (cfg >> 5) & 0x03,
+ .Alternate = af
+ };
+ HAL_GPIO_Init(IO_GPIO(io), &init);
+}
#elif defined(STM32F3) || defined(STM32F4)
void IOConfigGPIO(IO_t io, ioConfig_t cfg)
diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h
index 5afa4abb76..d3f85eaf77 100644
--- a/src/main/drivers/io.h
+++ b/src/main/drivers/io.h
@@ -28,6 +28,23 @@
#define IOCFG_IPU IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_2MHz)
#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_2MHz)
+#elif defined(STM32F7)
+
+//speed is packed inside modebits 5 and 2,
+#define IO_CONFIG(mode, speed, pupd) ((mode) | ((speed) << 2) | ((pupd) << 5))
+
+#define IOCFG_OUT_PP IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_NOPULL)
+#define IOCFG_OUT_OD IO_CONFIG(GPIO_MODE_OUTPUT_OD, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_AF_PP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN)
+#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
+#define IOCFG_AF_OD IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_IPD IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN)
+#define IOCFG_IPU IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
+#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_IPU_25 IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_HIGH, GPIO_PULLUP)
+
#elif defined(STM32F3) || defined(STM32F4)
#define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5))
@@ -74,7 +91,7 @@ resourceType_t IOGetResources(IO_t io);
IO_t IOGetByTag(ioTag_t tag);
void IOConfigGPIO(IO_t io, ioConfig_t cfg);
-#if defined(STM32F3) || defined(STM32F4)
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af);
#endif
diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c
index bd935432dc..c7110799c9 100644
--- a/src/main/drivers/light_ws2811strip.c
+++ b/src/main/drivers/light_ws2811strip.c
@@ -39,7 +39,7 @@
#include "dma.h"
#include "light_ws2811strip.h"
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(STM32F7)
uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
#else
uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h
index c24712502c..c980353abb 100644
--- a/src/main/drivers/light_ws2811strip.h
+++ b/src/main/drivers/light_ws2811strip.h
@@ -28,6 +28,9 @@
#if defined(STM32F40_41xxx)
#define BIT_COMPARE_1 67 // timer compare value for logical 1
#define BIT_COMPARE_0 33 // timer compare value for logical 0
+#elif defined(STM32F7)
+#define BIT_COMPARE_1 76 // timer compare value for logical 1
+#define BIT_COMPARE_0 38 // timer compare value for logical 0
#else
#define BIT_COMPARE_1 17 // timer compare value for logical 1
#define BIT_COMPARE_0 9 // timer compare value for logical 0
@@ -51,7 +54,7 @@ void setStripColors(const hsvColor_t *colors);
bool isWS2811LedStripReady(void);
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(STM32F7)
extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
#else
extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c
new file mode 100644
index 0000000000..a9e6061bba
--- /dev/null
+++ b/src/main/drivers/light_ws2811strip_hal.c
@@ -0,0 +1,176 @@
+/*
+ * 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 "platform.h"
+
+#ifdef LED_STRIP
+
+#include "common/color.h"
+#include "light_ws2811strip.h"
+#include "nvic.h"
+#include "dma.h"
+#include "io.h"
+#include "system.h"
+#include "rcc.h"
+#include "timer.h"
+
+#if !defined(WS2811_PIN)
+#define WS2811_PIN PA0
+#define WS2811_TIMER TIM5
+#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
+#define WS2811_DMA_STREAM DMA1_Stream2
+#define WS2811_DMA_IT DMA_IT_TCIF2
+#define WS2811_DMA_CHANNEL DMA_Channel_6
+#define WS2811_TIMER_CHANNEL TIM_Channel_1
+#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM5
+#endif
+
+static IO_t ws2811IO = IO_NONE;
+static uint16_t timDMASource = 0;
+bool ws2811Initialised = false;
+
+static TIM_HandleTypeDef TimHandle;
+
+void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
+{
+ if(htim->Instance==WS2811_TIMER)
+ {
+ //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL);
+ ws2811LedDataTransferInProgress = 0;
+ }
+}
+
+void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
+{
+ HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]);
+}
+
+void ws2811LedStripHardwareInit(void)
+{
+ TimHandle.Instance = WS2811_TIMER;
+
+ TimHandle.Init.Prescaler = 1;
+ TimHandle.Init.Period = 135; // 800kHz
+ TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ if(HAL_TIM_PWM_Init(&TimHandle) != HAL_OK)
+ {
+ /* Initialization Error */
+ return;
+ }
+
+ static DMA_HandleTypeDef hdma_tim;
+
+ ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
+ /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */
+ IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0);
+ IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), WS2811_TIMER_GPIO_AF);
+
+ __DMA1_CLK_ENABLE();
+
+
+ /* Set the parameters to be configured */
+ hdma_tim.Init.Channel = WS2811_DMA_CHANNEL;
+ hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE;
+ hdma_tim.Init.MemInc = DMA_MINC_ENABLE;
+ hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ;
+ hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ;
+ hdma_tim.Init.Mode = DMA_NORMAL;
+ hdma_tim.Init.Priority = DMA_PRIORITY_HIGH;
+ hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+ hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE;
+ hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE;
+
+ /* Set hdma_tim instance */
+ hdma_tim.Instance = WS2811_DMA_STREAM;
+
+ uint32_t channelAddress = 0;
+ switch (WS2811_TIMER_CHANNEL) {
+ case TIM_CHANNEL_1:
+ timDMASource = TIM_DMA_ID_CC1;
+ channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
+ break;
+ case TIM_CHANNEL_2:
+ timDMASource = TIM_DMA_ID_CC2;
+ channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
+ break;
+ case TIM_CHANNEL_3:
+ timDMASource = TIM_DMA_ID_CC3;
+ channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
+ break;
+ case TIM_CHANNEL_4:
+ timDMASource = TIM_DMA_ID_CC4;
+ channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
+ break;
+ }
+
+ /* Link hdma_tim to hdma[x] (channelx) */
+ __HAL_LINKDMA(&TimHandle, hdma[timDMASource], hdma_tim);
+
+ dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, timDMASource);
+
+ /* Initialize TIMx DMA handle */
+ if(HAL_DMA_Init(TimHandle.hdma[timDMASource]) != HAL_OK)
+ {
+ /* Initialization Error */
+ return;
+ }
+
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
+
+ /* PWM1 Mode configuration: Channel1 */
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
+ TIM_OCInitStructure.Pulse = 0;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
+ TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+
+ if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, WS2811_TIMER_CHANNEL) != HAL_OK)
+ {
+ /* Configuration Error */
+ return;
+ }
+
+ const hsvColor_t hsv_white = { 0, 255, 255};
+ ws2811Initialised = true;
+ setStripColor(&hsv_white);
+}
+
+
+void ws2811LedStripDMAEnable(void)
+{
+ if (!ws2811Initialised)
+ {
+ ws2811LedDataTransferInProgress = 0;
+ return;
+ }
+
+ if( HAL_TIM_PWM_Start_DMA(&TimHandle, WS2811_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK)
+ {
+ /* Starting PWM generation Error */
+ ws2811LedDataTransferInProgress = 0;
+ return;
+ }
+
+}
+#endif
diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c
index 41d978f2b9..46bcdd0e46 100644
--- a/src/main/drivers/light_ws2811strip_stm32f4xx.c
+++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c
@@ -30,13 +30,13 @@
#include "system.h"
#include "rcc.h"
#include "timer.h"
+#include "timer_stm32f4xx.h"
#if !defined(WS2811_PIN)
#define WS2811_PIN PA0
#define WS2811_TIMER TIM5
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream2
-#define WS2811_DMA_IT DMA_IT_TCIF2
#define WS2811_DMA_CHANNEL DMA_Channel_6
#define WS2811_TIMER_CHANNEL TIM_Channel_1
#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5
@@ -94,33 +94,9 @@ void ws2811LedStripHardwareInit(void)
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = 0;
- uint32_t channelAddress = 0;
- switch (WS2811_TIMER_CHANNEL) {
- case TIM_Channel_1:
- TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure);
- timDMASource = TIM_DMA_CC1;
- channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
- TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_2:
- TIM_OC2Init(WS2811_TIMER, &TIM_OCInitStructure);
- timDMASource = TIM_DMA_CC2;
- channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
- TIM_OC2PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_3:
- TIM_OC3Init(WS2811_TIMER, &TIM_OCInitStructure);
- timDMASource = TIM_DMA_CC3;
- channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
- TIM_OC3PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_4:
- TIM_OC4Init(WS2811_TIMER, &TIM_OCInitStructure);
- timDMASource = TIM_DMA_CC4;
- channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
- TIM_OC4PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
- break;
- }
+ timerOCInit(WS2811_TIMER, WS2811_TIMER_CHANNEL, &TIM_OCInitStructure);
+ timerOCPreloadConfig(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_OCPreload_Enable);
+ timDMASource = timerDmaSource(WS2811_TIMER_CHANNEL);
TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE);
TIM_ARRPreloadConfig(WS2811_TIMER, ENABLE);
@@ -133,7 +109,7 @@ void ws2811LedStripHardwareInit(void)
DMA_DeInit(WS2811_DMA_STREAM);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL;
- DMA_InitStructure.DMA_PeripheralBaseAddr = channelAddress;
+ DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(WS2811_TIMER, WS2811_TIMER_CHANNEL);
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE;
@@ -151,7 +127,7 @@ void ws2811LedStripHardwareInit(void)
DMA_Init(WS2811_DMA_STREAM, &DMA_InitStructure);
DMA_ITConfig(WS2811_DMA_STREAM, DMA_IT_TC, ENABLE);
- DMA_ClearITPendingBit(WS2811_DMA_STREAM, WS2811_DMA_IT);
+ DMA_ClearITPendingBit(WS2811_DMA_STREAM, dmaFlag_IT_TCIF(WS2811_DMA_STREAM));
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0);
diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index 9712f89153..ff053dbc0b 100644
--- a/src/main/drivers/nvic.h
+++ b/src/main/drivers/nvic.h
@@ -1,7 +1,6 @@
#pragma once
-#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
// can't use 0
#define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1)
@@ -30,6 +29,12 @@
#define NVIC_PRIO_SERIALUART6_TXDMA NVIC_BUILD_PRIORITY(1, 0)
#define NVIC_PRIO_SERIALUART6_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART6 NVIC_BUILD_PRIORITY(1, 2)
+#define NVIC_PRIO_SERIALUART7_TXDMA NVIC_BUILD_PRIORITY(1, 0)
+#define NVIC_PRIO_SERIALUART7_RXDMA NVIC_BUILD_PRIORITY(1, 1)
+#define NVIC_PRIO_SERIALUART7 NVIC_BUILD_PRIORITY(1, 2)
+#define NVIC_PRIO_SERIALUART8_TXDMA NVIC_BUILD_PRIORITY(1, 0)
+#define NVIC_PRIO_SERIALUART8_RXDMA NVIC_BUILD_PRIORITY(1, 1)
+#define NVIC_PRIO_SERIALUART8 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_I2C_ER NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_I2C_EV NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0)
@@ -40,7 +45,16 @@
#define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0)
+#ifdef USE_HAL_DRIVER
// utility macros to join/split priority
+#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_2
+#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING)))))<<4)&0xf0)
+#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING))))>>4)
+#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING))))>>4)
+#else
+// utility macros to join/split priority
+#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0)
#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
+#endif
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 82a286417d..59ddad65e9 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -54,24 +54,8 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
- switch (channel) {
- case TIM_Channel_1:
- TIM_OC1Init(tim, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_2:
- TIM_OC2Init(tim, &TIM_OCInitStructure);
- TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_3:
- TIM_OC3Init(tim, &TIM_OCInitStructure);
- TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_4:
- TIM_OC4Init(tim, &TIM_OCInitStructure);
- TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
- break;
- }
+ timerOCInit(tim, channel, &TIM_OCInitStructure);
+ timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable);
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
@@ -84,20 +68,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
}
TIM_Cmd(timerHardware->tim, ENABLE);
- switch (timerHardware->channel) {
- case TIM_Channel_1:
- port->ccr = &timerHardware->tim->CCR1;
- break;
- case TIM_Channel_2:
- port->ccr = &timerHardware->tim->CCR2;
- break;
- case TIM_Channel_3:
- port->ccr = &timerHardware->tim->CCR3;
- break;
- case TIM_Channel_4:
- port->ccr = &timerHardware->tim->CCR4;
- break;
- }
+ port->ccr = timerChCCR(timerHardware);
port->period = period;
port->tim = timerHardware->tim;
@@ -140,7 +111,9 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
- *motors[index].ccr = 0;
+ if (motors[index].ccr) {
+ *motors[index].ccr = 0;
+ }
}
}
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 2c437ea704..0c1a61832a 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -39,6 +39,11 @@ typedef enum {
#define ONESHOT42_TIMER_MHZ 21
#define MULTISHOT_TIMER_MHZ 84
#define PWM_BRUSHED_TIMER_MHZ 21
+#elif defined(STM32F7) // must be multiples of timer clock
+#define ONESHOT125_TIMER_MHZ 9
+#define ONESHOT42_TIMER_MHZ 27
+#define MULTISHOT_TIMER_MHZ 54
+#define PWM_BRUSHED_TIMER_MHZ 27
#else
#define ONESHOT125_TIMER_MHZ 8
#define ONESHOT42_TIMER_MHZ 24
@@ -58,11 +63,15 @@ typedef struct {
const timerHardware_t *timerHardware;
uint16_t value;
uint16_t timerDmaSource;
-#if defined(STM32F3) || defined(STM32F4)
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
#else
uint8_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
#endif
+#if defined(STM32F7)
+ TIM_HandleTypeDef TimHandle;
+ uint32_t Channel;
+#endif
} motorDmaOutput_t;
struct timerHardware_s;
diff --git a/src/main/drivers/pwm_output_hal.c b/src/main/drivers/pwm_output_hal.c
new file mode 100644
index 0000000000..290b386007
--- /dev/null
+++ b/src/main/drivers/pwm_output_hal.c
@@ -0,0 +1,300 @@
+/*
+ * 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 "platform.h"
+
+#include "io.h"
+#include "timer.h"
+#include "pwm_output.h"
+
+#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
+#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
+
+static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
+static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
+
+#ifdef USE_SERVOS
+static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
+#endif
+
+static bool pwmMotorsEnabled = true;
+
+static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
+{
+ TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
+ if(Handle == NULL) return;
+
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM2;
+ TIM_OCInitStructure.Pulse = value;
+ TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW;
+ TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
+ TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
+ TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+
+ HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
+ //HAL_TIM_PWM_Start(Handle, channel);
+}
+
+static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
+{
+ TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
+ if(Handle == NULL) return;
+
+ configTimeBase(timerHardware->tim, period, mhz);
+ pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
+
+ if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
+ HAL_TIM_PWM_Start(Handle, timerHardware->channel);
+ } else {
+ HAL_TIM_PWM_Stop(Handle, timerHardware->channel);
+ }
+ HAL_TIM_Base_Start(Handle);
+
+ switch (timerHardware->channel) {
+ case TIM_CHANNEL_1:
+ port->ccr = &timerHardware->tim->CCR1;
+ break;
+ case TIM_CHANNEL_2:
+ port->ccr = &timerHardware->tim->CCR2;
+ break;
+ case TIM_CHANNEL_3:
+ port->ccr = &timerHardware->tim->CCR3;
+ break;
+ case TIM_CHANNEL_4:
+ port->ccr = &timerHardware->tim->CCR4;
+ break;
+ }
+ port->period = period;
+ port->tim = timerHardware->tim;
+
+ *port->ccr = 0;
+}
+
+static void pwmWriteBrushed(uint8_t index, uint16_t value)
+{
+ *motors[index].ccr = (value - 1000) * motors[index].period / 1000;
+}
+
+static void pwmWriteStandard(uint8_t index, uint16_t value)
+{
+ *motors[index].ccr = value;
+}
+
+static void pwmWriteOneShot125(uint8_t index, uint16_t value)
+{
+ *motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
+}
+
+static void pwmWriteOneShot42(uint8_t index, uint16_t value)
+{
+ *motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
+}
+
+static void pwmWriteMultiShot(uint8_t index, uint16_t value)
+{
+ *motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
+}
+
+void pwmWriteMotor(uint8_t index, uint16_t value)
+{
+ if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) {
+ motors[index].pwmWritePtr(index, value);
+ }
+}
+
+void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
+{
+ for (int index = 0; index < motorCount; index++) {
+ // Set the compare register to 0, which stops the output pulsing if the timer overflows
+ *motors[index].ccr = 0;
+ }
+}
+
+void pwmDisableMotors(void)
+{
+ pwmMotorsEnabled = false;
+}
+
+void pwmEnableMotors(void)
+{
+ pwmMotorsEnabled = true;
+}
+
+static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
+{
+ for (int index = 0; index < motorCount; index++) {
+ bool overflowed = false;
+ // If we have not already overflowed this timer
+ for (int j = 0; j < index; j++) {
+ if (motors[j].tim == motors[index].tim) {
+ overflowed = true;
+ break;
+ }
+ }
+ if (!overflowed) {
+ timerForceOverflow(motors[index].tim);
+ }
+ // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
+ // This compare register will be set to the output value on the next main loop.
+ *motors[index].ccr = 0;
+ }
+}
+
+void pwmCompleteMotorUpdate(uint8_t motorCount)
+{
+ if (pwmCompleteWritePtr) {
+ pwmCompleteWritePtr(motorCount);
+ }
+}
+
+void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
+{
+ uint32_t timerMhzCounter;
+ pwmWriteFuncPtr pwmWritePtr;
+ bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
+ bool isDigital = false;
+
+ switch (motorConfig->motorPwmProtocol) {
+ default:
+ case PWM_TYPE_ONESHOT125:
+ timerMhzCounter = ONESHOT125_TIMER_MHZ;
+ pwmWritePtr = pwmWriteOneShot125;
+ break;
+ case PWM_TYPE_ONESHOT42:
+ timerMhzCounter = ONESHOT42_TIMER_MHZ;
+ pwmWritePtr = pwmWriteOneShot42;
+ break;
+ case PWM_TYPE_MULTISHOT:
+ timerMhzCounter = MULTISHOT_TIMER_MHZ;
+ pwmWritePtr = pwmWriteMultiShot;
+ break;
+ case PWM_TYPE_BRUSHED:
+ timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
+ pwmWritePtr = pwmWriteBrushed;
+ useUnsyncedPwm = true;
+ idlePulse = 0;
+ break;
+ case PWM_TYPE_STANDARD:
+ timerMhzCounter = PWM_TIMER_MHZ;
+ pwmWritePtr = pwmWriteStandard;
+ useUnsyncedPwm = true;
+ idlePulse = 0;
+ break;
+#ifdef USE_DSHOT
+ case PWM_TYPE_DSHOT600:
+ case PWM_TYPE_DSHOT150:
+ pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
+ isDigital = true;
+ break;
+#endif
+ }
+
+ if (!useUnsyncedPwm && !isDigital) {
+ pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
+ }
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+ const ioTag_t tag = motorConfig->ioTags[motorIndex];
+
+ if (!tag) {
+ break;
+ }
+
+ const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
+
+ if (timerHardware == NULL) {
+ /* flag failure and disable ability to arm */
+ break;
+ }
+
+#ifdef USE_DSHOT
+ if (isDigital) {
+ pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
+ motors[motorIndex].pwmWritePtr = pwmWriteDigital;
+ motors[motorIndex].enabled = true;
+ continue;
+ }
+#endif
+ motors[motorIndex].io = IOGetByTag(tag);
+
+ IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_OUTPUT, RESOURCE_INDEX(motorIndex));
+ //IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
+ IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
+
+ motors[motorIndex].pwmWritePtr = pwmWritePtr;
+ if (useUnsyncedPwm) {
+ const uint32_t hz = timerMhzCounter * 1000000;
+ pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmProtocol, idlePulse);
+ } else {
+ pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
+ }
+ motors[motorIndex].enabled = true;
+ }
+}
+
+bool pwmIsSynced(void)
+{
+ return pwmCompleteWritePtr != NULL;
+}
+
+pwmOutputPort_t *pwmGetMotors(void)
+{
+ return motors;
+}
+
+#ifdef USE_SERVOS
+void pwmWriteServo(uint8_t index, uint16_t value)
+{
+ if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
+ *servos[index].ccr = value;
+ }
+}
+
+void servoInit(const servoConfig_t *servoConfig)
+{
+ for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
+ const ioTag_t tag = servoConfig->ioTags[servoIndex];
+
+ if (!tag) {
+ break;
+ }
+
+ servos[servoIndex].io = IOGetByTag(tag);
+
+ IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_OUTPUT, RESOURCE_INDEX(servoIndex));
+ //IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
+
+ const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
+ IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
+
+ if (timer == NULL) {
+ /* flag failure and disable ability to arm */
+ break;
+ }
+
+ pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
+ servos[servoIndex].enabled = true;
+ }
+}
+
+#endif
diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c
index 8579088d8e..f45b0483e7 100644
--- a/src/main/drivers/pwm_output_stm32f3xx.c
+++ b/src/main/drivers/pwm_output_stm32f3xx.c
@@ -58,26 +58,22 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
{
motorDmaOutput_t * const motor = &dmaMotors[index];
- motor->value = value;
-
- motor->dmaBuffer[0] = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[1] = (value & 0x200) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[2] = (value & 0x100) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[3] = (value & 0x80) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[4] = (value & 0x40) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[5] = (value & 0x20) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[6] = (value & 0x10) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[7] = (value & 0x8) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[8] = (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[9] = (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[10] = (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[11] = MOTOR_BIT_0; /* telemetry is always false for the moment */
-
- /* check sum */
- motor->dmaBuffer[12] = (value & 0x400) ^ (value & 0x40) ^ (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[13] = (value & 0x200) ^ (value & 0x20) ^ (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[14] = (value & 0x100) ^ (value & 0x10) ^ (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[15] = (value & 0x80) ^ (value & 0x8) ^ (0x0) ? MOTOR_BIT_1 : MOTOR_BIT_0;
+ uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now)
+ // compute checksum
+ int csum = 0;
+ int csum_data = packet;
+ for (int i = 0; i < 3; i++) {
+ csum ^= csum_data; // xor data by nibbles
+ csum_data >>= 4;
+ }
+ csum &= 0xf;
+ // append checksum
+ packet = (packet << 4) | csum;
+ // generate pulses for whole packet
+ for (int i = 0; i < 16; i++) {
+ motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
+ packet <<= 1;
+ }
DMA_SetCurrDataCounter(motor->timerHardware->dmaChannel, MOTOR_DMA_BUFFER_SIZE);
DMA_Cmd(motor->timerHardware->dmaChannel, ENABLE);
@@ -153,36 +149,12 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
}
TIM_OCInitStructure.TIM_Pulse = 0;
- uint32_t timerChannelAddress = 0;
- switch (timerHardware->channel) {
- case TIM_Channel_1:
- TIM_OC1Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC1;
- timerChannelAddress = (uint32_t)(&timer->CCR1);
- TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_2:
- TIM_OC2Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC2;
- timerChannelAddress = (uint32_t)(&timer->CCR2);
- TIM_OC2PreloadConfig(timer, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_3:
- TIM_OC3Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC3;
- timerChannelAddress = (uint32_t)(&timer->CCR3);
- TIM_OC3PreloadConfig(timer, TIM_OCPreload_Enable);
- break;
- case TIM_Channel_4:
- TIM_OC4Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC4;
- timerChannelAddress = (uint32_t)(&timer->CCR4);
- TIM_OC4PreloadConfig(timer, TIM_OCPreload_Enable);
- break;
- }
+ timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure);
+ timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
+ motor->timerDmaSource = timerDmaSource(timerHardware->channel);
dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
- TIM_CCxCmd(timer, motor->timerHardware->channel, TIM_CCx_Enable);
+ TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
if (configureTimer) {
TIM_CtrlPWMOutputs(timer, ENABLE);
@@ -197,7 +169,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
DMA_Cmd(channel, DISABLE);
DMA_DeInit(channel);
DMA_StructInit(&DMA_InitStructure);
- DMA_InitStructure.DMA_PeripheralBaseAddr = timerChannelAddress;
+ DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE;
diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c
index f2a3744825..f5a92fb371 100644
--- a/src/main/drivers/pwm_output_stm32f4xx.c
+++ b/src/main/drivers/pwm_output_stm32f4xx.c
@@ -22,6 +22,7 @@
#include "io.h"
#include "timer.h"
+#include "timer_stm32f4xx.h"
#include "pwm_output.h"
#include "nvic.h"
#include "dma.h"
@@ -58,26 +59,22 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
{
motorDmaOutput_t * const motor = &dmaMotors[index];
- motor->value = value;
-
- motor->dmaBuffer[0] = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[1] = (value & 0x200) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[2] = (value & 0x100) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[3] = (value & 0x80) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[4] = (value & 0x40) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[5] = (value & 0x20) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[6] = (value & 0x10) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[7] = (value & 0x8) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[8] = (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[9] = (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[10] = (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[11] = MOTOR_BIT_0; /* telemetry is always false for the moment */
-
- /* check sum */
- motor->dmaBuffer[12] = (value & 0x400) ^ (value & 0x40) ^ (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[13] = (value & 0x200) ^ (value & 0x20) ^ (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[14] = (value & 0x100) ^ (value & 0x10) ^ (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0;
- motor->dmaBuffer[15] = (value & 0x80) ^ (value & 0x8) ^ (0x0) ? MOTOR_BIT_1 : MOTOR_BIT_0;
+ uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now)
+ // compute checksum
+ int csum = 0;
+ int csum_data = packet;
+ for (int i = 0; i < 3; i++) {
+ csum ^= csum_data; // xor data by nibbles
+ csum_data >>= 4;
+ }
+ csum &= 0xf;
+ // append checksum
+ packet = (packet << 4) | csum;
+ // generate pulses for whole packet
+ for (int i = 0; i < 16; i++) {
+ motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
+ packet <<= 1;
+ }
DMA_SetCurrDataCounter(motor->timerHardware->dmaStream, MOTOR_DMA_BUFFER_SIZE);
DMA_Cmd(motor->timerHardware->dmaStream, ENABLE);
@@ -143,38 +140,9 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = 0;
- uint32_t timerChannelAddress = 0;
- uint32_t dmaItFlag = 0;
- switch (timerHardware->channel) {
- case TIM_Channel_1:
- TIM_OC1Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC1;
- timerChannelAddress = (uint32_t)(&timer->CCR1);
- TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable);
- dmaItFlag = DMA_IT_TCIF1;
- break;
- case TIM_Channel_2:
- TIM_OC2Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC2;
- timerChannelAddress = (uint32_t)(&timer->CCR2);
- TIM_OC2PreloadConfig(timer, TIM_OCPreload_Enable);
- dmaItFlag = DMA_IT_TCIF2;
- break;
- case TIM_Channel_3:
- TIM_OC3Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC3;
- timerChannelAddress = (uint32_t)(&timer->CCR3);
- TIM_OC3PreloadConfig(timer, TIM_OCPreload_Enable);
- dmaItFlag = DMA_IT_TCIF3;
- break;
- case TIM_Channel_4:
- TIM_OC4Init(timer, &TIM_OCInitStructure);
- motor->timerDmaSource = TIM_DMA_CC4;
- timerChannelAddress = (uint32_t)(&timer->CCR4);
- TIM_OC4PreloadConfig(timer, TIM_OCPreload_Enable);
- dmaItFlag = DMA_IT_TCIF4;
- break;
- }
+ timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure);
+ timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
+ motor->timerDmaSource = timerDmaSource(timerHardware->channel);
dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
TIM_CCxCmd(timer, motor->timerHardware->channel, TIM_CCx_Enable);
@@ -191,7 +159,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
DMA_DeInit(stream);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel;
- DMA_InitStructure.DMA_PeripheralBaseAddr = timerChannelAddress;
+ DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)motor->dmaBuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE;
@@ -209,7 +177,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
DMA_Init(stream, &DMA_InitStructure);
DMA_ITConfig(stream, DMA_IT_TC, ENABLE);
- DMA_ClearITPendingBit(stream, dmaItFlag);
+ DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(timerHardware->dmaStream));
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
}
diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c
new file mode 100644
index 0000000000..4d47bfc990
--- /dev/null
+++ b/src/main/drivers/pwm_output_stm32f7xx.c
@@ -0,0 +1,220 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight 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.
+ *
+ * Betaflight 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 Betaflight. If not, see .
+ */
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "io.h"
+#include "timer.h"
+#include "pwm_output.h"
+#include "nvic.h"
+#include "dma.h"
+#include "system.h"
+#include "rcc.h"
+
+#ifdef USE_DSHOT
+
+#define MAX_DMA_TIMERS 8
+
+#define MOTOR_DSHOT600_MHZ 12
+#define MOTOR_DSHOT150_MHZ 3
+
+#define MOTOR_BIT_0 7
+#define MOTOR_BIT_1 14
+#define MOTOR_BITLENGTH 19
+
+static uint8_t dmaMotorTimerCount = 0;
+static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
+static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
+
+uint8_t getTimerIndex(TIM_TypeDef *timer)
+{
+ for (int i = 0; i < dmaMotorTimerCount; i++) {
+ if (dmaMotorTimers[i].timer == timer) {
+ return i;
+ }
+ }
+ dmaMotorTimers[dmaMotorTimerCount++].timer = timer;
+ return dmaMotorTimerCount-1;
+}
+
+void pwmWriteDigital(uint8_t index, uint16_t value)
+{
+ motorDmaOutput_t * const motor = &dmaMotors[index];
+
+ uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now)
+ // compute checksum
+ int csum = 0;
+ int csum_data = packet;
+ for (int i = 0; i < 3; i++) {
+ csum ^= csum_data; // xor data by nibbles
+ csum_data >>= 4;
+ }
+ csum &= 0xf;
+ // append checksum
+ packet = (packet << 4) | csum;
+ // generate pulses for whole packet
+ for (int i = 0; i < 16; i++) {
+ motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
+ packet <<= 1;
+ }
+
+ if( HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK)
+ {
+ /* Starting PWM generation Error */
+ return;
+ }
+}
+
+void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
+{
+ UNUSED(motorCount);
+
+ for (uint8_t i = 0; i < dmaMotorTimerCount; i++) {
+ //TIM_SetCounter(dmaMotorTimers[i].timer, 0);
+ //TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
+ }
+}
+
+
+static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
+{
+ motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
+ HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
+}
+
+/*static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
+{
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
+ motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
+ DMA_Cmd(descriptor->stream, DISABLE);
+ TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+ }
+}*/
+
+void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
+{
+ motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
+ motor->timerHardware = timerHardware;
+
+ TIM_TypeDef *timer = timerHardware->tim;
+ const IO_t motorIO = IOGetByTag(timerHardware->tag);
+
+ const uint8_t timerIndex = getTimerIndex(timer);
+ const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
+
+ IOInit(motorIO, OWNER_MOTOR, RESOURCE_OUTPUT, 0);
+ IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction);
+
+ __DMA1_CLK_ENABLE();
+
+ if (configureTimer) {
+ RCC_ClockCmd(timerRCC(timer), ENABLE);
+
+ uint32_t hz = (pwmProtocolType == PWM_TYPE_DSHOT600 ? MOTOR_DSHOT600_MHZ : MOTOR_DSHOT150_MHZ) * 1000000;
+ motor->TimHandle.Instance = timerHardware->tim;
+ motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;;
+ motor->TimHandle.Init.Period = MOTOR_BITLENGTH;
+ motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ if(HAL_TIM_PWM_Init(&motor->TimHandle) != HAL_OK)
+ {
+ /* Initialization Error */
+ return;
+ }
+
+ }
+ else
+ {
+ motor->TimHandle = dmaMotors[timerIndex].TimHandle;
+ }
+
+ switch (timerHardware->channel) {
+ case TIM_CHANNEL_1:
+ motor->timerDmaSource = TIM_DMA_ID_CC1;
+ break;
+ case TIM_CHANNEL_2:
+ motor->timerDmaSource = TIM_DMA_ID_CC2;
+ break;
+ case TIM_CHANNEL_3:
+ motor->timerDmaSource = TIM_DMA_ID_CC3;
+ break;
+ case TIM_CHANNEL_4:
+ motor->timerDmaSource = TIM_DMA_ID_CC4;
+ break;
+ }
+
+ dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
+
+
+ static DMA_HandleTypeDef hdma_tim;
+
+ /* Set the parameters to be configured */
+ hdma_tim.Init.Channel = timerHardware->dmaChannel;
+ hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE;
+ hdma_tim.Init.MemInc = DMA_MINC_ENABLE;
+ hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ;
+ hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ;
+ hdma_tim.Init.Mode = DMA_NORMAL;
+ hdma_tim.Init.Priority = DMA_PRIORITY_HIGH;
+ hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+ hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE;
+ hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE;
+
+ /* Set hdma_tim instance */
+ if(timerHardware->dmaStream == NULL)
+ {
+ /* Initialization Error */
+ return;
+ }
+ hdma_tim.Instance = timerHardware->dmaStream;
+
+ /* Link hdma_tim to hdma[x] (channelx) */
+ __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], hdma_tim);
+
+ dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
+
+ /* Initialize TIMx DMA handle */
+ if(HAL_DMA_Init(motor->TimHandle.hdma[motor->timerDmaSource]) != HAL_OK)
+ {
+ /* Initialization Error */
+ return;
+ }
+
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
+
+ /* PWM1 Mode configuration: Channel1 */
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
+ TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+ TIM_OCInitStructure.Pulse = 0;
+
+ if(HAL_TIM_PWM_ConfigChannel(&motor->TimHandle, &TIM_OCInitStructure, motor->timerHardware->channel) != HAL_OK)
+ {
+ /* Configuration Error */
+ return;
+ }
+}
+
+#endif
diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c
index 791c5f25d3..f055683976 100644
--- a/src/main/drivers/pwm_rx.c
+++ b/src/main/drivers/pwm_rx.c
@@ -320,7 +320,11 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture
if (pwmInputPort->state == 0) {
pwmInputPort->rise = capture;
pwmInputPort->state = 1;
+#if defined(USE_HAL_DRIVER)
+ pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_FALLING);
+#else
pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
+#endif
} else {
pwmInputPort->fall = capture;
@@ -330,11 +334,37 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture
// switch state
pwmInputPort->state = 0;
+#if defined(USE_HAL_DRIVER)
+ pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_RISING);
+#else
pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
+#endif
pwmInputPort->missedEvents = 0;
}
}
+#ifdef USE_HAL_DRIVER
+
+void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
+{
+ TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
+ if(Handle == NULL) return;
+
+ TIM_IC_InitTypeDef TIM_ICInitStructure;
+
+ TIM_ICInitStructure.ICPolarity = polarity;
+ TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_DIRECTTI;
+ TIM_ICInitStructure.ICPrescaler = TIM_ICPSC_DIV1;
+
+ if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
+ TIM_ICInitStructure.ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
+ } else {
+ TIM_ICInitStructure.ICFilter = 0x00;
+ }
+
+ HAL_TIM_IC_ConfigChannel(Handle, &TIM_ICInitStructure, channel);
+}
+#else
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{
TIM_ICInitTypeDef TIM_ICInitStructure;
@@ -354,6 +384,9 @@ 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++) {
@@ -377,8 +410,11 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel));
IOConfigGPIO(io, timer->ioMode);
- pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
-
+#if defined(USE_HAL_DRIVER)
+ pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
+#else
+ pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
+#endif
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
@@ -438,7 +474,11 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0);
IOConfigGPIO(io, timer->ioMode);
+#if defined(USE_HAL_DRIVER)
+ pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
+#else
pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
+#endif
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
diff --git a/src/main/drivers/rcc.c b/src/main/drivers/rcc.c
index b1e759f972..7047a0181a 100644
--- a/src/main/drivers/rcc.c
+++ b/src/main/drivers/rcc.c
@@ -6,6 +6,11 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
{
int tag = periphTag >> 5;
uint32_t mask = 1 << (periphTag & 0x1f);
+#if defined(USE_HAL_DRIVER)
+ (void)tag;
+ (void)mask;
+ (void)NewState;
+#else
switch (tag) {
#if defined(STM32F3) || defined(STM32F1)
case RCC_AHB:
@@ -24,12 +29,18 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
break;
#endif
}
+#endif
}
void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
{
int tag = periphTag >> 5;
uint32_t mask = 1 << (periphTag & 0x1f);
+#if defined(USE_HAL_DRIVER)
+ (void)tag;
+ (void)mask;
+ (void)NewState;
+#else
switch (tag) {
#if defined(STM32F3) || defined(STM32F10X_CL)
case RCC_AHB:
@@ -48,4 +59,5 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
break;
#endif
}
+#endif
}
diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c
index a86fc74577..f02df997d8 100644
--- a/src/main/drivers/sdcard.c
+++ b/src/main/drivers/sdcard.c
@@ -108,6 +108,9 @@ static sdcard_t sdcard;
#ifdef SDCARD_DMA_CHANNEL_TX
static bool useDMAForTx;
+#if defined(USE_HAL_DRIVER)
+ DMA_HandleTypeDef *sdDMAHandle;
+#endif
#else
// DMA channel not available so we can hard-code this to allow the non-DMA paths to be stripped by optimization
static const bool useDMAForTx = false;
@@ -413,6 +416,9 @@ static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite)
if (useDMAForTx) {
#ifdef SDCARD_DMA_CHANNEL_TX
+#if defined(USE_HAL_DRIVER)
+ sdDMAHandle = spiSetDMATransmit(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL, SDCARD_SPI_INSTANCE, buffer, SDCARD_BLOCK_SIZE);
+#else
// Queue the transmission of the sector payload
#ifdef SDCARD_DMA_CLK
RCC_AHB1PeriphClockCmd(SDCARD_DMA_CLK, ENABLE);
@@ -446,6 +452,7 @@ static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite)
DMA_Cmd(SDCARD_DMA_CHANNEL_TX, ENABLE);
SPI_I2S_DMACmd(SDCARD_SPI_INSTANCE, SPI_I2S_DMAReq_Tx, ENABLE);
+#endif
#endif
}
else {
@@ -729,6 +736,28 @@ bool sdcard_poll(void)
sendComplete = false;
#ifdef SDCARD_DMA_CHANNEL_TX
+#if defined(USE_HAL_DRIVER)
+ //if (useDMAForTx && __HAL_DMA_GET_FLAG(sdDMAHandle, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) {
+ //if (useDMAForTx && HAL_DMA_PollForTransfer(sdDMAHandle, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY) == HAL_OK) {
+ if (useDMAForTx && (sdDMAHandle->State == HAL_DMA_STATE_READY)) {
+ //__HAL_DMA_CLEAR_FLAG(sdDMAHandle, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG);
+
+ //__HAL_DMA_DISABLE(sdDMAHandle);
+
+ // Drain anything left in the Rx FIFO (we didn't read it during the write)
+ while (__HAL_SPI_GET_FLAG(spiHandleByInstance(SDCARD_SPI_INSTANCE), SPI_FLAG_RXNE) == SET) {
+ SDCARD_SPI_INSTANCE->DR;
+ }
+
+ // Wait for the final bit to be transmitted
+ while (spiIsBusBusy(SDCARD_SPI_INSTANCE)) {
+ }
+
+ HAL_SPI_DMAStop(spiHandleByInstance(SDCARD_SPI_INSTANCE));
+
+ sendComplete = true;
+ }
+#else
#ifdef SDCARD_DMA_CHANNEL
if (useDMAForTx && DMA_GetFlagStatus(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG) == SET) {
DMA_ClearFlag(SDCARD_DMA_CHANNEL_TX, SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG);
@@ -752,6 +781,7 @@ bool sdcard_poll(void)
sendComplete = true;
}
+#endif
#endif
if (!useDMAForTx) {
// Send another chunk
diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h
index 5697fcc9f3..d487df3781 100644
--- a/src/main/drivers/serial_uart.h
+++ b/src/main/drivers/serial_uart.h
@@ -35,11 +35,19 @@
#define UART5_TX_BUFFER_SIZE 256
#define UART6_RX_BUFFER_SIZE 256
#define UART6_TX_BUFFER_SIZE 256
+#define UART7_RX_BUFFER_SIZE 256
+#define UART7_TX_BUFFER_SIZE 256
+#define UART8_RX_BUFFER_SIZE 256
+#define UART8_TX_BUFFER_SIZE 256
typedef struct {
serialPort_t port;
-#ifdef STM32F4
+#if defined(STM32F7)
+ DMA_HandleTypeDef rxDMAHandle;
+ DMA_HandleTypeDef txDMAHandle;
+#endif
+#if defined(STM32F4) || defined(STM32F7)
DMA_Stream_TypeDef *rxDMAStream;
DMA_Stream_TypeDef *txDMAStream;
uint32_t rxDMAChannel;
@@ -48,7 +56,6 @@ typedef struct {
DMA_Channel_TypeDef *rxDMAChannel;
DMA_Channel_TypeDef *txDMAChannel;
#endif
-
uint32_t rxDMAIrq;
uint32_t txDMAIrq;
@@ -58,6 +65,10 @@ typedef struct {
uint32_t txDMAPeripheralBaseAddr;
uint32_t rxDMAPeripheralBaseAddr;
+#ifdef USE_HAL_DRIVER
+ // All USARTs can also be used as UART, and we use them only as UART.
+ UART_HandleTypeDef Handle;
+#endif
USART_TypeDef *USARTx;
} uartPort_t;
diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c
new file mode 100644
index 0000000000..8fc2573293
--- /dev/null
+++ b/src/main/drivers/serial_uart_hal.c
@@ -0,0 +1,390 @@
+/*
+ * 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 .
+ */
+
+/*
+ * Authors:
+ * Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups.
+ * Hamasaki/Timecop - Initial baseflight code
+*/
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+#include "io.h"
+#include "nvic.h"
+#include "inverter.h"
+
+#include "serial.h"
+#include "serial_uart.h"
+#include "serial_uart_impl.h"
+
+static void usartConfigurePinInversion(uartPort_t *uartPort) {
+ bool inverted = uartPort->port.options & SERIAL_INVERTED;
+
+ if(inverted)
+ {
+ if (uartPort->port.mode & MODE_RX)
+ {
+ uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_RXINVERT_INIT;
+ uartPort->Handle.AdvancedInit.RxPinLevelInvert = UART_ADVFEATURE_RXINV_ENABLE;
+ }
+ if (uartPort->port.mode & MODE_TX)
+ {
+ uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_TXINVERT_INIT;
+ uartPort->Handle.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE;
+ }
+ }
+}
+
+static void uartReconfigure(uartPort_t *uartPort)
+{
+ HAL_StatusTypeDef status = HAL_ERROR;
+ /*RCC_PeriphCLKInitTypeDef RCC_PeriphClkInit;
+ RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2|RCC_PERIPHCLK_USART3|
+ RCC_PERIPHCLK_UART4|RCC_PERIPHCLK_UART5|RCC_PERIPHCLK_USART6|RCC_PERIPHCLK_UART7|RCC_PERIPHCLK_UART8;
+ RCC_PeriphClkInit.Usart1ClockSelection = RCC_USART1CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Usart2ClockSelection = RCC_USART2CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Usart3ClockSelection = RCC_USART3CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Uart4ClockSelection = RCC_UART4CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Uart5ClockSelection = RCC_UART5CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Usart6ClockSelection = RCC_USART6CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Uart7ClockSelection = RCC_UART7CLKSOURCE_SYSCLK;
+ RCC_PeriphClkInit.Uart8ClockSelection = RCC_UART8CLKSOURCE_SYSCLK;
+ HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit);*/
+
+ HAL_UART_DeInit(&uartPort->Handle);
+ uartPort->Handle.Init.BaudRate = uartPort->port.baudRate;
+ uartPort->Handle.Init.WordLength = UART_WORDLENGTH_8B;
+ uartPort->Handle.Init.StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_STOPBITS_2 : USART_STOPBITS_1;
+ uartPort->Handle.Init.Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_PARITY_EVEN : USART_PARITY_NONE;
+ uartPort->Handle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+ uartPort->Handle.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
+ uartPort->Handle.Init.Mode = 0;
+
+ if (uartPort->port.mode & MODE_RX)
+ uartPort->Handle.Init.Mode |= UART_MODE_RX;
+ if (uartPort->port.mode & MODE_TX)
+ uartPort->Handle.Init.Mode |= UART_MODE_TX;
+
+
+ usartConfigurePinInversion(uartPort);
+
+ if(uartPort->port.options & SERIAL_BIDIR)
+ {
+ status = HAL_HalfDuplex_Init(&uartPort->Handle);
+ }
+ else
+ {
+ status = HAL_UART_Init(&uartPort->Handle);
+ }
+
+ // Receive DMA or IRQ
+ if (uartPort->port.mode & MODE_RX)
+ {
+ if (uartPort->rxDMAStream)
+ {
+ uartPort->rxDMAHandle.Instance = uartPort->rxDMAStream;
+ uartPort->rxDMAHandle.Init.Channel = uartPort->rxDMAChannel;
+ uartPort->rxDMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+ uartPort->rxDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ uartPort->rxDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
+ uartPort->rxDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ uartPort->rxDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ uartPort->rxDMAHandle.Init.Mode = DMA_CIRCULAR;
+ uartPort->rxDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ uartPort->rxDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
+ uartPort->rxDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+ uartPort->rxDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+ uartPort->rxDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
+
+
+ HAL_DMA_DeInit(&uartPort->rxDMAHandle);
+ HAL_DMA_Init(&uartPort->rxDMAHandle);
+ /* Associate the initialized DMA handle to the UART handle */
+ __HAL_LINKDMA(&uartPort->Handle, hdmarx, uartPort->rxDMAHandle);
+
+ HAL_UART_Receive_DMA(&uartPort->Handle, (uint8_t*)uartPort->port.rxBuffer, uartPort->port.rxBufferSize);
+
+ uartPort->rxDMAPos = __HAL_DMA_GET_COUNTER(&uartPort->rxDMAHandle);
+
+ }
+ else
+ {
+ /* Enable the UART Parity Error Interrupt */
+ SET_BIT(uartPort->USARTx->CR1, USART_CR1_PEIE);
+
+ /* Enable the UART Error Interrupt: (Frame error, noise error, overrun error) */
+ SET_BIT(uartPort->USARTx->CR3, USART_CR3_EIE);
+
+ /* Enable the UART Data Register not empty Interrupt */
+ SET_BIT(uartPort->USARTx->CR1, USART_CR1_RXNEIE);
+ }
+ }
+
+ // Transmit DMA or IRQ
+ if (uartPort->port.mode & MODE_TX) {
+
+ if (uartPort->txDMAStream) {
+ uartPort->txDMAHandle.Instance = uartPort->txDMAStream;
+ uartPort->txDMAHandle.Init.Channel = uartPort->txDMAChannel;
+ uartPort->txDMAHandle.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ uartPort->txDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ uartPort->txDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
+ uartPort->txDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ uartPort->txDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ uartPort->txDMAHandle.Init.Mode = DMA_NORMAL;
+ uartPort->txDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ uartPort->txDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
+ uartPort->txDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+ uartPort->txDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+ uartPort->txDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
+
+
+ HAL_DMA_DeInit(&uartPort->txDMAHandle);
+ HAL_StatusTypeDef status = HAL_DMA_Init(&uartPort->txDMAHandle);
+ if(status != HAL_OK)
+ {
+ while(1);
+ }
+ /* Associate the initialized DMA handle to the UART handle */
+ __HAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle);
+
+ __HAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
+ } else {
+ __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE);
+ }
+ }
+ return;
+}
+
+serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ uartPort_t *s = NULL;
+
+ if (USARTx == USART1) {
+ s = serialUART1(baudRate, mode, options);
+#ifdef USE_UART2
+ } else if (USARTx == USART2) {
+ s = serialUART2(baudRate, mode, options);
+#endif
+#ifdef USE_UART3
+ } else if (USARTx == USART3) {
+ s = serialUART3(baudRate, mode, options);
+#endif
+#ifdef USE_UART4
+ } else if (USARTx == UART4) {
+ s = serialUART4(baudRate, mode, options);
+#endif
+#ifdef USE_UART5
+ } else if (USARTx == UART5) {
+ s = serialUART5(baudRate, mode, options);
+#endif
+#ifdef USE_UART6
+ } else if (USARTx == USART6) {
+ s = serialUART6(baudRate, mode, options);
+#endif
+#ifdef USE_UART7
+ } else if (USARTx == UART7) {
+ s = serialUART7(baudRate, mode, options);
+#endif
+#ifdef USE_UART8
+ } else if (USARTx == UART8) {
+ s = serialUART8(baudRate, mode, options);
+#endif
+ } else {
+ return (serialPort_t *)s;
+ }
+
+
+ s->txDMAEmpty = true;
+
+ // common serial initialisation code should move to serialPort::init()
+ s->port.rxBufferHead = s->port.rxBufferTail = 0;
+ s->port.txBufferHead = s->port.txBufferTail = 0;
+ // callback works for IRQ-based RX ONLY
+ s->port.rxCallback = callback;
+ s->port.mode = mode;
+ s->port.baudRate = baudRate;
+ s->port.options = options;
+
+ uartReconfigure(s);
+
+ return (serialPort_t *)s;
+}
+
+void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
+{
+ uartPort_t *uartPort = (uartPort_t *)instance;
+ uartPort->port.baudRate = baudRate;
+ uartReconfigure(uartPort);
+}
+
+void uartSetMode(serialPort_t *instance, portMode_t mode)
+{
+ uartPort_t *uartPort = (uartPort_t *)instance;
+ uartPort->port.mode = mode;
+ uartReconfigure(uartPort);
+}
+
+void uartStartTxDMA(uartPort_t *s)
+{
+ uint16_t size = 0;
+ uint32_t fromwhere=0;
+ HAL_UART_StateTypeDef state = HAL_UART_GetState(&s->Handle);
+ if((state & HAL_UART_STATE_BUSY_TX) == HAL_UART_STATE_BUSY_TX)
+ return;
+
+ if (s->port.txBufferHead > s->port.txBufferTail) {
+ size = s->port.txBufferHead - s->port.txBufferTail;
+ fromwhere = s->port.txBufferTail;
+ s->port.txBufferTail = s->port.txBufferHead;
+ } else {
+ size = s->port.txBufferSize - s->port.txBufferTail;
+ fromwhere = s->port.txBufferTail;
+ s->port.txBufferTail = 0;
+ }
+ s->txDMAEmpty = false;
+ HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size);
+}
+
+uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
+{
+ uartPort_t *s = (uartPort_t*)instance;
+
+ if (s->rxDMAStream) {
+ uint32_t rxDMAHead = __HAL_DMA_GET_COUNTER(s->Handle.hdmarx);
+
+ if (rxDMAHead >= s->rxDMAPos) {
+ return rxDMAHead - s->rxDMAPos;
+ } else {
+ return s->port.rxBufferSize + rxDMAHead - s->rxDMAPos;
+ }
+ }
+
+ if (s->port.rxBufferHead >= s->port.rxBufferTail) {
+ return s->port.rxBufferHead - s->port.rxBufferTail;
+ } else {
+ return s->port.rxBufferSize + s->port.rxBufferHead - s->port.rxBufferTail;
+ }
+}
+
+uint32_t uartTotalTxBytesFree(const serialPort_t *instance)
+{
+ uartPort_t *s = (uartPort_t*)instance;
+
+ uint32_t bytesUsed;
+
+ if (s->port.txBufferHead >= s->port.txBufferTail) {
+ bytesUsed = s->port.txBufferHead - s->port.txBufferTail;
+ } else {
+ bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail;
+ }
+
+ if (s->txDMAStream) {
+ /*
+ * When we queue up a DMA request, we advance the Tx buffer tail before the transfer finishes, so we must add
+ * the remaining size of that in-progress transfer here instead:
+ */
+ bytesUsed += __HAL_DMA_GET_COUNTER(s->Handle.hdmatx);
+
+ /*
+ * If the Tx buffer is being written to very quickly, we might have advanced the head into the buffer
+ * space occupied by the current DMA transfer. In that case the "bytesUsed" total will actually end up larger
+ * than the total Tx buffer size, because we'll end up transmitting the same buffer region twice. (So we'll be
+ * transmitting a garbage mixture of old and new bytes).
+ *
+ * Be kind to callers and pretend like our buffer can only ever be 100% full.
+ */
+ if (bytesUsed >= s->port.txBufferSize - 1) {
+ return 0;
+ }
+ }
+
+ return (s->port.txBufferSize - 1) - bytesUsed;
+}
+
+bool isUartTransmitBufferEmpty(const serialPort_t *instance)
+{
+ uartPort_t *s = (uartPort_t *)instance;
+ if (s->txDMAStream)
+
+ return s->txDMAEmpty;
+ else
+ return s->port.txBufferTail == s->port.txBufferHead;
+}
+
+uint8_t uartRead(serialPort_t *instance)
+{
+ uint8_t ch;
+ uartPort_t *s = (uartPort_t *)instance;
+
+
+ if (s->rxDMAStream) {
+
+ ch = s->port.rxBuffer[s->port.rxBufferSize - s->rxDMAPos];
+ if (--s->rxDMAPos == 0)
+ s->rxDMAPos = s->port.rxBufferSize;
+ } else {
+ ch = s->port.rxBuffer[s->port.rxBufferTail];
+ if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) {
+ s->port.rxBufferTail = 0;
+ } else {
+ s->port.rxBufferTail++;
+ }
+ }
+
+ return ch;
+}
+
+void uartWrite(serialPort_t *instance, uint8_t ch)
+{
+ uartPort_t *s = (uartPort_t *)instance;
+ s->port.txBuffer[s->port.txBufferHead] = ch;
+ if (s->port.txBufferHead + 1 >= s->port.txBufferSize) {
+ s->port.txBufferHead = 0;
+ } else {
+ s->port.txBufferHead++;
+ }
+
+ if (s->txDMAStream) {
+ if (!(s->txDMAStream->CR & 1))
+ uartStartTxDMA(s);
+ } else {
+ __HAL_UART_ENABLE_IT(&s->Handle, UART_IT_TXE);
+ }
+}
+
+const struct serialPortVTable uartVTable[] = {
+ {
+ .serialWrite = uartWrite,
+ .serialTotalRxWaiting = uartTotalRxBytesWaiting,
+ .serialTotalTxFree = uartTotalTxBytesFree,
+ .serialRead = uartRead,
+ .serialSetBaudRate = uartSetBaudRate,
+ .isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty,
+ .setMode = uartSetMode,
+ .writeBuf = NULL,
+ .beginWrite = NULL,
+ .endWrite = NULL,
+ }
+};
+
diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h
index a4549de344..d983570285 100644
--- a/src/main/drivers/serial_uart_impl.h
+++ b/src/main/drivers/serial_uart_impl.h
@@ -29,4 +29,6 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option
uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t options);
uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t options);
uartPort_t *serialUART6(uint32_t baudRate, portMode_t mode, portOptions_t options);
+uartPort_t *serialUART7(uint32_t baudRate, portMode_t mode, portOptions_t options);
+uartPort_t *serialUART8(uint32_t baudRate, portMode_t mode, portOptions_t options);
diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c
new file mode 100644
index 0000000000..b6aeb7c320
--- /dev/null
+++ b/src/main/drivers/serial_uart_stm32f7xx.c
@@ -0,0 +1,550 @@
+/*
+ * 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 "platform.h"
+
+#include "system.h"
+#include "io.h"
+#include "rcc.h"
+#include "nvic.h"
+#include "dma.h"
+
+#include "serial.h"
+#include "serial_uart.h"
+#include "serial_uart_impl.h"
+
+static void handleUsartTxDma(uartPort_t *s);
+
+#define UART_RX_BUFFER_SIZE UART1_RX_BUFFER_SIZE
+#define UART_TX_BUFFER_SIZE UART1_TX_BUFFER_SIZE
+
+typedef enum UARTDevice {
+ UARTDEV_1 = 0,
+ UARTDEV_2 = 1,
+ UARTDEV_3 = 2,
+ UARTDEV_4 = 3,
+ UARTDEV_5 = 4,
+ UARTDEV_6 = 5,
+ UARTDEV_7 = 6,
+ UARTDEV_8 = 7
+} UARTDevice;
+
+typedef struct uartDevice_s {
+ USART_TypeDef* dev;
+ uartPort_t port;
+ uint32_t DMAChannel;
+ DMA_Stream_TypeDef *txDMAStream;
+ DMA_Stream_TypeDef *rxDMAStream;
+ ioTag_t rx;
+ ioTag_t tx;
+ volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE];
+ volatile uint8_t txBuffer[UART_TX_BUFFER_SIZE];
+ uint32_t rcc_ahb1;
+ rccPeriphTag_t rcc_apb2;
+ rccPeriphTag_t rcc_apb1;
+ uint8_t af;
+ uint8_t txIrq;
+ uint8_t rxIrq;
+ uint32_t txPriority;
+ uint32_t rxPriority;
+} uartDevice_t;
+
+//static uartPort_t uartPort[MAX_UARTS];
+#ifdef USE_UART1
+static uartDevice_t uart1 =
+{
+ .DMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART1_RX_DMA
+ .rxDMAStream = DMA2_Stream5,
+#endif
+ .txDMAStream = DMA2_Stream7,
+ .dev = USART1,
+ .rx = IO_TAG(UART1_RX_PIN),
+ .tx = IO_TAG(UART1_TX_PIN),
+ .af = GPIO_AF7_USART1,
+#ifdef UART1_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART1_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb2 = RCC_APB2(USART1),
+ .txIrq = DMA2_ST7_HANDLER,
+ .rxIrq = USART1_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART1_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART1
+};
+#endif
+
+#ifdef USE_UART2
+static uartDevice_t uart2 =
+{
+ .DMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART2_RX_DMA
+ .rxDMAStream = DMA1_Stream5,
+#endif
+ .txDMAStream = DMA1_Stream6,
+ .dev = USART2,
+ .rx = IO_TAG(UART2_RX_PIN),
+ .tx = IO_TAG(UART2_TX_PIN),
+ .af = GPIO_AF7_USART2,
+#ifdef UART2_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART2_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb1 = RCC_APB1(USART2),
+ .txIrq = DMA1_ST6_HANDLER,
+ .rxIrq = USART2_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART2_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART2
+};
+#endif
+
+#ifdef USE_UART3
+static uartDevice_t uart3 =
+{
+ .DMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART3_RX_DMA
+ .rxDMAStream = DMA1_Stream1,
+#endif
+ .txDMAStream = DMA1_Stream3,
+ .dev = USART3,
+ .rx = IO_TAG(UART3_RX_PIN),
+ .tx = IO_TAG(UART3_TX_PIN),
+ .af = GPIO_AF7_USART3,
+#ifdef UART3_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART3_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb1 = RCC_APB1(USART3),
+ .txIrq = DMA1_ST3_HANDLER,
+ .rxIrq = USART3_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART3_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART3
+};
+#endif
+
+#ifdef USE_UART4
+static uartDevice_t uart4 =
+{
+ .DMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART4_RX_DMA
+ .rxDMAStream = DMA1_Stream2,
+#endif
+ .txDMAStream = DMA1_Stream4,
+ .dev = UART4,
+ .rx = IO_TAG(UART4_RX_PIN),
+ .tx = IO_TAG(UART4_TX_PIN),
+ .af = GPIO_AF8_UART4,
+#ifdef UART4_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART4_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb1 = RCC_APB1(UART4),
+ .txIrq = DMA1_ST4_HANDLER,
+ .rxIrq = UART4_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART4_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART4
+};
+#endif
+
+#ifdef USE_UART5
+static uartDevice_t uart5 =
+{
+ .DMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART5_RX_DMA
+ .rxDMAStream = DMA1_Stream0,
+#endif
+ .txDMAStream = DMA1_Stream7,
+ .dev = UART5,
+ .rx = IO_TAG(UART5_RX_PIN),
+ .tx = IO_TAG(UART5_TX_PIN),
+ .af = GPIO_AF8_UART5,
+#ifdef UART5_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART5_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb1 = RCC_APB1(UART5),
+ .txIrq = DMA1_ST7_HANDLER,
+ .rxIrq = UART5_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART5_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART5
+};
+#endif
+
+#ifdef USE_UART6
+static uartDevice_t uart6 =
+{
+ .DMAChannel = DMA_CHANNEL_5,
+#ifdef USE_UART6_RX_DMA
+ .rxDMAStream = DMA2_Stream1,
+#endif
+ .txDMAStream = DMA2_Stream6,
+ .dev = USART6,
+ .rx = IO_TAG(UART6_RX_PIN),
+ .tx = IO_TAG(UART6_TX_PIN),
+ .af = GPIO_AF8_USART6,
+#ifdef UART6_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART6_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb2 = RCC_APB2(USART6),
+ .txIrq = DMA2_ST6_HANDLER,
+ .rxIrq = USART6_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART6_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART6
+};
+#endif
+
+#ifdef USE_UART7
+static uartDevice_t uart7 =
+{
+ .DMAChannel = DMA_CHANNEL_5,
+#ifdef USE_UART7_RX_DMA
+ .rxDMAStream = DMA1_Stream3,
+#endif
+ .txDMAStream = DMA1_Stream1,
+ .dev = UART7,
+ .rx = IO_TAG(UART7_RX_PIN),
+ .tx = IO_TAG(UART7_TX_PIN),
+ .af = GPIO_AF8_UART7,
+#ifdef UART7_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART7_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb1 = RCC_APB1(UART7),
+ .txIrq = DMA1_ST1_HANDLER,
+ .rxIrq = UART7_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART7_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART7
+};
+#endif
+#ifdef USE_UART8
+static uartDevice_t uart8 =
+{
+ .DMAChannel = DMA_CHANNEL_5,
+#ifdef USE_UART8_RX_DMA
+ .rxDMAStream = DMA1_Stream6,
+#endif
+ .txDMAStream = DMA1_Stream0,
+ .dev = UART8,
+ .rx = IO_TAG(UART8_RX_PIN),
+ .tx = IO_TAG(UART8_TX_PIN),
+ .af = GPIO_AF8_UART8,
+#ifdef UART8_AHB1_PERIPHERALS
+ .rcc_ahb1 = UART8_AHB1_PERIPHERALS,
+#endif
+ .rcc_apb1 = RCC_APB1(UART8),
+ .txIrq = DMA1_ST0_HANDLER,
+ .rxIrq = UART8_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART8_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART8
+};
+#endif
+
+
+
+static uartDevice_t* uartHardwareMap[] = {
+#ifdef USE_UART1
+ &uart1,
+#else
+ NULL,
+#endif
+#ifdef USE_UART2
+ &uart2,
+#else
+ NULL,
+#endif
+#ifdef USE_UART3
+ &uart3,
+#else
+ NULL,
+#endif
+#ifdef USE_UART4
+ &uart4,
+#else
+ NULL,
+#endif
+#ifdef USE_UART5
+ &uart5,
+#else
+ NULL,
+#endif
+#ifdef USE_UART6
+ &uart6,
+#else
+ NULL,
+#endif
+#ifdef USE_UART7
+ &uart7,
+#else
+ NULL,
+#endif
+#ifdef USE_UART8
+ &uart8,
+#else
+ NULL,
+#endif
+};
+
+void uartIrqHandler(uartPort_t *s)
+{
+ UART_HandleTypeDef *huart = &s->Handle;
+ /* UART in mode Receiver ---------------------------------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET))
+ {
+ uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff);
+
+ if (s->port.rxCallback) {
+ s->port.rxCallback(rbyte);
+ } else {
+ s->port.rxBuffer[s->port.rxBufferHead] = rbyte;
+ s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
+ }
+ CLEAR_BIT(huart->Instance->CR1, (USART_CR1_PEIE));
+
+ /* Disable the UART Error Interrupt: (Frame error, noise error, overrun error) */
+ CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE);
+
+ __HAL_UART_SEND_REQ(huart, UART_RXDATA_FLUSH_REQUEST);
+ }
+
+ /* UART parity error interrupt occurred -------------------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET))
+ {
+ __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF);
+ }
+
+ /* UART frame error interrupt occurred --------------------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET))
+ {
+ __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF);
+ }
+
+ /* UART noise error interrupt occurred --------------------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET))
+ {
+ __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF);
+ }
+
+ /* UART Over-Run interrupt occurred -----------------------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET))
+ {
+ __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF);
+ }
+
+ /* UART in mode Transmitter ------------------------------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET))
+ {
+ HAL_UART_IRQHandler(huart);
+ }
+
+ /* UART in mode Transmitter (transmission end) -----------------------------*/
+ if((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET))
+ {
+ HAL_UART_IRQHandler(huart);
+ handleUsartTxDma(s);
+ }
+
+}
+
+static void handleUsartTxDma(uartPort_t *s)
+{
+ if (s->port.txBufferHead != s->port.txBufferTail)
+ uartStartTxDMA(s);
+ else
+ {
+ s->txDMAEmpty = true;
+ }
+}
+
+void dmaIRQHandler(dmaChannelDescriptor_t* descriptor)
+{
+ uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port);
+ HAL_DMA_IRQHandler(&s->txDMAHandle);
+}
+
+uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ uartPort_t *s;
+
+ uartDevice_t *uart = uartHardwareMap[device];
+ if (!uart) return NULL;
+
+ s = &(uart->port);
+ s->port.vTable = uartVTable;
+
+ s->port.baudRate = baudRate;
+
+ s->port.rxBuffer = uart->rxBuffer;
+ s->port.txBuffer = uart->txBuffer;
+ s->port.rxBufferSize = sizeof(uart->rxBuffer);
+ s->port.txBufferSize = sizeof(uart->txBuffer);
+
+ s->USARTx = uart->dev;
+ if (uart->rxDMAStream) {
+ s->rxDMAChannel = uart->DMAChannel;
+ s->rxDMAStream = uart->rxDMAStream;
+ }
+ s->txDMAChannel = uart->DMAChannel;
+ s->txDMAStream = uart->txDMAStream;
+
+ s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR;
+ s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR;
+
+ s->Handle.Instance = uart->dev;
+
+ IO_t tx = IOGetByTag(uart->tx);
+ IO_t rx = IOGetByTag(uart->rx);
+
+ if (options & SERIAL_BIDIR) {
+ IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device));
+ IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
+ }
+ else {
+ if (mode & MODE_TX) {
+ IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(device));
+ IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
+ }
+
+ if (mode & MODE_RX) {
+ IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device));
+ IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
+ }
+ }
+
+ // DMA TX Interrupt
+ dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart);
+
+
+ //HAL_NVIC_SetPriority(uart->txIrq, NVIC_PRIORITY_BASE(uart->txPriority), NVIC_PRIORITY_SUB(uart->txPriority));
+ //HAL_NVIC_EnableIRQ(uart->txIrq);
+
+ if(!s->rxDMAChannel)
+ {
+ HAL_NVIC_SetPriority(uart->rxIrq, NVIC_PRIORITY_BASE(uart->rxPriority), NVIC_PRIORITY_SUB(uart->rxPriority));
+ HAL_NVIC_EnableIRQ(uart->rxIrq);
+ }
+
+ return s;
+}
+
+#ifdef USE_UART1
+uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_1, baudRate, mode, options);
+}
+
+// USART1 Rx/Tx IRQ Handler
+void USART1_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART2
+uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_2, baudRate, mode, options);
+}
+
+// USART2 Rx/Tx IRQ Handler
+void USART2_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART3
+uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_3, baudRate, mode, options);
+}
+
+// USART3 Rx/Tx IRQ Handler
+void USART3_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_3]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART4
+uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_4, baudRate, mode, options);
+}
+
+// UART4 Rx/Tx IRQ Handler
+void UART4_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_4]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART5
+uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_5, baudRate, mode, options);
+}
+
+// UART5 Rx/Tx IRQ Handler
+void UART5_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_5]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART6
+uartPort_t *serialUART6(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_6, baudRate, mode, options);
+}
+
+// USART6 Rx/Tx IRQ Handler
+void USART6_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_6]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART7
+uartPort_t *serialUART7(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_7, baudRate, mode, options);
+}
+
+// UART7 Rx/Tx IRQ Handler
+void UART7_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_7]->port);
+ uartIrqHandler(s);
+}
+#endif
+
+#ifdef USE_UART8
+uartPort_t *serialUART8(uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ return serialUART(UARTDEV_8, baudRate, mode, options);
+}
+
+// UART8 Rx/Tx IRQ Handler
+void UART8_IRQHandler(void)
+{
+ uartPort_t *s = &(uartHardwareMap[UARTDEV_8]->port);
+ uartIrqHandler(s);
+}
+#endif
diff --git a/src/main/drivers/serial_usb_vcp_hal.c b/src/main/drivers/serial_usb_vcp_hal.c
new file mode 100644
index 0000000000..e81e1bcc19
--- /dev/null
+++ b/src/main/drivers/serial_usb_vcp_hal.c
@@ -0,0 +1,193 @@
+/*
+ * 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 "platform.h"
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+#include "io.h"
+
+#include "vcp_hal/usbd_cdc_interface.h"
+
+#include "system.h"
+
+#include "serial.h"
+#include "serial_usb_vcp.h"
+
+
+#define USB_TIMEOUT 50
+
+static vcpPort_t vcpPort;
+USBD_HandleTypeDef USBD_Device;
+
+static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
+{
+ UNUSED(instance);
+ UNUSED(baudRate);
+
+ // TODO implement
+}
+
+static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
+{
+ UNUSED(instance);
+ UNUSED(mode);
+
+ // TODO implement
+}
+
+static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
+{
+ UNUSED(instance);
+ return true;
+}
+
+static uint32_t usbVcpAvailable(const serialPort_t *instance)
+{
+ UNUSED(instance);
+ uint32_t receiveLength = vcpAvailable();
+ return receiveLength;
+}
+
+static uint8_t usbVcpRead(serialPort_t *instance)
+{
+ UNUSED(instance);
+ return vcpRead();
+}
+
+static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count)
+{
+ UNUSED(instance);
+
+
+ if (!vcpIsConnected()) {
+ return;
+ }
+
+ uint32_t start = millis();
+ const uint8_t *p = data;
+ while (count > 0) {
+ uint32_t txed = vcpWrite(p, count);
+ count -= txed;
+ p += txed;
+
+ if (millis() - start > USB_TIMEOUT) {
+ break;
+ }
+ }
+}
+
+static bool usbVcpFlush(vcpPort_t *port)
+{
+ uint8_t count = port->txAt;
+ port->txAt = 0;
+
+ if (count == 0) {
+ return true;
+ }
+ if (!vcpIsConnected()) {
+ return false;
+ }
+
+ uint32_t start = millis();
+ uint8_t *p = port->txBuf;
+ while (count > 0) {
+ uint32_t txed = vcpWrite(p, count);
+ count -= txed;
+ p += txed;
+
+ if (millis() - start > USB_TIMEOUT) {
+ break;
+ }
+ }
+ return count == 0;
+}
+
+static void usbVcpWrite(serialPort_t *instance, uint8_t c)
+{
+ vcpPort_t *port = container_of(instance, vcpPort_t, port);
+
+ port->txBuf[port->txAt++] = c;
+ if (!port->buffering || port->txAt >= ARRAYLEN(port->txBuf)) {
+ usbVcpFlush(port);
+ }
+}
+
+static void usbVcpBeginWrite(serialPort_t *instance)
+{
+ vcpPort_t *port = container_of(instance, vcpPort_t, port);
+ port->buffering = true;
+}
+
+uint32_t usbTxBytesFree()
+{
+ return CDC_Send_FreeBytes();
+}
+
+static void usbVcpEndWrite(serialPort_t *instance)
+{
+ vcpPort_t *port = container_of(instance, vcpPort_t, port);
+ port->buffering = false;
+ usbVcpFlush(port);
+}
+
+static const struct serialPortVTable usbVTable[] = {
+ {
+ .serialWrite = usbVcpWrite,
+ .serialTotalRxWaiting = usbVcpAvailable,
+ .serialTotalTxFree = usbTxBytesFree,
+ .serialRead = usbVcpRead,
+ .serialSetBaudRate = usbVcpSetBaudRate,
+ .isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
+ .setMode = usbVcpSetMode,
+ .writeBuf = usbVcpWriteBuf,
+ .beginWrite = usbVcpBeginWrite,
+ .endWrite = usbVcpEndWrite
+ }
+};
+
+serialPort_t *usbVcpOpen(void)
+{
+ vcpPort_t *s;
+
+ /* Init Device Library */
+ USBD_Init(&USBD_Device, &VCP_Desc, 0);
+
+ /* Add Supported Class */
+ USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
+
+ /* Add CDC Interface Class */
+ USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
+
+ /* Start Device Process */
+ USBD_Start(&USBD_Device);
+
+ s = &vcpPort;
+ s->port.vTable = usbVTable;
+
+ return (serialPort_t *)s;
+}
+uint32_t usbVcpGetBaudRate(serialPort_t *instance)
+{
+ UNUSED(instance);
+
+ return vcpBaudrate();
+}
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 80e231054f..1d85160335 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -37,9 +37,13 @@ uint32_t cachedRccCsrValue;
void cycleCounterInit(void)
{
+#if defined(USE_HAL_DRIVER)
+ usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
+#else
RCC_ClocksTypeDef clocks;
RCC_GetClocksFreq(&clocks);
usTicks = clocks.SYSCLK_Frequency / 1000000;
+#endif
}
// SysTick
@@ -53,6 +57,10 @@ void SysTick_Handler(void)
sysTickPending = 0;
(void)(SysTick->CTRL);
}
+#ifdef USE_HAL_DRIVER
+ // used by the HAL for some timekeeping and timeouts, should always be 1ms
+ HAL_IncTick();
+#endif
}
// Return system uptime in microseconds (rollover in 70minutes)
diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c
new file mode 100644
index 0000000000..a2acf4b5ab
--- /dev/null
+++ b/src/main/drivers/system_stm32f7xx.c
@@ -0,0 +1,204 @@
+/*
+ * 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
+
+#include "platform.h"
+
+#include "accgyro_mpu.h"
+#include "exti.h"
+#include "nvic.h"
+#include "system.h"
+
+#include "exti.h"
+
+#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
+void SystemClock_Config(void);
+
+void systemReset(void)
+{
+ if (mpuConfiguration.reset)
+ mpuConfiguration.reset();
+
+ __disable_irq();
+ NVIC_SystemReset();
+}
+
+void systemResetToBootloader(void)
+{
+ if (mpuConfiguration.reset)
+ mpuConfiguration.reset();
+
+
+ (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot
+
+ __disable_irq();
+ NVIC_SystemReset();
+}
+
+void enableGPIOPowerUsageAndNoiseReductions(void)
+{
+
+ // AHB1
+ __HAL_RCC_BKPSRAM_CLK_ENABLE();
+ __HAL_RCC_DTCMRAMEN_CLK_ENABLE();
+ __HAL_RCC_DMA2_CLK_ENABLE();
+ __HAL_RCC_DMA2D_CLK_ENABLE();
+ __HAL_RCC_USB_OTG_HS_CLK_ENABLE();
+ __HAL_RCC_USB_OTG_HS_ULPI_CLK_ENABLE();
+ __HAL_RCC_GPIOA_CLK_ENABLE();
+ __HAL_RCC_GPIOB_CLK_ENABLE();
+ __HAL_RCC_GPIOC_CLK_ENABLE();
+ __HAL_RCC_GPIOD_CLK_ENABLE();
+ __HAL_RCC_GPIOE_CLK_ENABLE();
+ __HAL_RCC_GPIOF_CLK_ENABLE();
+ __HAL_RCC_GPIOG_CLK_ENABLE();
+ __HAL_RCC_GPIOH_CLK_ENABLE();
+ __HAL_RCC_GPIOI_CLK_ENABLE();
+ __HAL_RCC_GPIOJ_CLK_ENABLE();
+ __HAL_RCC_GPIOK_CLK_ENABLE();
+
+ //APB1
+ __HAL_RCC_TIM2_CLK_ENABLE();
+ __HAL_RCC_TIM3_CLK_ENABLE();
+ __HAL_RCC_TIM4_CLK_ENABLE();
+ __HAL_RCC_TIM5_CLK_ENABLE();
+ __HAL_RCC_TIM6_CLK_ENABLE();
+ __HAL_RCC_TIM7_CLK_ENABLE();
+ __HAL_RCC_TIM12_CLK_ENABLE();
+ __HAL_RCC_TIM13_CLK_ENABLE();
+ __HAL_RCC_TIM14_CLK_ENABLE();
+ __HAL_RCC_LPTIM1_CLK_ENABLE();
+ __HAL_RCC_SPI2_CLK_ENABLE();
+ __HAL_RCC_SPI3_CLK_ENABLE();
+ __HAL_RCC_USART2_CLK_ENABLE();
+ __HAL_RCC_USART3_CLK_ENABLE();
+ __HAL_RCC_UART4_CLK_ENABLE();
+ __HAL_RCC_UART5_CLK_ENABLE();
+ __HAL_RCC_I2C1_CLK_ENABLE();
+ __HAL_RCC_I2C2_CLK_ENABLE();
+ __HAL_RCC_I2C3_CLK_ENABLE();
+ __HAL_RCC_I2C4_CLK_ENABLE();
+ __HAL_RCC_CAN1_CLK_ENABLE();
+ __HAL_RCC_CAN2_CLK_ENABLE();
+ __HAL_RCC_CEC_CLK_ENABLE();
+ __HAL_RCC_DAC_CLK_ENABLE();
+ __HAL_RCC_UART7_CLK_ENABLE();
+ __HAL_RCC_UART8_CLK_ENABLE();
+
+ //APB2
+ __HAL_RCC_TIM1_CLK_ENABLE();
+ __HAL_RCC_TIM8_CLK_ENABLE();
+ __HAL_RCC_USART1_CLK_ENABLE();
+ __HAL_RCC_USART6_CLK_ENABLE();
+ __HAL_RCC_ADC1_CLK_ENABLE();
+ __HAL_RCC_ADC2_CLK_ENABLE();
+ __HAL_RCC_ADC3_CLK_ENABLE();
+ __HAL_RCC_SDMMC1_CLK_ENABLE();
+ __HAL_RCC_SPI1_CLK_ENABLE();
+ __HAL_RCC_SPI4_CLK_ENABLE();
+ __HAL_RCC_TIM9_CLK_ENABLE();
+ __HAL_RCC_TIM10_CLK_ENABLE();
+ __HAL_RCC_TIM11_CLK_ENABLE();
+ __HAL_RCC_SPI5_CLK_ENABLE();
+ __HAL_RCC_SPI6_CLK_ENABLE();
+ __HAL_RCC_SAI1_CLK_ENABLE();
+ __HAL_RCC_SAI2_CLK_ENABLE();
+//
+// GPIO_InitTypeDef GPIO_InitStructure;
+// GPIO_StructInit(&GPIO_InitStructure);
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input
+//
+// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
+// GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone
+//
+// GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14); // leave JTAG pins alone
+// GPIO_Init(GPIOA, &GPIO_InitStructure);
+//
+// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
+// GPIO_Init(GPIOB, &GPIO_InitStructure);
+//
+// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
+// GPIO_Init(GPIOC, &GPIO_InitStructure);
+// GPIO_Init(GPIOD, &GPIO_InitStructure);
+// GPIO_Init(GPIOE, &GPIO_InitStructure);
+}
+
+bool isMPUSoftReset(void)
+{
+ if (RCC->CSR & RCC_CSR_SFTRSTF)
+ return true;
+ else
+ return false;
+}
+
+void systemInit(void)
+{
+ checkForBootLoaderRequest();
+
+ //SystemClock_Config();
+
+ // Configure NVIC preempt/priority groups
+ //NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
+
+ // cache RCC->CSR value to use it in isMPUSoftreset() and others
+ cachedRccCsrValue = RCC->CSR;
+
+ /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */
+ //extern void *isr_vector_table_base;
+ //NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0);
+ //__HAL_RCC_USB_OTG_FS_CLK_DISABLE;
+
+ //RCC_ClearFlag();
+
+ enableGPIOPowerUsageAndNoiseReductions();
+
+ // Init cycle counter
+ cycleCounterInit();
+
+ // SysTick
+ //SysTick_Config(SystemCoreClock / 1000);
+ HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
+
+ HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
+}
+
+void(*bootJump)(void);
+void checkForBootLoaderRequest(void)
+{
+ uint32_t bt;
+ __PWR_CLK_ENABLE();
+ __BKPSRAM_CLK_ENABLE();
+ HAL_PWR_EnableBkUpAccess();
+
+ bt = (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) ;
+ if ( bt == 0xDEADBEEF ) {
+ (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xCAFEFEED; // Reset our trigger
+
+ void (*SysMemBootJump)(void);
+ __SYSCFG_CLK_ENABLE();
+ SYSCFG->MEMRMP |= SYSCFG_MEM_BOOT_ADD0 ;
+ uint32_t p = (*((uint32_t *) 0x1ff00000));
+ __set_MSP(p); //Set the main stack pointer to its defualt values
+ SysMemBootJump = (void (*)(void)) (*((uint32_t *) 0x1ff00004)); // Point the PC to the System Memory reset vector (+4)
+ SysMemBootJump();
+ while (1);
+ }
+}
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index c0425eb137..7dfef2033f 100755
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -481,8 +481,6 @@ volatile timCCR_t* timerChCCRLo(const timerHardware_t *timHw)
return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + (timHw->channel & ~TIM_Channel_2));
}
-
-
volatile timCCR_t* timerChCCR(const timerHardware_t *timHw)
{
return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + timHw->channel);
@@ -770,4 +768,62 @@ const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag)
}
}
return NULL;
+}
+
+#if !defined(USE_HAL_DRIVER)
+void timerOCInit(TIM_TypeDef *tim, uint8_t channel, TIM_OCInitTypeDef *init)
+{
+ switch (channel) {
+ case TIM_Channel_1:
+ TIM_OC1Init(tim, init);
+ break;
+ case TIM_Channel_2:
+ TIM_OC2Init(tim, init);
+ break;
+ case TIM_Channel_3:
+ TIM_OC3Init(tim, init);
+ break;
+ case TIM_Channel_4:
+ TIM_OC4Init(tim, init);
+ break;
+ }
+}
+
+void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload)
+{
+ switch (channel) {
+ case TIM_Channel_1:
+ TIM_OC1PreloadConfig(tim, preload);
+ break;
+ case TIM_Channel_2:
+ TIM_OC2PreloadConfig(tim, preload);
+ break;
+ case TIM_Channel_3:
+ TIM_OC3PreloadConfig(tim, preload);
+ break;
+ case TIM_Channel_4:
+ TIM_OC4PreloadConfig(tim, preload);
+ break;
+ }
+}
+#endif
+
+volatile timCCR_t* timerCCR(TIM_TypeDef *tim, uint8_t channel)
+{
+ return (volatile timCCR_t*)((volatile char*)&tim->CCR1 + channel);
+}
+
+uint16_t timerDmaSource(uint8_t channel)
+{
+ switch (channel) {
+ case TIM_Channel_1:
+ return TIM_DMA_CC1;
+ case TIM_Channel_2:
+ return TIM_DMA_CC2;
+ case TIM_Channel_3:
+ return TIM_DMA_CC3;
+ case TIM_Channel_4:
+ return TIM_DMA_CC4;
+ }
+ return 0;
}
\ No newline at end of file
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 244ca3bec0..cf4d12b220 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -30,6 +30,11 @@ typedef uint32_t timCCR_t;
typedef uint32_t timCCER_t;
typedef uint32_t timSR_t;
typedef uint32_t timCNT_t;
+#elif defined(STM32F7)
+typedef uint32_t timCCR_t;
+typedef uint32_t timCCER_t;
+typedef uint32_t timSR_t;
+typedef uint32_t timCNT_t;
#elif defined(STM32F3)
typedef uint32_t timCCR_t;
typedef uint32_t timCCER_t;
@@ -77,11 +82,11 @@ typedef struct timerHardware_s {
uint8_t irq;
uint8_t output;
ioConfig_t ioMode;
-#if defined(STM32F3) || defined(STM32F4)
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
uint8_t alternateFunction;
#endif
#if defined(USE_DSHOT)
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(STM32F7)
DMA_Stream_TypeDef *dmaStream;
uint32_t dmaChannel;
#elif defined(STM32F3)
@@ -110,6 +115,8 @@ typedef enum {
#define HARDWARE_TIMER_DEFINITION_COUNT 10
#elif defined(STM32F4)
#define HARDWARE_TIMER_DEFINITION_COUNT 14
+#elif defined(STM32F7)
+#define HARDWARE_TIMER_DEFINITION_COUNT 14
#endif
extern const timerHardware_t timerHardware[];
@@ -164,4 +171,14 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO -
rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
-const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag);
\ No newline at end of file
+const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag);
+
+#if defined(USE_HAL_DRIVER)
+TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim);
+#else
+void timerOCInit(TIM_TypeDef *tim, uint8_t channel, TIM_OCInitTypeDef *init);
+void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload);
+#endif
+
+volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel);
+uint16_t timerDmaSource(uint8_t channel);
\ No newline at end of file
diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c
new file mode 100644
index 0000000000..05c01a5210
--- /dev/null
+++ b/src/main/drivers/timer_hal.c
@@ -0,0 +1,872 @@
+/*
+ * 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 "platform.h"
+
+#include "build/atomic.h"
+
+#include "common/utils.h"
+
+#include "nvic.h"
+
+#include "io.h"
+#include "rcc.h"
+#include "system.h"
+
+#include "timer.h"
+#include "timer_impl.h"
+
+#define TIM_N(n) (1 << (n))
+
+/*
+ Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
+ TIM1 2 channels
+ TIM2 4 channels
+ TIM3 4 channels
+ TIM4 4 channels
+*/
+
+
+/// TODO: HAL in a lot af calls lookupTimerIndex is used. Instead of passing the timer instance the index should be passed.
+
+#define USED_TIMER_COUNT BITCOUNT(USED_TIMERS)
+#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
+
+#define TIM_IT_CCx(ch) (TIM_IT_CC1 << ((ch) / 4))
+
+typedef struct timerConfig_s {
+ timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
+ timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
+ timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
+ uint32_t forcedOverflowTimerValue;
+} timerConfig_t;
+timerConfig_t timerConfig[USED_TIMER_COUNT+1];
+
+typedef struct {
+ channelType_t type;
+} timerChannelInfo_t;
+timerChannelInfo_t timerChannelInfo[USABLE_TIMER_CHANNEL_COUNT];
+
+typedef struct {
+ uint8_t priority;
+} timerInfo_t;
+timerInfo_t timerInfo[USED_TIMER_COUNT+1];
+
+typedef struct
+{
+ TIM_HandleTypeDef Handle;
+} timerHandle_t;
+timerHandle_t timerHandle[USED_TIMER_COUNT+1];
+
+// return index of timer in timer table. Lowest timer has index 0
+#define TIMER_INDEX(i) BITCOUNT((TIM_N(i) - 1) & USED_TIMERS)
+
+static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
+{
+#define _CASE_SHF 10 // amount we can safely shift timer address to the right. gcc will throw error if some timers overlap
+#define _CASE_(tim, index) case ((unsigned)tim >> _CASE_SHF): return index; break
+#define _CASE(i) _CASE_(TIM##i##_BASE, TIMER_INDEX(i))
+
+// let gcc do the work, switch should be quite optimized
+ switch((unsigned)tim >> _CASE_SHF) {
+#if USED_TIMERS & TIM_N(1)
+ _CASE(1);
+#endif
+#if USED_TIMERS & TIM_N(2)
+ _CASE(2);
+#endif
+#if USED_TIMERS & TIM_N(3)
+ _CASE(3);
+#endif
+#if USED_TIMERS & TIM_N(4)
+ _CASE(4);
+#endif
+#if USED_TIMERS & TIM_N(5)
+ _CASE(5);
+#endif
+#if USED_TIMERS & TIM_N(6)
+ _CASE(6);
+#endif
+#if USED_TIMERS & TIM_N(7)
+ _CASE(7);
+#endif
+#if USED_TIMERS & TIM_N(8)
+ _CASE(8);
+#endif
+#if USED_TIMERS & TIM_N(9)
+ _CASE(9);
+#endif
+#if USED_TIMERS & TIM_N(10)
+ _CASE(10);
+#endif
+#if USED_TIMERS & TIM_N(11)
+ _CASE(11);
+#endif
+#if USED_TIMERS & TIM_N(12)
+ _CASE(12);
+#endif
+#if USED_TIMERS & TIM_N(13)
+ _CASE(13);
+#endif
+#if USED_TIMERS & TIM_N(14)
+ _CASE(14);
+#endif
+#if USED_TIMERS & TIM_N(15)
+ _CASE(15);
+#endif
+#if USED_TIMERS & TIM_N(16)
+ _CASE(16);
+#endif
+#if USED_TIMERS & TIM_N(17)
+ _CASE(17);
+#endif
+ default: return ~1; // make sure final index is out of range
+ }
+#undef _CASE
+#undef _CASE_
+}
+
+TIM_TypeDef * const usedTimers[USED_TIMER_COUNT] = {
+#define _DEF(i) TIM##i
+
+#if USED_TIMERS & TIM_N(1)
+ _DEF(1),
+#endif
+#if USED_TIMERS & TIM_N(2)
+ _DEF(2),
+#endif
+#if USED_TIMERS & TIM_N(3)
+ _DEF(3),
+#endif
+#if USED_TIMERS & TIM_N(4)
+ _DEF(4),
+#endif
+#if USED_TIMERS & TIM_N(5)
+ _DEF(5),
+#endif
+#if USED_TIMERS & TIM_N(6)
+ _DEF(6),
+#endif
+#if USED_TIMERS & TIM_N(7)
+ _DEF(7),
+#endif
+#if USED_TIMERS & TIM_N(8)
+ _DEF(8),
+#endif
+#if USED_TIMERS & TIM_N(9)
+ _DEF(9),
+#endif
+#if USED_TIMERS & TIM_N(10)
+ _DEF(10),
+#endif
+#if USED_TIMERS & TIM_N(11)
+ _DEF(11),
+#endif
+#if USED_TIMERS & TIM_N(12)
+ _DEF(12),
+#endif
+#if USED_TIMERS & TIM_N(13)
+ _DEF(13),
+#endif
+#if USED_TIMERS & TIM_N(14)
+ _DEF(14),
+#endif
+#if USED_TIMERS & TIM_N(15)
+ _DEF(15),
+#endif
+#if USED_TIMERS & TIM_N(16)
+ _DEF(16),
+#endif
+#if USED_TIMERS & TIM_N(17)
+ _DEF(17),
+#endif
+#undef _DEF
+};
+
+static inline uint8_t lookupChannelIndex(const uint16_t channel)
+{
+ return channel >> 2;
+}
+
+rccPeriphTag_t timerRCC(TIM_TypeDef *tim)
+{
+ for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+ if (timerDefinitions[i].TIMx == tim) {
+ return timerDefinitions[i].rcc;
+ }
+ }
+ return 0;
+}
+
+void timerNVICConfigure(uint8_t irq)
+{
+ HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ HAL_NVIC_EnableIRQ(irq);
+}
+
+TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT)
+ return NULL;
+
+ return &timerHandle[timerIndex].Handle;
+}
+
+void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ if(timerHandle[timerIndex].Handle.Instance == tim)
+ {
+ // already configured
+ return;
+ }
+
+ timerHandle[timerIndex].Handle.Instance = tim;
+
+ timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+ timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
+
+ timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000;
+
+ HAL_TIM_Base_Init(&timerHandle[timerIndex].Handle);
+ if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9)
+ {
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ if (HAL_TIM_ConfigClockSource(&timerHandle[timerIndex].Handle, &sClockSourceConfig) != HAL_OK)
+ {
+ return;
+ }
+ }
+ if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 )
+ {
+ TIM_MasterConfigTypeDef sMasterConfig;
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ if (HAL_TIMEx_MasterConfigSynchronization(&timerHandle[timerIndex].Handle, &sMasterConfig) != HAL_OK)
+ {
+ return;
+ }
+ }
+
+}
+
+// old interface for PWM inputs. It should be replaced
+void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
+{
+ uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ configTimeBase(timerHardwarePtr->tim, period, mhz);
+ HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
+ timerNVICConfigure(timerHardwarePtr->irq);
+ // HACK - enable second IRQ on timers that need it
+ switch(timerHardwarePtr->irq) {
+
+ case TIM1_CC_IRQn:
+ timerNVICConfigure(TIM1_UP_TIM10_IRQn);
+ break;
+ case TIM8_CC_IRQn:
+ timerNVICConfigure(TIM8_UP_TIM13_IRQn);
+ break;
+
+ }
+}
+
+// allocate and configure timer channel. Timer priority is set to highest priority of its channels
+void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ unsigned channel = timHw - timerHardware;
+ if(channel >= USABLE_TIMER_CHANNEL_COUNT)
+ return;
+
+ timerChannelInfo[channel].type = type;
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if(timer >= USED_TIMER_COUNT)
+ return;
+ if(irqPriority < timerInfo[timer].priority) {
+ // it would be better to set priority in the end, but current startup sequence is not ready
+ configTimeBase(usedTimers[timer], 0, 1);
+ HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
+
+
+ HAL_NVIC_SetPriority(timHw->irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+ HAL_NVIC_EnableIRQ(timHw->irq);
+
+ timerInfo[timer].priority = irqPriority;
+ }
+}
+
+void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn)
+{
+ self->fn = fn;
+}
+
+void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn)
+{
+ self->fn = fn;
+ self->next = NULL;
+}
+
+// update overflow callback list
+// some synchronization mechanism is neccesary to avoid disturbing other channels (BASEPRI used now)
+static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) {
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
+ if(cfg->overflowCallback[i]) {
+ *chain = cfg->overflowCallback[i];
+ chain = &cfg->overflowCallback[i]->next;
+ }
+ *chain = NULL;
+ }
+ // enable or disable IRQ
+ if(cfg->overflowCallbackActive)
+ __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE);
+ else
+ __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_UPDATE);
+}
+
+// config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive
+void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ uint8_t channelIndex = lookupChannelIndex(timHw->channel);
+ if(edgeCallback == NULL) // disable irq before changing callback to NULL
+ __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+ // setup callback info
+ timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback;
+ timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
+ // enable channel IRQ
+ if(edgeCallback)
+ __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
+}
+
+// configure callbacks for pair of channels (1+2 or 3+4).
+// Hi(2,4) and Lo(1,3) callbacks are specified, it is not important which timHw channel is used.
+// This is intended for dual capture mode (each channel handles one transition)
+void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_t *edgeCallbackLo, timerCCHandlerRec_t *edgeCallbackHi, timerOvrHandlerRec_t *overflowCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ uint16_t chLo = timHw->channel & ~TIM_CHANNEL_2; // lower channel
+ uint16_t chHi = timHw->channel | TIM_CHANNEL_2; // upper channel
+ uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel
+
+ if(edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL
+ __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
+ if(edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL
+ __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
+
+ // setup callback info
+ timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallbackLo;
+ timerConfig[timerIndex].edgeCallback[channelIndex + 1] = edgeCallbackHi;
+ timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
+ timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL;
+
+ // enable channel IRQs
+ if(edgeCallbackLo) {
+ __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
+ __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
+ }
+ if(edgeCallbackHi) {
+ __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
+ __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
+ }
+
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
+}
+
+// enable/disable IRQ for low channel in dual configuration
+//void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newState) {
+// TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel&~TIM_Channel_2), newState);
+//}
+// enable/disable IRQ for low channel in dual configuration
+void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newState) {
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ if(newState)
+ __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
+ else
+ __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
+}
+
+//// enable or disable IRQ
+//void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
+//{
+// TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), newState);
+//}
+// enable or disable IRQ
+void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ if(newState)
+ __HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+ else
+ __HAL_TIM_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+}
+
+// clear Compare/Capture flag for channel
+//void timerChClearCCFlag(const timerHardware_t *timHw)
+//{
+// TIM_ClearFlag(timHw->tim, TIM_IT_CCx(timHw->channel));
+//}
+// clear Compare/Capture flag for channel
+void timerChClearCCFlag(const timerHardware_t *timHw)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ __HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+}
+
+// configure timer channel GPIO mode
+void timerChConfigGPIO(const timerHardware_t* timHw, ioConfig_t mode)
+{
+ IOInit(IOGetByTag(timHw->tag), OWNER_TIMER, RESOURCE_TIMER, 0);
+ IOConfigGPIO(IOGetByTag(timHw->tag), mode);
+}
+
+// calculate input filter constant
+// TODO - we should probably setup DTS to higher value to allow reasonable input filtering
+// - notice that prescaler[0] does use DTS for sampling - the sequence won't be monotonous anymore
+static unsigned getFilter(unsigned ticks)
+{
+ static const unsigned ftab[16] = {
+ 1*1, // fDTS !
+ 1*2, 1*4, 1*8, // fCK_INT
+ 2*6, 2*8, // fDTS/2
+ 4*6, 4*8,
+ 8*6, 8*8,
+ 16*5, 16*6, 16*8,
+ 32*5, 32*6, 32*8
+ };
+ for(unsigned i = 1; i < ARRAYLEN(ftab); i++)
+ if(ftab[i] > ticks)
+ return i - 1;
+ return 0x0f;
+}
+
+// Configure input captupre
+void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
+{
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if(timer >= USED_TIMER_COUNT)
+ return;
+
+ TIM_IC_InitTypeDef TIM_ICInitStructure;
+
+ TIM_ICInitStructure.ICPolarity = polarityRising ? TIM_ICPOLARITY_RISING : TIM_ICPOLARITY_FALLING;
+ TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_DIRECTTI;
+ TIM_ICInitStructure.ICPrescaler = TIM_ICPSC_DIV1;
+ TIM_ICInitStructure.ICFilter = getFilter(inputFilterTicks);
+ HAL_TIM_IC_ConfigChannel(&timerHandle[timer].Handle,&TIM_ICInitStructure, timHw->channel);
+}
+
+// configure dual channel input channel for capture
+// polarity is for Low channel (capture order is always Lo - Hi)
+void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
+{
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if(timer >= USED_TIMER_COUNT)
+ return;
+
+ TIM_IC_InitTypeDef TIM_ICInitStructure;
+ bool directRising = (timHw->channel & TIM_CHANNEL_2) ? !polarityRising : polarityRising;
+
+ // configure direct channel
+ TIM_ICInitStructure.ICPolarity = directRising ? TIM_ICPOLARITY_RISING : TIM_ICPOLARITY_FALLING;
+ TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_DIRECTTI;
+ TIM_ICInitStructure.ICPrescaler = TIM_ICPSC_DIV1;
+ TIM_ICInitStructure.ICFilter = getFilter(inputFilterTicks);
+ HAL_TIM_IC_ConfigChannel(&timerHandle[timer].Handle,&TIM_ICInitStructure, timHw->channel);
+
+ // configure indirect channel
+ TIM_ICInitStructure.ICPolarity = directRising ? TIM_ICPOLARITY_FALLING : TIM_ICPOLARITY_RISING;
+ TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_INDIRECTTI;
+ HAL_TIM_IC_ConfigChannel(&timerHandle[timer].Handle,&TIM_ICInitStructure, timHw->channel ^ TIM_CHANNEL_2);
+}
+
+void timerChICPolarity(const timerHardware_t *timHw, bool polarityRising)
+{
+ timCCER_t tmpccer = timHw->tim->CCER;
+ tmpccer &= ~(TIM_CCER_CC1P << timHw->channel);
+ tmpccer |= polarityRising ? (TIM_ICPOLARITY_RISING << timHw->channel) : (TIM_ICPOLARITY_FALLING << timHw->channel);
+ timHw->tim->CCER = tmpccer;
+}
+
+volatile timCCR_t* timerChCCRHi(const timerHardware_t *timHw)
+{
+ return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + (timHw->channel | TIM_CHANNEL_2));
+}
+
+volatile timCCR_t* timerChCCRLo(const timerHardware_t *timHw)
+{
+ return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + (timHw->channel & ~TIM_CHANNEL_2));
+}
+
+
+
+volatile timCCR_t* timerChCCR(const timerHardware_t *timHw)
+{
+ return (volatile timCCR_t*)((volatile char*)&timHw->tim->CCR1 + timHw->channel);
+}
+
+void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh)
+{
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if(timer >= USED_TIMER_COUNT)
+ return;
+
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
+
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_INACTIVE;
+ TIM_OCInitStructure.Pulse = 0x00000000;
+ TIM_OCInitStructure.OCPolarity = stateHigh ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW;
+ TIM_OCInitStructure.OCNPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
+ TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+
+ HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
+
+ if(outEnable) {
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_INACTIVE;
+ HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
+ HAL_TIM_OC_Start(&timerHandle[timer].Handle, timHw->channel);
+ } else {
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+ HAL_TIM_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
+ HAL_TIM_OC_Start_IT(&timerHandle[timer].Handle, timHw->channel);
+ }
+}
+
+static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
+{
+ uint16_t capture;
+ unsigned tim_status;
+ tim_status = tim->SR & tim->DIER;
+#if 1
+ while(tim_status) {
+ // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
+ // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
+ unsigned bit = __builtin_clz(tim_status);
+ unsigned mask = ~(0x80000000 >> bit);
+ tim->SR = mask;
+ tim_status &= mask;
+ switch(bit) {
+ case __builtin_clz(TIM_IT_UPDATE): {
+
+ if(timerConfig->forcedOverflowTimerValue != 0){
+ capture = timerConfig->forcedOverflowTimerValue - 1;
+ timerConfig->forcedOverflowTimerValue = 0;
+ } else {
+ capture = tim->ARR;
+ }
+
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while(cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ break;
+ }
+ case __builtin_clz(TIM_IT_CC1):
+ timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1);
+ break;
+ case __builtin_clz(TIM_IT_CC2):
+ timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2);
+ break;
+ case __builtin_clz(TIM_IT_CC3):
+ timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3);
+ break;
+ case __builtin_clz(TIM_IT_CC4):
+ timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4);
+ break;
+ }
+ }
+#else
+ if (tim_status & (int)TIM_IT_Update) {
+ tim->SR = ~TIM_IT_Update;
+ capture = tim->ARR;
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while(cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ }
+ if (tim_status & (int)TIM_IT_CC1) {
+ tim->SR = ~TIM_IT_CC1;
+ timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1);
+ }
+ if (tim_status & (int)TIM_IT_CC2) {
+ tim->SR = ~TIM_IT_CC2;
+ timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2);
+ }
+ if (tim_status & (int)TIM_IT_CC3) {
+ tim->SR = ~TIM_IT_CC3;
+ timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3);
+ }
+ if (tim_status & (int)TIM_IT_CC4) {
+ tim->SR = ~TIM_IT_CC4;
+ timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4);
+ }
+#endif
+}
+
+// handler for shared interrupts when both timers need to check status bits
+#define _TIM_IRQ_HANDLER2(name, i, j) \
+ void name(void) \
+ { \
+ timCCxHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]); \
+ timCCxHandler(TIM ## j, &timerConfig[TIMER_INDEX(j)]); \
+ } struct dummy
+
+#define _TIM_IRQ_HANDLER(name, i) \
+ void name(void) \
+ { \
+ timCCxHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]); \
+ } struct dummy
+
+#if USED_TIMERS & TIM_N(1)
+_TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
+# if USED_TIMERS & TIM_N(10)
+_TIM_IRQ_HANDLER2(TIM1_UP_TIM10_IRQHandler, 1, 10); // both timers are in use
+# else
+_TIM_IRQ_HANDLER(TIM1_UP_TIM10_IRQHandler, 1); // timer10 is not used
+# endif
+#endif
+
+#if USED_TIMERS & TIM_N(2)
+_TIM_IRQ_HANDLER(TIM2_IRQHandler, 2);
+#endif
+#if USED_TIMERS & TIM_N(3)
+_TIM_IRQ_HANDLER(TIM3_IRQHandler, 3);
+#endif
+#if USED_TIMERS & TIM_N(4)
+_TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
+#endif
+#if USED_TIMERS & TIM_N(5)
+_TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
+#endif
+
+#if USED_TIMERS & TIM_N(8)
+_TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
+
+# if USED_TIMERS & TIM_N(13)
+_TIM_IRQ_HANDLER2(TIM8_UP_TIM13_IRQHandler, 8, 13); // both timers are in use
+# else
+_TIM_IRQ_HANDLER(TIM8_UP_TIM13_IRQHandler, 8); // timer13 is not used
+# endif
+#endif
+
+#if USED_TIMERS & TIM_N(9)
+_TIM_IRQ_HANDLER(TIM1_BRK_TIM9_IRQHandler, 9);
+#endif
+# if USED_TIMERS & TIM_N(11)
+_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM11_IRQHandler, 11);
+# endif
+#if USED_TIMERS & TIM_N(12)
+_TIM_IRQ_HANDLER(TIM8_BRK_TIM12_IRQHandler, 12);
+#endif
+#if USED_TIMERS & TIM_N(15)
+_TIM_IRQ_HANDLER(TIM1_BRK_TIM15_IRQHandler, 15);
+#endif
+#if defined(STM32F303xC) && ((USED_TIMERS & (TIM_N(1)|TIM_N(16))) == (TIM_N(16)))
+_TIM_IRQ_HANDLER(TIM1_UP_TIM16_IRQHandler, 16); // only timer16 is used, not timer1
+#endif
+#if USED_TIMERS & TIM_N(17)
+_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM17_IRQHandler, 17);
+#endif
+
+void timerInit(void)
+{
+ memset(timerConfig, 0, sizeof (timerConfig));
+
+
+#if USED_TIMERS & TIM_N(1)
+ __HAL_RCC_TIM1_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(2)
+ __HAL_RCC_TIM2_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(3)
+ __HAL_RCC_TIM3_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(4)
+ __HAL_RCC_TIM4_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(5)
+ __HAL_RCC_TIM5_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(6)
+ __HAL_RCC_TIM6_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(7)
+ __HAL_RCC_TIM7_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(8)
+ __HAL_RCC_TIM8_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(9)
+ __HAL_RCC_TIM9_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(10)
+ __HAL_RCC_TIM10_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(11)
+ __HAL_RCC_TIM11_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(12)
+ __HAL_RCC_TIM12_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(13)
+ __HAL_RCC_TIM13_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(14)
+ __HAL_RCC_TIM14_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(15)
+ __HAL_RCC_TIM15_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(16)
+ __HAL_RCC_TIM16_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(17)
+ __HAL_RCC_TIM17_CLK_ENABLE();
+#endif
+
+ /* enable the timer peripherals */
+ for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE);
+ }
+
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
+ for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) {
+ const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex];
+ IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction);
+ }
+#endif
+
+ // initialize timer channel structures
+ for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ timerChannelInfo[i].type = TYPE_FREE;
+ }
+ for(int i = 0; i < USED_TIMER_COUNT; i++) {
+ timerInfo[i].priority = ~0;
+ }
+}
+
+// finish configuring timers after allocation phase
+// start timers
+// TODO - Work in progress - initialization routine must be modified/verified to start correctly without timers
+void timerStart(void)
+{
+#if 0
+ for(unsigned timer = 0; timer < USED_TIMER_COUNT; timer++) {
+ int priority = -1;
+ int irq = -1;
+ for(unsigned hwc = 0; hwc < USABLE_TIMER_CHANNEL_COUNT; hwc++)
+ if((timerChannelInfo[hwc].type != TYPE_FREE) && (timerHardware[hwc].tim == usedTimers[timer])) {
+ // TODO - move IRQ to timer info
+ irq = timerHardware[hwc].irq;
+ }
+ // TODO - aggregate required timer paramaters
+ configTimeBase(usedTimers[timer], 0, 1);
+ TIM_Cmd(usedTimers[timer], ENABLE);
+ if(priority >= 0) { // maybe none of the channels was configured
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ NVIC_InitStructure.NVIC_IRQChannel = irq;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SPLIT_PRIORITY_BASE(priority);
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_SPLIT_PRIORITY_SUB(priority);
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ }
+ }
+#endif
+}
+
+/**
+ * Force an overflow for a given timer.
+ * Saves the current value of the counter in the relevant timerConfig's forcedOverflowTimerValue variable.
+ * @param TIM_Typedef *tim The timer to overflow
+ * @return void
+ **/
+void timerForceOverflow(TIM_TypeDef *tim)
+{
+ uint8_t timerIndex = lookupTimerIndex((const TIM_TypeDef *)tim);
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ // Save the current count so that PPM reading will work on the same timer that was forced to overflow
+ timerConfig[timerIndex].forcedOverflowTimerValue = tim->CNT + 1;
+
+ // Force an overflow by setting the UG bit
+ tim->EGR |= TIM_EGR_UG;
+ }
+}
+
+const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag)
+{
+ for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+ if (timerHardware[i].tag == tag) {
+ if (flag && (timerHardware[i].output & flag) == flag) {
+ return &timerHardware[i];
+ } else if (!flag && timerHardware[i].output == flag) {
+ // TODO: shift flag by one so not to be 0
+ return &timerHardware[i];
+ }
+ }
+ }
+ return NULL;
+}
diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c
index be299b1168..da13ae77f3 100644
--- a/src/main/drivers/timer_stm32f4xx.c
+++ b/src/main/drivers/timer_stm32f4xx.c
@@ -136,4 +136,3 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
}
-
diff --git a/src/main/drivers/timer_stm32f4xx.h b/src/main/drivers/timer_stm32f4xx.h
index 52094cbc03..4f99107858 100644
--- a/src/main/drivers/timer_stm32f4xx.h
+++ b/src/main/drivers/timer_stm32f4xx.h
@@ -3,4 +3,4 @@
#include "stm32f4xx.h"
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);
\ No newline at end of file
diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c
new file mode 100644
index 0000000000..180116c0fd
--- /dev/null
+++ b/src/main/drivers/timer_stm32f7xx.c
@@ -0,0 +1,132 @@
+/*
+ modified version of StdPeriph function is located here.
+ TODO - what license does apply here?
+ original file was lincesed under MCD-ST Liberty SW License Agreement V2
+ http://www.st.com/software_license_agreement_liberty_v2
+*/
+
+#include
+#include
+
+#include "platform.h"
+
+#include "common/utils.h"
+
+#include "stm32f7xx.h"
+#include "rcc.h"
+#include "timer.h"
+
+/**
+ * @brief Selects the TIM Output Compare Mode.
+ * @note This function does NOT disable the selected channel before changing the Output
+ * Compare Mode.
+ * @param TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
+ * @param TIM_Channel: specifies the TIM Channel
+ * This parameter can be one of the following values:
+ * @arg TIM_Channel_1: TIM Channel 1
+ * @arg TIM_Channel_2: TIM Channel 2
+ * @arg TIM_Channel_3: TIM Channel 3
+ * @arg TIM_Channel_4: TIM Channel 4
+ * @param TIM_OCMode: specifies the TIM Output Compare Mode.
+ * This parameter can be one of the following values:
+ * @arg TIM_OCMode_Timing
+ * @arg TIM_OCMode_Active
+ * @arg TIM_OCMode_Toggle
+ * @arg TIM_OCMode_PWM1
+ * @arg TIM_OCMode_PWM2
+ * @arg TIM_ForcedAction_Active
+ * @arg TIM_ForcedAction_InActive
+ * @retval None
+ */
+
+#define CCMR_Offset ((uint16_t)0x0018)
+
+const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
+ { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) },
+ { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) },
+ { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) },
+ { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) },
+ { .TIMx = TIM5, .rcc = RCC_APB1(TIM5) },
+ { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) },
+ { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) },
+ { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) },
+ { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) },
+ { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) },
+ { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) },
+ { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) },
+ { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) },
+ { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) },
+};
+
+/*
+ need a mapping from dma and timers to pins, and the values should all be set here to the dmaMotors array.
+ this mapping could be used for both these motors and for led strip.
+
+ only certain pins have OC output (already used in normal PWM et al) but then
+ there are only certain DMA streams/channels available for certain timers and channels.
+ *** (this may highlight some hardware limitations on some targets) ***
+
+ DMA1
+
+ Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
+ 0
+ 1
+ 2 TIM4_CH1 TIM4_CH2 TIM4_CH3
+ 3 TIM2_CH3 TIM2_CH1 TIM2_CH1 TIM2_CH4
+ TIM2_CH4
+ 4
+ 5 TIM3_CH4 TIM3_CH1 TIM3_CH2 TIM3_CH3
+ 6 TIM5_CH3 TIM5_CH4 TIM5_CH1 TIM5_CH4 TIM5_CH2
+ 7
+
+ DMA2
+
+ Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
+ 0 TIM8_CH1 TIM1_CH1
+ TIM8_CH2 TIM1_CH2
+ TIM8_CH3 TIM1_CH3
+ 1
+ 2
+ 3
+ 4
+ 5
+ 6 TIM1_CH1 TIM1_CH2 TIM1_CH1 TIM1_CH4 TIM1_CH3
+ 7 TIM8_CH1 TIM8_CH2 TIM8_CH3 TIM8_CH4
+*/
+
+uint8_t timerClockDivisor(TIM_TypeDef *tim)
+{
+ return 1;
+}
+
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
+{
+ uint32_t tmp = 0;
+
+ /* Check the parameters */
+ assert_param(IS_TIM_LIST8_PERIPH(TIMx));
+ assert_param(IS_TIM_CHANNEL(TIM_Channel));
+ assert_param(IS_TIM_OCM(TIM_OCMode));
+
+ tmp = (uint32_t) TIMx;
+ tmp += CCMR_Offset;
+
+ if((TIM_Channel == TIM_CHANNEL_1) ||(TIM_Channel == TIM_CHANNEL_3)) {
+ tmp += (TIM_Channel>>1);
+
+ /* Reset the OCxM bits in the CCMRx register */
+ *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
+
+ /* Configure the OCxM bits in the CCMRx register */
+ *(__IO uint32_t *) tmp |= TIM_OCMode;
+ } else {
+ tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
+
+ /* Reset the OCxM bits in the CCMRx register */
+ *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
+
+ /* Configure the OCxM bits in the CCMRx register */
+ *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
+ }
+}
+
diff --git a/src/main/drivers/timer_stm32f7xx.h b/src/main/drivers/timer_stm32f7xx.h
new file mode 100644
index 0000000000..286291aab8
--- /dev/null
+++ b/src/main/drivers/timer_stm32f7xx.h
@@ -0,0 +1,6 @@
+
+#pragma once
+
+#include "stm32f7xx.h"
+
+void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 015a603394..6eb8c50092 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -449,7 +449,7 @@ uint16_t getCurrentMinthrottle(void)
return masterConfig.motorConfig.minthrottle;
}
-// Default settings
+
void createDefaultConfig(master_t *config)
{
// Clear all configuration
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index db9043f439..d3b2c92e16 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -23,15 +23,16 @@
#include "platform.h"
+#include "blackbox/blackbox.h"
+
#include "build/build_config.h"
#include "build/debug.h"
#include "build/version.h"
-#include "blackbox/blackbox.h"
-
#include "common/axis.h"
#include "common/color.h"
#include "common/maths.h"
+#include "common/streambuf.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
@@ -70,8 +71,8 @@
#include "io/serial_4way.h"
//#include "io/vtx.h"
-#include "msp/msp_protocol.h"
#include "msp/msp.h"
+#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "rx/rx.h"
@@ -121,9 +122,6 @@ extern serialPort_t *debugSerialPort;
extern uint16_t cycleTime; // FIXME dependency on mw.c
extern void resetProfile(profile_t *profile);
-// cause reboot after MSP processing complete
-static mspPort_t *currentPort;
-
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
@@ -197,10 +195,6 @@ typedef enum {
#define RATEPROFILE_MASK (1 << 7)
-#define JUMBO_FRAME_SIZE_LIMIT 255
-
-#define DATAFLASH_BUFFER_SIZE 4096
-
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
static void msp4WayIfFn(serialPort_t *serialPort)
{
@@ -224,90 +218,11 @@ static void mspRebootFn(serialPort_t *serialPort)
while (true) ;
}
-static void serialize8(uint8_t a)
-{
- bufWriterAppend(writer, a);
- currentPort->checksum ^= a;
-}
-
-static void serialize16(uint16_t a)
-{
- serialize8((uint8_t)(a >> 0));
- serialize8((uint8_t)(a >> 8));
-}
-
-static void serialize32(uint32_t a)
-{
- serialize16((uint16_t)(a >> 0));
- serialize16((uint16_t)(a >> 16));
-}
-
-static uint8_t read8(void)
-{
- return currentPort->inBuf[currentPort->indRX++] & 0xff;
-}
-
-static uint16_t read16(void)
-{
- uint16_t t = read8();
- t += (uint16_t)read8() << 8;
- return t;
-}
-
-static uint32_t read32(void)
-{
- uint32_t t = read16();
- t += (uint32_t)read16() << 16;
- return t;
-}
-
-static void headSerialResponse(uint8_t err, uint16_t responseBodySize)
-{
- serialBeginWrite(currentPort->port);
-
- serialize8('$');
- serialize8('M');
- serialize8(err ? '!' : '>');
- currentPort->checksum = 0; // start calculating a new checksum
- if (responseBodySize < JUMBO_FRAME_SIZE_LIMIT) {
- serialize8(responseBodySize);
- } else {
- serialize8(JUMBO_FRAME_SIZE_LIMIT);
- }
- serialize8(currentPort->cmdMSP);
- if (responseBodySize >= JUMBO_FRAME_SIZE_LIMIT) {
- serialize16(responseBodySize);
- }
-}
-
-static void headSerialReply(uint16_t responseBodySize)
-{
- headSerialResponse(0, responseBodySize);
-}
-
-static void headSerialError(uint8_t responseBodySize)
-{
- headSerialResponse(1, responseBodySize);
-}
-
-static void tailSerialReply(void)
-{
- serialize8(currentPort->checksum);
- serialEndWrite(currentPort->port);
-}
-
-static void s_struct(uint8_t *cb, uint8_t siz)
-{
- headSerialReply(siz);
- while (siz--)
- serialize8(*cb++);
-}
-
-static void serializeNames(const char *s)
+static void serializeNames(sbuf_t *dst, const char *s)
{
const char *c;
for (c = s; *c; c++)
- serialize8(*c);
+ sbufWriteU8(dst, *c);
}
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
@@ -336,7 +251,7 @@ static const box_t *findBoxByPermenantId(uint8_t permenantId)
return NULL;
}
-static void serializeBoxNamesReply(void)
+static void serializeBoxNamesReply(sbuf_t *dst)
{
int i, activeBoxId, j, flag = 1, count = 0, len;
const box_t *box;
@@ -357,26 +272,23 @@ reset:
count += len;
} else {
for (j = 0; j < len; j++)
- serialize8(box->boxName[j]);
+ sbufWriteU8(dst, box->boxName[j]);
}
}
if (flag) {
- headSerialReply(count);
flag = 0;
goto reset;
}
}
-static void serializeSDCardSummaryReply(void)
+static void serializeSDCardSummaryReply(sbuf_t *dst)
{
- headSerialReply(3 + 4 + 4);
-
#ifdef USE_SDCARD
uint8_t flags = 1 /* SD card supported */ ;
uint8_t state = 0;
- serialize8(flags);
+ sbufWriteU8(dst, flags);
// Merge the card and filesystem states together
if (!sdcard_isInserted()) {
@@ -402,71 +314,68 @@ static void serializeSDCardSummaryReply(void)
}
}
- serialize8(state);
- serialize8(afatfs_getLastError());
+ sbufWriteU8(dst, state);
+ sbufWriteU8(dst, afatfs_getLastError());
// Write free space and total space in kilobytes
- serialize32(afatfs_getContiguousFreeSpace() / 1024);
- serialize32(sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
+ sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024);
+ sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
#else
- serialize8(0);
- serialize8(0);
- serialize8(0);
- serialize32(0);
- serialize32(0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU32(dst, 0);
+ sbufWriteU32(dst, 0);
#endif
}
-static void serializeDataflashSummaryReply(void)
+static void serializeDataflashSummaryReply(sbuf_t *dst)
{
- headSerialReply(1 + 3 * 4);
#ifdef USE_FLASHFS
const flashGeometry_t *geometry = flashfsGetGeometry();
uint8_t flags = (flashfsIsReady() ? 1 : 0) | 2 /* FlashFS is supported */;
- serialize8(flags);
- serialize32(geometry->sectors);
- serialize32(geometry->totalSize);
- serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
+ sbufWriteU8(dst, flags);
+ sbufWriteU32(dst, geometry->sectors);
+ sbufWriteU32(dst, geometry->totalSize);
+ sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
#else
- serialize8(0); // FlashFS is neither ready nor supported
- serialize32(0);
- serialize32(0);
- serialize32(0);
+ sbufWriteU8(dst, 0); // FlashFS is neither ready nor supported
+ sbufWriteU32(dst, 0);
+ sbufWriteU32(dst, 0);
+ sbufWriteU32(dst, 0);
#endif
}
#ifdef USE_FLASHFS
-static void serializeDataflashReadReply(uint32_t address, uint16_t size, bool useLegacyFormat)
+static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat)
{
- static uint8_t buffer[DATAFLASH_BUFFER_SIZE];
+ BUILD_BUG_ON(MSP_PORT_DATAFLASH_INFO_SIZE < 16);
- if (size > sizeof(buffer)) {
- size = sizeof(buffer);
+ uint16_t readLen = size;
+ const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE;
+ if (readLen > bytesRemainingInBuf) {
+ readLen = bytesRemainingInBuf;
+ }
+ // size will be lower than that requested if we reach end of volume
+ if (readLen > flashfsGetSize() - address) {
+ // truncate the request
+ readLen = flashfsGetSize() - address;
+ }
+ sbufWriteU32(dst, address);
+ if (!useLegacyFormat) {
+ // new format supports variable read lengths
+ sbufWriteU16(dst, readLen);
+ sbufWriteU8(dst, 0); // placeholder for compression format
}
- // bytesRead will be lower than that requested if we reach end of volume
- int bytesRead = flashfsReadAbs(address, buffer, size);
+ // bytesRead will equal readLen
+ const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
+ sbufAdvance(dst, bytesRead);
if (useLegacyFormat) {
- headSerialReply(sizeof(uint32_t) + size);
-
- serialize32(address);
- } else {
- headSerialReply(sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint8_t) + bytesRead);
-
- serialize32(address);
- serialize16(bytesRead);
- serialize8(0); // placeholder for compression format
- }
-
- int i;
- for (i = 0; i < bytesRead; i++) {
- serialize8(buffer[i]);
- }
-
- if (useLegacyFormat) {
- for (; i < size; i++) {
- serialize8(0);
+ // pad the buffer with zeros
+ for (int i = bytesRead; i < size; i++) {
+ sbufWriteU8(dst, 0);
}
}
}
@@ -617,704 +526,580 @@ static uint32_t packFlightModeFlags(void)
return junk;
}
-static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProcessFn)
+static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
uint32_t i;
-#ifdef USE_FLASHFS
- const unsigned int dataSize = currentPort->dataSize;
-#endif
-#ifdef GPS
- uint8_t wp_no;
- int32_t lat = 0, lon = 0;
-#endif
switch (cmdMSP) {
case MSP_API_VERSION:
- headSerialReply(
- 1 + // protocol version length
- API_VERSION_LENGTH
- );
- serialize8(MSP_PROTOCOL_VERSION);
-
- serialize8(API_VERSION_MAJOR);
- serialize8(API_VERSION_MINOR);
+ sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
+ sbufWriteU8(dst, API_VERSION_MAJOR);
+ sbufWriteU8(dst, API_VERSION_MINOR);
break;
case MSP_FC_VARIANT:
- headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
-
for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
- serialize8(flightControllerIdentifier[i]);
+ sbufWriteU8(dst, flightControllerIdentifier[i]);
}
break;
case MSP_FC_VERSION:
- headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);
-
- serialize8(FC_VERSION_MAJOR);
- serialize8(FC_VERSION_MINOR);
- serialize8(FC_VERSION_PATCH_LEVEL);
+ sbufWriteU8(dst, FC_VERSION_MAJOR);
+ sbufWriteU8(dst, FC_VERSION_MINOR);
+ sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
break;
case MSP_BOARD_INFO:
- headSerialReply(
- BOARD_IDENTIFIER_LENGTH +
- BOARD_HARDWARE_REVISION_LENGTH
- );
for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
- serialize8(boardIdentifier[i]);
+ sbufWriteU8(dst, boardIdentifier[i]);
}
#ifdef USE_HARDWARE_REVISION_DETECTION
- serialize16(hardwareRevision);
+ sbufWriteU16(dst, hardwareRevision);
#else
- serialize16(0); // No other build targets currently have hardware revision detection.
+ sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
#endif
break;
case MSP_BUILD_INFO:
- headSerialReply(
- BUILD_DATE_LENGTH +
- BUILD_TIME_LENGTH +
- GIT_SHORT_REVISION_LENGTH
- );
-
for (i = 0; i < BUILD_DATE_LENGTH; i++) {
- serialize8(buildDate[i]);
+ sbufWriteU8(dst, buildDate[i]);
}
for (i = 0; i < BUILD_TIME_LENGTH; i++) {
- serialize8(buildTime[i]);
+ sbufWriteU8(dst, buildTime[i]);
}
for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
- serialize8(shortGitRevision[i]);
+ sbufWriteU8(dst, shortGitRevision[i]);
}
break;
// DEPRECATED - Use MSP_API_VERSION
case MSP_IDENT:
- headSerialReply(7);
- serialize8(MW_VERSION);
- serialize8(masterConfig.mixerMode);
- serialize8(MSP_PROTOCOL_VERSION);
- serialize32(CAP_DYNBALANCE); // "capability"
+ sbufWriteU8(dst, MW_VERSION);
+ sbufWriteU8(dst, masterConfig.mixerMode);
+ sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
+ sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
break;
case MSP_STATUS_EX:
- headSerialReply(15);
- serialize16(cycleTime);
+ sbufWriteU16(dst, cycleTime);
#ifdef USE_I2C
- serialize16(i2cGetErrorCounter());
+ sbufWriteU16(dst, i2cGetErrorCounter());
#else
- serialize16(0);
+ sbufWriteU16(dst, 0);
#endif
- serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
- serialize32(packFlightModeFlags());
- serialize8(getCurrentProfile());
- serialize16(constrain(averageSystemLoadPercent, 0, 100));
- serialize8(MAX_PROFILE_COUNT);
- serialize8(getCurrentControlRateProfile());
+ sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
+ sbufWriteU32(dst, packFlightModeFlags());
+ sbufWriteU8(dst, getCurrentProfile());
+ sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
+ sbufWriteU8(dst, MAX_PROFILE_COUNT);
+ sbufWriteU8(dst, getCurrentControlRateProfile());
break;
case MSP_NAME:
{
const unsigned int nameLen = strlen(masterConfig.name);
- headSerialReply(nameLen);
for (i = 0; i < nameLen; i++) {
- serialize8(masterConfig.name[i]);
+ sbufWriteU8(dst, masterConfig.name[i]);
}
}
break;
case MSP_STATUS:
- headSerialReply(11);
- serialize16(cycleTime);
+ sbufWriteU16(dst, cycleTime);
#ifdef USE_I2C
- serialize16(i2cGetErrorCounter());
+ sbufWriteU16(dst, i2cGetErrorCounter());
#else
- serialize16(0);
+ sbufWriteU16(dst, 0);
#endif
- serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
- serialize32(packFlightModeFlags());
- serialize8(masterConfig.current_profile_index);
+ sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
+ sbufWriteU32(dst, packFlightModeFlags());
+ sbufWriteU8(dst, masterConfig.current_profile_index);
break;
case MSP_RAW_IMU:
- headSerialReply(18);
-
- // Hack scale due to choice of units for sensor data in multiwii
- const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1;
-
- for (i = 0; i < 3; i++)
- serialize16(accSmooth[i] / scale);
- for (i = 0; i < 3; i++)
- serialize16(gyroADC[i]);
- for (i = 0; i < 3; i++)
- serialize16(magADC[i]);
+ {
+ // Hack scale due to choice of units for sensor data in multiwii
+ const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1;
+ for (i = 0; i < 3; i++)
+ sbufWriteU16(dst, accSmooth[i] / scale);
+ for (i = 0; i < 3; i++)
+ sbufWriteU16(dst, gyroADC[i]);
+ for (i = 0; i < 3; i++)
+ sbufWriteU16(dst, magADC[i]);
+ }
break;
#ifdef USE_SERVOS
case MSP_SERVO:
- s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
+ sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
break;
case MSP_SERVO_CONFIGURATIONS:
- headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t));
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
- serialize16(masterConfig.servoConf[i].min);
- serialize16(masterConfig.servoConf[i].max);
- serialize16(masterConfig.servoConf[i].middle);
- serialize8(masterConfig.servoConf[i].rate);
- serialize8(masterConfig.servoConf[i].angleAtMin);
- serialize8(masterConfig.servoConf[i].angleAtMax);
- serialize8(masterConfig.servoConf[i].forwardFromChannel);
- serialize32(masterConfig.servoConf[i].reversedSources);
+ sbufWriteU16(dst, masterConfig.servoConf[i].min);
+ sbufWriteU16(dst, masterConfig.servoConf[i].max);
+ sbufWriteU16(dst, masterConfig.servoConf[i].middle);
+ sbufWriteU8(dst, masterConfig.servoConf[i].rate);
+ sbufWriteU8(dst, masterConfig.servoConf[i].angleAtMin);
+ sbufWriteU8(dst, masterConfig.servoConf[i].angleAtMax);
+ sbufWriteU8(dst, masterConfig.servoConf[i].forwardFromChannel);
+ sbufWriteU32(dst, masterConfig.servoConf[i].reversedSources);
}
break;
case MSP_SERVO_MIX_RULES:
- headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t));
for (i = 0; i < MAX_SERVO_RULES; i++) {
- serialize8(masterConfig.customServoMixer[i].targetChannel);
- serialize8(masterConfig.customServoMixer[i].inputSource);
- serialize8(masterConfig.customServoMixer[i].rate);
- serialize8(masterConfig.customServoMixer[i].speed);
- serialize8(masterConfig.customServoMixer[i].min);
- serialize8(masterConfig.customServoMixer[i].max);
- serialize8(masterConfig.customServoMixer[i].box);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].targetChannel);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].inputSource);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].rate);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].speed);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].min);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].max);
+ sbufWriteU8(dst, masterConfig.customServoMixer[i].box);
}
break;
#endif
case MSP_MOTOR:
- s_struct((uint8_t *)motor, 16);
+ for (unsigned i = 0; i < 8; i++) {
+ if (i >= MAX_SUPPORTED_MOTORS || !pwmGetMotors()[i].enabled) {
+ sbufWriteU16(dst, 0);
+ continue;
+ }
+ if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_DSHOT150 || masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_DSHOT600)
+ sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator
+ else
+ sbufWriteU16(dst, motor[i]);
+ }
break;
case MSP_RC:
- headSerialReply(2 * rxRuntimeConfig.channelCount);
for (i = 0; i < rxRuntimeConfig.channelCount; i++)
- serialize16(rcData[i]);
+ sbufWriteU16(dst, rcData[i]);
break;
case MSP_ATTITUDE:
- headSerialReply(6);
- serialize16(attitude.values.roll);
- serialize16(attitude.values.pitch);
- serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+ sbufWriteU16(dst, attitude.values.roll);
+ sbufWriteU16(dst, attitude.values.pitch);
+ sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
break;
case MSP_ALTITUDE:
- headSerialReply(6);
#if defined(BARO) || defined(SONAR)
- serialize32(altitudeHoldGetEstimatedAltitude());
+ sbufWriteU32(dst, altitudeHoldGetEstimatedAltitude());
#else
- serialize32(0);
+ sbufWriteU32(dst, 0);
#endif
- serialize16(vario);
+ sbufWriteU16(dst, vario);
break;
case MSP_SONAR_ALTITUDE:
- headSerialReply(4);
#if defined(SONAR)
- serialize32(sonarGetLatestAltitude());
+ sbufWriteU32(dst, sonarGetLatestAltitude());
#else
- serialize32(0);
+ sbufWriteU32(dst, 0);
#endif
break;
case MSP_ANALOG:
- headSerialReply(7);
- serialize8((uint8_t)constrain(vbat, 0, 255));
- serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
- serialize16(rssi);
+ sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
+ sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
+ sbufWriteU16(dst, rssi);
if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
- serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
+ sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
} else
- serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
+ sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break;
case MSP_ARMING_CONFIG:
- headSerialReply(2);
- serialize8(masterConfig.auto_disarm_delay);
- serialize8(masterConfig.disarm_kill_switch);
+ sbufWriteU8(dst, masterConfig.auto_disarm_delay);
+ sbufWriteU8(dst, masterConfig.disarm_kill_switch);
break;
case MSP_LOOP_TIME:
- headSerialReply(2);
- serialize16((uint16_t)gyro.targetLooptime);
+ sbufWriteU16(dst, (uint16_t)gyro.targetLooptime);
break;
case MSP_RC_TUNING:
- headSerialReply(12);
- serialize8(currentControlRateProfile->rcRate8);
- serialize8(currentControlRateProfile->rcExpo8);
+ sbufWriteU8(dst, currentControlRateProfile->rcRate8);
+ sbufWriteU8(dst, currentControlRateProfile->rcExpo8);
for (i = 0 ; i < 3; i++) {
- serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
+ sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
}
- serialize8(currentControlRateProfile->dynThrPID);
- serialize8(currentControlRateProfile->thrMid8);
- serialize8(currentControlRateProfile->thrExpo8);
- serialize16(currentControlRateProfile->tpa_breakpoint);
- serialize8(currentControlRateProfile->rcYawExpo8);
- serialize8(currentControlRateProfile->rcYawRate8);
+ sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
+ sbufWriteU8(dst, currentControlRateProfile->thrMid8);
+ sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
+ sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
+ sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8);
+ sbufWriteU8(dst, currentControlRateProfile->rcYawRate8);
break;
case MSP_PID:
- headSerialReply(3 * PID_ITEM_COUNT);
for (i = 0; i < PID_ITEM_COUNT; i++) {
- serialize8(currentProfile->pidProfile.P8[i]);
- serialize8(currentProfile->pidProfile.I8[i]);
- serialize8(currentProfile->pidProfile.D8[i]);
+ sbufWriteU8(dst, currentProfile->pidProfile.P8[i]);
+ sbufWriteU8(dst, currentProfile->pidProfile.I8[i]);
+ sbufWriteU8(dst, currentProfile->pidProfile.D8[i]);
}
break;
case MSP_PIDNAMES:
- headSerialReply(sizeof(pidnames) - 1);
- serializeNames(pidnames);
+ serializeNames(dst, pidnames);
break;
case MSP_PID_CONTROLLER:
- headSerialReply(1);
- serialize8(PID_CONTROLLER_BETAFLIGHT); // Needs Cleanup in the future
+ sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
break;
case MSP_MODE_RANGES:
- headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i];
const box_t *box = &boxes[mac->modeId];
- serialize8(box->permanentId);
- serialize8(mac->auxChannelIndex);
- serialize8(mac->range.startStep);
- serialize8(mac->range.endStep);
+ sbufWriteU8(dst, box->permanentId);
+ sbufWriteU8(dst, mac->auxChannelIndex);
+ sbufWriteU8(dst, mac->range.startStep);
+ sbufWriteU8(dst, mac->range.endStep);
}
break;
case MSP_ADJUSTMENT_RANGES:
- headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT * (
- 1 + // adjustment index/slot
- 1 + // aux channel index
- 1 + // start step
- 1 + // end step
- 1 + // adjustment function
- 1 // aux switch channel index
- ));
for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i];
- serialize8(adjRange->adjustmentIndex);
- serialize8(adjRange->auxChannelIndex);
- serialize8(adjRange->range.startStep);
- serialize8(adjRange->range.endStep);
- serialize8(adjRange->adjustmentFunction);
- serialize8(adjRange->auxSwitchChannelIndex);
+ sbufWriteU8(dst, adjRange->adjustmentIndex);
+ sbufWriteU8(dst, adjRange->auxChannelIndex);
+ sbufWriteU8(dst, adjRange->range.startStep);
+ sbufWriteU8(dst, adjRange->range.endStep);
+ sbufWriteU8(dst, adjRange->adjustmentFunction);
+ sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
}
break;
case MSP_BOXNAMES:
- serializeBoxNamesReply();
+ serializeBoxNamesReply(dst);
break;
case MSP_BOXIDS:
- headSerialReply(activeBoxIdCount);
for (i = 0; i < activeBoxIdCount; i++) {
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
if (!box) {
continue;
}
- serialize8(box->permanentId);
+ sbufWriteU8(dst, box->permanentId);
}
break;
case MSP_MISC:
- headSerialReply(2 * 5 + 3 + 3 + 2 + 4);
- serialize16(masterConfig.rxConfig.midrc);
+ sbufWriteU16(dst, masterConfig.rxConfig.midrc);
- serialize16(masterConfig.motorConfig.minthrottle);
- serialize16(masterConfig.motorConfig.maxthrottle);
- serialize16(masterConfig.motorConfig.mincommand);
+ sbufWriteU16(dst, masterConfig.motorConfig.minthrottle);
+ sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle);
+ sbufWriteU16(dst, masterConfig.motorConfig.mincommand);
- serialize16(masterConfig.failsafeConfig.failsafe_throttle);
+ sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
#ifdef GPS
- serialize8(masterConfig.gpsConfig.provider); // gps_type
- serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
- serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
+ sbufWriteU8(dst, masterConfig.gpsConfig.provider); // gps_type
+ sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
+ sbufWriteU8(dst, masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
#else
- serialize8(0); // gps_type
- serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
- serialize8(0); // gps_ubx_sbas
+ sbufWriteU8(dst, 0); // gps_type
+ sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
+ sbufWriteU8(dst, 0); // gps_ubx_sbas
#endif
- serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
- serialize8(masterConfig.rxConfig.rssi_channel);
- serialize8(0);
+ sbufWriteU8(dst, masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
+ sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
+ sbufWriteU8(dst, 0);
- serialize16(masterConfig.mag_declination / 10);
+ sbufWriteU16(dst, masterConfig.mag_declination / 10);
- serialize8(masterConfig.batteryConfig.vbatscale);
- serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
- serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
- serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
break;
case MSP_MOTOR_PINS:
// FIXME This is hardcoded and should not be.
- headSerialReply(8);
for (i = 0; i < 8; i++)
- serialize8(i + 1);
+ sbufWriteU8(dst, i + 1);
break;
#ifdef GPS
case MSP_RAW_GPS:
- headSerialReply(16);
- serialize8(STATE(GPS_FIX));
- serialize8(GPS_numSat);
- serialize32(GPS_coord[LAT]);
- serialize32(GPS_coord[LON]);
- serialize16(GPS_altitude);
- serialize16(GPS_speed);
- serialize16(GPS_ground_course);
+ sbufWriteU8(dst, STATE(GPS_FIX));
+ sbufWriteU8(dst, GPS_numSat);
+ sbufWriteU32(dst, GPS_coord[LAT]);
+ sbufWriteU32(dst, GPS_coord[LON]);
+ sbufWriteU16(dst, GPS_altitude);
+ sbufWriteU16(dst, GPS_speed);
+ sbufWriteU16(dst, GPS_ground_course);
break;
case MSP_COMP_GPS:
- headSerialReply(5);
- serialize16(GPS_distanceToHome);
- serialize16(GPS_directionToHome);
- serialize8(GPS_update & 1);
- break;
- case MSP_WP:
- wp_no = read8(); // get the wp number
- headSerialReply(18);
- if (wp_no == 0) {
- lat = GPS_home[LAT];
- lon = GPS_home[LON];
- } else if (wp_no == 16) {
- lat = GPS_hold[LAT];
- lon = GPS_hold[LON];
- }
- serialize8(wp_no);
- serialize32(lat);
- serialize32(lon);
- serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
- serialize16(0); // heading will come here (deg)
- serialize16(0); // time to stay (ms) will come here
- serialize8(0); // nav flag will come here
+ sbufWriteU16(dst, GPS_distanceToHome);
+ sbufWriteU16(dst, GPS_directionToHome);
+ sbufWriteU8(dst, GPS_update & 1);
break;
case MSP_GPSSVINFO:
- headSerialReply(1 + (GPS_numCh * 4));
- serialize8(GPS_numCh);
+ sbufWriteU8(dst, GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
- serialize8(GPS_svinfo_chn[i]);
- serialize8(GPS_svinfo_svid[i]);
- serialize8(GPS_svinfo_quality[i]);
- serialize8(GPS_svinfo_cno[i]);
+ sbufWriteU8(dst, GPS_svinfo_chn[i]);
+ sbufWriteU8(dst, GPS_svinfo_svid[i]);
+ sbufWriteU8(dst, GPS_svinfo_quality[i]);
+ sbufWriteU8(dst, GPS_svinfo_cno[i]);
}
break;
#endif
case MSP_DEBUG:
- headSerialReply(DEBUG16_VALUE_COUNT * sizeof(debug[0]));
-
// output some useful QA statistics
// debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
for (i = 0; i < DEBUG16_VALUE_COUNT; i++)
- serialize16(debug[i]); // 4 variables are here for general monitoring purpose
+ sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose
break;
// Additional commands that are not compatible with MultiWii
case MSP_ACC_TRIM:
- headSerialReply(4);
- serialize16(masterConfig.accelerometerTrims.values.pitch);
- serialize16(masterConfig.accelerometerTrims.values.roll);
+ sbufWriteU16(dst, masterConfig.accelerometerTrims.values.pitch);
+ sbufWriteU16(dst, masterConfig.accelerometerTrims.values.roll);
break;
case MSP_UID:
- headSerialReply(12);
- serialize32(U_ID_0);
- serialize32(U_ID_1);
- serialize32(U_ID_2);
+ sbufWriteU32(dst, U_ID_0);
+ sbufWriteU32(dst, U_ID_1);
+ sbufWriteU32(dst, U_ID_2);
break;
case MSP_FEATURE:
- headSerialReply(4);
- serialize32(featureMask());
+ sbufWriteU32(dst, featureMask());
break;
case MSP_BOARD_ALIGNMENT:
- headSerialReply(6);
- serialize16(masterConfig.boardAlignment.rollDegrees);
- serialize16(masterConfig.boardAlignment.pitchDegrees);
- serialize16(masterConfig.boardAlignment.yawDegrees);
+ sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
+ sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
+ sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
break;
case MSP_VOLTAGE_METER_CONFIG:
- headSerialReply(4);
- serialize8(masterConfig.batteryConfig.vbatscale);
- serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
- serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
- serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
+ sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
break;
case MSP_CURRENT_METER_CONFIG:
- headSerialReply(7);
- serialize16(masterConfig.batteryConfig.currentMeterScale);
- serialize16(masterConfig.batteryConfig.currentMeterOffset);
- serialize8(masterConfig.batteryConfig.currentMeterType);
- serialize16(masterConfig.batteryConfig.batteryCapacity);
+ sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
+ sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
+ sbufWriteU8(dst, masterConfig.batteryConfig.currentMeterType);
+ sbufWriteU16(dst, masterConfig.batteryConfig.batteryCapacity);
break;
case MSP_MIXER:
- headSerialReply(1);
- serialize8(masterConfig.mixerMode);
+ sbufWriteU8(dst, masterConfig.mixerMode);
break;
case MSP_RX_CONFIG:
- headSerialReply(22);
- serialize8(masterConfig.rxConfig.serialrx_provider);
- serialize16(masterConfig.rxConfig.maxcheck);
- serialize16(masterConfig.rxConfig.midrc);
- serialize16(masterConfig.rxConfig.mincheck);
- serialize8(masterConfig.rxConfig.spektrum_sat_bind);
- serialize16(masterConfig.rxConfig.rx_min_usec);
- serialize16(masterConfig.rxConfig.rx_max_usec);
- serialize8(masterConfig.rxConfig.rcInterpolation);
- serialize8(masterConfig.rxConfig.rcInterpolationInterval);
- serialize16(masterConfig.rxConfig.airModeActivateThreshold);
- serialize8(masterConfig.rxConfig.rx_spi_protocol);
- serialize32(masterConfig.rxConfig.rx_spi_id);
- serialize8(masterConfig.rxConfig.rx_spi_rf_channel_count);
+ sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
+ sbufWriteU16(dst, masterConfig.rxConfig.maxcheck);
+ sbufWriteU16(dst, masterConfig.rxConfig.midrc);
+ sbufWriteU16(dst, masterConfig.rxConfig.mincheck);
+ sbufWriteU8(dst, masterConfig.rxConfig.spektrum_sat_bind);
+ sbufWriteU16(dst, masterConfig.rxConfig.rx_min_usec);
+ sbufWriteU16(dst, masterConfig.rxConfig.rx_max_usec);
+ sbufWriteU8(dst, masterConfig.rxConfig.rcInterpolation);
+ sbufWriteU8(dst, masterConfig.rxConfig.rcInterpolationInterval);
+ sbufWriteU16(dst, masterConfig.rxConfig.airModeActivateThreshold);
+ sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_protocol);
+ sbufWriteU32(dst, masterConfig.rxConfig.rx_spi_id);
+ sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_rf_channel_count);
break;
case MSP_FAILSAFE_CONFIG:
- headSerialReply(8);
- serialize8(masterConfig.failsafeConfig.failsafe_delay);
- serialize8(masterConfig.failsafeConfig.failsafe_off_delay);
- serialize16(masterConfig.failsafeConfig.failsafe_throttle);
- serialize8(masterConfig.failsafeConfig.failsafe_kill_switch);
- serialize16(masterConfig.failsafeConfig.failsafe_throttle_low_delay);
- serialize8(masterConfig.failsafeConfig.failsafe_procedure);
+ sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_delay);
+ sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_off_delay);
+ sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
+ sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_kill_switch);
+ sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle_low_delay);
+ sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_procedure);
break;
case MSP_RXFAIL_CONFIG:
- headSerialReply(3 * (rxRuntimeConfig.channelCount));
for (i = 0; i < rxRuntimeConfig.channelCount; i++) {
- serialize8(masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
- serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
+ sbufWriteU8(dst, masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
+ sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
}
break;
case MSP_RSSI_CONFIG:
- headSerialReply(1);
- serialize8(masterConfig.rxConfig.rssi_channel);
+ sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
break;
case MSP_RX_MAP:
- headSerialReply(MAX_MAPPABLE_RX_INPUTS);
for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++)
- serialize8(masterConfig.rxConfig.rcmap[i]);
+ sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]);
break;
case MSP_BF_CONFIG:
- headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
- serialize8(masterConfig.mixerMode);
+ sbufWriteU8(dst, masterConfig.mixerMode);
- serialize32(featureMask());
+ sbufWriteU32(dst, featureMask());
- serialize8(masterConfig.rxConfig.serialrx_provider);
+ sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
- serialize16(masterConfig.boardAlignment.rollDegrees);
- serialize16(masterConfig.boardAlignment.pitchDegrees);
- serialize16(masterConfig.boardAlignment.yawDegrees);
+ sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
+ sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
+ sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
- serialize16(masterConfig.batteryConfig.currentMeterScale);
- serialize16(masterConfig.batteryConfig.currentMeterOffset);
+ sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
+ sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
break;
case MSP_CF_SERIAL_CONFIG:
- headSerialReply(
- ((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * serialGetAvailablePortCount())
- );
for (i = 0; i < SERIAL_PORT_COUNT; i++) {
if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
continue;
};
- serialize8(masterConfig.serialConfig.portConfigs[i].identifier);
- serialize16(masterConfig.serialConfig.portConfigs[i].functionMask);
- serialize8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
- serialize8(masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex);
- serialize8(masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex);
- serialize8(masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex);
+ sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].identifier);
+ sbufWriteU16(dst, masterConfig.serialConfig.portConfigs[i].functionMask);
+ sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
+ sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex);
+ sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex);
+ sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex);
}
break;
#ifdef LED_STRIP
case MSP_LED_COLORS:
- headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4);
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.colors[i];
- serialize16(color->h);
- serialize8(color->s);
- serialize8(color->v);
+ sbufWriteU16(dst, color->h);
+ sbufWriteU8(dst, color->s);
+ sbufWriteU8(dst, color->v);
}
break;
case MSP_LED_STRIP_CONFIG:
- headSerialReply(LED_MAX_STRIP_LENGTH * 4);
for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
- serialize32(*ledConfig);
+ sbufWriteU32(dst, *ledConfig);
}
break;
case MSP_LED_STRIP_MODECOLOR:
- headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3);
for (int i = 0; i < LED_MODE_COUNT; i++) {
for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
- serialize8(i);
- serialize8(j);
- serialize8(masterConfig.modeColors[i].color[j]);
+ sbufWriteU8(dst, i);
+ sbufWriteU8(dst, j);
+ sbufWriteU8(dst, masterConfig.modeColors[i].color[j]);
}
}
for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
- serialize8(LED_MODE_COUNT);
- serialize8(j);
- serialize8(masterConfig.specialColors.color[j]);
+ sbufWriteU8(dst, LED_MODE_COUNT);
+ sbufWriteU8(dst, j);
+ sbufWriteU8(dst, masterConfig.specialColors.color[j]);
}
break;
#endif
case MSP_DATAFLASH_SUMMARY:
- serializeDataflashSummaryReply();
+ serializeDataflashSummaryReply(dst);
break;
-#ifdef USE_FLASHFS
- case MSP_DATAFLASH_READ:
- {
- uint32_t readAddress = read32();
- uint16_t readLength;
- bool useLegacyFormat;
- if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
- readLength = read16();
- useLegacyFormat = false;
- } else {
- readLength = 128;
- useLegacyFormat = true;
- }
-
- serializeDataflashReadReply(readAddress, readLength, useLegacyFormat);
- }
- break;
-#endif
-
case MSP_BLACKBOX_CONFIG:
- headSerialReply(4);
-
#ifdef BLACKBOX
- serialize8(1); //Blackbox supported
- serialize8(masterConfig.blackbox_device);
- serialize8(masterConfig.blackbox_rate_num);
- serialize8(masterConfig.blackbox_rate_denom);
+ sbufWriteU8(dst, 1); //Blackbox supported
+ sbufWriteU8(dst, masterConfig.blackbox_device);
+ sbufWriteU8(dst, masterConfig.blackbox_rate_num);
+ sbufWriteU8(dst, masterConfig.blackbox_rate_denom);
#else
- serialize8(0); // Blackbox not supported
- serialize8(0);
- serialize8(0);
- serialize8(0);
+ sbufWriteU8(dst, 0); // Blackbox not supported
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
+ sbufWriteU8(dst, 0);
#endif
break;
case MSP_SDCARD_SUMMARY:
- serializeSDCardSummaryReply();
+ serializeSDCardSummaryReply(dst);
break;
case MSP_TRANSPONDER_CONFIG:
#ifdef TRANSPONDER
- headSerialReply(1 + sizeof(masterConfig.transponderData));
-
- serialize8(1); //Transponder supported
-
+ sbufWriteU8(dst, 1); //Transponder supported
for (i = 0; i < sizeof(masterConfig.transponderData); i++) {
- serialize8(masterConfig.transponderData[i]);
+ sbufWriteU8(dst, masterConfig.transponderData[i]);
}
#else
- headSerialReply(1);
- serialize8(0); // Transponder not supported
+ sbufWriteU8(dst, 0); // Transponder not supported
#endif
break;
case MSP_OSD_CONFIG:
#ifdef OSD
- headSerialReply(10 + (OSD_MAX_ITEMS * 2));
- serialize8(1); // OSD supported
+ sbufWriteU8(dst, 1); // OSD supported
// send video system (AUTO/PAL/NTSC)
- serialize8(masterConfig.osdProfile.video_system);
- serialize8(masterConfig.osdProfile.units);
- serialize8(masterConfig.osdProfile.rssi_alarm);
- serialize16(masterConfig.osdProfile.cap_alarm);
- serialize16(masterConfig.osdProfile.time_alarm);
- serialize16(masterConfig.osdProfile.alt_alarm);
+ sbufWriteU8(dst, masterConfig.osdProfile.video_system);
+ sbufWriteU8(dst, masterConfig.osdProfile.units);
+ sbufWriteU8(dst, masterConfig.osdProfile.rssi_alarm);
+ sbufWriteU16(dst, masterConfig.osdProfile.cap_alarm);
+ sbufWriteU16(dst, masterConfig.osdProfile.time_alarm);
+ sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm);
for (i = 0; i < OSD_MAX_ITEMS; i++) {
- serialize16(masterConfig.osdProfile.item_pos[i]);
+ sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]);
}
#else
- headSerialReply(1);
- serialize8(0); // OSD not supported
+ sbufWriteU8(dst, 0); // OSD not supported
#endif
break;
case MSP_BF_BUILD_INFO:
- headSerialReply(11 + 4 + 4);
- for (i = 0; i < 11; i++)
- serialize8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
- serialize32(0); // future exp
- serialize32(0); // future exp
+ for (i = 0; i < 11; i++) {
+ sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
+ }
+ sbufWriteU32(dst, 0); // future exp
+ sbufWriteU32(dst, 0); // future exp
break;
case MSP_3D:
- headSerialReply(2 * 3);
- serialize16(masterConfig.flight3DConfig.deadband3d_low);
- serialize16(masterConfig.flight3DConfig.deadband3d_high);
- serialize16(masterConfig.flight3DConfig.neutral3d);
+ sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low);
+ sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high);
+ sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d);
break;
case MSP_RC_DEADBAND:
- headSerialReply(5);
- serialize8(masterConfig.rcControlsConfig.deadband);
- serialize8(masterConfig.rcControlsConfig.yaw_deadband);
- serialize8(masterConfig.rcControlsConfig.alt_hold_deadband);
- serialize16(masterConfig.flight3DConfig.deadband3d_throttle);
+ sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband);
+ sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband);
+ sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband);
+ sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle);
break;
case MSP_SENSOR_ALIGNMENT:
- headSerialReply(3);
- serialize8(masterConfig.sensorAlignmentConfig.gyro_align);
- serialize8(masterConfig.sensorAlignmentConfig.acc_align);
- serialize8(masterConfig.sensorAlignmentConfig.mag_align);
+ sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.gyro_align);
+ sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.acc_align);
+ sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.mag_align);
break;
case MSP_ADVANCED_CONFIG :
- headSerialReply(6);
if (masterConfig.gyro_lpf) {
- serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000
- serialize8(1);
+ sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
+ sbufWriteU8(dst, 1);
} else {
- serialize8(masterConfig.gyro_sync_denom);
- serialize8(masterConfig.pid_process_denom);
+ sbufWriteU8(dst, masterConfig.gyro_sync_denom);
+ sbufWriteU8(dst, masterConfig.pid_process_denom);
}
- serialize8(masterConfig.motorConfig.useUnsyncedPwm);
- serialize8(masterConfig.motorConfig.motorPwmProtocol);
- serialize16(masterConfig.motorConfig.motorPwmRate);
+ sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
+ sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol);
+ sbufWriteU16(dst, masterConfig.motorConfig.motorPwmRate);
break;
case MSP_FILTER_CONFIG :
- headSerialReply(17);
- serialize8(masterConfig.gyro_soft_lpf_hz);
- serialize16(currentProfile->pidProfile.dterm_lpf_hz);
- serialize16(currentProfile->pidProfile.yaw_lpf_hz);
- serialize16(masterConfig.gyro_soft_notch_hz_1);
- serialize16(masterConfig.gyro_soft_notch_cutoff_1);
- serialize16(currentProfile->pidProfile.dterm_notch_hz);
- serialize16(currentProfile->pidProfile.dterm_notch_cutoff);
- serialize16(masterConfig.gyro_soft_notch_hz_2);
- serialize16(masterConfig.gyro_soft_notch_cutoff_2);
+ sbufWriteU8(dst, masterConfig.gyro_soft_lpf_hz);
+ sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
+ sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
+ sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_1);
+ sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_1);
+ sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
+ sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
+ sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_2);
+ sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_2);
break;
case MSP_PID_ADVANCED:
- headSerialReply(17);
- serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate);
- serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
- serialize16(currentProfile->pidProfile.yaw_p_limit);
- serialize8(0); // reserved
- serialize8(currentProfile->pidProfile.vbatPidCompensation);
- serialize8(currentProfile->pidProfile.setpointRelaxRatio);
- serialize8(currentProfile->pidProfile.dtermSetpointWeight);
- serialize8(0); // reserved
- serialize8(0); // reserved
- serialize8(currentProfile->pidProfile.itermThrottleGain);
- serialize16(currentProfile->pidProfile.rateAccelLimit);
- serialize16(currentProfile->pidProfile.yawRateAccelLimit);
+ sbufWriteU16(dst, currentProfile->pidProfile.rollPitchItermIgnoreRate);
+ sbufWriteU16(dst, currentProfile->pidProfile.yawItermIgnoreRate);
+ sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit);
+ sbufWriteU8(dst, 0); // reserved
+ sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation);
+ sbufWriteU8(dst, currentProfile->pidProfile.setpointRelaxRatio);
+ sbufWriteU8(dst, currentProfile->pidProfile.dtermSetpointWeight);
+ sbufWriteU8(dst, 0); // reserved
+ sbufWriteU8(dst, 0); // reserved
+ sbufWriteU8(dst, currentProfile->pidProfile.itermThrottleGain);
+ sbufWriteU16(dst, currentProfile->pidProfile.rateAccelLimit);
+ sbufWriteU16(dst, currentProfile->pidProfile.yawRateAccelLimit);
break;
case MSP_SENSOR_CONFIG:
- headSerialReply(3);
- serialize8(masterConfig.acc_hardware);
- serialize8(masterConfig.baro_hardware);
- serialize8(masterConfig.mag_hardware);
+ sbufWriteU8(dst, masterConfig.acc_hardware);
+ sbufWriteU8(dst, masterConfig.baro_hardware);
+ sbufWriteU8(dst, masterConfig.mag_hardware);
break;
case MSP_REBOOT:
- headSerialReply(0);
if (mspPostProcessFn) {
*mspPostProcessFn = mspRebootFn;
}
@@ -1322,11 +1107,10 @@ static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProces
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
case MSP_SET_4WAY_IF:
- headSerialReply(1);
// get channel number
// switch all motor lines HI
// reply with the count of ESC found
- serialize8(esc4wayInit());
+ sbufWriteU8(dst, esc4wayInit());
if (mspPostProcessFn) {
*mspPostProcessFn = msp4WayIfFn;
}
@@ -1339,12 +1123,54 @@ static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProces
return true;
}
-static bool processInCommand(uint8_t cmdMSP)
+#ifdef GPS
+static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
+{
+ uint8_t wp_no;
+ int32_t lat = 0, lon = 0;
+ wp_no = sbufReadU8(src); // get the wp number
+ if (wp_no == 0) {
+ lat = GPS_home[LAT];
+ lon = GPS_home[LON];
+ } else if (wp_no == 16) {
+ lat = GPS_hold[LAT];
+ lon = GPS_hold[LON];
+ }
+ sbufWriteU8(dst, wp_no);
+ sbufWriteU32(dst, lat);
+ sbufWriteU32(dst, lon);
+ sbufWriteU32(dst, AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
+ sbufWriteU16(dst, 0); // heading will come here (deg)
+ sbufWriteU16(dst, 0); // time to stay (ms) will come here
+ sbufWriteU8(dst, 0); // nav flag will come here
+}
+#endif
+
+#ifdef USE_FLASHFS
+static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
+{
+ const unsigned int dataSize = sbufBytesRemaining(src);
+ const uint32_t readAddress = sbufReadU32(src);
+ uint16_t readLength;
+ bool useLegacyFormat;
+ if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
+ readLength = sbufReadU16(src);
+ useLegacyFormat = false;
+ } else {
+ readLength = 128;
+ useLegacyFormat = true;
+ }
+
+ serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat);
+}
+#endif
+
+static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
{
uint32_t i;
uint16_t tmp;
uint8_t value;
- const unsigned int dataSize = currentPort->dataSize;
+ const unsigned int dataSize = sbufBytesRemaining(src);
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
@@ -1354,7 +1180,7 @@ static bool processInCommand(uint8_t cmdMSP)
#endif
switch (cmdMSP) {
case MSP_SELECT_SETTING:
- value = read8();
+ value = sbufReadU8(src);
if ((value & RATEPROFILE_MASK) == 0) {
if (!ARMING_FLAG(ARMED)) {
if (value >= MAX_PROFILE_COUNT) {
@@ -1373,19 +1199,19 @@ static bool processInCommand(uint8_t cmdMSP)
break;
case MSP_SET_HEAD:
- magHold = read16();
+ magHold = sbufReadU16(src);
break;
case MSP_SET_RAW_RC:
#ifndef SKIP_RX_MSP
{
uint8_t channelCount = dataSize / sizeof(uint16_t);
if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
} else {
uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
for (i = 0; i < channelCount; i++) {
- frame[i] = read16();
+ frame[i] = sbufReadU16(src);
}
rxMspFrameReceive(frame, channelCount);
@@ -1394,174 +1220,174 @@ static bool processInCommand(uint8_t cmdMSP)
#endif
break;
case MSP_SET_ACC_TRIM:
- masterConfig.accelerometerTrims.values.pitch = read16();
- masterConfig.accelerometerTrims.values.roll = read16();
+ masterConfig.accelerometerTrims.values.pitch = sbufReadU16(src);
+ masterConfig.accelerometerTrims.values.roll = sbufReadU16(src);
break;
case MSP_SET_ARMING_CONFIG:
- masterConfig.auto_disarm_delay = read8();
- masterConfig.disarm_kill_switch = read8();
+ masterConfig.auto_disarm_delay = sbufReadU8(src);
+ masterConfig.disarm_kill_switch = sbufReadU8(src);
break;
case MSP_SET_LOOP_TIME:
- read16();
+ sbufReadU16(src);
break;
case MSP_SET_PID_CONTROLLER:
break;
case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) {
- currentProfile->pidProfile.P8[i] = read8();
- currentProfile->pidProfile.I8[i] = read8();
- currentProfile->pidProfile.D8[i] = read8();
+ currentProfile->pidProfile.P8[i] = sbufReadU8(src);
+ currentProfile->pidProfile.I8[i] = sbufReadU8(src);
+ currentProfile->pidProfile.D8[i] = sbufReadU8(src);
}
break;
case MSP_SET_MODE_RANGE:
- i = read8();
+ i = sbufReadU8(src);
if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i];
- i = read8();
+ i = sbufReadU8(src);
const box_t *box = findBoxByPermenantId(i);
if (box) {
mac->modeId = box->boxId;
- mac->auxChannelIndex = read8();
- mac->range.startStep = read8();
- mac->range.endStep = read8();
+ mac->auxChannelIndex = sbufReadU8(src);
+ mac->range.startStep = sbufReadU8(src);
+ mac->range.endStep = sbufReadU8(src);
useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile);
} else {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
}
} else {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
}
break;
case MSP_SET_ADJUSTMENT_RANGE:
- i = read8();
+ i = sbufReadU8(src);
if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i];
- i = read8();
+ i = sbufReadU8(src);
if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
adjRange->adjustmentIndex = i;
- adjRange->auxChannelIndex = read8();
- adjRange->range.startStep = read8();
- adjRange->range.endStep = read8();
- adjRange->adjustmentFunction = read8();
- adjRange->auxSwitchChannelIndex = read8();
+ adjRange->auxChannelIndex = sbufReadU8(src);
+ adjRange->range.startStep = sbufReadU8(src);
+ adjRange->range.endStep = sbufReadU8(src);
+ adjRange->adjustmentFunction = sbufReadU8(src);
+ adjRange->auxSwitchChannelIndex = sbufReadU8(src);
} else {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
}
} else {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
}
break;
case MSP_SET_RC_TUNING:
if (dataSize >= 10) {
- currentControlRateProfile->rcRate8 = read8();
- currentControlRateProfile->rcExpo8 = read8();
+ currentControlRateProfile->rcRate8 = sbufReadU8(src);
+ currentControlRateProfile->rcExpo8 = sbufReadU8(src);
for (i = 0; i < 3; i++) {
- value = read8();
+ value = sbufReadU8(src);
currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
}
- value = read8();
+ value = sbufReadU8(src);
currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
- currentControlRateProfile->thrMid8 = read8();
- currentControlRateProfile->thrExpo8 = read8();
- currentControlRateProfile->tpa_breakpoint = read16();
+ currentControlRateProfile->thrMid8 = sbufReadU8(src);
+ currentControlRateProfile->thrExpo8 = sbufReadU8(src);
+ currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
if (dataSize >= 11) {
- currentControlRateProfile->rcYawExpo8 = read8();
+ currentControlRateProfile->rcYawExpo8 = sbufReadU8(src);
}
if (dataSize >= 12) {
- currentControlRateProfile->rcYawRate8 = read8();
+ currentControlRateProfile->rcYawRate8 = sbufReadU8(src);
}
} else {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
}
break;
case MSP_SET_MISC:
- tmp = read16();
+ tmp = sbufReadU16(src);
if (tmp < 1600 && tmp > 1400)
masterConfig.rxConfig.midrc = tmp;
- masterConfig.motorConfig.minthrottle = read16();
- masterConfig.motorConfig.maxthrottle = read16();
- masterConfig.motorConfig.mincommand = read16();
+ masterConfig.motorConfig.minthrottle = sbufReadU16(src);
+ masterConfig.motorConfig.maxthrottle = sbufReadU16(src);
+ masterConfig.motorConfig.mincommand = sbufReadU16(src);
- masterConfig.failsafeConfig.failsafe_throttle = read16();
+ masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
#ifdef GPS
- masterConfig.gpsConfig.provider = read8(); // gps_type
- read8(); // gps_baudrate
- masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas
+ masterConfig.gpsConfig.provider = sbufReadU8(src); // gps_type
+ sbufReadU8(src); // gps_baudrate
+ masterConfig.gpsConfig.sbasMode = sbufReadU8(src); // gps_ubx_sbas
#else
- read8(); // gps_type
- read8(); // gps_baudrate
- read8(); // gps_ubx_sbas
+ sbufReadU8(src); // gps_type
+ sbufReadU8(src); // gps_baudrate
+ sbufReadU8(src); // gps_ubx_sbas
#endif
- masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8();
- masterConfig.rxConfig.rssi_channel = read8();
- read8();
+ masterConfig.batteryConfig.multiwiiCurrentMeterOutput = sbufReadU8(src);
+ masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
+ sbufReadU8(src);
- masterConfig.mag_declination = read16() * 10;
+ masterConfig.mag_declination = sbufReadU16(src) * 10;
- masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended
- masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
- masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
- masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
+ masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
+ masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
+ masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
+ masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break;
case MSP_SET_MOTOR:
for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
- motor_disarmed[i] = read16();
+ motor_disarmed[i] = sbufReadU16(src);
break;
case MSP_SET_SERVO_CONFIGURATION:
#ifdef USE_SERVOS
if (dataSize != 1 + sizeof(servoParam_t)) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
break;
}
- i = read8();
+ i = sbufReadU8(src);
if (i >= MAX_SUPPORTED_SERVOS) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
} else {
- masterConfig.servoConf[i].min = read16();
- masterConfig.servoConf[i].max = read16();
- masterConfig.servoConf[i].middle = read16();
- masterConfig.servoConf[i].rate = read8();
- masterConfig.servoConf[i].angleAtMin = read8();
- masterConfig.servoConf[i].angleAtMax = read8();
- masterConfig.servoConf[i].forwardFromChannel = read8();
- masterConfig.servoConf[i].reversedSources = read32();
+ masterConfig.servoConf[i].min = sbufReadU16(src);
+ masterConfig.servoConf[i].max = sbufReadU16(src);
+ masterConfig.servoConf[i].middle = sbufReadU16(src);
+ masterConfig.servoConf[i].rate = sbufReadU8(src);
+ masterConfig.servoConf[i].angleAtMin = sbufReadU8(src);
+ masterConfig.servoConf[i].angleAtMax = sbufReadU8(src);
+ masterConfig.servoConf[i].forwardFromChannel = sbufReadU8(src);
+ masterConfig.servoConf[i].reversedSources = sbufReadU32(src);
}
#endif
break;
case MSP_SET_SERVO_MIX_RULE:
#ifdef USE_SERVOS
- i = read8();
+ i = sbufReadU8(src);
if (i >= MAX_SERVO_RULES) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
} else {
- masterConfig.customServoMixer[i].targetChannel = read8();
- masterConfig.customServoMixer[i].inputSource = read8();
- masterConfig.customServoMixer[i].rate = read8();
- masterConfig.customServoMixer[i].speed = read8();
- masterConfig.customServoMixer[i].min = read8();
- masterConfig.customServoMixer[i].max = read8();
- masterConfig.customServoMixer[i].box = read8();
+ masterConfig.customServoMixer[i].targetChannel = sbufReadU8(src);
+ masterConfig.customServoMixer[i].inputSource = sbufReadU8(src);
+ masterConfig.customServoMixer[i].rate = sbufReadU8(src);
+ masterConfig.customServoMixer[i].speed = sbufReadU8(src);
+ masterConfig.customServoMixer[i].min = sbufReadU8(src);
+ masterConfig.customServoMixer[i].max = sbufReadU8(src);
+ masterConfig.customServoMixer[i].box = sbufReadU8(src);
loadCustomServoMixer();
}
#endif
break;
case MSP_SET_3D:
- masterConfig.flight3DConfig.deadband3d_low = read16();
- masterConfig.flight3DConfig.deadband3d_high = read16();
- masterConfig.flight3DConfig.neutral3d = read16();
- masterConfig.flight3DConfig.deadband3d_throttle = read16();
+ masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src);
+ masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src);
+ masterConfig.flight3DConfig.neutral3d = sbufReadU16(src);
+ masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src);
break;
case MSP_SET_RC_DEADBAND:
- masterConfig.rcControlsConfig.deadband = read8();
- masterConfig.rcControlsConfig.yaw_deadband = read8();
- masterConfig.rcControlsConfig.alt_hold_deadband = read8();
+ masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
+ masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
+ masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
break;
case MSP_SET_RESET_CURR_PID:
@@ -1569,9 +1395,9 @@ static bool processInCommand(uint8_t cmdMSP)
break;
case MSP_SET_SENSOR_ALIGNMENT:
- masterConfig.sensorAlignmentConfig.gyro_align = read8();
- masterConfig.sensorAlignmentConfig.acc_align = read8();
- masterConfig.sensorAlignmentConfig.mag_align = read8();
+ masterConfig.sensorAlignmentConfig.gyro_align = sbufReadU8(src);
+ masterConfig.sensorAlignmentConfig.acc_align = sbufReadU8(src);
+ masterConfig.sensorAlignmentConfig.mag_align = sbufReadU8(src);
break;
case MSP_RESET_CONF:
@@ -1590,7 +1416,7 @@ static bool processInCommand(uint8_t cmdMSP)
break;
case MSP_EEPROM_WRITE:
if (ARMING_FLAG(ARMED)) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
return true;
}
writeEEPROM();
@@ -1601,9 +1427,9 @@ static bool processInCommand(uint8_t cmdMSP)
case MSP_SET_BLACKBOX_CONFIG:
// Don't allow config to be updated while Blackbox is logging
if (blackboxMayEditConfig()) {
- masterConfig.blackbox_device = read8();
- masterConfig.blackbox_rate_num = read8();
- masterConfig.blackbox_rate_denom = read8();
+ masterConfig.blackbox_device = sbufReadU8(src);
+ masterConfig.blackbox_rate_num = sbufReadU8(src);
+ masterConfig.blackbox_rate_denom = sbufReadU8(src);
}
break;
#endif
@@ -1611,12 +1437,12 @@ static bool processInCommand(uint8_t cmdMSP)
#ifdef TRANSPONDER
case MSP_SET_TRANSPONDER_CONFIG:
if (dataSize != sizeof(masterConfig.transponderData)) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
break;
}
for (i = 0; i < sizeof(masterConfig.transponderData); i++) {
- masterConfig.transponderData[i] = read8();
+ masterConfig.transponderData[i] = sbufReadU8(src);
}
transponderUpdateData(masterConfig.transponderData);
@@ -1624,25 +1450,25 @@ static bool processInCommand(uint8_t cmdMSP)
#endif
#ifdef OSD
case MSP_SET_OSD_CONFIG:
- addr = read8();
+ addr = sbufReadU8(src);
// set all the other settings
if ((int8_t)addr == -1) {
- masterConfig.osdProfile.video_system = read8();
- masterConfig.osdProfile.units = read8();
- masterConfig.osdProfile.rssi_alarm = read8();
- masterConfig.osdProfile.cap_alarm = read16();
- masterConfig.osdProfile.time_alarm = read16();
- masterConfig.osdProfile.alt_alarm = read16();
+ masterConfig.osdProfile.video_system = sbufReadU8(src);
+ masterConfig.osdProfile.units = sbufReadU8(src);
+ masterConfig.osdProfile.rssi_alarm = sbufReadU8(src);
+ masterConfig.osdProfile.cap_alarm = sbufReadU16(src);
+ masterConfig.osdProfile.time_alarm = sbufReadU16(src);
+ masterConfig.osdProfile.alt_alarm = sbufReadU16(src);
}
// set a position setting
else {
- masterConfig.osdProfile.item_pos[addr] = read16();
+ masterConfig.osdProfile.item_pos[addr] = sbufReadU16(src);
}
break;
case MSP_OSD_CHAR_WRITE:
- addr = read8();
+ addr = sbufReadU8(src);
for (i = 0; i < 54; i++) {
- font_data[i] = read8();
+ font_data[i] = sbufReadU8(src);
}
max7456WriteNvm(addr, font_data);
break;
@@ -1650,7 +1476,7 @@ static bool processInCommand(uint8_t cmdMSP)
#ifdef USE_RTC6705
case MSP_SET_VTX_CONFIG:
- tmp = read16();
+ tmp = sbufReadU16(src);
if (tmp < 40)
masterConfig.vtx_channel = tmp;
if (current_vtx_channel != masterConfig.vtx_channel) {
@@ -1668,26 +1494,26 @@ static bool processInCommand(uint8_t cmdMSP)
#ifdef GPS
case MSP_SET_RAW_GPS:
- if (read8()) {
+ if (sbufReadU8(src)) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
- GPS_numSat = read8();
- GPS_coord[LAT] = read32();
- GPS_coord[LON] = read32();
- GPS_altitude = read16();
- GPS_speed = read16();
+ GPS_numSat = sbufReadU8(src);
+ GPS_coord[LAT] = sbufReadU32(src);
+ GPS_coord[LON] = sbufReadU32(src);
+ GPS_altitude = sbufReadU16(src);
+ GPS_speed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;
case MSP_SET_WP:
- wp_no = read8(); //get the wp number
- lat = read32();
- lon = read32();
- alt = read32(); // to set altitude (cm)
- read16(); // future: to set heading (deg)
- read16(); // future: to set time to stay (ms)
- read8(); // future: to set nav flag
+ wp_no = sbufReadU8(src); //get the wp number
+ lat = sbufReadU32(src);
+ lon = sbufReadU32(src);
+ alt = sbufReadU32(src); // to set altitude (cm)
+ sbufReadU16(src); // future: to set heading (deg)
+ sbufReadU16(src); // future: to set time to stay (ms)
+ sbufReadU8(src); // future: to set nav flag
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
@@ -1707,104 +1533,104 @@ static bool processInCommand(uint8_t cmdMSP)
#endif
case MSP_SET_FEATURE:
featureClearAll();
- featureSet(read32()); // features bitmap
+ featureSet(sbufReadU32(src)); // features bitmap
break;
case MSP_SET_BOARD_ALIGNMENT:
- masterConfig.boardAlignment.rollDegrees = read16();
- masterConfig.boardAlignment.pitchDegrees = read16();
- masterConfig.boardAlignment.yawDegrees = read16();
+ masterConfig.boardAlignment.rollDegrees = sbufReadU16(src);
+ masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src);
+ masterConfig.boardAlignment.yawDegrees = sbufReadU16(src);
break;
case MSP_SET_VOLTAGE_METER_CONFIG:
- masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended
- masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
- masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
- masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
+ masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
+ masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
+ masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
+ masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break;
case MSP_SET_CURRENT_METER_CONFIG:
- masterConfig.batteryConfig.currentMeterScale = read16();
- masterConfig.batteryConfig.currentMeterOffset = read16();
- masterConfig.batteryConfig.currentMeterType = read8();
- masterConfig.batteryConfig.batteryCapacity = read16();
+ masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
+ masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
+ masterConfig.batteryConfig.currentMeterType = sbufReadU8(src);
+ masterConfig.batteryConfig.batteryCapacity = sbufReadU16(src);
break;
#ifndef USE_QUAD_MIXER_ONLY
case MSP_SET_MIXER:
- masterConfig.mixerMode = read8();
+ masterConfig.mixerMode = sbufReadU8(src);
break;
#endif
case MSP_SET_RX_CONFIG:
- masterConfig.rxConfig.serialrx_provider = read8();
- masterConfig.rxConfig.maxcheck = read16();
- masterConfig.rxConfig.midrc = read16();
- masterConfig.rxConfig.mincheck = read16();
- masterConfig.rxConfig.spektrum_sat_bind = read8();
+ masterConfig.rxConfig.serialrx_provider = sbufReadU8(src);
+ masterConfig.rxConfig.maxcheck = sbufReadU16(src);
+ masterConfig.rxConfig.midrc = sbufReadU16(src);
+ masterConfig.rxConfig.mincheck = sbufReadU16(src);
+ masterConfig.rxConfig.spektrum_sat_bind = sbufReadU8(src);
if (dataSize > 8) {
- masterConfig.rxConfig.rx_min_usec = read16();
- masterConfig.rxConfig.rx_max_usec = read16();
+ masterConfig.rxConfig.rx_min_usec = sbufReadU16(src);
+ masterConfig.rxConfig.rx_max_usec = sbufReadU16(src);
}
if (dataSize > 12) {
- masterConfig.rxConfig.rcInterpolation = read8();
- masterConfig.rxConfig.rcInterpolationInterval = read8();
- masterConfig.rxConfig.airModeActivateThreshold = read16();
+ masterConfig.rxConfig.rcInterpolation = sbufReadU8(src);
+ masterConfig.rxConfig.rcInterpolationInterval = sbufReadU8(src);
+ masterConfig.rxConfig.airModeActivateThreshold = sbufReadU16(src);
}
if (dataSize > 16) {
- masterConfig.rxConfig.rx_spi_protocol = read8();
- masterConfig.rxConfig.rx_spi_id = read32();
- masterConfig.rxConfig.rx_spi_rf_channel_count = read8();
+ masterConfig.rxConfig.rx_spi_protocol = sbufReadU8(src);
+ masterConfig.rxConfig.rx_spi_id = sbufReadU32(src);
+ masterConfig.rxConfig.rx_spi_rf_channel_count = sbufReadU8(src);
}
break;
case MSP_SET_FAILSAFE_CONFIG:
- masterConfig.failsafeConfig.failsafe_delay = read8();
- masterConfig.failsafeConfig.failsafe_off_delay = read8();
- masterConfig.failsafeConfig.failsafe_throttle = read16();
- masterConfig.failsafeConfig.failsafe_kill_switch = read8();
- masterConfig.failsafeConfig.failsafe_throttle_low_delay = read16();
- masterConfig.failsafeConfig.failsafe_procedure = read8();
+ masterConfig.failsafeConfig.failsafe_delay = sbufReadU8(src);
+ masterConfig.failsafeConfig.failsafe_off_delay = sbufReadU8(src);
+ masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
+ masterConfig.failsafeConfig.failsafe_kill_switch = sbufReadU8(src);
+ masterConfig.failsafeConfig.failsafe_throttle_low_delay = sbufReadU16(src);
+ masterConfig.failsafeConfig.failsafe_procedure = sbufReadU8(src);
break;
case MSP_SET_RXFAIL_CONFIG:
- i = read8();
+ i = sbufReadU8(src);
if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
- masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8();
- masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16());
+ masterConfig.rxConfig.failsafe_channel_configurations[i].mode = sbufReadU8(src);
+ masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
} else {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
}
break;
case MSP_SET_RSSI_CONFIG:
- masterConfig.rxConfig.rssi_channel = read8();
+ masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
break;
case MSP_SET_RX_MAP:
for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
- masterConfig.rxConfig.rcmap[i] = read8();
+ masterConfig.rxConfig.rcmap[i] = sbufReadU8(src);
}
break;
case MSP_SET_BF_CONFIG:
#ifdef USE_QUAD_MIXER_ONLY
- read8(); // mixerMode ignored
+ sbufReadU8(src); // mixerMode ignored
#else
- masterConfig.mixerMode = read8(); // mixerMode
+ masterConfig.mixerMode = sbufReadU8(src); // mixerMode
#endif
featureClearAll();
- featureSet(read32()); // features bitmap
+ featureSet(sbufReadU32(src)); // features bitmap
- masterConfig.rxConfig.serialrx_provider = read8(); // serialrx_type
+ masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); // serialrx_type
- masterConfig.boardAlignment.rollDegrees = read16(); // board_align_roll
- masterConfig.boardAlignment.pitchDegrees = read16(); // board_align_pitch
- masterConfig.boardAlignment.yawDegrees = read16(); // board_align_yaw
+ masterConfig.boardAlignment.rollDegrees = sbufReadU16(src); // board_align_roll
+ masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src); // board_align_pitch
+ masterConfig.boardAlignment.yawDegrees = sbufReadU16(src); // board_align_yaw
- masterConfig.batteryConfig.currentMeterScale = read16();
- masterConfig.batteryConfig.currentMeterOffset = read16();
+ masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
+ masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
break;
case MSP_SET_CF_SERIAL_CONFIG:
@@ -1812,27 +1638,27 @@ static bool processInCommand(uint8_t cmdMSP)
uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
if (dataSize % portConfigSize != 0) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
break;
}
uint8_t remainingPortsInPacket = dataSize / portConfigSize;
while (remainingPortsInPacket--) {
- uint8_t identifier = read8();
+ uint8_t identifier = sbufReadU8(src);
serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
if (!portConfig) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
break;
}
portConfig->identifier = identifier;
- portConfig->functionMask = read16();
- portConfig->msp_baudrateIndex = read8();
- portConfig->gps_baudrateIndex = read8();
- portConfig->telemetry_baudrateIndex = read8();
- portConfig->blackbox_baudrateIndex = read8();
+ portConfig->functionMask = sbufReadU16(src);
+ portConfig->msp_baudrateIndex = sbufReadU8(src);
+ portConfig->gps_baudrateIndex = sbufReadU8(src);
+ portConfig->telemetry_baudrateIndex = sbufReadU8(src);
+ portConfig->blackbox_baudrateIndex = sbufReadU8(src);
}
}
break;
@@ -1841,108 +1667,124 @@ static bool processInCommand(uint8_t cmdMSP)
case MSP_SET_LED_COLORS:
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.colors[i];
- color->h = read16();
- color->s = read8();
- color->v = read8();
+ color->h = sbufReadU16(src);
+ color->s = sbufReadU8(src);
+ color->v = sbufReadU8(src);
}
break;
case MSP_SET_LED_STRIP_CONFIG:
{
- i = read8();
+ i = sbufReadU8(src);
if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
- headSerialError(0);
+ return MSP_RESULT_ERROR;
break;
}
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
- *ledConfig = read32();
+ *ledConfig = sbufReadU32(src);
reevaluateLedConfig();
}
break;
case MSP_SET_LED_STRIP_MODECOLOR:
{
- ledModeIndex_e modeIdx = read8();
- int funIdx = read8();
- int color = read8();
+ ledModeIndex_e modeIdx = sbufReadU8(src);
+ int funIdx = sbufReadU8(src);
+ int color = sbufReadU8(src);
if (!setModeColor(modeIdx, funIdx, color))
- return false;
+ return MSP_RESULT_ERROR;
}
break;
#endif
case MSP_SET_ADVANCED_CONFIG :
- masterConfig.gyro_sync_denom = read8();
- masterConfig.pid_process_denom = read8();
- masterConfig.motorConfig.useUnsyncedPwm = read8();
+ masterConfig.gyro_sync_denom = sbufReadU8(src);
+ masterConfig.pid_process_denom = sbufReadU8(src);
+ masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
- masterConfig.motorConfig.motorPwmProtocol = constrain(read8(), 0, PWM_TYPE_MAX - 1);
+ masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
#else
- masterConfig.motorConfig.motorPwmProtocol = constrain(read8(), 0, PWM_TYPE_BRUSHED);
+ masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
#endif
- masterConfig.motorConfig.motorPwmRate = read16();
+ masterConfig.motorConfig.motorPwmRate = sbufReadU16(src);
break;
case MSP_SET_FILTER_CONFIG :
- masterConfig.gyro_soft_lpf_hz = read8();
- currentProfile->pidProfile.dterm_lpf_hz = read16();
- currentProfile->pidProfile.yaw_lpf_hz = read16();
+ masterConfig.gyro_soft_lpf_hz = sbufReadU8(src);
+ currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
+ currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
if (dataSize > 5) {
- masterConfig.gyro_soft_notch_hz_1 = read16();
- masterConfig.gyro_soft_notch_cutoff_1 = read16();
- currentProfile->pidProfile.dterm_notch_hz = read16();
- currentProfile->pidProfile.dterm_notch_cutoff = read16();
+ masterConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
+ masterConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
+ currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
+ currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
}
- if (currentPort->dataSize > 13) {
- masterConfig.gyro_soft_notch_hz_2 = read16();
- masterConfig.gyro_soft_notch_cutoff_2 = read16();
+ if (dataSize > 13) {
+ masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
+ masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
break;
case MSP_SET_PID_ADVANCED:
- currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
- currentProfile->pidProfile.yawItermIgnoreRate = read16();
- currentProfile->pidProfile.yaw_p_limit = read16();
- read8(); // reserved
- currentProfile->pidProfile.vbatPidCompensation = read8();
- currentProfile->pidProfile.setpointRelaxRatio = read8();
- currentProfile->pidProfile.dtermSetpointWeight = read8();
- read8(); // reserved
- read8(); // reserved
- currentProfile->pidProfile.itermThrottleGain = read8();
- currentProfile->pidProfile.rateAccelLimit = read16();
- currentProfile->pidProfile.yawRateAccelLimit = read16();
+ currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src);
+ currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src);
+ currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src);
+ sbufReadU8(src); // reserved
+ currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src);
+ currentProfile->pidProfile.setpointRelaxRatio = sbufReadU8(src);
+ currentProfile->pidProfile.dtermSetpointWeight = sbufReadU8(src);
+ sbufReadU8(src); // reserved
+ sbufReadU8(src); // reserved
+ currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
+ currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
+ currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
break;
case MSP_SET_SENSOR_CONFIG:
- masterConfig.acc_hardware = read8();
- masterConfig.baro_hardware = read8();
- masterConfig.mag_hardware = read8();
+ masterConfig.acc_hardware = sbufReadU8(src);
+ masterConfig.baro_hardware = sbufReadU8(src);
+ masterConfig.mag_hardware = sbufReadU8(src);
break;
case MSP_SET_NAME:
memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
for (i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
- masterConfig.name[i] = read8();
+ masterConfig.name[i] = sbufReadU8(src);
}
break;
default:
// we do not know how to handle the (valid) message, indicate error MSP $M!
- return false;
+ return MSP_RESULT_ERROR;
}
- headSerialReply(0);
- return true;
+ return MSP_RESULT_ACK;
}
-mspResult_e mspFcProcessCommand(mspPort_t *mspPort, mspPostProcessFnPtr *mspPostProcessFn)
+/*
+ * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
+ */
+mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
- mspResult_e ret = MSP_RESULT_ACK;
- currentPort = mspPort;
- mspPostProcessFn = NULL;
- if (!(processOutCommand(mspPort->cmdMSP, mspPostProcessFn) || processInCommand(mspPort->cmdMSP))) {
- headSerialError(0);
- ret = MSP_RESULT_ERROR;
+ int ret = MSP_RESULT_ACK;
+ sbuf_t *dst = &reply->buf;
+ sbuf_t *src = &cmd->buf;
+ const uint8_t cmdMSP = cmd->cmd;
+ // initialize reply by default
+ reply->cmd = cmd->cmd;
+
+ if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
+ ret = MSP_RESULT_ACK;
+#ifdef GPS
+ } else if (cmdMSP == MSP_WP) {
+ mspFcWpCommand(dst, src);
+ ret = MSP_RESULT_ACK;
+#endif
+#ifdef USE_FLASHFS
+ } else if (cmdMSP == MSP_DATAFLASH_READ) {
+ mspFcDataFlashReadCommand(dst, src);
+ ret = MSP_RESULT_ACK;
+#endif
+ } else {
+ ret = mspFcProcessInCommand(cmdMSP, src);
}
- tailSerialReply();
- mspPort->c_state = MSP_IDLE;
+ reply->result = ret;
return ret;
}
@@ -1955,18 +1797,13 @@ mspProcessCommandFnPtr mspFcInit(void)
return mspFcProcessCommand;
}
-void mspServerPush(mspPort_t *mspPort, uint8_t cmd, uint8_t *data, int len)
+void mspServerPush(mspPacket_t *push, uint8_t *data, int len)
{
- currentPort = mspPort;
- mspPort->cmdMSP = cmd;
-
- headSerialReply(len);
+ sbuf_t *dst = &push->buf;
while (len--) {
- serialize8(*data++);
+ sbufWriteU8(dst, *data++);
}
-
- tailSerialReply();
}
mspPushCommandFnPtr mspFcPushInit(void)
diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c
index 3c9ccd3807..fa684bdcdb 100644
--- a/src/main/io/canvas.c
+++ b/src/main/io/canvas.c
@@ -9,12 +9,20 @@
#ifdef CANVAS
+#include "drivers/system.h"
+
#include "io/cms_types.h"
#include "fc/fc_msp.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
+void canvasOutput(uint8_t cmd, uint8_t *buf, int len)
+{
+ mspSerialPush(cmd, buf, len);
+ delayMicroseconds(len * 150); // XXX Kludge!!!
+}
+
void canvasGetSize(uint8_t *pRows, uint8_t *pCols)
{
*pRows = 13;
@@ -25,7 +33,7 @@ void canvasBegin(void)
{
uint8_t subcmd[] = { 0 };
- mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd));
+ canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd));
}
void canvasHeartBeat(void)
@@ -37,21 +45,18 @@ void canvasEnd(void)
{
uint8_t subcmd[] = { 1 };
- mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd));
+ canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd));
}
void canvasClear(void)
{
uint8_t subcmd[] = { 2 };
- mspSerialPush(MSP_CANVAS, subcmd, sizeof(subcmd));
+ canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd));
}
void canvasWrite(uint8_t col, uint8_t row, char *string)
{
-
-//debug[0]++; // Let's capture excess canvas writes
-
int len;
char buf[30 + 4];
@@ -64,7 +69,7 @@ void canvasWrite(uint8_t col, uint8_t row, char *string)
buf[3] = 0;
memcpy((char *)&buf[4], string, len);
- mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4);
+ canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4);
}
screenFnVTable_t canvasVTable = {
diff --git a/src/main/io/cms.c b/src/main/io/cms.c
index a507f22b6c..014f9d00ac 100644
--- a/src/main/io/cms.c
+++ b/src/main/io/cms.c
@@ -715,14 +715,128 @@ static void simple_ftoa(int32_t value, char *floatString)
floatString[0] = ' ';
}
+void cmsDrawMenuEntry(OSD_Entry *p, uint8_t row, bool drawPolled)
+{
+ char buff[10];
+
+ switch (p->type) {
+ case OME_POS:; // Semi-colon required to add an empty statement
+#ifdef OSD
+ uint32_t address = (uint32_t)p->data;
+ uint16_t *val;
+
+ val = (uint16_t *)address;
+ if (!(*val & VISIBLE_FLAG))
+ break;
+#endif
+
+ case OME_Submenu:
+ if (cmsScreenCleared)
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, ">");
+ break;
+ case OME_Bool:
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ if (*((uint8_t *)(p->data))) {
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES");
+ } else {
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO ");
+ }
+ p->changed = false;
+ }
+ break;
+ case OME_TAB: {
+ if (p->changed || cmsScreenCleared) {
+ OSD_TAB_t *ptr = p->data;
+ cmsScreenWrite(RIGHT_MENU_COLUMN - 5, row, (char *)ptr->names[*ptr->val]);
+ p->changed = false;
+ }
+ break;
+ }
+ case OME_VISIBLE:
+#ifdef OSD
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ uint32_t address = (uint32_t)p->data;
+ uint16_t *val;
+
+ val = (uint16_t *)address;
+
+ if (VISIBLE(*val)) {
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, "YES");
+ } else {
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, "NO ");
+ }
+ p->changed = false;
+ }
+#endif
+ break;
+ case OME_UINT8:
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ OSD_UINT8_t *ptr = p->data;
+ itoa(*ptr->val, buff, 10);
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, " ");
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff);
+ p->changed = false;
+ }
+ break;
+ case OME_INT8:
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ OSD_INT8_t *ptr = p->data;
+ itoa(*ptr->val, buff, 10);
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, " ");
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff);
+ p->changed = false;
+ }
+ break;
+ case OME_UINT16:
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ OSD_UINT16_t *ptr = p->data;
+ itoa(*ptr->val, buff, 10);
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, " ");
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff);
+ p->changed = false;
+ }
+ break;
+ case OME_INT16:
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ OSD_UINT16_t *ptr = p->data;
+ itoa(*ptr->val, buff, 10);
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, " ");
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff);
+ p->changed = false;
+ }
+ break;
+ case OME_Poll_INT16:
+ if (p->data && drawPolled) {
+ OSD_UINT16_t *ptr = p->data;
+ itoa(*ptr->val, buff, 10);
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, " ");
+ cmsScreenWrite(RIGHT_MENU_COLUMN, row, buff);
+ }
+ break;
+ case OME_FLOAT:
+ if ((p->changed || cmsScreenCleared) && p->data) {
+ OSD_FLOAT_t *ptr = p->data;
+ simple_ftoa(*ptr->val * ptr->multipler, buff);
+ cmsScreenWrite(RIGHT_MENU_COLUMN - 1, row, " ");
+ cmsScreenWrite(RIGHT_MENU_COLUMN - 1, row, buff);
+ p->changed = false;
+ }
+ break;
+ case OME_OSD_Exit:
+ case OME_Label:
+ case OME_END:
+ case OME_Back:
+ break;
+ }
+}
+
void cmsDrawMenu(void)
{
uint8_t i = 0;
OSD_Entry *p;
- char buff[10];
uint8_t top = (cmsRows - currentMenuIdx) / 2 - 1;
- // XXX Need denom based on absolute time?
+ // XXX Need to denom based on absolute time
static uint8_t pollDenom = 0;
bool drawPolled = (++pollDenom % 8 == 0);
@@ -745,115 +859,7 @@ void cmsDrawMenu(void)
if (cmsScreenCleared)
cmsScreenWrite(LEFT_MENU_COLUMN + 2, i + top, p->text);
- switch (p->type) {
- case OME_POS:; // Semi-colon required to add an empty statement
-#ifdef OSD
- uint32_t address = (uint32_t)p->data;
- uint16_t *val;
-
- val = (uint16_t *)address;
- if (!(*val & VISIBLE_FLAG))
- break;
-#endif
-
- case OME_Submenu:
- if (cmsScreenCleared)
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, ">");
- break;
- case OME_Bool:
- if ((p->changed || cmsScreenCleared) && p->data) {
- if (*((uint8_t *)(p->data))) {
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES");
- } else {
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO ");
- }
- p->changed = false;
- }
- break;
- case OME_TAB: {
- if (p->changed || cmsScreenCleared) {
- OSD_TAB_t *ptr = p->data;
- cmsScreenWrite(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]);
- p->changed = false;
- }
- break;
- }
- case OME_VISIBLE:
-#ifdef OSD
- if ((p->changed || cmsScreenCleared) && p->data) {
- uint32_t address = (uint32_t)p->data;
- uint16_t *val;
-
- val = (uint16_t *)address;
-
- if (VISIBLE(*val)) {
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "YES");
- } else {
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, "NO ");
- }
- p->changed = false;
- }
-#endif
- break;
- case OME_UINT8:
- if ((p->changed || cmsScreenCleared) && p->data) {
- OSD_UINT8_t *ptr = p->data;
- itoa(*ptr->val, buff, 10);
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " ");
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff);
- p->changed = false;
- }
- break;
- case OME_INT8:
- if ((p->changed || cmsScreenCleared) && p->data) {
- OSD_INT8_t *ptr = p->data;
- itoa(*ptr->val, buff, 10);
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " ");
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff);
- p->changed = false;
- }
- break;
- case OME_UINT16:
- if ((p->changed || cmsScreenCleared) && p->data) {
- OSD_UINT16_t *ptr = p->data;
- itoa(*ptr->val, buff, 10);
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " ");
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff);
- p->changed = false;
- }
- break;
- case OME_INT16:
- if ((p->changed || cmsScreenCleared) && p->data) {
- OSD_UINT16_t *ptr = p->data;
- itoa(*ptr->val, buff, 10);
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " ");
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff);
- p->changed = false;
- }
- break;
- case OME_Poll_INT16:
- if (p->data && drawPolled) {
- OSD_UINT16_t *ptr = p->data;
- itoa(*ptr->val, buff, 10);
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, " ");
- cmsScreenWrite(RIGHT_MENU_COLUMN, i + top, buff);
- }
- break;
- case OME_FLOAT:
- if ((p->changed || cmsScreenCleared) && p->data) {
- OSD_FLOAT_t *ptr = p->data;
- simple_ftoa(*ptr->val * ptr->multipler, buff);
- cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, " ");
- cmsScreenWrite(RIGHT_MENU_COLUMN - 1, i + top, buff);
- p->changed = false;
- }
- break;
- case OME_OSD_Exit:
- case OME_Label:
- case OME_END:
- case OME_Back:
- break;
- }
+ cmsDrawMenuEntry(p, top + i, drawPolled);
i++;
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index b5529d5110..10887ec389 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -74,6 +74,12 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
#ifdef USE_UART6
SERIAL_PORT_USART6,
#endif
+#ifdef USE_UART7
+ SERIAL_PORT_USART7,
+#endif
+#ifdef USE_UART8
+ SERIAL_PORT_USART8,
+#endif
#ifdef USE_SOFTSERIAL1
SERIAL_PORT_SOFTSERIAL1,
#endif
@@ -307,19 +313,29 @@ serialPort_t *openSerialPort(
break;
#endif
#ifdef USE_UART4
- case SERIAL_PORT_USART4:
- serialPort = uartOpen(UART4, rxCallback, baudRate, mode, options);
- break;
+ case SERIAL_PORT_USART4:
+ serialPort = uartOpen(UART4, rxCallback, baudRate, mode, options);
+ break;
#endif
#ifdef USE_UART5
- case SERIAL_PORT_USART5:
- serialPort = uartOpen(UART5, rxCallback, baudRate, mode, options);
- break;
+ case SERIAL_PORT_USART5:
+ serialPort = uartOpen(UART5, rxCallback, baudRate, mode, options);
+ break;
#endif
#ifdef USE_UART6
- case SERIAL_PORT_USART6:
- serialPort = uartOpen(USART6, rxCallback, baudRate, mode, options);
- break;
+ case SERIAL_PORT_USART6:
+ serialPort = uartOpen(USART6, rxCallback, baudRate, mode, options);
+ break;
+#endif
+#ifdef USE_UART7
+ case SERIAL_PORT_USART7:
+ serialPort = uartOpen(UART7, rxCallback, baudRate, mode, options);
+ break;
+#endif
+#ifdef USE_UART8
+ case SERIAL_PORT_USART8:
+ serialPort = uartOpen(UART8, rxCallback, baudRate, mode, options);
+ break;
#endif
#ifdef USE_SOFTSERIAL1
case SERIAL_PORT_SOFTSERIAL1:
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 4d67c1eb97..c72a267a98 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -69,6 +69,8 @@ typedef enum {
SERIAL_PORT_USART4,
SERIAL_PORT_USART5,
SERIAL_PORT_USART6,
+ SERIAL_PORT_USART7,
+ SERIAL_PORT_USART8,
SERIAL_PORT_USB_VCP = 20,
SERIAL_PORT_SOFTSERIAL1 = 30,
SERIAL_PORT_SOFTSERIAL2,
diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c
index 712c5b47c2..f0bed4cfe8 100644
--- a/src/main/io/serial_4way.c
+++ b/src/main/io/serial_4way.c
@@ -44,6 +44,10 @@
#include "io/serial_4way_stk500v2.h"
#endif
+#if defined(USE_HAL_DRIVER)
+#define Bit_RESET GPIO_PIN_RESET
+#endif
+
#define USE_TXRX_LED
#ifdef USE_TXRX_LED
diff --git a/src/main/main.c b/src/main/main.c
index 216c2c1dcd..af2b46896e 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -129,15 +129,6 @@ extern uint8_t motorControlEnable;
serialPort_t *loopbackPort;
#endif
-#ifdef USE_DPRINTF
-#include "common/printf.h"
-#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
-serialPort_t *debugSerialPort = NULL;
-#define dprintf(x) if (debugSerialPort) printf x
-#else
-#define dprintf(x)
-#endif
-
typedef enum {
SYSTEM_STATE_INITIALISING = 0,
SYSTEM_STATE_CONFIG_LOADED = (1 << 0),
@@ -151,6 +142,10 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING;
void init(void)
{
+#ifdef USE_HAL_DRIVER
+ HAL_Init();
+#endif
+
printfSupportInit();
initEEPROM();
@@ -240,7 +235,9 @@ void init(void)
timerInit(); // timer must be initialized before any channel is allocated
+#if !defined(USE_HAL_DRIVER)
dmaInit();
+#endif
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
@@ -255,16 +252,6 @@ void init(void)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
-#ifdef USE_DPRINTF
- // Setup debugSerialPort
-
- debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
- if (debugSerialPort) {
- setPrintfSerialPort(debugSerialPort);
- dprintf(("debugSerialPort: OK\r\n"));
- }
-#endif
-
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
@@ -303,13 +290,13 @@ void init(void)
#endif
mixerConfigureOutput();
-
+ // pwmInit() needs to be called as soon as possible for ESC compatibility reasons
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
-
+/* temp until PGs are implemented. */
#ifdef INVERTER
initInverter();
#endif
@@ -334,6 +321,9 @@ void init(void)
spiInit(SPIDEV_3);
#endif
#endif
+#ifdef USE_SPI_DEVICE_4
+ spiInit(SPIDEV_4);
+#endif
#endif
#ifdef VTX
@@ -542,7 +532,7 @@ void init(void)
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
-#ifdef STM32F4
+#if defined(STM32F4) || defined(STM32F7)
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
@@ -652,7 +642,6 @@ int main(void)
}
#endif
-
#ifdef DEBUG_HARDFAULTS
//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/
/**
diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h
index 1f8aae7487..eaca6e2868 100644
--- a/src/main/msp/msp.h
+++ b/src/main/msp/msp.h
@@ -17,6 +17,8 @@
#pragma once
+#include "common/streambuf.h"
+
// return positive for ACK, negative on error, zero for no reply
typedef enum {
MSP_RESULT_ACK = 1,
@@ -24,8 +26,13 @@ typedef enum {
MSP_RESULT_NO_REPLY = 0
} mspResult_e;
+typedef struct mspPacket_s {
+ sbuf_t buf;
+ int16_t cmd;
+ int16_t result;
+} mspPacket_t;
+
struct serialPort_s;
typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
-struct mspPort_s;
-typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn);
-typedef void (*mspPushCommandFnPtr)(struct mspPort_s *, uint8_t, uint8_t *, int);
+typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
+typedef void (*mspPushCommandFnPtr)(mspPacket_t *push, uint8_t *, int);
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 29d23ce8be..212890e977 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -21,23 +21,19 @@
#include "platform.h"
+#include "common/streambuf.h"
#include "common/utils.h"
-#include "drivers/buf_writer.h"
#include "drivers/serial.h"
-#include "fc/runtime_config.h"
-
#include "io/serial.h"
#include "msp/msp.h"
#include "msp/msp_serial.h"
-
static mspProcessCommandFnPtr mspProcessCommandFn;
static mspPushCommandFnPtr mspPushCommandFn;
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
-bufWriter_t *writer;
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
@@ -79,7 +75,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
}
}
-bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
+static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
if (mspPort->c_state == MSP_IDLE) {
if (c == '$') {
@@ -94,12 +90,10 @@ bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
} else if (mspPort->c_state == MSP_HEADER_ARROW) {
if (c > MSP_PORT_INBUF_SIZE) {
mspPort->c_state = MSP_IDLE;
-
} else {
mspPort->dataSize = c;
mspPort->offset = 0;
mspPort->checksum = 0;
- mspPort->indRX = 0;
mspPort->checksum ^= c;
mspPort->c_state = MSP_HEADER_SIZE;
}
@@ -120,12 +114,64 @@ bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
return true;
}
-static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
+static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
{
- mspPostProcessFnPtr mspPostProcessFn = NULL;
- mspProcessCommandFn(mspPort, &mspPostProcessFn);
+ while (len-- > 0) {
+ checksum ^= *data++;
+ }
+ return checksum;
+}
- mspPort->c_state = MSP_IDLE;
+#define JUMBO_FRAME_SIZE_LIMIT 255
+
+static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
+{
+ serialBeginWrite(msp->port);
+ const int len = sbufBytesRemaining(&packet->buf);
+ const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
+ const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
+ serialWriteBuf(msp->port, hdr, sizeof(hdr));
+ uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field
+ if (len >= JUMBO_FRAME_SIZE_LIMIT) {
+ serialWrite(msp->port, len & 0xff);
+ checksum ^= len & 0xff;
+ serialWrite(msp->port, (len >> 8) & 0xff);
+ checksum ^= (len >> 8) & 0xff;
+ }
+ if (len > 0) {
+ serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
+ checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
+ }
+ serialWrite(msp->port, checksum);
+ serialEndWrite(msp->port);
+}
+
+static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
+{
+ static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
+
+ mspPacket_t reply = {
+ .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
+ .cmd = -1,
+ .result = 0,
+ };
+ uint8_t *outBufHead = reply.buf.ptr;
+
+ mspPacket_t command = {
+ .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
+ .cmd = msp->cmdMSP,
+ .result = 0,
+ };
+
+ mspPostProcessFnPtr mspPostProcessFn = NULL;
+ const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
+
+ if (status != MSP_RESULT_NO_REPLY) {
+ sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
+ mspSerialEncode(msp, &reply);
+ }
+
+ msp->c_state = MSP_IDLE;
return mspPostProcessFn;
}
@@ -141,11 +187,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
if (!mspPort->port) {
continue;
}
-
- // Big enough to fit a MSP_STATUS in one write.
- uint8_t buf[sizeof(bufWriter_t) + 20];
- writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
-
mspPostProcessFnPtr mspPostProcessFn = NULL;
while (serialRxBytesWaiting(mspPort->port)) {
@@ -161,9 +202,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
break; // process one command at a time so as not to block.
}
}
-
- bufWriterFlush(writer);
-
if (mspPostProcessFn) {
waitForSerialPortToFinishTransmitting(mspPort->port);
mspPostProcessFn(mspPort->port);
@@ -178,22 +216,27 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
mspSerialAllocatePorts();
}
-void mspSerialPush(uint8_t cmd, uint8_t *data, int buflen)
+void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
{
+ static uint8_t pushBuf[30];
+
+ mspPacket_t push = {
+ .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), },
+ .cmd = cmd,
+ .result = 0,
+ };
+
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
mspPort_t * const mspPort = &mspPorts[portIndex];
if (!mspPort->port) {
continue;
}
- // Big enough for a OSD line
- uint8_t buf[sizeof(bufWriter_t) + 30];
+ mspPushCommandFn(&push, data, datalen);
- writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
+ sbufSwitchToReader(&push.buf, pushBuf);
- mspPushCommandFn(mspPort, cmd, data, buflen);
-
- bufWriterFlush(writer);
+ mspSerialEncode(mspPort, &push);
}
}
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index ceb6b2cf89..6624dcbd36 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -17,7 +17,6 @@
#pragma once
-
#include "msp/msp.h"
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
@@ -38,7 +37,18 @@ typedef enum {
MSP_SKIP_NON_MSP_DATA
} mspEvaluateNonMspData_e;
-#define MSP_PORT_INBUF_SIZE 64
+#define MSP_PORT_INBUF_SIZE 192
+#ifdef USE_FLASHFS
+#ifdef STM32F1
+#define MSP_PORT_DATAFLASH_BUFFER_SIZE 1024
+#else
+#define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096
+#endif
+#define MSP_PORT_DATAFLASH_INFO_SIZE 16
+#define MSP_PORT_OUTBUF_SIZE (MSP_PORT_DATAFLASH_BUFFER_SIZE + MSP_PORT_DATAFLASH_INFO_SIZE)
+#else
+#define MSP_PORT_OUTBUF_SIZE 256
+#endif
struct serialPort_s;
typedef struct mspPort_s {
@@ -46,16 +56,12 @@ typedef struct mspPort_s {
uint8_t offset;
uint8_t dataSize;
uint8_t checksum;
- uint8_t indRX;
- uint8_t inBuf[MSP_PORT_INBUF_SIZE];
- mspState_e c_state;
uint8_t cmdMSP;
+ mspState_e c_state;
+ uint8_t inBuf[MSP_PORT_INBUF_SIZE];
} mspPort_t;
-struct bufWriter_s;
-extern struct bufWriter_s *writer;
-
void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
void mspSerialAllocatePorts(void);
diff --git a/src/main/platform.h b/src/main/platform.h
index 458146c9b2..221222263a 100644
--- a/src/main/platform.h
+++ b/src/main/platform.h
@@ -17,6 +17,18 @@
#pragma once
+#if defined(STM32F745xx)
+#include "stm32f7xx.h"
+#include "stm32f7xx_hal.h"
+
+// Chip Unique ID on F7
+#define U_ID_0 (*(uint32_t*)0x1ff0f420)
+#define U_ID_1 (*(uint32_t*)0x1ff0f424)
+#define U_ID_2 (*(uint32_t*)0x1ff0f428)
+
+#define STM32F7
+#endif
+
#if defined(STM32F40_41xxx) || defined (STM32F411xE)
#include "stm32f4xx_conf.h"
#include "stm32f4xx_rcc.h"
diff --git a/src/main/startup/startup_stm32f745xx.s b/src/main/startup/startup_stm32f745xx.s
new file mode 100644
index 0000000000..f548aafdc5
--- /dev/null
+++ b/src/main/startup/startup_stm32f745xx.s
@@ -0,0 +1,587 @@
+/**
+ ******************************************************************************
+ * @file startup_stm32f745xx.s
+ * @author MCD Application Team
+ * @version V1.1.0
+ * @date 22-April-2016
+ * @brief STM32F745xx Devices vector table for GCC toolchain based application.
+ * This module performs:
+ * - Set the initial SP
+ * - Set the initial PC == Reset_Handler,
+ * - Set the vector table entries with the exceptions ISR address
+ * - Branches to main in the C library (which eventually
+ * calls main()).
+ * After Reset the Cortex-M7 processor is in Thread mode,
+ * priority is Privileged, and the Stack is set to Main.
+ ******************************************************************************
+ * @attention
+ *
+ * © COPYRIGHT 2016 STMicroelectronics
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+ .syntax unified
+ .cpu cortex-m7
+ .fpu softvfp
+ .thumb
+
+.global g_pfnVectors
+.global Default_Handler
+
+/* start address for the initialization values of the .data section.
+defined in linker script */
+.word _sidata
+/* start address for the .data section. defined in linker script */
+.word _sdata
+/* end address for the .data section. defined in linker script */
+.word _edata
+/* start address for the .bss section. defined in linker script */
+.word _sbss
+/* end address for the .bss section. defined in linker script */
+.word _ebss
+/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
+
+/**
+ * @brief This is the code that gets called when the processor first
+ * starts execution following a reset event. Only the absolutely
+ * necessary set is performed, after which the application
+ * supplied main() routine is called.
+ * @param None
+ * @retval : None
+*/
+
+ .section .text.Reset_Handler
+ .weak Reset_Handler
+ .type Reset_Handler, %function
+Reset_Handler:
+/* Copy the data segment initializers from flash to SRAM */
+ movs r1, #0
+ b LoopCopyDataInit
+
+CopyDataInit:
+ ldr r3, =_sidata
+ ldr r3, [r3, r1]
+ str r3, [r0, r1]
+ adds r1, r1, #4
+
+LoopCopyDataInit:
+ ldr r0, =_sdata
+ ldr r3, =_edata
+ adds r2, r0, r1
+ cmp r2, r3
+ bcc CopyDataInit
+ ldr r2, =_sbss
+ b LoopFillZerobss
+/* Zero fill the bss segment. */
+FillZerobss:
+ movs r3, #0
+ str r3, [r2], #4
+
+LoopFillZerobss:
+ ldr r3, = _ebss
+ cmp r2, r3
+ bcc FillZerobss
+
+/*FPU settings*/
+ ldr r0, =0xE000ED88 /* Enable CP10,CP11 */
+ ldr r1,[r0]
+ orr r1,r1,#(0xF << 20)
+ str r1,[r0]
+
+/* Call the clock system initialization function.*/
+ bl SystemInit
+/* Call the application's entry point.*/
+ bl main
+ bx lr
+
+LoopForever:
+ b LoopForever
+
+.size Reset_Handler, .-Reset_Handler
+
+/**
+ * @brief This is the code that gets called when the processor receives an
+ * unexpected interrupt. This simply enters an infinite loop, preserving
+ * the system state for examination by a debugger.
+ * @param None
+ * @retval None
+*/
+ .section .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+ b Infinite_Loop
+ .size Default_Handler, .-Default_Handler
+/******************************************************************************
+*
+* The minimal vector table for a Cortex M7. Note that the proper constructs
+* must be placed on this to ensure that it ends up at physical address
+* 0x0000.0000.
+*
+*******************************************************************************/
+ .section .isr_vector,"a",%progbits
+ .type g_pfnVectors, %object
+ .size g_pfnVectors, .-g_pfnVectors
+
+
+g_pfnVectors:
+ .word _estack
+ .word Reset_Handler
+ .word NMI_Handler
+ .word HardFault_Handler
+ .word MemManage_Handler
+ .word BusFault_Handler
+ .word UsageFault_Handler
+ .word 0
+ .word 0
+ .word 0
+ .word 0
+ .word SVC_Handler
+ .word DebugMon_Handler
+ .word 0
+ .word PendSV_Handler
+ .word SysTick_Handler
+
+ /* External Interrupts */
+ .word WWDG_IRQHandler /* Window WatchDog */
+ .word PVD_IRQHandler /* PVD through EXTI Line detection */
+ .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */
+ .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */
+ .word FLASH_IRQHandler /* FLASH */
+ .word RCC_IRQHandler /* RCC */
+ .word EXTI0_IRQHandler /* EXTI Line0 */
+ .word EXTI1_IRQHandler /* EXTI Line1 */
+ .word EXTI2_IRQHandler /* EXTI Line2 */
+ .word EXTI3_IRQHandler /* EXTI Line3 */
+ .word EXTI4_IRQHandler /* EXTI Line4 */
+ .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */
+ .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */
+ .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */
+ .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */
+ .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */
+ .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */
+ .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */
+ .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */
+ .word CAN1_TX_IRQHandler /* CAN1 TX */
+ .word CAN1_RX0_IRQHandler /* CAN1 RX0 */
+ .word CAN1_RX1_IRQHandler /* CAN1 RX1 */
+ .word CAN1_SCE_IRQHandler /* CAN1 SCE */
+ .word EXTI9_5_IRQHandler /* External Line[9:5]s */
+ .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */
+ .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */
+ .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */
+ .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */
+ .word TIM2_IRQHandler /* TIM2 */
+ .word TIM3_IRQHandler /* TIM3 */
+ .word TIM4_IRQHandler /* TIM4 */
+ .word I2C1_EV_IRQHandler /* I2C1 Event */
+ .word I2C1_ER_IRQHandler /* I2C1 Error */
+ .word I2C2_EV_IRQHandler /* I2C2 Event */
+ .word I2C2_ER_IRQHandler /* I2C2 Error */
+ .word SPI1_IRQHandler /* SPI1 */
+ .word SPI2_IRQHandler /* SPI2 */
+ .word USART1_IRQHandler /* USART1 */
+ .word USART2_IRQHandler /* USART2 */
+ .word USART3_IRQHandler /* USART3 */
+ .word EXTI15_10_IRQHandler /* External Line[15:10]s */
+ .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */
+ .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */
+ .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */
+ .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */
+ .word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */
+ .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */
+ .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */
+ .word FMC_IRQHandler /* FMC */
+ .word SDMMC1_IRQHandler /* SDMMC1 */
+ .word TIM5_IRQHandler /* TIM5 */
+ .word SPI3_IRQHandler /* SPI3 */
+ .word UART4_IRQHandler /* UART4 */
+ .word UART5_IRQHandler /* UART5 */
+ .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */
+ .word TIM7_IRQHandler /* TIM7 */
+ .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */
+ .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */
+ .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */
+ .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */
+ .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */
+ .word ETH_IRQHandler /* Ethernet */
+ .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */
+ .word CAN2_TX_IRQHandler /* CAN2 TX */
+ .word CAN2_RX0_IRQHandler /* CAN2 RX0 */
+ .word CAN2_RX1_IRQHandler /* CAN2 RX1 */
+ .word CAN2_SCE_IRQHandler /* CAN2 SCE */
+ .word OTG_FS_IRQHandler /* USB OTG FS */
+ .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */
+ .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */
+ .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */
+ .word USART6_IRQHandler /* USART6 */
+ .word I2C3_EV_IRQHandler /* I2C3 event */
+ .word I2C3_ER_IRQHandler /* I2C3 error */
+ .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */
+ .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */
+ .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */
+ .word OTG_HS_IRQHandler /* USB OTG HS */
+ .word DCMI_IRQHandler /* DCMI */
+ .word 0 /* Reserved */
+ .word RNG_IRQHandler /* Rng */
+ .word FPU_IRQHandler /* FPU */
+ .word UART7_IRQHandler /* UART7 */
+ .word UART8_IRQHandler /* UART8 */
+ .word SPI4_IRQHandler /* SPI4 */
+ .word SPI5_IRQHandler /* SPI5 */
+ .word SPI6_IRQHandler /* SPI6 */
+ .word SAI1_IRQHandler /* SAI1 */
+ .word 0 /* Reserved */
+ .word 0 /* Reserved */
+ .word DMA2D_IRQHandler /* DMA2D */
+ .word SAI2_IRQHandler /* SAI2 */
+ .word QUADSPI_IRQHandler /* QUADSPI */
+ .word LPTIM1_IRQHandler /* LPTIM1 */
+ .word CEC_IRQHandler /* HDMI_CEC */
+ .word I2C4_EV_IRQHandler /* I2C4 Event */
+ .word I2C4_ER_IRQHandler /* I2C4 Error */
+ .word SPDIF_RX_IRQHandler /* SPDIF_RX */
+
+/*******************************************************************************
+*
+* Provide weak aliases for each Exception handler to the Default_Handler.
+* As they are weak aliases, any function with the same name will override
+* this definition.
+*
+*******************************************************************************/
+ .weak NMI_Handler
+ .thumb_set NMI_Handler,Default_Handler
+
+ .weak HardFault_Handler
+ .thumb_set HardFault_Handler,Default_Handler
+
+ .weak MemManage_Handler
+ .thumb_set MemManage_Handler,Default_Handler
+
+ .weak BusFault_Handler
+ .thumb_set BusFault_Handler,Default_Handler
+
+ .weak UsageFault_Handler
+ .thumb_set UsageFault_Handler,Default_Handler
+
+ .weak SVC_Handler
+ .thumb_set SVC_Handler,Default_Handler
+
+ .weak DebugMon_Handler
+ .thumb_set DebugMon_Handler,Default_Handler
+
+ .weak PendSV_Handler
+ .thumb_set PendSV_Handler,Default_Handler
+
+ .weak SysTick_Handler
+ .thumb_set SysTick_Handler,Default_Handler
+
+ .weak WWDG_IRQHandler
+ .thumb_set WWDG_IRQHandler,Default_Handler
+
+ .weak PVD_IRQHandler
+ .thumb_set PVD_IRQHandler,Default_Handler
+
+ .weak TAMP_STAMP_IRQHandler
+ .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
+
+ .weak RTC_WKUP_IRQHandler
+ .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+ .weak FLASH_IRQHandler
+ .thumb_set FLASH_IRQHandler,Default_Handler
+
+ .weak RCC_IRQHandler
+ .thumb_set RCC_IRQHandler,Default_Handler
+
+ .weak EXTI0_IRQHandler
+ .thumb_set EXTI0_IRQHandler,Default_Handler
+
+ .weak EXTI1_IRQHandler
+ .thumb_set EXTI1_IRQHandler,Default_Handler
+
+ .weak EXTI2_IRQHandler
+ .thumb_set EXTI2_IRQHandler,Default_Handler
+
+ .weak EXTI3_IRQHandler
+ .thumb_set EXTI3_IRQHandler,Default_Handler
+
+ .weak EXTI4_IRQHandler
+ .thumb_set EXTI4_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream0_IRQHandler
+ .thumb_set DMA1_Stream0_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream1_IRQHandler
+ .thumb_set DMA1_Stream1_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream2_IRQHandler
+ .thumb_set DMA1_Stream2_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream3_IRQHandler
+ .thumb_set DMA1_Stream3_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream4_IRQHandler
+ .thumb_set DMA1_Stream4_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream5_IRQHandler
+ .thumb_set DMA1_Stream5_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream6_IRQHandler
+ .thumb_set DMA1_Stream6_IRQHandler,Default_Handler
+
+ .weak ADC_IRQHandler
+ .thumb_set ADC_IRQHandler,Default_Handler
+
+ .weak CAN1_TX_IRQHandler
+ .thumb_set CAN1_TX_IRQHandler,Default_Handler
+
+ .weak CAN1_RX0_IRQHandler
+ .thumb_set CAN1_RX0_IRQHandler,Default_Handler
+
+ .weak CAN1_RX1_IRQHandler
+ .thumb_set CAN1_RX1_IRQHandler,Default_Handler
+
+ .weak CAN1_SCE_IRQHandler
+ .thumb_set CAN1_SCE_IRQHandler,Default_Handler
+
+ .weak EXTI9_5_IRQHandler
+ .thumb_set EXTI9_5_IRQHandler,Default_Handler
+
+ .weak TIM1_BRK_TIM9_IRQHandler
+ .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
+
+ .weak TIM1_UP_TIM10_IRQHandler
+ .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
+
+ .weak TIM1_TRG_COM_TIM11_IRQHandler
+ .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
+
+ .weak TIM1_CC_IRQHandler
+ .thumb_set TIM1_CC_IRQHandler,Default_Handler
+
+ .weak TIM2_IRQHandler
+ .thumb_set TIM2_IRQHandler,Default_Handler
+
+ .weak TIM3_IRQHandler
+ .thumb_set TIM3_IRQHandler,Default_Handler
+
+ .weak TIM4_IRQHandler
+ .thumb_set TIM4_IRQHandler,Default_Handler
+
+ .weak I2C1_EV_IRQHandler
+ .thumb_set I2C1_EV_IRQHandler,Default_Handler
+
+ .weak I2C1_ER_IRQHandler
+ .thumb_set I2C1_ER_IRQHandler,Default_Handler
+
+ .weak I2C2_EV_IRQHandler
+ .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+ .weak I2C2_ER_IRQHandler
+ .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+ .weak SPI1_IRQHandler
+ .thumb_set SPI1_IRQHandler,Default_Handler
+
+ .weak SPI2_IRQHandler
+ .thumb_set SPI2_IRQHandler,Default_Handler
+
+ .weak USART1_IRQHandler
+ .thumb_set USART1_IRQHandler,Default_Handler
+
+ .weak USART2_IRQHandler
+ .thumb_set USART2_IRQHandler,Default_Handler
+
+ .weak USART3_IRQHandler
+ .thumb_set USART3_IRQHandler,Default_Handler
+
+ .weak EXTI15_10_IRQHandler
+ .thumb_set EXTI15_10_IRQHandler,Default_Handler
+
+ .weak RTC_Alarm_IRQHandler
+ .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+ .weak OTG_FS_WKUP_IRQHandler
+ .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
+
+ .weak TIM8_BRK_TIM12_IRQHandler
+ .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
+
+ .weak TIM8_UP_TIM13_IRQHandler
+ .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
+
+ .weak TIM8_TRG_COM_TIM14_IRQHandler
+ .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
+
+ .weak TIM8_CC_IRQHandler
+ .thumb_set TIM8_CC_IRQHandler,Default_Handler
+
+ .weak DMA1_Stream7_IRQHandler
+ .thumb_set DMA1_Stream7_IRQHandler,Default_Handler
+
+ .weak FMC_IRQHandler
+ .thumb_set FMC_IRQHandler,Default_Handler
+
+ .weak SDMMC1_IRQHandler
+ .thumb_set SDMMC1_IRQHandler,Default_Handler
+
+ .weak TIM5_IRQHandler
+ .thumb_set TIM5_IRQHandler,Default_Handler
+
+ .weak SPI3_IRQHandler
+ .thumb_set SPI3_IRQHandler,Default_Handler
+
+ .weak UART4_IRQHandler
+ .thumb_set UART4_IRQHandler,Default_Handler
+
+ .weak UART5_IRQHandler
+ .thumb_set UART5_IRQHandler,Default_Handler
+
+ .weak TIM6_DAC_IRQHandler
+ .thumb_set TIM6_DAC_IRQHandler,Default_Handler
+
+ .weak TIM7_IRQHandler
+ .thumb_set TIM7_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream0_IRQHandler
+ .thumb_set DMA2_Stream0_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream1_IRQHandler
+ .thumb_set DMA2_Stream1_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream2_IRQHandler
+ .thumb_set DMA2_Stream2_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream3_IRQHandler
+ .thumb_set DMA2_Stream3_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream4_IRQHandler
+ .thumb_set DMA2_Stream4_IRQHandler,Default_Handler
+
+ .weak ETH_IRQHandler
+ .thumb_set ETH_IRQHandler,Default_Handler
+
+ .weak ETH_WKUP_IRQHandler
+ .thumb_set ETH_WKUP_IRQHandler,Default_Handler
+
+ .weak CAN2_TX_IRQHandler
+ .thumb_set CAN2_TX_IRQHandler,Default_Handler
+
+ .weak CAN2_RX0_IRQHandler
+ .thumb_set CAN2_RX0_IRQHandler,Default_Handler
+
+ .weak CAN2_RX1_IRQHandler
+ .thumb_set CAN2_RX1_IRQHandler,Default_Handler
+
+ .weak CAN2_SCE_IRQHandler
+ .thumb_set CAN2_SCE_IRQHandler,Default_Handler
+
+ .weak OTG_FS_IRQHandler
+ .thumb_set OTG_FS_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream5_IRQHandler
+ .thumb_set DMA2_Stream5_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream6_IRQHandler
+ .thumb_set DMA2_Stream6_IRQHandler,Default_Handler
+
+ .weak DMA2_Stream7_IRQHandler
+ .thumb_set DMA2_Stream7_IRQHandler,Default_Handler
+
+ .weak USART6_IRQHandler
+ .thumb_set USART6_IRQHandler,Default_Handler
+
+ .weak I2C3_EV_IRQHandler
+ .thumb_set I2C3_EV_IRQHandler,Default_Handler
+
+ .weak I2C3_ER_IRQHandler
+ .thumb_set I2C3_ER_IRQHandler,Default_Handler
+
+ .weak OTG_HS_EP1_OUT_IRQHandler
+ .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
+
+ .weak OTG_HS_EP1_IN_IRQHandler
+ .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
+
+ .weak OTG_HS_WKUP_IRQHandler
+ .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
+
+ .weak OTG_HS_IRQHandler
+ .thumb_set OTG_HS_IRQHandler,Default_Handler
+
+ .weak DCMI_IRQHandler
+ .thumb_set DCMI_IRQHandler,Default_Handler
+
+ .weak RNG_IRQHandler
+ .thumb_set RNG_IRQHandler,Default_Handler
+
+ .weak FPU_IRQHandler
+ .thumb_set FPU_IRQHandler,Default_Handler
+
+ .weak UART7_IRQHandler
+ .thumb_set UART7_IRQHandler,Default_Handler
+
+ .weak UART8_IRQHandler
+ .thumb_set UART8_IRQHandler,Default_Handler
+
+ .weak SPI4_IRQHandler
+ .thumb_set SPI4_IRQHandler,Default_Handler
+
+ .weak SPI5_IRQHandler
+ .thumb_set SPI5_IRQHandler,Default_Handler
+
+ .weak SPI6_IRQHandler
+ .thumb_set SPI6_IRQHandler,Default_Handler
+
+ .weak SAI1_IRQHandler
+ .thumb_set SAI1_IRQHandler,Default_Handler
+
+ .weak DMA2D_IRQHandler
+ .thumb_set DMA2D_IRQHandler,Default_Handler
+
+ .weak SAI2_IRQHandler
+ .thumb_set SAI2_IRQHandler,Default_Handler
+
+ .weak QUADSPI_IRQHandler
+ .thumb_set QUADSPI_IRQHandler,Default_Handler
+
+ .weak LPTIM1_IRQHandler
+ .thumb_set LPTIM1_IRQHandler,Default_Handler
+
+ .weak CEC_IRQHandler
+ .thumb_set CEC_IRQHandler,Default_Handler
+
+ .weak I2C4_EV_IRQHandler
+ .thumb_set I2C4_EV_IRQHandler,Default_Handler
+
+ .weak I2C4_ER_IRQHandler
+ .thumb_set I2C4_ER_IRQHandler,Default_Handler
+
+ .weak SPDIF_RX_IRQHandler
+ .thumb_set SPDIF_RX_IRQHandler,Default_Handler
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+
diff --git a/src/main/target/ANYFCF7/stm32f7xx_hal_conf.h b/src/main/target/ANYFCF7/stm32f7xx_hal_conf.h
new file mode 100644
index 0000000000..4bedcc08d6
--- /dev/null
+++ b/src/main/target/ANYFCF7/stm32f7xx_hal_conf.h
@@ -0,0 +1,446 @@
+/**
+ ******************************************************************************
+ * @file stm32f7xx_hal_conf.h
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief HAL configuration file.
+ ******************************************************************************
+ * @attention
+ *
+ * © COPYRIGHT(c) 2016 STMicroelectronics
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __STM32F7xx_HAL_CONF_H
+#define __STM32F7xx_HAL_CONF_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+
+/* ########################## Module Selection ############################## */
+/**
+ * @brief This is the list of modules to be used in the HAL driver
+ */
+#define HAL_MODULE_ENABLED
+ #define HAL_ADC_MODULE_ENABLED
+/* #define HAL_CAN_MODULE_ENABLED */
+/* #define HAL_CEC_MODULE_ENABLED */
+/* #define HAL_CRC_MODULE_ENABLED */
+/* #define HAL_CRYP_MODULE_ENABLED */
+/* #define HAL_DAC_MODULE_ENABLED */
+/* #define HAL_DCMI_MODULE_ENABLED */
+ #define HAL_DMA_MODULE_ENABLED
+// #define HAL_DMA2D_MODULE_ENABLED
+/* #define HAL_ETH_MODULE_ENABLED */
+#define HAL_FLASH_MODULE_ENABLED
+/* #define HAL_NAND_MODULE_ENABLED */
+/* #define HAL_NOR_MODULE_ENABLED */
+/* #define HAL_SRAM_MODULE_ENABLED */
+/* #define HAL_SDRAM_MODULE_ENABLED */
+/* #define HAL_HASH_MODULE_ENABLED */
+#define HAL_GPIO_MODULE_ENABLED
+ #define HAL_I2C_MODULE_ENABLED
+/* #define HAL_I2S_MODULE_ENABLED */
+/* #define HAL_IWDG_MODULE_ENABLED */
+/* #define HAL_LPTIM_MODULE_ENABLED */
+/* #define HAL_LTDC_MODULE_ENABLED */
+#define HAL_PWR_MODULE_ENABLED
+/* #define HAL_QSPI_MODULE_ENABLED */
+#define HAL_RCC_MODULE_ENABLED
+/* #define HAL_RNG_MODULE_ENABLED */
+//#define HAL_RTC_MODULE_ENABLED
+/* #define HAL_SAI_MODULE_ENABLED */
+/* #define HAL_SD_MODULE_ENABLED */
+/* #define HAL_SPDIFRX_MODULE_ENABLED */
+ #define HAL_SPI_MODULE_ENABLED
+ #define HAL_TIM_MODULE_ENABLED
+ #define HAL_UART_MODULE_ENABLED
+ #define HAL_USART_MODULE_ENABLED
+/* #define HAL_IRDA_MODULE_ENABLED */
+/* #define HAL_SMARTCARD_MODULE_ENABLED */
+/* #define HAL_WWDG_MODULE_ENABLED */
+#define HAL_CORTEX_MODULE_ENABLED
+#define HAL_PCD_MODULE_ENABLED
+/* #define HAL_HCD_MODULE_ENABLED */
+/* #define HAL_DFSDM_MODULE_ENABLED */
+/* #define HAL_DSI_MODULE_ENABLED */
+/* #define HAL_JPEG_MODULE_ENABLED */
+/* #define HAL_MDIOS_MODULE_ENABLED */
+
+
+/* ########################## HSE/HSI Values adaptation ##################### */
+/**
+ * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
+ * This value is used by the RCC HAL module to compute the system frequency
+ * (when HSE is used as system clock source, directly or through the PLL).
+ */
+#if !defined (HSE_VALUE)
+ #define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined (HSE_STARTUP_TIMEOUT)
+ #define HSE_STARTUP_TIMEOUT ((uint32_t)100U) /*!< Time out for HSE start up, in ms */
+#endif /* HSE_STARTUP_TIMEOUT */
+
+/**
+ * @brief Internal High Speed oscillator (HSI) value.
+ * This value is used by the RCC HAL module to compute the system frequency
+ * (when HSI is used as system clock source, directly or through the PLL).
+ */
+#if !defined (HSI_VALUE)
+ #define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+ * @brief Internal Low Speed oscillator (LSI) value.
+ */
+#if !defined (LSI_VALUE)
+ #define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/
+#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
+ The real value may vary depending on the variations
+ in voltage and temperature. */
+/**
+ * @brief External Low Speed oscillator (LSE) value.
+ */
+#if !defined (LSE_VALUE)
+ #define LSE_VALUE ((uint32_t)32768U) /*!< Value of the External Low Speed oscillator in Hz */
+#endif /* LSE_VALUE */
+
+#if !defined (LSE_STARTUP_TIMEOUT)
+ #define LSE_STARTUP_TIMEOUT ((uint32_t)5000U) /*!< Time out for LSE start up, in ms */
+#endif /* LSE_STARTUP_TIMEOUT */
+
+/**
+ * @brief External clock source for I2S peripheral
+ * This value is used by the I2S HAL module to compute the I2S clock source
+ * frequency, this source is inserted directly through I2S_CKIN pad.
+ */
+#if !defined (EXTERNAL_CLOCK_VALUE)
+ #define EXTERNAL_CLOCK_VALUE ((uint32_t)12288000U) /*!< Value of the Internal oscillator in Hz*/
+#endif /* EXTERNAL_CLOCK_VALUE */
+
+/* Tip: To avoid modifying this file each time you need to use different HSE,
+ === you can define the HSE value in your toolchain compiler preprocessor. */
+
+/* ########################### System Configuration ######################### */
+/**
+ * @brief This is the HAL system configuration section
+ */
+#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
+#define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */
+#define USE_RTOS 0U
+#define PREFETCH_ENABLE 1U
+#define ART_ACCLERATOR_ENABLE 1U /* To enable instruction cache and prefetch */
+
+/* ########################## Assert Selection ############################## */
+/**
+ * @brief Uncomment the line below to expanse the "assert_param" macro in the
+ * HAL drivers code
+ */
+/* #define USE_FULL_ASSERT 1 */
+
+/* ################## Ethernet peripheral configuration for NUCLEO 144 board ##################### */
+
+/* Section 1 : Ethernet peripheral configuration */
+
+/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */
+#define MAC_ADDR0 2U
+#define MAC_ADDR1 0U
+#define MAC_ADDR2 0U
+#define MAC_ADDR3 0U
+#define MAC_ADDR4 0U
+#define MAC_ADDR5 0U
+
+/* Definition of the Ethernet driver buffers size and count */
+#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
+#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
+#define ETH_RXBUFNB ((uint32_t)5) /* 5 Rx buffers of size ETH_RX_BUF_SIZE */
+#define ETH_TXBUFNB ((uint32_t)5) /* 5 Tx buffers of size ETH_TX_BUF_SIZE */
+
+/* Section 2: PHY configuration section */
+/* LAN8742A PHY Address*/
+#define LAN8742A_PHY_ADDRESS 0x00
+/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
+#define PHY_RESET_DELAY ((uint32_t)0x00000FFF)
+/* PHY Configuration delay */
+#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFF)
+
+#define PHY_READ_TO ((uint32_t)0x0000FFFF)
+#define PHY_WRITE_TO ((uint32_t)0x0000FFFF)
+
+/* Section 3: Common PHY Registers */
+
+#define PHY_BCR ((uint16_t)0x00) /*!< Transceiver Basic Control Register */
+#define PHY_BSR ((uint16_t)0x01) /*!< Transceiver Basic Status Register */
+
+#define PHY_RESET ((uint16_t)0x8000) /*!< PHY Reset */
+#define PHY_LOOPBACK ((uint16_t)0x4000) /*!< Select loop-back mode */
+#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100) /*!< Set the full-duplex mode at 100 Mb/s */
+#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000) /*!< Set the half-duplex mode at 100 Mb/s */
+#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100) /*!< Set the full-duplex mode at 10 Mb/s */
+#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000) /*!< Set the half-duplex mode at 10 Mb/s */
+#define PHY_AUTONEGOTIATION ((uint16_t)0x1000) /*!< Enable auto-negotiation function */
+#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200) /*!< Restart auto-negotiation function */
+#define PHY_POWERDOWN ((uint16_t)0x0800) /*!< Select the power down mode */
+#define PHY_ISOLATE ((uint16_t)0x0400) /*!< Isolate PHY from MII */
+
+#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020) /*!< Auto-Negotiation process completed */
+#define PHY_LINKED_STATUS ((uint16_t)0x0004) /*!< Valid link established */
+#define PHY_JABBER_DETECTION ((uint16_t)0x0002) /*!< Jabber condition detected */
+
+/* Section 4: Extended PHY Registers */
+
+#define PHY_SR ((uint16_t)0x1F) /*!< PHY special control/ status register Offset */
+
+#define PHY_SPEED_STATUS ((uint16_t)0x0004) /*!< PHY Speed mask */
+#define PHY_DUPLEX_STATUS ((uint16_t)0x0010) /*!< PHY Duplex mask */
+
+
+#define PHY_ISFR ((uint16_t)0x1D) /*!< PHY Interrupt Source Flag register Offset */
+#define PHY_ISFR_INT4 ((uint16_t)0x0010) /*!< PHY Link down inturrupt */
+
+/* ################## SPI peripheral configuration ########################## */
+
+/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver
+* Activated: CRC code is present inside driver
+* Deactivated: CRC code cleaned from driver
+*/
+
+#define USE_SPI_CRC 1U
+
+/* Includes ------------------------------------------------------------------*/
+/**
+ * @brief Include module's header file
+ */
+
+#ifdef HAL_RCC_MODULE_ENABLED
+ #include "stm32f7xx_hal_rcc.h"
+#endif /* HAL_RCC_MODULE_ENABLED */
+
+#ifdef HAL_GPIO_MODULE_ENABLED
+ #include "stm32f7xx_hal_gpio.h"
+#endif /* HAL_GPIO_MODULE_ENABLED */
+
+#ifdef HAL_DMA_MODULE_ENABLED
+ #include "stm32f7xx_hal_dma.h"
+#endif /* HAL_DMA_MODULE_ENABLED */
+
+#ifdef HAL_CORTEX_MODULE_ENABLED
+ #include "stm32f7xx_hal_cortex.h"
+#endif /* HAL_CORTEX_MODULE_ENABLED */
+
+#ifdef HAL_ADC_MODULE_ENABLED
+ #include "stm32f7xx_hal_adc.h"
+#endif /* HAL_ADC_MODULE_ENABLED */
+
+#ifdef HAL_CAN_MODULE_ENABLED
+ #include "stm32f7xx_hal_can.h"
+#endif /* HAL_CAN_MODULE_ENABLED */
+
+#ifdef HAL_CEC_MODULE_ENABLED
+ #include "stm32f7xx_hal_cec.h"
+#endif /* HAL_CEC_MODULE_ENABLED */
+
+#ifdef HAL_CRC_MODULE_ENABLED
+ #include "stm32f7xx_hal_crc.h"
+#endif /* HAL_CRC_MODULE_ENABLED */
+
+#ifdef HAL_CRYP_MODULE_ENABLED
+ #include "stm32f7xx_hal_cryp.h"
+#endif /* HAL_CRYP_MODULE_ENABLED */
+
+#ifdef HAL_DMA2D_MODULE_ENABLED
+ #include "stm32f7xx_hal_dma2d.h"
+#endif /* HAL_DMA2D_MODULE_ENABLED */
+
+#ifdef HAL_DAC_MODULE_ENABLED
+ #include "stm32f7xx_hal_dac.h"
+#endif /* HAL_DAC_MODULE_ENABLED */
+
+#ifdef HAL_DCMI_MODULE_ENABLED
+ #include "stm32f7xx_hal_dcmi.h"
+#endif /* HAL_DCMI_MODULE_ENABLED */
+
+#ifdef HAL_ETH_MODULE_ENABLED
+ #include "stm32f7xx_hal_eth.h"
+#endif /* HAL_ETH_MODULE_ENABLED */
+
+#ifdef HAL_FLASH_MODULE_ENABLED
+ #include "stm32f7xx_hal_flash.h"
+#endif /* HAL_FLASH_MODULE_ENABLED */
+
+#ifdef HAL_SRAM_MODULE_ENABLED
+ #include "stm32f7xx_hal_sram.h"
+#endif /* HAL_SRAM_MODULE_ENABLED */
+
+#ifdef HAL_NOR_MODULE_ENABLED
+ #include "stm32f7xx_hal_nor.h"
+#endif /* HAL_NOR_MODULE_ENABLED */
+
+#ifdef HAL_NAND_MODULE_ENABLED
+ #include "stm32f7xx_hal_nand.h"
+#endif /* HAL_NAND_MODULE_ENABLED */
+
+#ifdef HAL_SDRAM_MODULE_ENABLED
+ #include "stm32f7xx_hal_sdram.h"
+#endif /* HAL_SDRAM_MODULE_ENABLED */
+
+#ifdef HAL_HASH_MODULE_ENABLED
+ #include "stm32f7xx_hal_hash.h"
+#endif /* HAL_HASH_MODULE_ENABLED */
+
+#ifdef HAL_I2C_MODULE_ENABLED
+ #include "stm32f7xx_hal_i2c.h"
+#endif /* HAL_I2C_MODULE_ENABLED */
+
+#ifdef HAL_I2S_MODULE_ENABLED
+ #include "stm32f7xx_hal_i2s.h"
+#endif /* HAL_I2S_MODULE_ENABLED */
+
+#ifdef HAL_IWDG_MODULE_ENABLED
+ #include "stm32f7xx_hal_iwdg.h"
+#endif /* HAL_IWDG_MODULE_ENABLED */
+
+#ifdef HAL_LPTIM_MODULE_ENABLED
+ #include "stm32f7xx_hal_lptim.h"
+#endif /* HAL_LPTIM_MODULE_ENABLED */
+
+#ifdef HAL_LTDC_MODULE_ENABLED
+ #include "stm32f7xx_hal_ltdc.h"
+#endif /* HAL_LTDC_MODULE_ENABLED */
+
+#ifdef HAL_PWR_MODULE_ENABLED
+ #include "stm32f7xx_hal_pwr.h"
+#endif /* HAL_PWR_MODULE_ENABLED */
+
+#ifdef HAL_QSPI_MODULE_ENABLED
+ #include "stm32f7xx_hal_qspi.h"
+#endif /* HAL_QSPI_MODULE_ENABLED */
+
+#ifdef HAL_RNG_MODULE_ENABLED
+ #include "stm32f7xx_hal_rng.h"
+#endif /* HAL_RNG_MODULE_ENABLED */
+
+#ifdef HAL_RTC_MODULE_ENABLED
+ #include "stm32f7xx_hal_rtc.h"
+#endif /* HAL_RTC_MODULE_ENABLED */
+
+#ifdef HAL_SAI_MODULE_ENABLED
+ #include "stm32f7xx_hal_sai.h"
+#endif /* HAL_SAI_MODULE_ENABLED */
+
+#ifdef HAL_SD_MODULE_ENABLED
+ #include "stm32f7xx_hal_sd.h"
+#endif /* HAL_SD_MODULE_ENABLED */
+
+#ifdef HAL_SPDIFRX_MODULE_ENABLED
+ #include "stm32f7xx_hal_spdifrx.h"
+#endif /* HAL_SPDIFRX_MODULE_ENABLED */
+
+#ifdef HAL_SPI_MODULE_ENABLED
+ #include "stm32f7xx_hal_spi.h"
+#endif /* HAL_SPI_MODULE_ENABLED */
+
+#ifdef HAL_TIM_MODULE_ENABLED
+ #include "stm32f7xx_hal_tim.h"
+#endif /* HAL_TIM_MODULE_ENABLED */
+
+#ifdef HAL_UART_MODULE_ENABLED
+ #include "stm32f7xx_hal_uart.h"
+#endif /* HAL_UART_MODULE_ENABLED */
+
+#ifdef HAL_USART_MODULE_ENABLED
+ #include "stm32f7xx_hal_usart.h"
+#endif /* HAL_USART_MODULE_ENABLED */
+
+#ifdef HAL_IRDA_MODULE_ENABLED
+ #include "stm32f7xx_hal_irda.h"
+#endif /* HAL_IRDA_MODULE_ENABLED */
+
+#ifdef HAL_SMARTCARD_MODULE_ENABLED
+ #include "stm32f7xx_hal_smartcard.h"
+#endif /* HAL_SMARTCARD_MODULE_ENABLED */
+
+#ifdef HAL_WWDG_MODULE_ENABLED
+ #include "stm32f7xx_hal_wwdg.h"
+#endif /* HAL_WWDG_MODULE_ENABLED */
+
+#ifdef HAL_PCD_MODULE_ENABLED
+ #include "stm32f7xx_hal_pcd.h"
+#endif /* HAL_PCD_MODULE_ENABLED */
+
+#ifdef HAL_HCD_MODULE_ENABLED
+ #include "stm32f7xx_hal_hcd.h"
+#endif /* HAL_HCD_MODULE_ENABLED */
+
+#ifdef HAL_DFSDM_MODULE_ENABLED
+ #include "stm32f7xx_hal_dfsdm.h"
+#endif /* HAL_DFSDM_MODULE_ENABLED */
+
+#ifdef HAL_DSI_MODULE_ENABLED
+ #include "stm32f7xx_hal_dsi.h"
+#endif /* HAL_DSI_MODULE_ENABLED */
+
+#ifdef HAL_JPEG_MODULE_ENABLED
+ #include "stm32f7xx_hal_jpeg.h"
+#endif /* HAL_JPEG_MODULE_ENABLED */
+
+#ifdef HAL_MDIOS_MODULE_ENABLED
+ #include "stm32f7xx_hal_mdios.h"
+#endif /* HAL_MDIOS_MODULE_ENABLED */
+
+/* Exported macro ------------------------------------------------------------*/
+#ifdef USE_FULL_ASSERT
+/**
+ * @brief The assert_param macro is used for function's parameters check.
+ * @param expr: If expr is false, it calls assert_failed function
+ * which reports the name of the source file and the source
+ * line number of the call that failed.
+ * If expr is true, it returns no value.
+ * @retval None
+ */
+ #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
+/* Exported functions ------------------------------------------------------- */
+ void assert_failed(uint8_t* file, uint32_t line);
+#else
+ #define assert_param(expr) ((void)0)
+#endif /* USE_FULL_ASSERT */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __STM32F7xx_HAL_CONF_H */
+
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c
new file mode 100644
index 0000000000..d3b944ee70
--- /dev/null
+++ b/src/main/target/ANYFCF7/target.c
@@ -0,0 +1,88 @@
+/*
+ * 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/dma.h"
+
+#include "drivers/timer.h"
+
+// DSHOT TEST
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+ { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN
+ { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN
+ { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN
+ { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN
+ { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN
+ { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN
+
+ { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_5, DMA1_ST7_HANDLER }, // S10_OUT 1
+ { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2
+ { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4
+ { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT
+ { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT
+ { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT
+ { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3
+ { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT
+ { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT
+ { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT
+};
+
+/* STANDARD LAYOUT
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+ { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S1_IN
+ { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S2_IN
+ { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S3_IN
+ { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S4_IN
+ { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S5_IN
+ { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S6_IN
+
+ { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4}, // S10_OUT 1
+ { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S6_OUT 2
+ { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4}, // S5_OUT 3
+ { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S1_OUT 4
+ { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S2_OUT
+ { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9}, // S3_OUT
+ { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S4_OUT
+ { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S7_OUT
+ { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S8_OUT
+ { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S9_OUT
+};
+*/
+
+// ALTERNATE LAYOUT
+//const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+// { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S1_IN
+// { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S2_IN
+// { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S3_IN
+// { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S4_IN
+// { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S5_IN
+// { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S6_IN
+//
+// { TIM10, IO_TAG(PB8), TIM_CHANNEL_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM10}, // S10_OUT
+// { TIM9, IO_TAG(PA2), TIM_CHANNEL_1, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9}, // S6_OUT
+// { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S1_OUT
+// { TIM11, IO_TAG(PB9), TIM_CHANNEL_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM11}, // S5_OUT
+// { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S2_OUT
+// { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9}, // S3_OUT
+// { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S4_OUT
+// { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S7_OUT
+// { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S8_OUT
+// { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S9_OUT
+//};
diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h
new file mode 100644
index 0000000000..c36c5b529a
--- /dev/null
+++ b/src/main/target/ANYFCF7/target.h
@@ -0,0 +1,171 @@
+/*
+ * 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 "ANY7"
+
+#define CONFIG_START_FLASH_ADDRESS (0x080C0000)
+
+#define USBD_PRODUCT_STRING "AnyFCF7"
+
+#define USE_DSHOT
+
+#define LED0 PB7
+#define LED1 PB6
+
+//#define BEEPER PB2
+//#define BEEPER_INVERTED
+
+#define MPU6000_CS_PIN PA4
+#define MPU6000_SPI_INSTANCE SPI1
+
+#define ACC
+#define USE_ACC_SPI_MPU6000
+#define ACC_MPU6000_ALIGN CW270_DEG
+
+#define GYRO
+#define USE_GYRO_SPI_MPU6000
+#define GYRO_MPU6000_ALIGN CW270_DEG
+
+// MPU6000 interrupts
+#define USE_MPU_DATA_READY_SIGNAL
+#define MPU_INT_EXTI PC4
+#define USE_EXTI
+
+#define MAG
+//#define USE_MAG_HMC5883
+//#define HMC5883_BUS I2C_DEVICE_EXT
+//#define MAG_HMC5883_ALIGN CW270_DEG_FLIP
+//#define MAG_HMC5883_ALIGN CW90_DEG
+
+#define BARO
+#define USE_BARO_MS5611
+
+#define USABLE_TIMER_CHANNEL_COUNT 16
+
+#define USE_VCP
+#define VBUS_SENSING_PIN PA8
+
+#define USE_UART1
+#define UART1_RX_PIN PA10
+#define UART1_TX_PIN PA9
+
+#define USE_UART2
+#define UART2_RX_PIN PD6
+#define UART2_TX_PIN PD5
+
+#define USE_UART3
+#define UART3_RX_PIN PD9
+#define UART3_TX_PIN PD8
+
+#define USE_UART4
+#define UART4_RX_PIN PC11
+#define UART4_TX_PIN PC10
+
+#define USE_UART5
+#define UART5_RX_PIN PD2
+#define UART5_TX_PIN PC12
+
+#define USE_UART6
+#define UART6_RX_PIN PC7
+#define UART6_TX_PIN PC6
+
+#define USE_UART7
+#define UART7_RX_PIN PE7
+#define UART7_TX_PIN PE8
+
+#define USE_UART8
+#define UART8_RX_PIN PE0
+#define UART8_TX_PIN PE1
+
+#define SERIAL_PORT_COUNT 9 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, USART7, USART8
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_4
+
+#define SPI1_NSS_PIN PA4
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define SPI4_NSS_PIN PE11
+#define SPI4_SCK_PIN PE12
+#define SPI4_MISO_PIN PE13
+#define SPI4_MOSI_PIN PE14
+
+#define USE_SDCARD
+#define SDCARD_DETECT_INVERTED
+#define SDCARD_DETECT_PIN PD3
+#define SDCARD_DETECT_EXTI_LINE EXTI_Line3
+#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource3
+#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD
+#define SDCARD_DETECT_EXTI_IRQn EXTI3_IRQn
+
+#define SDCARD_SPI_INSTANCE SPI4
+#define SDCARD_SPI_CS_PIN SPI4_NSS_PIN
+
+#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz
+// Divide to under 25MHz for normal operation:
+#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz
+
+#define SDCARD_DMA_CHANNEL_TX DMA2_Stream1
+#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5
+#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2
+#define SDCARD_DMA_CHANNEL DMA_CHANNEL_4
+
+#define USE_I2C
+#define I2C_DEVICE (I2CDEV_4)
+//#define I2C_DEVICE_EXT (I2CDEV_2)
+
+#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
+
+#define USE_ADC
+#define VBAT_ADC_PIN PC0
+#define CURRENT_METER_ADC_PIN PC1
+#define RSSI_ADC_GPIO_PIN PC2
+
+#define LED_STRIP
+
+// LED Strip can run off Pin 6 (PA0) of the ESC outputs.
+#define WS2811_PIN PA1
+#define WS2811_TIMER TIM5
+#define WS2811_TIMER_CHANNEL TIM_CHANNEL_2
+#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER
+#define WS2811_DMA_STREAM DMA1_Stream4
+#define WS2811_DMA_FLAG DMA_FLAG_TCIF4
+#define WS2811_DMA_IT DMA_IT_TCIF4
+#define WS2811_DMA_CHANNEL DMA_CHANNEL_6
+#define WS2811_DMA_IRQ DMA1_Stream4_IRQn
+#define WS2811_TIMER_GPIO_AF GPIO_AF2_TIM5
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
+#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SBUS
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTE 0xffff
+
+#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11))
diff --git a/src/main/target/ANYFCF7/target.mk b/src/main/target/ANYFCF7/target.mk
new file mode 100644
index 0000000000..158958769f
--- /dev/null
+++ b/src/main/target/ANYFCF7/target.mk
@@ -0,0 +1,9 @@
+F7X5XG_TARGETS += $(TARGET)
+FEATURES += SDCARD VCP
+
+TARGET_SRC = \
+ drivers/accgyro_spi_mpu6000.c \
+ drivers/barometer_ms5611.c \
+ drivers/light_ws2811strip.c \
+ drivers/light_ws2811strip_hal.c
+
diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index a013e3fb2d..022b51da5e 100644
--- a/src/main/target/BLUEJAYF4/target.h
+++ b/src/main/target/BLUEJAYF4/target.h
@@ -148,7 +148,6 @@
#define WS2811_TIMER_CHANNEL TIM_Channel_4
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream2
-#define WS2811_DMA_IT DMA_IT_TCIF2
#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM3
#define WS2811_DMA_CHANNEL DMA_Channel_5
#define WS2811_DMA_IRQ DMA1_Stream2_IRQn
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index b6ef71a966..7dc877cd1d 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -121,6 +121,7 @@
#undef BARO
#undef SONAR
#undef LED_STRIP
+#undef USE_SERIAL_4WAY_BLHELI_INTERFACE
#endif
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c
deleted file mode 100644
index e73d34f83a..0000000000
--- a/src/main/target/EUSTM32F103RC/target.c
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * 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"
-
-const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
- { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1
- { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2
- { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3
- { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4
- { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5
- { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6
- { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7
- { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8
- { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1
- { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2
- { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3
- { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4
- { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5
- { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6
-};
-
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
deleted file mode 100644
index 506e2b05c3..0000000000
--- a/src/main/target/EUSTM32F103RC/target.h
+++ /dev/null
@@ -1,118 +0,0 @@
-/*
- * 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 "EUF1"
-
-#define LED0 PB3
-#define LED1 PB4
-
-#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO
-#define INVERTER_USART USART2
-
-#define USE_EXTI
-
-#define MPU6000_CS_GPIO GPIOB
-#define MPU6000_CS_PIN PB12
-#define MPU6000_SPI_INSTANCE SPI2
-
-#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB
-#define MPU6500_CS_GPIO GPIOB
-#define MPU6500_CS_PIN PB12
-#define MPU6500_SPI_INSTANCE SPI2
-
-#define GYRO
-#define USE_FAKE_GYRO
-//#define USE_GYRO_L3G4200D
-//#define USE_GYRO_L3GD20
-//#define USE_GYRO_MPU3050
-#define USE_GYRO_MPU6050
-#define USE_GYRO_SPI_MPU6000
-#define USE_GYRO_SPI_MPU6500
-
-#define GYRO_MPU6050_ALIGN CW0_DEG
-
-#define ACC
-#define USE_FAKE_ACC
-#define USE_ACC_ADXL345
-#define USE_ACC_BMA280
-#define USE_ACC_MMA8452
-#define USE_ACC_MPU6050
-//#define USE_ACC_SPI_MPU6000
-#define USE_ACC_SPI_MPU6500
-
-#define ACC_MPU6050_ALIGN CW0_DEG
-
-#define BARO
-#define USE_BARO_MS5611
-#define USE_BARO_BMP085
-#define USE_BARO_BMP280
-
-#define MAG
-#define USE_MAG_HMC5883
-#define USE_MAG_AK8975
-
-#define MAG_AK8975_ALIGN CW180_DEG_FLIP
-
-#define SONAR
-#define SONAR_TRIGGER_PIN PB0
-#define SONAR_ECHO_PIN PB1
-#define SONAR_TRIGGER_PIN_PWM PB8
-#define SONAR_ECHO_PIN_PWM PB9
-
-#define DISPLAY
-
-#define USE_UART1
-#define USE_UART2
-#define USE_SOFTSERIAL1
-#define USE_SOFTSERIAL2
-#define SERIAL_PORT_COUNT 4
-
-#define SOFTSERIAL_1_TIMER TIM3
-#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
-#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
-#define SOFTSERIAL_2_TIMER TIM3
-#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
-#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
-
-#define USE_I2C
-#define I2C_DEVICE (I2CDEV_2)
-
-// #define SOFT_I2C // enable to test software i2c
-// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
-// #define SOFT_I2C_PB67
-
-#define USE_ADC
-#define CURRENT_METER_ADC_PIN PB1
-#define VBAT_ADC_PIN PA4
-#define RSSI_ADC_PIN PA1
-#define EXTERNAL1_ADC_PIN PA5
-
-#define SPEKTRUM_BIND
-// USART2, PA3
-#define BIND_PIN PA3
-
-// IO - stm32f103RCT6 in 64pin package (TODO)
-#define TARGET_IO_PORTA 0xffff
-#define TARGET_IO_PORTB 0xffff
-#define TARGET_IO_PORTC 0xffff
-#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2))
-
-#define USABLE_TIMER_CHANNEL_COUNT 14
-#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4))
-
diff --git a/src/main/target/EUSTM32F103RC/target.mk b/src/main/target/EUSTM32F103RC/target.mk
deleted file mode 100644
index 718d86d26b..0000000000
--- a/src/main/target/EUSTM32F103RC/target.mk
+++ /dev/null
@@ -1,22 +0,0 @@
-F1_TARGETS += $(TARGET)
-FEATURES = ONBOARDFLASH HIGHEND
-
-DEVICE_FLAGS = -DSTM32F10X_HD
-
-TARGET_SRC = \
- drivers/accgyro_adxl345.c \
- drivers/accgyro_bma280.c \
- drivers/accgyro_l3g4200d.c \
- drivers/accgyro_mma845x.c \
- drivers/accgyro_mpu.c \
- drivers/accgyro_mpu3050.c \
- drivers/accgyro_mpu6050.c \
- drivers/accgyro_mpu6500.c \
- drivers/accgyro_spi_mpu6000.c \
- drivers/accgyro_spi_mpu6500.c \
- drivers/barometer_bmp085.c \
- drivers/barometer_bmp280.c \
- drivers/barometer_ms5611.c \
- drivers/compass_ak8975.c \
- drivers/compass_hmc5883l.c
-
diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c
index e838c785ba..af48f988d7 100644
--- a/src/main/target/NAZE/target.c
+++ b/src/main/target/NAZE/target.c
@@ -33,9 +33,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1
{ TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2
- { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3
- { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4
- { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5
- { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6
+ { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_IPD }, // PWM11 - OUT3
+ { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_IPD }, // PWM12 - OUT4
+ { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_IPD }, // PWM13 - OUT5
+ { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_IPD } // PWM14 - OUT6
};
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 59f53e88cd..ec9358222c 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -103,14 +103,14 @@
#define ACC_BMA280_ALIGN CW0_DEG
#define ACC_MPU6500_ALIGN CW0_DEG
-//#define BARO
-//#define USE_BARO_MS5611
-//#define USE_BARO_BMP085
-//#define USE_BARO_BMP280
+#define BARO
+#define USE_BARO_MS5611
+#define USE_BARO_BMP085
+#define USE_BARO_BMP280
-//#define MAG
-//#define USE_MAG_HMC5883
-//#define MAG_HMC5883_ALIGN CW180_DEG
+#define MAG
+#define USE_MAG_HMC5883
+#define MAG_HMC5883_ALIGN CW180_DEG
//#define SONAR
//#define SONAR_TRIGGER_PIN PB0
@@ -151,11 +151,11 @@
#define RSSI_ADC_PIN PA1
#define EXTERNAL1_ADC_PIN PA5
-//#define LED_STRIP
-//#define WS2811_TIMER TIM3
-//#define WS2811_PIN PA6
-//#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6
-//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
+#define LED_STRIP
+#define WS2811_TIMER TIM3
+#define WS2811_PIN PA6
+#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6
+#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
#undef GPS
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index d1e205b8d8..1ef2f55c74 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -127,8 +127,6 @@
#define WS2811_DMA_STREAM DMA1_Stream4
#define WS2811_DMA_CHANNEL DMA_Channel_6
#define WS2811_DMA_IRQ DMA1_Stream4_IRQn
-#define WS2811_DMA_FLAG DMA_FLAG_TCIF4
-#define WS2811_DMA_IT DMA_IT_TCIF4
#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5
#define SENSORS_SET (SENSOR_ACC)
diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c
deleted file mode 100644
index 1b2c76f515..0000000000
--- a/src/main/target/PORT103R/target.c
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * 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"
-
-const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
- { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1
- { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2
- { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3
- { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4
- { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5
- { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6
- { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7
- { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8
- { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1
- { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2
- { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3
- { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4
- { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5
- { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6
-};
-
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
deleted file mode 100644
index 545674aa14..0000000000
--- a/src/main/target/PORT103R/target.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * 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 "103R"
-
-#define LED0 PB3
-#define LED1 PB4
-#define LED2 PD2 // PD2 (LED) - Labelled LED4
-
-#define BEEPER PA12 // PA12 (Beeper)
-
-#define BARO_XCLR_GPIO GPIOC
-#define BARO_XCLR_PIN PC13
-#define BARO_EOC_GPIO GPIOC
-#define BARO_EOC_PIN PC14
-#define BARO_APB2_PERIPHERALS RCC_APB2Periph_GPIOC
-
-#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO
-#define INVERTER_USART USART2
-
-#define USE_SPI
-#define USE_SPI_DEVICE_2
-
-#define PORT103R_SPI_INSTANCE SPI2
-#define PORT103R_SPI_CS_PIN PB12
-
-// We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision:
-#define M25P16_CS_PIN PORT103R_SPI_CS_PIN
-#define M25P16_SPI_INSTANCE PORT103R_SPI_INSTANCE
-
-#define MPU6000_CS_PIN PORT103R_SPI_CS_PIN
-#define MPU6000_SPI_INSTANCE PORT103R_SPI_INSTANCE
-
-#define MPU6500_CS_PIN PORT103R_SPI_CS_PIN
-#define MPU6500_SPI_INSTANCE PORT103R_SPI_INSTANCE
-
-#define GYRO
-#define USE_FAKE_GYRO
-//#define USE_GYRO_L3G4200D
-//#define USE_GYRO_L3GD20
-//#define USE_GYRO_MPU3050
-#define USE_GYRO_MPU6050
-#define USE_GYRO_SPI_MPU6000
-#define USE_GYRO_SPI_MPU6500
-
-#define ACC
-#define USE_FAKE_ACC
-//#define USE_ACC_ADXL345
-//#define USE_ACC_BMA280
-//#define USE_ACC_MMA8452
-#define USE_ACC_MPU6050
-#define USE_ACC_SPI_MPU6000
-#define USE_ACC_SPI_MPU6500
-
-#define BARO
-#define USE_BARO_MS5611
-#define USE_BARO_BMP085
-#define USE_BARO_BMP280
-
-#define MAG
-#define USE_MAG_HMC5883
-#define USE_MAG_AK8975
-
-#define USE_FLASHFS
-#define USE_FLASHTOOLS
-#define USE_FLASH_M25P16
-
-//#define SONAR
-#define SONAR_TRIGGER_PIN PB0
-#define SONAR_ECHO_PIN PB1
-#define SONAR_TRIGGER_PIN_PWM PB8
-#define SONAR_ECHO_PIN_PWM PB9
-
-//#define DISPLAY
-
-#define USE_UART1
-#define USE_UART2
-#define USE_SOFTSERIAL1
-#define USE_SOFTSERIAL2
-#define SERIAL_PORT_COUNT 4
-
-#define SOFTSERIAL_1_TIMER TIM3
-#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
-#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
-#define SOFTSERIAL_2_TIMER TIM3
-#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
-#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
-
-#define USE_I2C
-#define I2C_DEVICE (I2CDEV_2)
-
-// #define SOFT_I2C // enable to test software i2c
-// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
-// #define SOFT_I2C_PB67
-
-#define USE_ADC
-#define CURRENT_METER_ADC_PIN PB1
-#define VBAT_ADC_PIN PA4
-#define RSSI_ADC_PIN PA1
-#define EXTERNAL1_ADC_PIN PA5
-
-//#define LED_STRIP
-//#define LED_STRIP_TIMER TIM3
-
-#define SKIP_CLI_COMMAND_HELP
-#define SKIP_PID_LUXFLOAT
-
-#define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-// IO - stm32f103RCT6 in 64pin package
-#define TARGET_IO_PORTA 0xffff
-#define TARGET_IO_PORTB 0xffff
-#define TARGET_IO_PORTC 0xffff
-#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2))
-
-#define USABLE_TIMER_CHANNEL_COUNT 14
-#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4))
-
diff --git a/src/main/target/PORT103R/target.mk b/src/main/target/PORT103R/target.mk
deleted file mode 100644
index bb3de6bc72..0000000000
--- a/src/main/target/PORT103R/target.mk
+++ /dev/null
@@ -1,25 +0,0 @@
-F1_TARGETS += $(TARGET)
-256K_TARGETS += $(TARGET)
-FLASH_SIZE = 256
-
-FEATURES = ONBOARDFLASH HIGHEND
-
-DEVICE_FLAGS = -DSTM32F10X_HD
-
-TARGET_SRC = \
- drivers/accgyro_adxl345.c \
- drivers/accgyro_bma280.c \
- drivers/accgyro_l3g4200d.c \
- drivers/accgyro_mma845x.c \
- drivers/accgyro_mpu.c \
- drivers/accgyro_mpu3050.c \
- drivers/accgyro_mpu6050.c \
- drivers/accgyro_mpu6500.c \
- drivers/accgyro_spi_mpu6000.c \
- drivers/accgyro_spi_mpu6500.c \
- drivers/barometer_bmp085.c \
- drivers/barometer_bmp280.c \
- drivers/barometer_ms5611.c \
- drivers/compass_ak8975.c \
- drivers/compass_hmc5883l.c
-
diff --git a/src/main/target/SOULF4/target.h b/src/main/target/SOULF4/target.h
index e8cbf5a077..f49652428f 100644
--- a/src/main/target/SOULF4/target.h
+++ b/src/main/target/SOULF4/target.h
@@ -105,7 +105,6 @@
#define WS2811_TIMER TIM5
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream2
-#define WS2811_DMA_IT DMA_IT_TCIF2
#define WS2811_DMA_CHANNEL DMA_Channel_6
#define WS2811_TIMER_CHANNEL TIM_Channel_1
#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index f8361330d4..13345dc8e2 100644
--- a/src/main/target/YUPIF4/target.h
+++ b/src/main/target/YUPIF4/target.h
@@ -131,8 +131,6 @@
#define WS2811_TIMER_CHANNEL TIM_Channel_4
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream2
-#define WS2811_DMA_FLAG DMA_FLAG_TCIF2
-#define WS2811_DMA_IT DMA_IT_TCIF2
#define WS2811_DMA_CHANNEL DMA_Channel_5
#define WS2811_DMA_IRQ DMA1_Stream2_IRQn
#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM3
diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld
new file mode 100644
index 0000000000..62270ea0f0
--- /dev/null
+++ b/src/main/target/link/stm32_flash_f745.ld
@@ -0,0 +1,24 @@
+/*
+*****************************************************************************
+**
+** File : stm32_flash_f745.ld
+**
+** Abstract : Linker script for STM32F745VGTx Device with
+** 1024KByte FLASH, 320KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Specify the memory areas */
+MEMORY
+{
+ FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1M
+ RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 320K
+ MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
+}
+/* note CCM could be used for stack */
+
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/system_stm32f7xx.c b/src/main/target/system_stm32f7xx.c
new file mode 100644
index 0000000000..df533bbf1b
--- /dev/null
+++ b/src/main/target/system_stm32f7xx.c
@@ -0,0 +1,385 @@
+/**
+ ******************************************************************************
+ * @file system_stm32f7xx.c
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief CMSIS Cortex-M7 Device Peripheral Access Layer System Source File.
+ *
+ * This file provides two functions and one global variable to be called from
+ * user application:
+ * - SystemInit(): This function is called at startup just after reset and
+ * before branch to main program. This call is made inside
+ * the "startup_stm32f7xx.s" file.
+ *
+ * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+ * by the user application to setup the SysTick
+ * timer or configure other parameters.
+ *
+ * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+ * be called whenever the core clock is changed
+ * during program execution.
+ *
+ *
+ ******************************************************************************
+ * @attention
+ *
+ * © COPYRIGHT 2016 STMicroelectronics
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/** @addtogroup CMSIS
+ * @{
+ */
+
+/** @addtogroup stm32f7xx_system
+ * @{
+ */
+
+/** @addtogroup STM32F7xx_System_Private_Includes
+ * @{
+ */
+
+#include "stm32f7xx.h"
+
+#if !defined (HSE_VALUE)
+ #define HSE_VALUE ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined (HSI_VALUE)
+ #define HSI_VALUE ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F7xx_System_Private_TypesDefinitions
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F7xx_System_Private_Defines
+ * @{
+ */
+
+/************************* Miscellaneous Configuration ************************/
+
+/*!< Uncomment the following line if you need to relocate your vector Table in
+ Internal SRAM. */
+/* #define VECT_TAB_SRAM */
+#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field.
+ This value must be a multiple of 0x200. */
+/******************************************************************************/
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F7xx_System_Private_Macros
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F7xx_System_Private_Variables
+ * @{
+ */
+
+ /* This variable is updated in three ways:
+ 1) by calling CMSIS function SystemCoreClockUpdate()
+ 2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+ 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+ Note: If you use this function to configure the system clock; then there
+ is no need to call the 2 first functions listed above, since SystemCoreClock
+ variable is updated automatically.
+ */
+ uint32_t SystemCoreClock = 216000000;
+ const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+ const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4};
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F7xx_System_Private_FunctionPrototypes
+ * @{
+ */
+
+ /// TODO: F7 check if this is the best configuration for the clocks.
+ // current settings are just a copy from one of the example projects
+ void SystemClock_Config(void)
+ {
+ RCC_ClkInitTypeDef RCC_ClkInitStruct;
+ RCC_OscInitTypeDef RCC_OscInitStruct;
+ RCC_PeriphCLKInitTypeDef PeriphClkInitStruct;
+ HAL_StatusTypeDef ret = HAL_OK;
+
+ __HAL_RCC_PWR_CLK_ENABLE();
+
+ __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
+
+ /* Enable HSE Oscillator and activate PLL with HSE as source */
+ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
+ RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+ RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+ RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+ RCC_OscInitStruct.PLL.PLLM = 8;
+ RCC_OscInitStruct.PLL.PLLN = 432;
+ RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
+ RCC_OscInitStruct.PLL.PLLQ = 9;
+
+ ret = HAL_RCC_OscConfig(&RCC_OscInitStruct);
+ if(ret != HAL_OK)
+ {
+ while(1) { ; }
+ }
+
+ /* Activate the OverDrive to reach the 216 MHz Frequency */
+ ret = HAL_PWREx_EnableOverDrive();
+ if(ret != HAL_OK)
+ {
+ while(1) { ; }
+ }
+ /* Select PLLSAI output as USB clock source */
+ PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_CLK48;
+ PeriphClkInitStruct.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP;
+ PeriphClkInitStruct.PLLSAI.PLLSAIN = 384;
+ PeriphClkInitStruct.PLLSAI.PLLSAIQ = 7;
+ PeriphClkInitStruct.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8;
+ if(HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
+ {
+ while(1) {};
+ }
+
+ /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */
+ RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2);
+ RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+ RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+ RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
+ RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
+
+ ret = HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+ if(ret != HAL_OK)
+ {
+ while(1) { ; }
+ }
+
+ PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2
+ |RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_USART6
+ |RCC_PERIPHCLK_UART4|RCC_PERIPHCLK_UART5
+ |RCC_PERIPHCLK_UART7|RCC_PERIPHCLK_UART8
+ |RCC_PERIPHCLK_I2C2|RCC_PERIPHCLK_I2C4;
+ PeriphClkInitStruct.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2;
+ PeriphClkInitStruct.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.Usart3ClockSelection = RCC_USART3CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.Uart4ClockSelection = RCC_UART4CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.Uart5ClockSelection = RCC_UART5CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;
+ PeriphClkInitStruct.Uart7ClockSelection = RCC_UART7CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.Uart8ClockSelection = RCC_UART8CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1;
+ PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1;
+ ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct);
+ if (ret != HAL_OK)
+ {
+ while(1) { ; }
+ }
+
+ // Activating the timerprescalers while the APBx prescalers are 1/2/4 will connect the TIMxCLK to HCLK which has been configured to 216MHz
+ __HAL_RCC_TIMCLKPRESCALER(RCC_TIMPRES_ACTIVATED);
+
+ SystemCoreClockUpdate();
+ }
+
+/**
+ * @}
+ */
+
+/** @addtogroup STM32F7xx_System_Private_Functions
+ * @{
+ */
+
+/**
+ * @brief Setup the microcontroller system
+ * Initialize the Embedded Flash Interface, the PLL and update the
+ * SystemFrequency variable.
+ * @param None
+ * @retval None
+ */
+void SystemInit(void)
+{
+ /* FPU settings ------------------------------------------------------------*/
+ #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
+ #endif
+ /* Reset the RCC clock configuration to the default reset state ------------*/
+ /* Set HSION bit */
+ RCC->CR |= (uint32_t)0x00000001;
+
+ /* Reset CFGR register */
+ RCC->CFGR = 0x00000000;
+
+ /* Reset HSEON, CSSON and PLLON bits */
+ RCC->CR &= (uint32_t)0xFEF6FFFF;
+
+ /* Reset PLLCFGR register */
+ RCC->PLLCFGR = 0x24003010;
+
+ /* Reset HSEBYP bit */
+ RCC->CR &= (uint32_t)0xFFFBFFFF;
+
+ /* Disable all interrupts */
+ RCC->CIR = 0x00000000;
+
+ /* Configure the Vector Table location add offset address ------------------*/
+#ifdef VECT_TAB_SRAM
+ SCB->VTOR = RAMDTCM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
+#else
+ SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
+#endif
+
+ /* Enable I-Cache */
+ //SCB_EnableICache();
+
+ /* Enable D-Cache */
+ //SCB_EnableDCache();
+
+ /* Configure the system clock to 216 MHz */
+ SystemClock_Config();
+
+ //if(SystemCoreClock != 260000000)
+ {
+ //while(1)
+ {
+ // There is a mismatch between the configured clock and the expected clock in portable.h
+ }
+ }
+
+
+}
+
+/**
+ * @brief Update SystemCoreClock variable according to Clock Register Values.
+ * The SystemCoreClock variable contains the core clock (HCLK), it can
+ * be used by the user application to setup the SysTick timer or configure
+ * other parameters.
+ *
+ * @note Each time the core clock (HCLK) changes, this function must be called
+ * to update SystemCoreClock variable value. Otherwise, any configuration
+ * based on this variable will be incorrect.
+ *
+ * @note - The system frequency computed by this function is not the real
+ * frequency in the chip. It is calculated based on the predefined
+ * constant and the selected clock source:
+ *
+ * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+ *
+ * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+ *
+ * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
+ * or HSI_VALUE(*) multiplied/divided by the PLL factors.
+ *
+ * (*) HSI_VALUE is a constant defined in stm32f7xx_hal_conf.h file (default value
+ * 16 MHz) but the real value may vary depending on the variations
+ * in voltage and temperature.
+ *
+ * (**) HSE_VALUE is a constant defined in stm32f7xx_hal_conf.h file (default value
+ * 25 MHz), user has to ensure that HSE_VALUE is same as the real
+ * frequency of the crystal used. Otherwise, this function may
+ * have wrong result.
+ *
+ * - The result of this function could be not correct when using fractional
+ * value for HSE crystal.
+ *
+ * @param None
+ * @retval None
+ */
+void SystemCoreClockUpdate(void)
+{
+ uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
+
+ /* Get SYSCLK source -------------------------------------------------------*/
+ tmp = RCC->CFGR & RCC_CFGR_SWS;
+
+ switch (tmp)
+ {
+ case 0x00: /* HSI used as system clock source */
+ SystemCoreClock = HSI_VALUE;
+ break;
+ case 0x04: /* HSE used as system clock source */
+ SystemCoreClock = HSE_VALUE;
+ break;
+ case 0x08: /* PLL used as system clock source */
+
+ /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
+ SYSCLK = PLL_VCO / PLL_P
+ */
+ pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
+ pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
+
+ if (pllsource != 0)
+ {
+ /* HSE used as PLL clock source */
+ pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
+ }
+ else
+ {
+ /* HSI used as PLL clock source */
+ pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
+ }
+
+ pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
+ SystemCoreClock = pllvco/pllp;
+ break;
+ default:
+ SystemCoreClock = HSI_VALUE;
+ break;
+ }
+ /* Compute HCLK frequency --------------------------------------------------*/
+ /* Get HCLK prescaler */
+ tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+ /* HCLK frequency */
+ SystemCoreClock >>= tmp;
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/target/system_stm32f7xx.h b/src/main/target/system_stm32f7xx.h
new file mode 100644
index 0000000000..431d59f8bf
--- /dev/null
+++ b/src/main/target/system_stm32f7xx.h
@@ -0,0 +1,45 @@
+/**
+ ******************************************************************************
+ * @file system_stm32f7xx.h
+ * @author MCD Application Team
+ * @version V1.6.1
+ * @date 21-October-2015
+ * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices.
+ ******************************************************************************
+ * @attention
+ *
+ * © COPYRIGHT 2015 STMicroelectronics
+ *
+ * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
+ * You may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ * http://www.st.com/software_license_agreement_liberty_v2
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ ******************************************************************************
+ */
+
+#ifndef __SYSTEM_STM32F7XX_H
+#define __SYSTEM_STM32F7XX_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
+extern void SystemInit(void);
+extern void SystemClock_Config(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_STM32F7XX_H */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c
new file mode 100644
index 0000000000..a141965cc3
--- /dev/null
+++ b/src/main/vcp_hal/usbd_cdc_interface.c
@@ -0,0 +1,393 @@
+/**
+ ******************************************************************************
+ * @file USB_Device/CDC_Standalone/Src/usbd_cdc_interface.c
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief Source file for USBD CDC interface
+ ******************************************************************************
+ * @attention
+ *
+ * © Copyright (c) 2016 STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted, provided that the following conditions are met:
+ *
+ * 1. Redistribution of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of other
+ * contributors to this software may be used to endorse or promote products
+ * derived from this software without specific written permission.
+ * 4. This software, including modifications and/or derivative works of this
+ * software, must execute solely and exclusively on microcontroller or
+ * microprocessor devices manufactured by or for STMicroelectronics.
+ * 5. Redistribution and use of this software other than as permitted under
+ * this license is void and will automatically terminate your rights under
+ * this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
+ * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
+ * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f7xx_hal.h"
+#include "usbd_core.h"
+#include "usbd_desc.h"
+#include "usbd_cdc.h"
+#include "usbd_cdc_interface.h"
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+#define APP_RX_DATA_SIZE 2048
+#define APP_TX_DATA_SIZE 2048
+
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+USBD_CDC_LineCodingTypeDef LineCoding =
+{
+ 115200, /* baud rate*/
+ 0x00, /* stop bits-1*/
+ 0x00, /* parity - none*/
+ 0x08 /* nb. of bits 8*/
+};
+
+uint8_t UserRxBuffer[APP_RX_DATA_SIZE];/* Received Data over USB are stored in this buffer */
+uint8_t UserTxBuffer[APP_TX_DATA_SIZE];/* Received Data over UART (CDC interface) are stored in this buffer */
+uint32_t BuffLength;
+uint32_t UserTxBufPtrIn = 0;/* Increment this pointer or roll it back to
+ start address when data are received over USART */
+uint32_t UserTxBufPtrOut = 0; /* Increment this pointer or roll it back to
+ start address when data are sent over USB */
+
+uint32_t rxAvailable = 0;
+uint8_t* rxBuffPtr = NULL;
+
+/* TIM handler declaration */
+TIM_HandleTypeDef TimHandle;
+/* USB handler declaration */
+extern USBD_HandleTypeDef USBD_Device;
+
+/* Private function prototypes -----------------------------------------------*/
+static int8_t CDC_Itf_Init(void);
+static int8_t CDC_Itf_DeInit(void);
+static int8_t CDC_Itf_Control(uint8_t cmd, uint8_t* pbuf, uint16_t length);
+static int8_t CDC_Itf_Receive(uint8_t* pbuf, uint32_t *Len);
+
+static void TIM_Config(void);
+static void Error_Handler(void);
+
+USBD_CDC_ItfTypeDef USBD_CDC_fops =
+{
+ CDC_Itf_Init,
+ CDC_Itf_DeInit,
+ CDC_Itf_Control,
+ CDC_Itf_Receive
+};
+
+
+void TIMx_IRQHandler(void)
+{
+ HAL_TIM_IRQHandler(&TimHandle);
+}
+
+/* Private functions ---------------------------------------------------------*/
+
+/**
+ * @brief CDC_Itf_Init
+ * Initializes the CDC media low layer
+ * @param None
+ * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+ */
+static int8_t CDC_Itf_Init(void)
+{
+ /*##-3- Configure the TIM Base generation #################################*/
+ TIM_Config();
+
+ /*##-4- Start the TIM Base generation in interrupt mode ####################*/
+ /* Start Channel1 */
+ if(HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK)
+ {
+ /* Starting Error */
+ Error_Handler();
+ }
+
+ /*##-5- Set Application Buffers ############################################*/
+ USBD_CDC_SetTxBuffer(&USBD_Device, UserTxBuffer, 0);
+ USBD_CDC_SetRxBuffer(&USBD_Device, UserRxBuffer);
+
+ return (USBD_OK);
+}
+
+/**
+ * @brief CDC_Itf_DeInit
+ * DeInitializes the CDC media low layer
+ * @param None
+ * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+ */
+static int8_t CDC_Itf_DeInit(void)
+{
+
+ return (USBD_OK);
+}
+
+/**
+ * @brief CDC_Itf_Control
+ * Manage the CDC class requests
+ * @param Cmd: Command code
+ * @param Buf: Buffer containing command data (request parameters)
+ * @param Len: Number of data to be sent (in bytes)
+ * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+ */
+static int8_t CDC_Itf_Control (uint8_t cmd, uint8_t* pbuf, uint16_t length)
+{
+ UNUSED(length);
+ switch (cmd)
+ {
+ case CDC_SEND_ENCAPSULATED_COMMAND:
+ /* Add your code here */
+ break;
+
+ case CDC_GET_ENCAPSULATED_RESPONSE:
+ /* Add your code here */
+ break;
+
+ case CDC_SET_COMM_FEATURE:
+ /* Add your code here */
+ break;
+
+ case CDC_GET_COMM_FEATURE:
+ /* Add your code here */
+ break;
+
+ case CDC_CLEAR_COMM_FEATURE:
+ /* Add your code here */
+ break;
+
+ case CDC_SET_LINE_CODING:
+ LineCoding.bitrate = (uint32_t)(pbuf[0] | (pbuf[1] << 8) |\
+ (pbuf[2] << 16) | (pbuf[3] << 24));
+ LineCoding.format = pbuf[4];
+ LineCoding.paritytype = pbuf[5];
+ LineCoding.datatype = pbuf[6];
+
+ break;
+
+ case CDC_GET_LINE_CODING:
+ pbuf[0] = (uint8_t)(LineCoding.bitrate);
+ pbuf[1] = (uint8_t)(LineCoding.bitrate >> 8);
+ pbuf[2] = (uint8_t)(LineCoding.bitrate >> 16);
+ pbuf[3] = (uint8_t)(LineCoding.bitrate >> 24);
+ pbuf[4] = LineCoding.format;
+ pbuf[5] = LineCoding.paritytype;
+ pbuf[6] = LineCoding.datatype;
+ break;
+
+ case CDC_SET_CONTROL_LINE_STATE:
+ /* Add your code here */
+ break;
+
+ case CDC_SEND_BREAK:
+ /* Add your code here */
+ break;
+
+ default:
+ break;
+ }
+
+ return (USBD_OK);
+}
+
+/**
+ * @brief TIM period elapsed callback
+ * @param htim: TIM handle
+ * @retval None
+ */
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
+{
+ if(htim->Instance != TIMusb) return;
+
+ uint32_t buffptr;
+ uint32_t buffsize;
+
+ if(UserTxBufPtrOut != UserTxBufPtrIn)
+ {
+ if(UserTxBufPtrOut > UserTxBufPtrIn) /* Roll-back */
+ {
+ buffsize = APP_RX_DATA_SIZE - UserTxBufPtrOut;
+ }
+ else
+ {
+ buffsize = UserTxBufPtrIn - UserTxBufPtrOut;
+ }
+
+ buffptr = UserTxBufPtrOut;
+
+ USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t*)&UserTxBuffer[buffptr], buffsize);
+
+ if(USBD_CDC_TransmitPacket(&USBD_Device) == USBD_OK)
+ {
+ UserTxBufPtrOut += buffsize;
+ if (UserTxBufPtrOut == APP_RX_DATA_SIZE)
+ {
+ UserTxBufPtrOut = 0;
+ }
+ }
+ }
+}
+
+/**
+ * @brief CDC_Itf_DataRx
+ * Data received over USB OUT endpoint are sent over CDC interface
+ * through this function.
+ * @param Buf: Buffer of data to be transmitted
+ * @param Len: Number of data received (in bytes)
+ * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+ */
+static int8_t CDC_Itf_Receive(uint8_t* Buf, uint32_t *Len)
+{
+ rxAvailable = *Len;
+ rxBuffPtr = Buf;
+ return (USBD_OK);
+}
+
+/**
+ * @brief TIM_Config: Configure TIMusb timer
+ * @param None.
+ * @retval None
+ */
+static void TIM_Config(void)
+{
+ /* Set TIMusb instance */
+ TimHandle.Instance = TIMusb;
+
+ /* Initialize TIMx peripheral as follow:
+ + Period = 10000 - 1
+ + Prescaler = ((SystemCoreClock/2)/10000) - 1
+ + ClockDivision = 0
+ + Counter direction = Up
+ */
+ TimHandle.Init.Period = (CDC_POLLING_INTERVAL*1000) - 1;
+ TimHandle.Init.Prescaler = 84-1;
+ TimHandle.Init.ClockDivision = 0;
+ TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ if(HAL_TIM_Base_Init(&TimHandle) != HAL_OK)
+ {
+ /* Initialization Error */
+ Error_Handler();
+ }
+
+ /*##-6- Enable TIM peripherals Clock #######################################*/
+ TIMx_CLK_ENABLE();
+
+ /*##-7- Configure the NVIC for TIMx ########################################*/
+ /* Set Interrupt Group Priority */
+ HAL_NVIC_SetPriority(TIMx_IRQn, 6, 0);
+
+ /* Enable the TIMx global Interrupt */
+ HAL_NVIC_EnableIRQ(TIMx_IRQn);
+}
+
+/**
+ * @brief This function is executed in case of error occurrence.
+ * @param None
+ * @retval None
+ */
+static void Error_Handler(void)
+{
+ /* Add your own code here */
+}
+
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+
+uint8_t vcpRead()
+{
+ uint8_t ch = 0;
+ if( (rxBuffPtr != NULL) && (rxAvailable > 0) )
+ {
+ ch = rxBuffPtr[0];
+ rxBuffPtr++;
+ rxAvailable--;
+ }
+
+ if(rxAvailable < 1)
+ USBD_CDC_ReceivePacket(&USBD_Device);
+
+ return ch;
+}
+
+uint8_t vcpAvailable()
+{
+ return rxAvailable;
+}
+
+uint32_t CDC_Send_FreeBytes(void)
+{
+ /*
+ return the bytes free in the circular buffer
+
+ functionally equivalent to:
+ (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in)
+ but without the impact of the condition check.
+ */
+ return ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_RX_DATA_SIZE)) - 1;
+}
+
+
+/**
+ * @brief vcpWrite
+ * CDC received data to be send over USB IN endpoint are managed in
+ * this function.
+ * @param Buf: Buffer of data to be sent
+ * @param Len: Number of data to be sent (in bytes)
+ * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL
+ */
+uint32_t vcpWrite(const uint8_t* Buf, uint32_t Len)
+{
+ uint32_t ptr_head = UserTxBufPtrIn;
+ uint32_t ptr_tail = UserTxBufPtrOut;
+ uint32_t i = 0;
+
+ for (i = 0; i < Len; i++)
+ {
+ // head reached tail
+ if(ptr_head == (ptr_tail-1))
+ {
+ break;
+ }
+
+ UserTxBuffer[ptr_head++] = Buf[i];
+ if(ptr_head == (APP_RX_DATA_SIZE))
+ {
+ ptr_head = 0;
+ }
+ }
+ UserTxBufPtrIn = ptr_head;
+ return i;
+}
+
+uint32_t vcpBaudrate()
+{
+ return LineCoding.bitrate;
+}
+
+uint8_t vcpIsConnected()
+{
+ return ((USBD_Device.dev_state == USBD_STATE_CONFIGURED)
+ || (USBD_Device.dev_state == USBD_STATE_SUSPENDED));
+}
diff --git a/src/main/vcp_hal/usbd_cdc_interface.h b/src/main/vcp_hal/usbd_cdc_interface.h
new file mode 100644
index 0000000000..18c35408e5
--- /dev/null
+++ b/src/main/vcp_hal/usbd_cdc_interface.h
@@ -0,0 +1,81 @@
+/**
+ ******************************************************************************
+ * @file USB_Device/CDC_Standalone/Inc/usbd_cdc_interface.h
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief Header for usbd_cdc_interface.c file.
+ ******************************************************************************
+ * @attention
+ *
+ * © Copyright (c) 2016 STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted, provided that the following conditions are met:
+ *
+ * 1. Redistribution of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of other
+ * contributors to this software may be used to endorse or promote products
+ * derived from this software without specific written permission.
+ * 4. This software, including modifications and/or derivative works of this
+ * software, must execute solely and exclusively on microcontroller or
+ * microprocessor devices manufactured by or for STMicroelectronics.
+ * 5. Redistribution and use of this software other than as permitted under
+ * this license is void and will automatically terminate your rights under
+ * this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
+ * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
+ * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __USBD_CDC_IF_H
+#define __USBD_CDC_IF_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "usbd_cdc.h"
+#include "stm32f7xx_hal.h"
+#include "usbd_core.h"
+#include "usbd_desc.h"
+
+/* Definition for TIMx clock resources */
+#define TIMusb TIM7
+#define TIMx_IRQn TIM7_IRQn
+#define TIMx_IRQHandler TIM7_IRQHandler
+#define TIMx_CLK_ENABLE __HAL_RCC_TIM7_CLK_ENABLE
+
+/* Periodically, the state of the buffer "UserTxBuffer" is checked.
+ The period depends on CDC_POLLING_INTERVAL */
+#define CDC_POLLING_INTERVAL 1 /* in ms. The max is 65 and the min is 1 */
+
+extern USBD_CDC_ItfTypeDef USBD_CDC_fops;
+
+uint8_t vcpRead();
+uint8_t vcpAvailable();
+uint32_t vcpWrite(const uint8_t* Buf, uint32_t Len);
+uint32_t vcpBaudrate();
+uint8_t vcpIsConnected();
+uint32_t CDC_Send_FreeBytes(void);
+
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions ------------------------------------------------------- */
+#endif /* __USBD_CDC_IF_H */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/vcp_hal/usbd_conf.c b/src/main/vcp_hal/usbd_conf.c
new file mode 100644
index 0000000000..9413513bbe
--- /dev/null
+++ b/src/main/vcp_hal/usbd_conf.c
@@ -0,0 +1,639 @@
+/**
+ ******************************************************************************
+ * @file USB_Device/CDC_Standalone/Src/usbd_conf.c
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief This file implements the USB Device library callbacks and MSP
+ ******************************************************************************
+ * @attention
+ *
+ * © Copyright (c) 2016 STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted, provided that the following conditions are met:
+ *
+ * 1. Redistribution of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of other
+ * contributors to this software may be used to endorse or promote products
+ * derived from this software without specific written permission.
+ * 4. This software, including modifications and/or derivative works of this
+ * software, must execute solely and exclusively on microcontroller or
+ * microprocessor devices manufactured by or for STMicroelectronics.
+ * 5. Redistribution and use of this software other than as permitted under
+ * this license is void and will automatically terminate your rights under
+ * this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
+ * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
+ * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f7xx_hal.h"
+#include "usbd_core.h"
+#include "usbd_desc.h"
+#include "usbd_cdc.h"
+#include "usbd_conf.h"
+#include "usbd_cdc_interface.h"
+
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+PCD_HandleTypeDef hpcd;
+
+/* Private function prototypes -----------------------------------------------*/
+/* Private functions ---------------------------------------------------------*/
+
+/*******************************************************************************
+ PCD BSP Routines
+*******************************************************************************/
+
+#ifdef USE_USB_FS
+void OTG_FS_IRQHandler(void)
+#else
+void OTG_HS_IRQHandler(void)
+#endif
+{
+ HAL_PCD_IRQHandler(&hpcd);
+}
+
+/**
+ * @brief Initializes the PCD MSP.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
+{
+ GPIO_InitTypeDef GPIO_InitStruct;
+
+ if(hpcd->Instance == USB_OTG_FS)
+ {
+ /* Configure USB FS GPIOs */
+ __HAL_RCC_GPIOA_CLK_ENABLE();
+
+ /* Configure DM DP Pins */
+ GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12);
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS;
+ HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+ if(hpcd->Init.vbus_sensing_enable == 1)
+ {
+ /* Configure VBUS Pin */
+ GPIO_InitStruct.Pin = GPIO_PIN_9;
+ GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+ }
+#if USE_USB_ID
+ /* Configure ID pin */
+ GPIO_InitStruct.Pin = GPIO_PIN_10;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+ GPIO_InitStruct.Pull = GPIO_PULLUP;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS;
+ HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+#endif
+ /* Enable USB FS Clock */
+ __HAL_RCC_USB_OTG_FS_CLK_ENABLE();
+
+ /* Set USBFS Interrupt priority */
+ HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0);
+
+ /* Enable USBFS Interrupt */
+ HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
+ }
+ else if(hpcd->Instance == USB_OTG_HS)
+ {
+#ifdef USE_USB_HS_IN_FS
+
+ __HAL_RCC_GPIOB_CLK_ENABLE();
+
+ /*Configure GPIO for HS on FS mode*/
+ GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_14 |GPIO_PIN_15;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF12_OTG_HS_FS;
+ HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+ if(hpcd->Init.vbus_sensing_enable == 1)
+ {
+ /* Configure VBUS Pin */
+ GPIO_InitStruct.Pin = GPIO_PIN_13 ;
+ GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+ }
+#else
+ /* Configure USB FS GPIOs */
+ __HAL_RCC_GPIOA_CLK_ENABLE();
+ __HAL_RCC_GPIOB_CLK_ENABLE();
+ __HAL_RCC_GPIOC_CLK_ENABLE();
+ __HAL_RCC_GPIOH_CLK_ENABLE();
+ __HAL_RCC_GPIOI_CLK_ENABLE();
+
+ /* CLK */
+ GPIO_InitStruct.Pin = GPIO_PIN_5;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
+ HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+ /* D0 */
+ GPIO_InitStruct.Pin = GPIO_PIN_3;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
+ HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+ /* D1 D2 D3 D4 D5 D6 D7 */
+ GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_5 |\
+ GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
+ HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+ /* STP */
+ GPIO_InitStruct.Pin = GPIO_PIN_0;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
+ HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+ /* NXT */
+ GPIO_InitStruct.Pin = GPIO_PIN_4;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
+ HAL_GPIO_Init(GPIOH, &GPIO_InitStruct);
+
+ /* DIR */
+ GPIO_InitStruct.Pin = GPIO_PIN_11;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
+ HAL_GPIO_Init(GPIOI, &GPIO_InitStruct);
+ __HAL_RCC_USB_OTG_HS_ULPI_CLK_ENABLE();
+#endif
+ /* Enable USB HS Clocks */
+ __HAL_RCC_USB_OTG_HS_CLK_ENABLE();
+
+ /* Set USBHS Interrupt to the lowest priority */
+ HAL_NVIC_SetPriority(OTG_HS_IRQn, 6, 0);
+
+ /* Enable USBHS Interrupt */
+ HAL_NVIC_EnableIRQ(OTG_HS_IRQn);
+ }
+}
+
+/**
+ * @brief De-Initializes the PCD MSP.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd)
+{
+ if(hpcd->Instance == USB_OTG_FS)
+ {
+ /* Disable USB FS Clock */
+ __HAL_RCC_USB_OTG_FS_CLK_DISABLE();
+ __HAL_RCC_SYSCFG_CLK_DISABLE();
+ }
+ else if(hpcd->Instance == USB_OTG_HS)
+ {
+ /* Disable USB HS Clocks */
+ __HAL_RCC_USB_OTG_HS_CLK_DISABLE();
+ __HAL_RCC_SYSCFG_CLK_DISABLE();
+ }
+}
+
+/*******************************************************************************
+ LL Driver Callbacks (PCD -> USB Device Library)
+*******************************************************************************/
+
+/**
+ * @brief SetupStage callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_LL_SetupStage(hpcd->pData, (uint8_t *)hpcd->Setup);
+}
+
+/**
+ * @brief DataOut Stage callback.
+ * @param hpcd: PCD handle
+ * @param epnum: Endpoint Number
+ * @retval None
+ */
+void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+{
+ USBD_LL_DataOutStage(hpcd->pData, epnum, hpcd->OUT_ep[epnum].xfer_buff);
+}
+
+/**
+ * @brief DataIn Stage callback.
+ * @param hpcd: PCD handle
+ * @param epnum: Endpoint Number
+ * @retval None
+ */
+void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+{
+ USBD_LL_DataInStage(hpcd->pData, epnum, hpcd->IN_ep[epnum].xfer_buff);
+}
+
+/**
+ * @brief SOF callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_SOFCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_LL_SOF(hpcd->pData);
+}
+
+/**
+ * @brief Reset callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_SpeedTypeDef speed = USBD_SPEED_FULL;
+
+ /* Set USB Current Speed */
+ switch(hpcd->Init.speed)
+ {
+ case PCD_SPEED_HIGH:
+ speed = USBD_SPEED_HIGH;
+ break;
+
+ case PCD_SPEED_FULL:
+ speed = USBD_SPEED_FULL;
+ break;
+
+ default:
+ speed = USBD_SPEED_FULL;
+ break;
+ }
+
+ /* Reset Device */
+ USBD_LL_Reset(hpcd->pData);
+
+ USBD_LL_SetSpeed(hpcd->pData, speed);
+}
+
+/**
+ * @brief Suspend callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_LL_Suspend(hpcd->pData);
+}
+
+/**
+ * @brief Resume callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_LL_Resume(hpcd->pData);
+}
+
+/**
+ * @brief ISOOUTIncomplete callback.
+ * @param hpcd: PCD handle
+ * @param epnum: Endpoint Number
+ * @retval None
+ */
+void HAL_PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+{
+ USBD_LL_IsoOUTIncomplete(hpcd->pData, epnum);
+}
+
+/**
+ * @brief ISOINIncomplete callback.
+ * @param hpcd: PCD handle
+ * @param epnum: Endpoint Number
+ * @retval None
+ */
+void HAL_PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+{
+ USBD_LL_IsoINIncomplete(hpcd->pData, epnum);
+}
+
+/**
+ * @brief ConnectCallback callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_LL_DevConnected(hpcd->pData);
+}
+
+/**
+ * @brief Disconnect callback.
+ * @param hpcd: PCD handle
+ * @retval None
+ */
+void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd)
+{
+ USBD_LL_DevDisconnected(hpcd->pData);
+}
+
+
+/*******************************************************************************
+ LL Driver Interface (USB Device Library --> PCD)
+*******************************************************************************/
+
+/**
+ * @brief Initializes the Low Level portion of the Device driver.
+ * @param pdev: Device handle
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev)
+{
+#ifdef USE_USB_FS
+ /* Set LL Driver parameters */
+ hpcd.Instance = USB_OTG_FS;
+ hpcd.Init.dev_endpoints = 4;
+ hpcd.Init.use_dedicated_ep1 = 0;
+ hpcd.Init.ep0_mps = 0x40;
+ hpcd.Init.dma_enable = 0;
+ hpcd.Init.low_power_enable = 0;
+ hpcd.Init.phy_itface = PCD_PHY_EMBEDDED;
+ hpcd.Init.Sof_enable = 0;
+ hpcd.Init.speed = PCD_SPEED_FULL;
+ hpcd.Init.vbus_sensing_enable = 0;
+ hpcd.Init.lpm_enable = 0;
+
+ /* Link The driver to the stack */
+ hpcd.pData = pdev;
+ pdev->pData = &hpcd;
+
+ /* Initialize LL Driver */
+ HAL_PCD_Init(&hpcd);
+
+ HAL_PCDEx_SetRxFiFo(&hpcd, 0x80);
+ HAL_PCDEx_SetTxFiFo(&hpcd, 0, 0x40);
+ HAL_PCDEx_SetTxFiFo(&hpcd, 1, 0x80);
+#endif
+
+#ifdef USE_USB_HS
+ /* Set LL Driver parameters */
+ hpcd.Instance = USB_OTG_HS;
+ hpcd.Init.dev_endpoints = 6;
+ hpcd.Init.use_dedicated_ep1 = 0;
+ hpcd.Init.ep0_mps = 0x40;
+
+ /* Be aware that enabling DMA mode will result in data being sent only by
+ multiple of 4 packet sizes. This is due to the fact that USB DMA does
+ not allow sending data from non word-aligned addresses.
+ For this specific application, it is advised to not enable this option
+ unless required. */
+ hpcd.Init.dma_enable = 0;
+ hpcd.Init.low_power_enable = 0;
+ hpcd.Init.lpm_enable = 0;
+
+#ifdef USE_USB_HS_IN_FS
+ hpcd.Init.phy_itface = PCD_PHY_EMBEDDED;
+#else
+ hpcd.Init.phy_itface = PCD_PHY_ULPI;
+#endif
+ hpcd.Init.Sof_enable = 0;
+ hpcd.Init.speed = PCD_SPEED_HIGH;
+ hpcd.Init.vbus_sensing_enable = 1;
+
+ /* Link The driver to the stack */
+ hpcd.pData = pdev;
+ pdev->pData = &hpcd;
+
+ /* Initialize LL Driver */
+ HAL_PCD_Init(&hpcd);
+
+ HAL_PCDEx_SetRxFiFo(&hpcd, 0x200);
+ HAL_PCDEx_SetTxFiFo(&hpcd, 0, 0x80);
+ HAL_PCDEx_SetTxFiFo(&hpcd, 1, 0x174);
+#endif
+
+ return USBD_OK;
+}
+
+/**
+ * @brief De-Initializes the Low Level portion of the Device driver.
+ * @param pdev: Device handle
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev)
+{
+ HAL_PCD_DeInit(pdev->pData);
+ return USBD_OK;
+}
+
+/**
+ * @brief Starts the Low Level portion of the Device driver.
+ * @param pdev: Device handle
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev)
+{
+ HAL_PCD_Start(pdev->pData);
+ return USBD_OK;
+}
+
+/**
+ * @brief Stops the Low Level portion of the Device driver.
+ * @param pdev: Device handle
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev)
+{
+ HAL_PCD_Stop(pdev->pData);
+ return USBD_OK;
+}
+
+/**
+ * @brief Opens an endpoint of the Low Level Driver.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @param ep_type: Endpoint Type
+ * @param ep_mps: Endpoint Max Packet Size
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev,
+ uint8_t ep_addr,
+ uint8_t ep_type,
+ uint16_t ep_mps)
+{
+ HAL_PCD_EP_Open(pdev->pData,
+ ep_addr,
+ ep_mps,
+ ep_type);
+
+ return USBD_OK;
+}
+
+/**
+ * @brief Closes an endpoint of the Low Level Driver.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
+{
+ HAL_PCD_EP_Close(pdev->pData, ep_addr);
+ return USBD_OK;
+}
+
+/**
+ * @brief Flushes an endpoint of the Low Level Driver.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
+{
+ HAL_PCD_EP_Flush(pdev->pData, ep_addr);
+ return USBD_OK;
+}
+
+/**
+ * @brief Sets a Stall condition on an endpoint of the Low Level Driver.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
+{
+ HAL_PCD_EP_SetStall(pdev->pData, ep_addr);
+ return USBD_OK;
+}
+
+/**
+ * @brief Clears a Stall condition on an endpoint of the Low Level Driver.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
+{
+ HAL_PCD_EP_ClrStall(pdev->pData, ep_addr);
+ return USBD_OK;
+}
+
+/**
+ * @brief Returns Stall condition.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval Stall (1: Yes, 0: No)
+ */
+uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
+{
+ PCD_HandleTypeDef *hpcd = pdev->pData;
+
+ if((ep_addr & 0x80) == 0x80)
+ {
+ return hpcd->IN_ep[ep_addr & 0x7F].is_stall;
+ }
+ else
+ {
+ return hpcd->OUT_ep[ep_addr & 0x7F].is_stall;
+ }
+}
+
+/**
+ * @brief Assigns a USB address to the device.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr)
+{
+ HAL_PCD_SetAddress(pdev->pData, dev_addr);
+ return USBD_OK;
+}
+
+/**
+ * @brief Transmits data over an endpoint.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @param pbuf: Pointer to data to be sent
+ * @param size: Data size
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev,
+ uint8_t ep_addr,
+ uint8_t *pbuf,
+ uint16_t size)
+{
+ HAL_PCD_EP_Transmit(pdev->pData, ep_addr, pbuf, size);
+ return USBD_OK;
+}
+
+/**
+ * @brief Prepares an endpoint for reception.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @param pbuf: Pointer to data to be received
+ * @param size: Data size
+ * @retval USBD Status
+ */
+USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev,
+ uint8_t ep_addr,
+ uint8_t *pbuf,
+ uint16_t size)
+{
+ HAL_PCD_EP_Receive(pdev->pData, ep_addr, pbuf, size);
+ return USBD_OK;
+}
+
+/**
+ * @brief Returns the last transferred packet size.
+ * @param pdev: Device handle
+ * @param ep_addr: Endpoint Number
+ * @retval Received Data Size
+ */
+uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
+{
+ return HAL_PCD_EP_GetRxCount(pdev->pData, ep_addr);
+}
+
+/**
+ * @brief Delays routine for the USB Device Library.
+ * @param Delay: Delay in ms
+ * @retval None
+ */
+void USBD_LL_Delay(uint32_t Delay)
+{
+ HAL_Delay(Delay);
+}
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/vcp_hal/usbd_conf.h b/src/main/vcp_hal/usbd_conf.h
new file mode 100644
index 0000000000..3c6843a77a
--- /dev/null
+++ b/src/main/vcp_hal/usbd_conf.h
@@ -0,0 +1,105 @@
+/**
+ ******************************************************************************
+ * @file USB_Device/CDC_Standalone/Inc/usbd_conf.h
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief General low level driver configuration
+ ******************************************************************************
+ * @attention
+ *
+ * © Copyright (c) 2016 STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted, provided that the following conditions are met:
+ *
+ * 1. Redistribution of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of other
+ * contributors to this software may be used to endorse or promote products
+ * derived from this software without specific written permission.
+ * 4. This software, including modifications and/or derivative works of this
+ * software, must execute solely and exclusively on microcontroller or
+ * microprocessor devices manufactured by or for STMicroelectronics.
+ * 5. Redistribution and use of this software other than as permitted under
+ * this license is void and will automatically terminate your rights under
+ * this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
+ * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
+ * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __USBD_CONF_H
+#define __USBD_CONF_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f7xx_hal.h"
+#include
+#include
+#include
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+/* Common Config */
+#define USBD_MAX_NUM_INTERFACES 1
+#define USBD_MAX_NUM_CONFIGURATION 1
+#define USBD_MAX_STR_DESC_SIZ 0x100
+#define USBD_SUPPORT_USER_STRING 0
+#define USBD_SELF_POWERED 1
+#define USBD_DEBUG_LEVEL 0
+#define USE_USB_FS
+
+/* Exported macro ------------------------------------------------------------*/
+/* Memory management macros */
+#define USBD_malloc malloc
+#define USBD_free free
+#define USBD_memset memset
+#define USBD_memcpy memcpy
+
+/* DEBUG macros */
+#if (USBD_DEBUG_LEVEL > 0)
+#define USBD_UsrLog(...) printf(__VA_ARGS__);\
+ printf("\n");
+#else
+#define USBD_UsrLog(...)
+#endif
+
+#if (USBD_DEBUG_LEVEL > 1)
+
+#define USBD_ErrLog(...) printf("ERROR: ") ;\
+ printf(__VA_ARGS__);\
+ printf("\n");
+#else
+#define USBD_ErrLog(...)
+#endif
+
+#if (USBD_DEBUG_LEVEL > 2)
+#define USBD_DbgLog(...) printf("DEBUG : ") ;\
+ printf(__VA_ARGS__);\
+ printf("\n");
+#else
+#define USBD_DbgLog(...)
+#endif
+
+/* Exported functions ------------------------------------------------------- */
+
+#endif /* __USBD_CONF_H */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/vcp_hal/usbd_desc.c
new file mode 100644
index 0000000000..d79eba51e0
--- /dev/null
+++ b/src/main/vcp_hal/usbd_desc.c
@@ -0,0 +1,304 @@
+/**
+ ******************************************************************************
+ * @file USB_Device/CDC_Standalone/Src/usbd_desc.c
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief This file provides the USBD descriptors and string formating method.
+ ******************************************************************************
+ * @attention
+ *
+ * © Copyright (c) 2016 STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted, provided that the following conditions are met:
+ *
+ * 1. Redistribution of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of other
+ * contributors to this software may be used to endorse or promote products
+ * derived from this software without specific written permission.
+ * 4. This software, including modifications and/or derivative works of this
+ * software, must execute solely and exclusively on microcontroller or
+ * microprocessor devices manufactured by or for STMicroelectronics.
+ * 5. Redistribution and use of this software other than as permitted under
+ * this license is void and will automatically terminate your rights under
+ * this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
+ * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
+ * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "usbd_core.h"
+#include "usbd_desc.h"
+#include "usbd_conf.h"
+
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+#define USBD_VID 0x0483
+#define USBD_PID 0x5740
+#define USBD_LANGID_STRING 0x409
+#define USBD_MANUFACTURER_STRING "STMicroelectronics"
+#define USBD_PRODUCT_HS_STRING "STM32 Virtual ComPort in HS Mode"
+#define USBD_PRODUCT_FS_STRING "STM32 Virtual ComPort in FS Mode"
+#define USBD_CONFIGURATION_HS_STRING "VCP Config"
+#define USBD_INTERFACE_HS_STRING "VCP Interface"
+#define USBD_CONFIGURATION_FS_STRING "VCP Config"
+#define USBD_INTERFACE_FS_STRING "VCP Interface"
+
+/* Private macro -------------------------------------------------------------*/
+/* Private function prototypes -----------------------------------------------*/
+uint8_t *USBD_VCP_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+uint8_t *USBD_VCP_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+uint8_t *USBD_VCP_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+uint8_t *USBD_VCP_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+uint8_t *USBD_VCP_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+uint8_t *USBD_VCP_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
+#ifdef USB_SUPPORT_USER_STRING_DESC
+uint8_t *USBD_VCP_USRStringDesc (USBD_SpeedTypeDef speed, uint8_t idx, uint16_t *length);
+#endif /* USB_SUPPORT_USER_STRING_DESC */
+
+/* Private variables ---------------------------------------------------------*/
+USBD_DescriptorsTypeDef VCP_Desc = {
+ USBD_VCP_DeviceDescriptor,
+ USBD_VCP_LangIDStrDescriptor,
+ USBD_VCP_ManufacturerStrDescriptor,
+ USBD_VCP_ProductStrDescriptor,
+ USBD_VCP_SerialStrDescriptor,
+ USBD_VCP_ConfigStrDescriptor,
+ USBD_VCP_InterfaceStrDescriptor,
+};
+
+/* USB Standard Device Descriptor */
+#if defined ( __ICCARM__ ) /*!< IAR Compiler */
+ #pragma data_alignment=4
+#endif
+__ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_LEN_DEV_DESC] __ALIGN_END = {
+ 0x12, /* bLength */
+ USB_DESC_TYPE_DEVICE, /* bDescriptorType */
+ 0x00, /* bcdUSB */
+ 0x02,
+ 0x00, /* bDeviceClass */
+ 0x00, /* bDeviceSubClass */
+ 0x00, /* bDeviceProtocol */
+ USB_MAX_EP0_SIZE, /* bMaxPacketSize */
+ LOBYTE(USBD_VID), /* idVendor */
+ HIBYTE(USBD_VID), /* idVendor */
+ LOBYTE(USBD_PID), /* idVendor */
+ HIBYTE(USBD_PID), /* idVendor */
+ 0x00, /* bcdDevice rel. 2.00 */
+ 0x02,
+ USBD_IDX_MFC_STR, /* Index of manufacturer string */
+ USBD_IDX_PRODUCT_STR, /* Index of product string */
+ USBD_IDX_SERIAL_STR, /* Index of serial number string */
+ USBD_MAX_NUM_CONFIGURATION /* bNumConfigurations */
+}; /* USB_DeviceDescriptor */
+
+/* USB Standard Device Descriptor */
+#if defined ( __ICCARM__ ) /*!< IAR Compiler */
+ #pragma data_alignment=4
+#endif
+__ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_LEN_LANGID_STR_DESC] __ALIGN_END = {
+ USB_LEN_LANGID_STR_DESC,
+ USB_DESC_TYPE_STRING,
+ LOBYTE(USBD_LANGID_STRING),
+ HIBYTE(USBD_LANGID_STRING),
+};
+
+uint8_t USBD_StringSerial[USB_SIZ_STRING_SERIAL] =
+{
+ USB_SIZ_STRING_SERIAL,
+ USB_DESC_TYPE_STRING,
+};
+
+#if defined ( __ICCARM__ ) /*!< IAR Compiler */
+ #pragma data_alignment=4
+#endif
+__ALIGN_BEGIN uint8_t USBD_StrDesc[USBD_MAX_STR_DESC_SIZ] __ALIGN_END;
+
+/* Private functions ---------------------------------------------------------*/
+static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len);
+static void Get_SerialNum(void);
+
+/**
+ * @brief Returns the device descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ (void)speed;
+ *length = sizeof(USBD_DeviceDesc);
+ return (uint8_t*)USBD_DeviceDesc;
+}
+
+/**
+ * @brief Returns the LangID string descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ (void)speed;
+ *length = sizeof(USBD_LangIDDesc);
+ return (uint8_t*)USBD_LangIDDesc;
+}
+
+/**
+ * @brief Returns the product string descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ if(speed == USBD_SPEED_HIGH)
+ {
+ USBD_GetString((uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length);
+ }
+ else
+ {
+ USBD_GetString((uint8_t *)USBD_PRODUCT_FS_STRING, USBD_StrDesc, length);
+ }
+ return USBD_StrDesc;
+}
+
+/**
+ * @brief Returns the manufacturer string descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ (void)speed;
+ USBD_GetString((uint8_t *)USBD_MANUFACTURER_STRING, USBD_StrDesc, length);
+ return USBD_StrDesc;
+}
+
+/**
+ * @brief Returns the serial number string descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ (void)speed;
+ *length = USB_SIZ_STRING_SERIAL;
+
+ /* Update the serial number string descriptor with the data from the unique ID*/
+ Get_SerialNum();
+
+ return (uint8_t*)USBD_StringSerial;
+}
+
+/**
+ * @brief Returns the configuration string descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ if(speed == USBD_SPEED_HIGH)
+ {
+ USBD_GetString((uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length);
+ }
+ else
+ {
+ USBD_GetString((uint8_t *)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length);
+ }
+ return USBD_StrDesc;
+}
+
+/**
+ * @brief Returns the interface string descriptor.
+ * @param speed: Current device speed
+ * @param length: Pointer to data length variable
+ * @retval Pointer to descriptor buffer
+ */
+uint8_t *USBD_VCP_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
+{
+ if(speed == USBD_SPEED_HIGH)
+ {
+ USBD_GetString((uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length);
+ }
+ else
+ {
+ USBD_GetString((uint8_t *)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length);
+ }
+ return USBD_StrDesc;
+}
+
+/**
+ * @brief Create the serial number string descriptor
+ * @param None
+ * @retval None
+ */
+static void Get_SerialNum(void)
+{
+ uint32_t deviceserial0, deviceserial1, deviceserial2;
+
+ deviceserial0 = *(uint32_t*)DEVICE_ID1;
+ deviceserial1 = *(uint32_t*)DEVICE_ID2;
+ deviceserial2 = *(uint32_t*)DEVICE_ID3;
+
+ deviceserial0 += deviceserial2;
+
+ if (deviceserial0 != 0)
+ {
+ IntToUnicode (deviceserial0, &USBD_StringSerial[2] ,8);
+ IntToUnicode (deviceserial1, &USBD_StringSerial[18] ,4);
+ }
+}
+
+/**
+ * @brief Convert Hex 32Bits value into char
+ * @param value: value to convert
+ * @param pbuf: pointer to the buffer
+ * @param len: buffer length
+ * @retval None
+ */
+static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len)
+{
+ uint8_t idx = 0;
+
+ for( idx = 0; idx < len; idx ++)
+ {
+ if( ((value >> 28)) < 0xA )
+ {
+ pbuf[ 2* idx] = (value >> 28) + '0';
+ }
+ else
+ {
+ pbuf[2* idx] = (value >> 28) + 'A' - 10;
+ }
+
+ value = value << 4;
+
+ pbuf[ 2* idx + 1] = 0;
+ }
+}
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+
diff --git a/src/main/vcp_hal/usbd_desc.h b/src/main/vcp_hal/usbd_desc.h
new file mode 100644
index 0000000000..70fc6b7efb
--- /dev/null
+++ b/src/main/vcp_hal/usbd_desc.h
@@ -0,0 +1,68 @@
+/**
+ ******************************************************************************
+ * @file USB_Device/CDC_Standalone/Inc/usbd_desc.h
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 22-April-2016
+ * @brief Header for usbd_desc.c module
+ ******************************************************************************
+ * @attention
+ *
+ * © Copyright (c) 2016 STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted, provided that the following conditions are met:
+ *
+ * 1. Redistribution of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of other
+ * contributors to this software may be used to endorse or promote products
+ * derived from this software without specific written permission.
+ * 4. This software, including modifications and/or derivative works of this
+ * software, must execute solely and exclusively on microcontroller or
+ * microprocessor devices manufactured by or for STMicroelectronics.
+ * 5. Redistribution and use of this software other than as permitted under
+ * this license is void and will automatically terminate your rights under
+ * this license.
+ *
+ * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
+ * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
+ * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __USBD_DESC_H
+#define __USBD_DESC_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "usbd_def.h"
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+#define DEVICE_ID1 (0x1FFF7A10)
+#define DEVICE_ID2 (0x1FFF7A14)
+#define DEVICE_ID3 (0x1FFF7A18)
+
+#define USB_SIZ_STRING_SERIAL 0x1A
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions ------------------------------------------------------- */
+extern USBD_DescriptorsTypeDef VCP_Desc;
+
+#endif /* __USBD_DESC_H */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/