diff --git a/.travis.sh b/.travis.sh index ee310af88b..0d6a3cdc1a 100755 --- a/.travis.sh +++ b/.travis.sh @@ -8,7 +8,6 @@ TARGET_FILE=obj/betaflight_${TARGET} TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} -MAKEFILE="-f Makefile" CURL_BASEOPTS=( "--retry" "10" @@ -22,12 +21,8 @@ CURL_PUB_BASEOPTS=( "--form" "github_repo=${TRAVIS_REPO_SLUG}" "--form" "build_name=${BUILDNAME}" ) -# A hacky way of running the unit tests at the same time as the normal builds. -if [ $RUNTESTS ] ; then - cd ./src/test && make test - # A hacky way of building the docs at the same time as the normal builds. -elif [ $PUBLISHDOCS ] ; then +if [ $PUBLISHDOCS ] ; then if [ $PUBLISH_URL ] ; then # Patch Gimli to fix underscores_inside_words @@ -50,9 +45,9 @@ elif [ $PUBLISHMETA ] ; then curl -k "${CURL_BASEOPTS[@]}" "${CURL_PUB_BASEOPTS[@]}" --form "recent_commits=${RECENT_COMMITS}" ${PUBLISH_URL} || true fi -else +elif [ $TARGET ] ; then + make $TARGET if [ $PUBLISH_URL ] ; then - make -j2 $MAKEFILE if [ -f ${TARGET_FILE}.bin ] ; then TARGET_FILE=${TARGET_FILE}.bin elif [ -f ${TARGET_FILE}.hex ] ; then @@ -64,7 +59,9 @@ else curl -k "${CURL_BASEOPTS[@]}" "${CURL_PUB_BASEOPTS[@]}" --form "file=@${TARGET_FILE}" ${PUBLISH_URL} || true exit 0; - else - make -j2 $MAKEFILE fi +elif [ $GOAL ] ; then + make V=0 $GOAL +else + make V=0 all fi diff --git a/.travis.yml b/.travis.yml index 8ba47b575c..5072324926 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,20 +1,25 @@ + env: -# - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True +# Specify the main Mafile supported goals. + - GOAL=test + - GOAL=all +# Or specify targets to run. # - TARGET=AFROMINI -# - TARGET=BEEBRAIN # - TARGET=AIORACERF3 # - TARGET=AIR32 +# - TARGET=AIRBOTF4 # - TARGET=AIRHEROF3 # - TARGET=ALIENFLIGHTF1 - - TARGET=ALIENFLIGHTF3 - - TARGET=ALIENFLIGHTF4 - - TARGET=ANYFCF7 - - TARGET=BETAFLIGHTF3 - - TARGET=BLUEJAYF4 - - TARGET=CC3D - - TARGET=CC3D_OPBL +# - TARGET=ALIENFLIGHTF3 +# - TARGET=ALIENFLIGHTF4 +# - TARGET=ANYFCF7 +# - TARGET=BEEBRAIN +# - TARGET=BETAFLIGHTF3 +# - TARGET=BLUEJAYF4 +# - TARGET=CC3D +# - TARGET=CC3D_OPBL # - TARGET=CHEBUZZF3 # - TARGET=CJMCU # - TARGET=COLIBRI @@ -23,50 +28,51 @@ env: # - TARGET=DOGE # - TARGET=F4BY # - TARGET=FURYF3 - - TARGET=FURYF4 +# - TARGET=FURYF4 +# - TARGET=FURYF7 +# - TARGET=IMPULSERCF3 # - TARGET=IRCFUSIONF3 # - TARGET=ISHAPEDF3 # - TARGET=KISSFC +# - TARGET=LUXV2_RACE # - TARGET=LUX_RACE # - TARGET=MICROSCISKY # - TARGET=MOTOLAB - - TARGET=NAZE +# - TARGET=NAZE # - TARGET=OMNIBUS # - TARGET=OMNIBUSF4 # - TARGET=PIKOBLX # - TARGET=RACEBASE - - TARGET=REVO +# - TARGET=RCEXPLORERF3 +# - TARGET=REVO +# - TARGET=REVOLT # - TARGET=REVONANO # - TARGET=REVO_OPBL # - TARGET=RMDO # - TARGET=SINGULARITY - - TARGET=SIRINFPV - - TARGET=SPARKY +# - TARGET=SIRINFPV +# - TARGET=SOULF4 +# - TARGET=SPARKY # - TARGET=SPARKY2 -# - TARGET=SPARKY_OPBL - - TARGET=SPRACINGF3 - - TARGET=SPRACINGF3EVO +# - TARGET=SPRACINGF3 +# - TARGET=SPRACINGF3EVO # - TARGET=SPRACINGF3MINI - - TARGET=STM32F3DISCOVERY +# - TARGET=STM32F3DISCOVERY # - TARGET=VRRACE # - TARGET=X_RACERSPI +# - TARGET=YUPIF4 # - TARGET=ZCOREF3 -# - TARGET=RCEXPLORERF3 # use new docker environment sudo: false +git: + depth: 5 + addons: apt: packages: - - build-essential - - git - libc6-i386 - - zlib1g-dev - - libssl-dev - - wkhtmltopdf - - libxml2-dev - - libxslt-dev # We use cpp for unit tests, and c for the main project. language: cpp @@ -75,11 +81,18 @@ compiler: clang install: - make arm_sdk_install -before_script: tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version +before_script: + - tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version + - clang --version + - clang++ --version + script: ./.travis.sh -cache: apt - +cache: + directories: + - downloads + - tools + #notifications: # irc: "chat.freenode.net#cleanflight" # use_notice: true @@ -93,4 +106,3 @@ notifications: on_success: always # options: [always|never|change] default: always on_failure: always # options: [always|never|change] default: always on_start: always # options: [always|never|change] default: always - diff --git a/Makefile b/Makefile index d234823bce..1a8ea3a565 100644 --- a/Makefile +++ b/Makefile @@ -45,10 +45,16 @@ export AT := @ ifndef V export V0 := export V1 := $(AT) +export STDOUT := else ifeq ($(V), 0) export V0 := $(AT) export V1 := $(AT) +export STDOUT:= "> /dev/null" +export MAKE := $(MAKE) --no-print-directory else ifeq ($(V), 1) +export V0 := +export V1 := +export STDOUT := endif ############################################################################### @@ -158,6 +164,10 @@ else STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S endif +ifeq ($(DEBUG_HARDFAULTS),F7) +CFLAGS += -DDEBUG_HARDFAULTS +endif + REVISION = $(shell git log -1 --format="%h") FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' ) @@ -364,7 +374,7 @@ endif 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 +DEVICE_FLAGS = -DSTM32F745xx -DUSE_HAL_DRIVER -D__FPU_PRESENT LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f745.ld else $(error Unknown MCU for F7 target) @@ -481,6 +491,7 @@ COMMON_SRC = \ drivers/bus_i2c_soft.c \ drivers/bus_spi.c \ drivers/bus_spi_soft.c \ + drivers/display.c \ drivers/exti.c \ drivers/gyro_sync.c \ drivers/io.c \ @@ -546,6 +557,14 @@ COMMON_SRC = \ HIGHEND_SRC = \ blackbox/blackbox.c \ blackbox/blackbox_io.c \ + cms/cms.c \ + cms/cms_menu_blackbox.c \ + cms/cms_menu_builtin.c \ + cms/cms_menu_imu.c \ + cms/cms_menu_ledstrip.c \ + cms/cms_menu_misc.c \ + cms/cms_menu_osd.c \ + cms/cms_menu_vtx.c \ common/colorconversion.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ @@ -555,9 +574,13 @@ HIGHEND_SRC = \ flight/gtune.c \ flight/navigation.c \ flight/gps_conversion.c \ + io/dashboard.c \ + io/displayport_max7456.c \ + io/displayport_msp.c \ + io/displayport_oled.c \ io/gps.c \ io/ledstrip.c \ - io/display.c \ + io/osd.c \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ @@ -650,7 +673,7 @@ STM32F7xx_COMMON_SRC = \ drivers/pwm_output_hal.c \ drivers/system_stm32f7xx.c \ drivers/serial_uart_stm32f7xx.c \ - drivers/serial_uart_hal.c + drivers/serial_uart_hal.c F7EXCLUDES = drivers/bus_spi.c \ drivers/bus_i2c.c \ @@ -716,8 +739,8 @@ CCACHE := endif # Tool names -CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc -CPP := $(CCACHE) $(ARM_SDK_PREFIX)g++ +CROSS_CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc +CROSS_CXX := $(CCACHE) $(ARM_SDK_PREFIX)g++ OBJCOPY := $(ARM_SDK_PREFIX)objcopy SIZE := $(ARM_SDK_PREFIX)size @@ -809,26 +832,26 @@ $(TARGET_BIN): $(TARGET_ELF) $(V0) $(OBJCOPY) -O binary $< $@ $(TARGET_ELF): $(TARGET_OBJS) - $(V1) echo LD $(notdir $@) - $(V1) $(CC) -o $@ $^ $(LDFLAGS) + $(V1) echo Linking $(TARGET) + $(V1) $(CROSS_CC) -o $@ $^ $(LDFLAGS) $(V0) $(SIZE) $(TARGET_ELF) # Compile $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) - $(V1) echo %% $(notdir $<) - $(V1) $(CC) -c -o $@ $(CFLAGS) $< + $(V1) echo "%% $(notdir $<)" "$(STDOUT)" + $(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $< # Assemble $(OBJECT_DIR)/$(TARGET)/%.o: %.s $(V1) mkdir -p $(dir $@) - $(V1) echo %% $(notdir $<) - $(V1) $(CC) -c -o $@ $(ASFLAGS) $< + $(V1) echo "%% $(notdir $<)" "$(STDOUT)" + $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< $(OBJECT_DIR)/$(TARGET)/%.o: %.S $(V1) mkdir -p $(dir $@) - $(V1) echo %% $(notdir $<) - $(V1) $(CC) -c -o $@ $(ASFLAGS) $< + $(V1) echo "%% $(notdir $<)" "$(STDOUT)" + $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< ## sample : Build all sample (travis) targets sample: $(SAMPLE_TARGETS) @@ -940,7 +963,7 @@ targets: ## test : run the cleanflight test suite ## junittest : run the cleanflight test suite, producing Junit XML result files. test junittest: - $(V0) cd src/test && $(MAKE) $@ || true + $(V0) cd src/test && $(MAKE) $@ # rebuild everything when makefile changes $(TARGET_OBJS) : Makefile diff --git a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index b2ca5f1648..d9befc711c 100644 --- a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -475,6 +475,7 @@ __ALIGN_BEGIN uint8_t USBD_CDC_OtherSpeedCfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIG static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev, uint8_t cfgidx) { + (void)cfgidx; uint8_t ret = 0; USBD_CDC_HandleTypeDef *hcdc; @@ -563,6 +564,7 @@ static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev, static uint8_t USBD_CDC_DeInit (USBD_HandleTypeDef *pdev, uint8_t cfgidx) { + (void)cfgidx; uint8_t ret = 0; /* Open EP IN */ @@ -663,6 +665,7 @@ static uint8_t USBD_CDC_Setup (USBD_HandleTypeDef *pdev, */ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) { + (void)epnum; USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; if(pdev->pClassData != NULL) diff --git a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c index 0158829c52..7ebb1873a1 100644 --- a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c +++ b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c @@ -212,6 +212,7 @@ USBD_StatusTypeDef USBD_Stop (USBD_HandleTypeDef *pdev) */ USBD_StatusTypeDef USBD_RunTestMode (USBD_HandleTypeDef *pdev) { + (void)pdev; return USBD_OK; } @@ -508,6 +509,8 @@ USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev) */ USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum) { + (void)pdev; + (void)epnum; return USBD_OK; } @@ -519,6 +522,8 @@ USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t ep */ USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum) { + (void)pdev; + (void)epnum; return USBD_OK; } @@ -530,6 +535,7 @@ USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t e */ USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev) { + (void)pdev; return USBD_OK; } diff --git a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c index 49330c667b..0f38e9af3e 100644 --- a/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c +++ b/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c @@ -716,6 +716,7 @@ void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata) void USBD_CtlError( USBD_HandleTypeDef *pdev , USBD_SetupReqTypedef *req) { + (void)req; USBD_LL_StallEP(pdev , 0x80); USBD_LL_StallEP(pdev , 0); } diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c index 4cb8ca8c5d..64416398e0 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_adc_ex.c @@ -756,6 +756,7 @@ HAL_StatusTypeDef HAL_ADCEx_MultiModeStop_DMA(ADC_HandleTypeDef* hadc) */ uint32_t HAL_ADCEx_MultiModeGetValue(ADC_HandleTypeDef* hadc) { + (void)hadc; /* Return the multi mode conversion value */ return ADC->CDR; } diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c index f761655d00..1e164b883a 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_i2c.c @@ -2845,6 +2845,7 @@ HAL_StatusTypeDef HAL_I2C_DisableListen_IT(I2C_HandleTypeDef *hi2c) */ HAL_StatusTypeDef HAL_I2C_Master_Abort_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress) { + (void)DevAddress; if(hi2c->Mode == HAL_I2C_MODE_MASTER) { /* Process Locked */ @@ -3684,6 +3685,7 @@ static HAL_StatusTypeDef I2C_RequestMemoryRead(I2C_HandleTypeDef *hi2c, uint16_t */ static void I2C_ITAddrCplt(I2C_HandleTypeDef *hi2c, uint32_t ITFlags) { + (void)ITFlags; uint8_t transferdirection = 0; uint16_t slaveaddrcode = 0; uint16_t ownadd1code = 0; @@ -4254,6 +4256,7 @@ static void I2C_DMASlaveTransmitCplt(DMA_HandleTypeDef *hdma) /* No specific action, Master fully manage the generation of STOP condition */ /* Mean that this generation can arrive at any time, at the end or during DMA process */ /* So STOP condition should be manage through Interrupt treatment */ + (void)hdma; } /** @@ -4308,6 +4311,7 @@ static void I2C_DMASlaveReceiveCplt(DMA_HandleTypeDef *hdma) /* No specific action, Master fully manage the generation of STOP condition */ /* Mean that this generation can arrive at any time, at the end or during DMA process */ /* So STOP condition should be manage through Interrupt treatment */ + (void)hdma; } /** diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c index 9d59c0124f..6811622868 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pwr.c @@ -404,6 +404,7 @@ void HAL_PWR_DisableWakeUpPin(uint32_t WakeUpPinx) */ void HAL_PWR_EnterSLEEPMode(uint32_t Regulator, uint8_t SLEEPEntry) { + (void)Regulator; /* Check the parameters */ assert_param(IS_PWR_REGULATOR(Regulator)); assert_param(IS_PWR_SLEEP_ENTRY(SLEEPEntry)); diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c index 4776fb4e34..de97e1feb3 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_tim.c @@ -2118,7 +2118,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Start(TIM_HandleTypeDef *htim, uint32_t Outpu No need to enable the counter, it's enabled automatically by hardware (the counter starts in response to a stimulus and generate a pulse */ - + (void)OutputChannel; TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_1, TIM_CCx_ENABLE); TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_2, TIM_CCx_ENABLE); @@ -2150,6 +2150,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Stop(TIM_HandleTypeDef *htim, uint32_t Output if TIM_CHANNEL_1 is used as input, the TIM_CHANNEL_2 will be used as output in all combinations, the TIM_CHANNEL_1 and TIM_CHANNEL_2 should be disabled together */ + (void)OutputChannel; TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_1, TIM_CCx_DISABLE); TIM_CCxChannelCmd(htim->Instance, TIM_CHANNEL_2, TIM_CCx_DISABLE); @@ -2187,6 +2188,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Start_IT(TIM_HandleTypeDef *htim, uint32_t Ou No need to enable the counter, it's enabled automatically by hardware (the counter starts in response to a stimulus and generate a pulse */ + (void)OutputChannel; /* Enable the TIM Capture/Compare 1 interrupt */ __HAL_TIM_ENABLE_IT(htim, TIM_IT_CC1); @@ -2218,6 +2220,7 @@ HAL_StatusTypeDef HAL_TIM_OnePulse_Start_IT(TIM_HandleTypeDef *htim, uint32_t Ou */ HAL_StatusTypeDef HAL_TIM_OnePulse_Stop_IT(TIM_HandleTypeDef *htim, uint32_t OutputChannel) { + (void)OutputChannel; /* Disable the TIM Capture/Compare 1 interrupt */ __HAL_TIM_DISABLE_IT(htim, TIM_IT_CC1); diff --git a/make/tools.mk b/make/tools.mk index 8d0b24d145..1e4323c111 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -25,36 +25,47 @@ ARM_SDK_URL_BASE := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update # source: https://launchpad.net/gcc-arm-embedded/5.0/ ifdef LINUX - arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2 + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2 endif ifdef MACOSX - arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-mac.tar.bz2 + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-mac.tar.bz2 endif ifdef WINDOWS - arm_sdk_install: ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip endif -arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) -# order-only prereq on directory existance: -arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR) -arm_sdk_install: arm_sdk_clean -ifneq ($(OSFAMILY), windows) - # download the source only if it's newer than what we already have - $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" +ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) +SDK_INSTALL_MARKER := $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc-$(GCC_REQUIRED_VERSION) + +# order-only prereq on directory existance: +arm_sdk_install: | $(TOOLS_DIR) + +arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER) + +$(SDK_INSTALL_MARKER): +ifneq ($(OSFAMILY), windows) # binary only release so just extract it $(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)" else - $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" $(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)" endif +.PHONY: arm_sdk_download +arm_sdk_download: | $(DL_DIR) +arm_sdk_download: $(DL_DIR)/$(ARM_SDK_FILE) +$(DL_DIR)/$(ARM_SDK_FILE): + # download the source only if it's newer than what we already have + $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" -z "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" + + ## arm_sdk_clean : Uninstall Arm SDK .PHONY: arm_sdk_clean arm_sdk_clean: $(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR) + $(V1) [ ! -d "$(DL_DIR)" ] || $(RM) -r $(DL_DIR) .PHONY: openocd_win_install diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f039f7ec14..7038c7ce20 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -36,6 +36,7 @@ #include "drivers/sensor.h" #include "drivers/compass.h" #include "drivers/system.h" +#include "drivers/pwm_output.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -895,7 +896,17 @@ void stopInTestMode(void) */ bool inMotorTestMode(void) { static uint32_t resetTime = 0; - uint16_t inactiveMotorCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.motorConfig.mincommand); + uint16_t inactiveMotorCommand; + if (feature(FEATURE_3D)) { + inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d; +#ifdef USE_DSHOT + } else if (isMotorProtocolDshot()) { + inactiveMotorCommand = DSHOT_DISARM_COMMAND; +#endif + } else { + inactiveMotorCommand = masterConfig.motorConfig.mincommand; + } + int i; bool atLeastOneMotorActivated = false; @@ -1158,7 +1169,7 @@ static bool blackboxWriteSysinfo() switch (xmitState.headerIndex) { BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); - BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName); + BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); diff --git a/src/main/build/version.h b/src/main/build/version.h index c3c6529ed3..59a418b265 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c new file mode 100644 index 0000000000..b8965ee9e3 --- /dev/null +++ b/src/main/cms/cms.c @@ -0,0 +1,871 @@ +/* + * 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 . + */ + +/* + Original OSD code created by Marcin Baliniak + OSD-CMS separation by jflyper + CMS-displayPort separation by jflyper and martinbudden + */ + +//#define CMS_MENU_DEBUG // For external menu content creators + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef CMS + +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + +#include "cms/cms.h" +#include "cms/cms_menu_builtin.h" +#include "cms/cms_types.h" + +#include "common/typeconversion.h" + +#include "drivers/system.h" + +// For 'ARM' related +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +// For rcData, stopAllMotors, stopPwmAllMotors +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +// For VISIBLE* (Actually, included by config_master.h) +#include "io/osd.h" + +// DisplayPort management + +#ifndef CMS_MAX_DEVICE +#define CMS_MAX_DEVICE 4 +#endif + +static displayPort_t *pCurrentDisplay; + +static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE]; +static int cmsDeviceCount; +static int cmsCurrentDevice = -1; + +bool cmsDisplayPortRegister(displayPort_t *pDisplay) +{ + if (cmsDeviceCount == CMS_MAX_DEVICE) + return false; + + cmsDisplayPorts[cmsDeviceCount++] = pDisplay; + + return true; +} + +static displayPort_t *cmsDisplayPortSelectCurrent(void) +{ + if (cmsDeviceCount == 0) + return NULL; + + if (cmsCurrentDevice < 0) + cmsCurrentDevice = 0; + + return cmsDisplayPorts[cmsCurrentDevice]; +} + +static displayPort_t *cmsDisplayPortSelectNext(void) +{ + if (cmsDeviceCount == 0) + return NULL; + + cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay + + return cmsDisplayPorts[cmsCurrentDevice]; +} + +#define CMS_UPDATE_INTERVAL_US 50000 // Interval of key scans (microsec) +#define CMS_POLL_INTERVAL_US 100000 // Interval of polling dynamic values (microsec) + +// XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted +// dynamically depending on size of the active output device, +// or statically to accomodate sizes of all supported devices. +// +// Device characteristics +// OLED +// 21 cols x 8 rows +// 128x64 with 5x7 (6x8) : 21 cols x 8 rows +// MAX7456 (PAL) +// 30 cols x 16 rows +// MAX7456 (NTSC) +// 30 cols x 13 rows +// HoTT Telemetry Screen +// 21 cols x 8 rows +// + +#define LEFT_MENU_COLUMN 1 +#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) +#define MAX_MENU_ITEMS(p) ((p)->rows - 2) + +static bool cmsInMenu = false; + +STATIC_UNIT_TESTED const CMS_Menu *currentMenu; // Points to top entry of the current page + +// XXX Does menu backing support backing into second page??? + +static const CMS_Menu *menuStack[10]; // Stack to save menu transition +static uint8_t menuStackHistory[10];// cursorRow in a stacked menu +static uint8_t menuStackIdx = 0; + +static OSD_Entry *pageTop; // Points to top entry of the current page +static OSD_Entry *pageTopAlt; // Only 2 pages are allowed (for now) +static uint8_t maxRow; // Max row in the current page + +static int8_t cursorRow; + +#ifdef CMS_MENU_DEBUG // For external menu content creators + +static char menuErrLabel[21 + 1] = "RANDOM DATA"; + +static OSD_Entry menuErrEntries[] = { + { "BROKEN MENU", OME_Label, NULL, NULL, 0 }, + { menuErrLabel, OME_Label, NULL, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu menuErr = { + "MENUERR", + OME_MENU, + NULL, + NULL, + NULL, + menuErrEntries, +}; +#endif + +// Stick/key detection + +#define IS_HI(X) (rcData[X] > 1750) +#define IS_LO(X) (rcData[X] < 1250) +#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) + +//key definiotion because API provide menu navigation over MSP/GUI app - not used NOW +#define KEY_ENTER 0 +#define KEY_UP 1 +#define KEY_DOWN 2 +#define KEY_LEFT 3 +#define KEY_RIGHT 4 +#define KEY_ESC 5 + +#define BUTTON_TIME 250 // msec +#define BUTTON_PAUSE 500 // msec + +static void cmsUpdateMaxRow(displayPort_t *instance) +{ + maxRow = 0; + + for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) { + maxRow++; + } + + if (maxRow > MAX_MENU_ITEMS(instance)) { + maxRow = MAX_MENU_ITEMS(instance); + } + + maxRow--; +} + +static void cmsFormatFloat(int32_t value, char *floatString) +{ + uint8_t k; + // np. 3450 + + itoa(100000 + value, floatString, 10); // Create string from abs of integer value + + // 103450 + + floatString[0] = floatString[1]; + floatString[1] = floatString[2]; + floatString[2] = '.'; + + // 03.450 + // usuwam koncowe zera i kropke + // Keep the first decimal place + for (k = 5; k > 3; k--) + if (floatString[k] == '0' || floatString[k] == '.') + floatString[k] = 0; + else + break; + + // oraz zero wiodonce + if (floatString[0] == '0') + floatString[0] = ' '; +} + +static void cmsPadToSize(char *buf, int size) +{ + int i; + + for (i = 0 ; i < size ; i++) { + if (buf[i] == 0) + break; + } + + for ( ; i < size ; i++) { + buf[i] = ' '; + } + + buf[size] = 0; +} + +static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) +{ + char buff[10]; + int cnt = 0; + + switch (p->type) { + case OME_String: + if (IS_PRINTVALUE(p) && p->data) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + CLR_PRINTVALUE(p); + } + break; + case OME_Submenu: + case OME_Funcall: + if (IS_PRINTVALUE(p)) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); + CLR_PRINTVALUE(p); + } + break; + case OME_Bool: + if (IS_PRINTVALUE(p) && p->data) { + if (*((uint8_t *)(p->data))) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + } else { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + } + CLR_PRINTVALUE(p); + } + break; + case OME_TAB: { + if (IS_PRINTVALUE(p)) { + OSD_TAB_t *ptr = p->data; + //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]); + CLR_PRINTVALUE(p); + } + break; + } +#ifdef OSD + case OME_VISIBLE: + if (IS_PRINTVALUE(p) && p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (VISIBLE(*val)) { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + } else { + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + } + CLR_PRINTVALUE(p); + } + break; +#endif + case OME_UINT8: + if (IS_PRINTVALUE(p) && p->data) { + OSD_UINT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsPadToSize(buff, 5); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + CLR_PRINTVALUE(p); + } + break; + case OME_INT8: + if (IS_PRINTVALUE(p) && p->data) { + OSD_INT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsPadToSize(buff, 5); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + CLR_PRINTVALUE(p); + } + break; + case OME_UINT16: + if (IS_PRINTVALUE(p) && p->data) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsPadToSize(buff, 5); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + CLR_PRINTVALUE(p); + } + break; + case OME_INT16: + if (IS_PRINTVALUE(p) && p->data) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + cmsPadToSize(buff, 5); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + CLR_PRINTVALUE(p); + } + break; + case OME_FLOAT: + if (IS_PRINTVALUE(p) && p->data) { + OSD_FLOAT_t *ptr = p->data; + cmsFormatFloat(*ptr->val * ptr->multipler, buff); + cmsPadToSize(buff, 5); + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? + CLR_PRINTVALUE(p); + } + break; + case OME_Label: + if (IS_PRINTVALUE(p) && p->data) { + // A label with optional string, immediately following text + cnt = displayWrite(pDisplay, LEFT_MENU_COLUMN + 2 + strlen(p->text), row, p->data); + CLR_PRINTVALUE(p); + } + break; + case OME_OSD_Exit: + case OME_END: + case OME_Back: + break; + case OME_MENU: + // Fall through + default: +#ifdef CMS_MENU_DEBUG + // Shouldn't happen. Notify creator of this menu content. + cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT"); +#endif + break; + } + + return cnt; +} + +static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) +{ + if (!pageTop) + return; + + uint8_t i; + OSD_Entry *p; + uint8_t top = (pDisplay->rows - maxRow) / 2 - 1; + + // Polled (dynamic) value display denominator. + + bool drawPolled = false; + static uint32_t lastPolledUs = 0; + + if (currentTimeUs > lastPolledUs + CMS_POLL_INTERVAL_US) { + drawPolled = true; + lastPolledUs = currentTimeUs; + } + + uint32_t room = displayTxBytesFree(pDisplay); + + if (pDisplay->cleared) { + for (p = pageTop, i= 0; p->type != OME_END; p++, i++) { + SET_PRINTLABEL(p); + SET_PRINTVALUE(p); + } + + if (i > MAX_MENU_ITEMS(pDisplay)) // max per page + { + pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay); + if (pageTopAlt->type == OME_END) + pageTopAlt = NULL; + } + + pDisplay->cleared = false; + } else if (drawPolled) { + for (p = pageTop ; p <= pageTop + maxRow ; p++) { + if (IS_DYNAMIC(p)) + SET_PRINTVALUE(p); + } + } + + // Cursor manipulation + + while ((pageTop + cursorRow)->type == OME_Label) // skip label + cursorRow++; + + if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " "); + } + + if (room < 30) + return; + + if (pDisplay->cursorRow != cursorRow) { + room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, cursorRow + top, " >"); + pDisplay->cursorRow = cursorRow; + } + + if (room < 30) + return; + + // Print text labels + for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + if (IS_PRINTLABEL(p)) { + uint8_t coloff = LEFT_MENU_COLUMN; + coloff += (p->type == OME_Label) ? 1 : 2; + room -= displayWrite(pDisplay, coloff, i + top, p->text); + CLR_PRINTLABEL(p); + if (room < 30) + return; + } + } + + // Print values + + // XXX Polled values at latter positions in the list may not be + // XXX printed if not enough room in the middle of the list. + + for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + if (IS_PRINTVALUE(p)) { + room -= cmsDrawMenuEntry(pDisplay, p, top + i); + if (room < 30) + return; + } + } +} + +long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) +{ + CMS_Menu *pMenu = (CMS_Menu *)ptr; + + if (pMenu) { +#ifdef CMS_MENU_DEBUG + if (pMenu->GUARD_type != OME_MENU) { + // ptr isn't pointing to a CMS_Menu. + if (pMenu->GUARD_type <= OME_MAX) { + strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1); + } else { + strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1); + } + pMenu = &menuErr; + } +#endif + + // Stack the current menu and move to a new menu. + // The (pMenu == curretMenu) case occurs when reopening for display sw + + if (pMenu != currentMenu) { + menuStack[menuStackIdx] = currentMenu; + cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value + menuStackHistory[menuStackIdx] = cursorRow; + menuStackIdx++; + + currentMenu = pMenu; + cursorRow = 0; + + if (pMenu->onEnter) + pMenu->onEnter(); + } + + pageTop = currentMenu->entries; + pageTopAlt = NULL; + + displayClear(pDisplay); + cmsUpdateMaxRow(pDisplay); + } + + return 0; +} + +STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) +{ + // Let onExit function decide whether to allow exit or not. + + if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0) + return -1; + + if (menuStackIdx) { + displayClear(pDisplay); + menuStackIdx--; + currentMenu = menuStack[menuStackIdx]; + cursorRow = menuStackHistory[menuStackIdx]; + + // cursorRow is absolute offset of a focused entry when stacked. + // Convert it back to page and relative offset. + + pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow() + cmsUpdateMaxRow(pDisplay); + + if (cursorRow > maxRow) { + // Cursor was in the second page. + pageTopAlt = currentMenu->entries; + pageTop = pageTopAlt + maxRow + 1; + cursorRow -= (maxRow + 1); + cmsUpdateMaxRow(pDisplay); // Update maxRow for the second page + } + } + + return 0; +} + +STATIC_UNIT_TESTED void cmsMenuOpen(void) +{ + if (!cmsInMenu) { + // New open + pCurrentDisplay = cmsDisplayPortSelectCurrent(); + if (!pCurrentDisplay) + return; + cmsInMenu = true; + currentMenu = &menuMain; + DISABLE_ARMING_FLAG(OK_TO_ARM); + } else { + // Switch display + displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); + if (pNextDisplay != pCurrentDisplay) { + displayRelease(pCurrentDisplay); + pCurrentDisplay = pNextDisplay; + } else { + return; + } + } + displayGrab(pCurrentDisplay); // grab the display for use by the CMS + cmsMenuChange(pCurrentDisplay, currentMenu); +} + +static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) +{ + debug[0]++; + + for (const OSD_Entry *p = pMenu->entries; p->type != OME_END ; p++) { + if (p->type == OME_Submenu) { + cmsTraverseGlobalExit(p->data); + } + } + + if (pMenu->onGlobalExit) { + debug[1]++; + pMenu->onGlobalExit(); + } +} + +long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) +{ + if (ptr) { + displayClear(pDisplay); + + displayWrite(pDisplay, 5, 3, "REBOOTING..."); + displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? + + stopMotors(); + stopPwmAllMotors(); + delay(200); + + cmsTraverseGlobalExit(&menuMain); + + if (currentMenu->onExit) + currentMenu->onExit((OSD_Entry *)NULL); // Forced exit + + saveConfigAndNotify(); + } + + cmsInMenu = false; + + displayRelease(pDisplay); + currentMenu = NULL; + + if (ptr) + systemReset(); + + ENABLE_ARMING_FLAG(OK_TO_ARM); + + return 0; +} + +STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) +{ + uint16_t res = BUTTON_TIME; + OSD_Entry *p; + + if (!currentMenu) + return res; + + if (key == KEY_ESC) { + cmsMenuBack(pDisplay); + return BUTTON_PAUSE; + } + + if (key == KEY_DOWN) { + if (cursorRow < maxRow) { + cursorRow++; + } else { + if (pageTopAlt) { // we have another page + displayClear(pDisplay); + p = pageTopAlt; + pageTopAlt = pageTop; + pageTop = (OSD_Entry *)p; + cmsUpdateMaxRow(pDisplay); + } + cursorRow = 0; // Goto top in any case + } + } + + if (key == KEY_UP) { + cursorRow--; + + if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0) + cursorRow--; + + if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) { + if (pageTopAlt) { + displayClear(pDisplay); + p = pageTopAlt; + pageTopAlt = pageTop; + pageTop = (OSD_Entry *)p; + cmsUpdateMaxRow(pDisplay); + } + cursorRow = maxRow; // Goto bottom in any case + } + } + + if (key == KEY_DOWN || key == KEY_UP) + return res; + + p = pageTop + cursorRow; + + switch (p->type) { + case OME_Submenu: + case OME_Funcall: + case OME_OSD_Exit: + if (p->func && key == KEY_RIGHT) { + p->func(pDisplay, p->data); + res = BUTTON_PAUSE; + } + break; + case OME_Back: + cmsMenuBack(pDisplay); + res = BUTTON_PAUSE; + break; + case OME_Bool: + if (p->data) { + uint8_t *val = p->data; + if (key == KEY_RIGHT) + *val = 1; + else + *val = 0; + SET_PRINTVALUE(p); + } + break; +#ifdef OSD + case OME_VISIBLE: + if (p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (key == KEY_RIGHT) + *val |= VISIBLE_FLAG; + else + *val %= ~VISIBLE_FLAG; + SET_PRINTVALUE(p); + } + break; +#endif + case OME_UINT8: + case OME_FLOAT: + if (p->data) { + OSD_UINT8_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_TAB: + if (p->type == OME_TAB) { + OSD_TAB_t *ptr = p->data; + + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += 1; + } + else { + if (*ptr->val > 0) + *ptr->val -= 1; + } + if (p->func) + p->func(pDisplay, p->data); + SET_PRINTVALUE(p); + } + break; + case OME_INT8: + if (p->data) { + OSD_INT8_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_UINT16: + if (p->data) { + OSD_UINT16_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_INT16: + if (p->data) { + OSD_INT16_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + SET_PRINTVALUE(p); + if (p->func) { + p->func(pDisplay, p); + } + } + break; + case OME_String: + break; + case OME_Label: + case OME_END: + break; + case OME_MENU: + // Shouldn't happen + break; + } + return res; +} + +static void cmsUpdate(uint32_t currentTimeUs) +{ + static int16_t rcDelayMs = BUTTON_TIME; + static uint32_t lastCalledMs = 0; + static uint32_t lastCmsHeartBeatMs = 0; + + const uint32_t currentTimeMs = currentTimeUs / 1000; + + if (!cmsInMenu) { + // Detect menu invocation + if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsMenuOpen(); + rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME + } + } else { + uint8_t key = 0; + if (rcDelayMs > 0) { + rcDelayMs -= (currentTimeMs - lastCalledMs); + } + else if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + // Double enter = display switching + cmsMenuOpen(); + rcDelayMs = BUTTON_PAUSE; + } + else if (IS_HI(PITCH)) { + key = KEY_UP; + rcDelayMs = BUTTON_TIME; + } + else if (IS_LO(PITCH)) { + key = KEY_DOWN; + rcDelayMs = BUTTON_TIME; + } + else if (IS_LO(ROLL)) { + key = KEY_LEFT; + rcDelayMs = BUTTON_TIME; + } + else if (IS_HI(ROLL)) { + key = KEY_RIGHT; + rcDelayMs = BUTTON_TIME; + } + else if (IS_HI(YAW) || IS_LO(YAW)) + { + key = KEY_ESC; + rcDelayMs = BUTTON_TIME; + } + + //lastCalled = currentTime; + + if (key) { + rcDelayMs = cmsHandleKey(pCurrentDisplay, key); + return; + } + + cmsDrawMenu(pCurrentDisplay, currentTimeUs); + + if (currentTimeMs > lastCmsHeartBeatMs + 500) { + // Heart beat for external CMS display device @ 500msec + // (Timeout @ 1000msec) + displayHeartbeat(pCurrentDisplay); + lastCmsHeartBeatMs = currentTimeMs; + } + } + lastCalledMs = currentTimeMs; +} + +void cmsHandler(uint32_t currentTime) +{ + if (cmsDeviceCount < 0) + return; + + static uint32_t lastCalled = 0; + + if (currentTime >= lastCalled + CMS_UPDATE_INTERVAL_US) { + lastCalled = currentTime; + cmsUpdate(currentTime); + } +} + +// Is initializing with menuMain better? +// Can it be done with the current main()? +void cmsInit(void) +{ + cmsDeviceCount = 0; + cmsCurrentDevice = -1; +} + +#endif // CMS diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h new file mode 100644 index 0000000000..8e5110af22 --- /dev/null +++ b/src/main/cms/cms.h @@ -0,0 +1,17 @@ +#pragma once + +#include "drivers/display.h" + +// Device management +bool cmsDisplayPortRegister(displayPort_t *pDisplay); + +// For main.c and scheduler +void cmsInit(void); +void cmsHandler(uint32_t currentTime); + +long cmsMenuChange(displayPort_t *pPort, const void *ptr); +long cmsMenuExit(displayPort_t *pPort, const void *ptr); + +#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c new file mode 100644 index 0000000000..0d65249fbc --- /dev/null +++ b/src/main/cms/cms_menu_blackbox.c @@ -0,0 +1,111 @@ +/* + * 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 . + */ + +// +// CMS things for blackbox and flashfs. +// + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_blackbox.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/flashfs.h" + +#ifdef USE_FLASHFS +static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(ptr); + + displayClear(pDisplay); + displayWrite(pDisplay, 5, 3, "ERASING FLASH..."); + displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + displayClear(pDisplay); + displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? + + return 0; +} +#endif // USE_FLASHFS + +static bool featureRead = false; +static uint8_t cmsx_FeatureBlackbox; + +static long cmsx_Blackbox_FeatureRead(void) +{ + if (!featureRead) { + cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + featureRead = true; + } + + return 0; +} + +static long cmsx_Blackbox_FeatureWriteback(void) +{ + if (cmsx_FeatureBlackbox) + featureSet(FEATURE_BLACKBOX); + else + featureClear(FEATURE_BLACKBOX); + + return 0; +} + +static OSD_Entry cmsx_menuBlackboxEntries[] = +{ + { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, + { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackbox_rate_denom,1,32,1 }, 0 }, + +#ifdef USE_FLASHFS + { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, +#endif // USE_FLASHFS + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuBlackbox = { + .GUARD_text = "MENUBB", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Blackbox_FeatureRead, + .onExit = NULL, + .onGlobalExit = cmsx_Blackbox_FeatureWriteback, + .entries = cmsx_menuBlackboxEntries +}; +#endif diff --git a/src/main/cms/cms_menu_blackbox.h b/src/main/cms/cms_menu_blackbox.h new file mode 100644 index 0000000000..a35ac4211b --- /dev/null +++ b/src/main/cms/cms_menu_blackbox.h @@ -0,0 +1,20 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuBlackbox; diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c new file mode 100644 index 0000000000..50a85a4c08 --- /dev/null +++ b/src/main/cms/cms_menu_builtin.c @@ -0,0 +1,143 @@ +/* + * 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 . + */ + +// +// Built-in menu contents and support functions +// + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef CMS + +#include "build/version.h" + +#include "drivers/system.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_builtin.h" + +// Sub menus + +#include "cms/cms_menu_imu.h" +#include "cms/cms_menu_blackbox.h" +#include "cms/cms_menu_vtx.h" +#include "cms/cms_menu_osd.h" +#include "cms/cms_menu_ledstrip.h" +#include "cms/cms_menu_misc.h" + + +// Info + +static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoTargetName[] = __TARGET__; + +#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere + +static long cmsx_InfoInit(void) +{ + for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') + infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; + else + infoGitRev[i] = shortGitRevision[i]; + } + + return 0; +} + +static OSD_Entry menuInfoEntries[] = { + { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, + { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, + { "GITREV", OME_String, NULL, infoGitRev, 0 }, + { "TARGET", OME_String, NULL, infoTargetName, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu menuInfo = { + .GUARD_text = "MENUINFO", + .GUARD_type = OME_MENU, + .onEnter = cmsx_InfoInit, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuInfoEntries +}; + +// Features + +static OSD_Entry menuFeaturesEntries[] = +{ + {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0}, +#if defined(VTX) || defined(USE_RTC6705) + {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, +#endif // VTX || USE_RTC6705 +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, +#endif // LED_STRIP + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +static CMS_Menu menuFeatures = { + .GUARD_text = "MENUFEATURES", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuFeaturesEntries, +}; + +// Main + +static OSD_Entry menuMainEntries[] = +{ + {"-- MAIN --", OME_Label, NULL, NULL, 0}, + + {"PROFILE", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0}, +#ifdef OSD + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0}, +#endif + {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0}, + {"MISC", OME_Submenu, cmsMenuChange, &cmsx_menuMisc, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, +#ifdef CMS_MENU_DEBUG + {"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0}, +#endif + + {NULL,OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuMain = { + .GUARD_text = "MENUMAIN", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMainEntries, +}; +#endif diff --git a/src/main/cms/cms_menu_builtin.h b/src/main/cms/cms_menu_builtin.h new file mode 100644 index 0000000000..8bb33757aa --- /dev/null +++ b/src/main/cms/cms_menu_builtin.h @@ -0,0 +1,22 @@ +/* + * 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 + +#include "cms/cms_types.h" + +extern CMS_Menu menuMain; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c new file mode 100644 index 0000000000..24b988cb07 --- /dev/null +++ b/src/main/cms/cms_menu_imu.c @@ -0,0 +1,384 @@ +/* + * 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 . + */ + +// Menu contents for PID, RATES, RC preview, misc +// Should be part of the relevant .c file. + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef CMS + +#include "build/version.h" + +#include "drivers/system.h" + +//#include "common/typeconversion.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_imu.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +// +// PID +// +static uint8_t tmpProfileIndex; +static uint8_t profileIndex; +static char profileIndexString[] = " p"; +static uint8_t tempPid[3][3]; + +static uint8_t tmpRateProfileIndex; +static uint8_t rateProfileIndex; +static char rateProfileIndexString[] = " p-r"; +static controlRateConfig_t rateProfile; + +static long cmsx_menuImu_onEnter(void) +{ + profileIndex = masterConfig.current_profile_index; + tmpProfileIndex = profileIndex + 1; + + rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + tmpRateProfileIndex = rateProfileIndex + 1; + + return 0; +} + +static long cmsx_menuImu_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.current_profile_index = profileIndex; + masterConfig.profile[profileIndex].activeRateProfile = rateProfileIndex; + + return 0; +} + +static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + profileIndex = tmpProfileIndex - 1; + + return 0; +} + +static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + rateProfileIndex = tmpRateProfileIndex - 1; + + return 0; +} + +static long cmsx_PidRead(void) +{ + + for (uint8_t i = 0; i < 3; i++) { + tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i]; + } + + return 0; +} + +static long cmsx_PidOnEnter(void) +{ + profileIndexString[1] = '0' + tmpProfileIndex; + cmsx_PidRead(); + + return 0; +} + +static long cmsx_PidWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + for (uint8_t i = 0; i < 3; i++) { + masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; + } + + return 0; +} + +static OSD_Entry cmsx_menuPidEntries[] = +{ + { "-- PID --", OME_Label, NULL, profileIndexString, 0}, + + { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][0], 0, 200, 1 }, 0 }, + { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][1], 0, 200, 1 }, 0 }, + { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDROLL][2], 0, 200, 1 }, 0 }, + + { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][0], 0, 200, 1 }, 0 }, + { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][1], 0, 200, 1 }, 0 }, + { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDPITCH][2], 0, 200, 1 }, 0 }, + + { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][0], 0, 200, 1 }, 0 }, + { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][1], 0, 200, 1 }, 0 }, + { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PIDYAW][2], 0, 200, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuPid = { + .GUARD_text = "XPID", + .GUARD_type = OME_MENU, + .onEnter = cmsx_PidOnEnter, + .onExit = cmsx_PidWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuPidEntries +}; + +// +// Rate & Expo +// + +static long cmsx_RateProfileRead(void) +{ + memcpy(&rateProfile, &masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], sizeof(controlRateConfig_t)); + + return 0; +} + +static long cmsx_RateProfileWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + memcpy(&masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], &rateProfile, sizeof(controlRateConfig_t)); + + return 0; +} + +static long cmsx_RateProfileOnEnter(void) +{ + rateProfileIndexString[1] = '0' + tmpProfileIndex; + rateProfileIndexString[3] = '0' + tmpRateProfileIndex; + cmsx_RateProfileRead(); + + return 0; +} + +static OSD_Entry cmsx_menuRateProfileEntries[] = +{ + { "-- RATE --", OME_Label, NULL, rateProfileIndexString, 0 }, + + { "RC RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRate8, 0, 255, 1, 10 }, 0 }, + { "RC YAW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawRate8, 0, 255, 1, 10 }, 0 }, + + { "ROLL SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[0], 0, 100, 1, 10 }, 0 }, + { "PITCH SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[1], 0, 100, 1, 10 }, 0 }, + { "YAW SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[2], 0, 100, 1, 10 }, 0 }, + + { "RC EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo8, 0, 100, 1, 10 }, 0 }, + { "RC YAW EXP", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcYawExpo8, 0, 100, 1, 10 }, 0 }, + + { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 100, 1, 10}, 0 }, + { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuRateProfile = { + .GUARD_text = "MENURATE", + .GUARD_type = OME_MENU, + .onEnter = cmsx_RateProfileOnEnter, + .onExit = cmsx_RateProfileWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuRateProfileEntries +}; + +static uint8_t cmsx_dtermSetpointWeight; +static uint8_t cmsx_setpointRelaxRatio; +static uint8_t cmsx_angleStrength; +static uint8_t cmsx_horizonStrength; +static uint8_t cmsx_horizonTransition; + +static long cmsx_profileOtherOnEnter(void) +{ + profileIndexString[1] = '0' + tmpProfileIndex; + + cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight; + cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio; + + cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; + cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; + cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; + + return 0; +} + +static long cmsx_profileOtherOnExit(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight; + masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio; + + masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength; + masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength; + masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition; + + return 0; +} + +static OSD_Entry cmsx_menuProfileOtherEntries[] = { + { "-- OTHER PP --", OME_Label, NULL, profileIndexString, 0 }, + + { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, + { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, + { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, + { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, + { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuProfileOther = { + .GUARD_text = "XPROFOTHER", + .GUARD_type = OME_MENU, + .onEnter = cmsx_profileOtherOnEnter, + .onExit = cmsx_profileOtherOnExit, + .onGlobalExit = NULL, + .entries = cmsx_menuProfileOtherEntries, +}; + +static OSD_Entry cmsx_menuFilterGlobalEntries[] = +{ + { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, + + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, + { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, + { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, + { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuFilterGlobal = { + .GUARD_text = "XFLTGLB", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuFilterGlobalEntries, +}; + +static uint16_t cmsx_dterm_lpf_hz; +static uint16_t cmsx_dterm_notch_hz; +static uint16_t cmsx_dterm_notch_cutoff; +static uint16_t cmsx_yaw_lpf_hz; +static uint16_t cmsx_yaw_p_limit; + +static long cmsx_FilterPerProfileRead(void) +{ + cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz; + cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz; + cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff; + cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz; + cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit; + + return 0; +} + +static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) +{ + UNUSED(self); + + masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz; + masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz; + masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff; + masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz; + masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit; + + return 0; +} + +static OSD_Entry cmsx_menuFilterPerProfileEntries[] = +{ + { "-- FILTER PP --", OME_Label, NULL, NULL, 0 }, + + { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf_hz, 0, 500, 1 }, 0 }, + { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, + { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, + { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lpf_hz, 0, 500, 1 }, 0 }, + { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_p_limit, 100, 500, 1 }, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu cmsx_menuFilterPerProfile = { + .GUARD_text = "XFLTPP", + .GUARD_type = OME_MENU, + .onEnter = cmsx_FilterPerProfileRead, + .onExit = cmsx_FilterPerProfileWriteback, + .onGlobalExit = NULL, + .entries = cmsx_menuFilterPerProfileEntries, +}; + +static OSD_Entry cmsx_menuImuEntries[] = +{ + { "-- IMU --", OME_Label, NULL, NULL, 0}, + + {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, + {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, + {"OTHER PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, + + {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, + {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, + + {"FLT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, + + {"FLT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, + + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuImu = { + .GUARD_text = "XIMU", + .GUARD_type = OME_MENU, + .onEnter = cmsx_menuImu_onEnter, + .onExit = cmsx_menuImu_onExit, + .onGlobalExit = NULL, + .entries = cmsx_menuImuEntries, +}; +#endif // CMS diff --git a/src/main/cms/cms_menu_imu.h b/src/main/cms/cms_menu_imu.h new file mode 100644 index 0000000000..a5e05e3103 --- /dev/null +++ b/src/main/cms/cms_menu_imu.h @@ -0,0 +1,20 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuImu; diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c new file mode 100644 index 0000000000..bd7efe2cd2 --- /dev/null +++ b/src/main/cms/cms_menu_ledstrip.c @@ -0,0 +1,82 @@ +/* + * 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 "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" + +#ifdef LED_STRIP + +static bool featureRead = false; +static uint8_t cmsx_FeatureLedstrip; + +static long cmsx_Ledstrip_FeatureRead(void) +{ + if (!featureRead) { + cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; + featureRead = true; + } + + return 0; +} + +static long cmsx_Ledstrip_FeatureWriteback(void) +{ + if (cmsx_FeatureLedstrip) + featureSet(FEATURE_LED_STRIP); + else + featureClear(FEATURE_LED_STRIP); + + return 0; +} + +static OSD_Entry cmsx_menuLedstripEntries[] = +{ + { "-- LED STRIP --", OME_Label, NULL, NULL, 0 }, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuLedstrip = { + .GUARD_text = "MENULED", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Ledstrip_FeatureRead, + .onExit = NULL, + .onGlobalExit = cmsx_Ledstrip_FeatureWriteback, + .entries = cmsx_menuLedstripEntries +}; +#endif // LED_STRIP +#endif // CMS diff --git a/src/main/cms/cms_menu_ledstrip.h b/src/main/cms/cms_menu_ledstrip.h new file mode 100644 index 0000000000..f740b8911c --- /dev/null +++ b/src/main/cms/cms_menu_ledstrip.h @@ -0,0 +1,20 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuLedstrip; diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c new file mode 100644 index 0000000000..684fbf8b62 --- /dev/null +++ b/src/main/cms/cms_menu_misc.c @@ -0,0 +1,104 @@ +/* + * 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 "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" + +// +// Misc +// + +static long cmsx_menuRcConfirmBack(const OSD_Entry *self) +{ + if (self && self->type == OME_Back) + return 0; + else + return -1; +} + +// +// RC preview +// +static OSD_Entry cmsx_menuRcEntries[] = +{ + { "-- RC PREV --", OME_Label, NULL, NULL, 0}, + + { "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC }, + { "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC }, + { "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC }, + { "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC }, + + { "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC }, + { "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC }, + { "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC }, + { "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC }, + + { "BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuRcPreview = { + .GUARD_text = "XRCPREV", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = cmsx_menuRcConfirmBack, + .onGlobalExit = NULL, + .entries = cmsx_menuRcEntries +}; + + +static OSD_Entry menuMiscEntries[]= +{ + { "-- MISC --", OME_Label, NULL, NULL, 0 }, + + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 }, + { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 }, + { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 }, + { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, + + { "BACK", OME_Back, NULL, NULL, 0}, + { NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuMisc = { + .GUARD_text = "XMISC", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuMiscEntries +}; + +#endif // CMS diff --git a/src/main/cms/cms_menu_misc.h b/src/main/cms/cms_menu_misc.h new file mode 100644 index 0000000000..8b0ed9fc67 --- /dev/null +++ b/src/main/cms/cms_menu_misc.h @@ -0,0 +1,20 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuMisc; diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c new file mode 100644 index 0000000000..1ef32700df --- /dev/null +++ b/src/main/cms/cms_menu_osd.c @@ -0,0 +1,112 @@ +/* + * 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/version.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#if defined(OSD) && defined(CMS) + +OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; +OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; +OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; +OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; + +OSD_Entry cmsx_menuAlarmsEntries[] = +{ + {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, + {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0}, + {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuAlarms = { + .GUARD_text = "MENUALARMS", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuAlarmsEntries, +}; + +OSD_Entry menuOsdActiveElemsEntries[] = +{ + {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, + {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu menuOsdActiveElems = { + .GUARD_text = "MENUOSDACT", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdActiveElemsEntries +}; + +OSD_Entry cmsx_menuOsdLayoutEntries[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, + {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuOsdLayout = { + .GUARD_text = "MENULAYOUT", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuOsdLayoutEntries +}; +#endif // CMS diff --git a/src/main/cms/cms_menu_osd.h b/src/main/cms/cms_menu_osd.h new file mode 100644 index 0000000000..9b0b001d28 --- /dev/null +++ b/src/main/cms/cms_menu_osd.h @@ -0,0 +1,21 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuAlarms; +extern CMS_Menu cmsx_menuOsdLayout; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c new file mode 100644 index 0000000000..3b0cb021a4 --- /dev/null +++ b/src/main/cms/cms_menu_vtx.c @@ -0,0 +1,146 @@ +/* + * 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/version.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_vtx.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + +static bool featureRead = false; +static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; + +static long cmsx_Vtx_FeatureRead(void) +{ + if (!featureRead) { + cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; + featureRead = true; + } + + return 0; +} + +static long cmsx_Vtx_FeatureWriteback(void) +{ + if (cmsx_featureVtx) + featureSet(FEATURE_VTX); + else + featureClear(FEATURE_VTX); + + return 0; +} + +static const char * const vtxBandNames[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +static OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; +static OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; + +static void cmsx_Vtx_ConfigRead(void) +{ +#ifdef VTX + cmsx_vtxBand = masterConfig.vtx_band; + cmsx_vtxChannel = masterConfig.vtx_channel + 1; +#endif // VTX + +#ifdef USE_RTC6705 + cmsx_vtxBand = masterConfig.vtx_channel / 8; + cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1; +#endif // USE_RTC6705 +} + +static void cmsx_Vtx_ConfigWriteback(void) +{ +#ifdef VTX + masterConfig.vtx_band = cmsx_vtxBand; + masterConfig.vtx_channel = cmsx_vtxChannel - 1; +#endif // VTX + +#ifdef USE_RTC6705 + masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; +#endif // USE_RTC6705 +} + +static long cmsx_Vtx_onEnter(void) +{ + cmsx_Vtx_FeatureRead(); + cmsx_Vtx_ConfigRead(); + + return 0; +} + +static long cmsx_Vtx_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + cmsx_Vtx_ConfigWriteback(); + + return 0; +} + +#ifdef VTX +static OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; +static OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; +#endif // VTX + +static OSD_Entry cmsx_menuVtxEntries[] = +{ + {"--- VTX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, +#ifdef VTX + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, +#endif // VTX + {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, +#ifdef USE_RTC6705 + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, +#endif // USE_RTC6705 + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +CMS_Menu cmsx_menuVtx = { + .GUARD_text = "MENUVTX", + .GUARD_type = OME_MENU, + .onEnter = cmsx_Vtx_onEnter, + .onExit= cmsx_Vtx_onExit, + .onGlobalExit = cmsx_Vtx_FeatureWriteback, + .entries = cmsx_menuVtxEntries +}; + +#endif // VTX || USE_RTC6705 +#endif // CMS diff --git a/src/main/cms/cms_menu_vtx.h b/src/main/cms/cms_menu_vtx.h new file mode 100644 index 0000000000..2e15372cad --- /dev/null +++ b/src/main/cms/cms_menu_vtx.h @@ -0,0 +1,20 @@ +/* + * 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 + +extern CMS_Menu cmsx_menuVtx; diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h new file mode 100644 index 0000000000..427e1d38e6 --- /dev/null +++ b/src/main/cms/cms_types.h @@ -0,0 +1,155 @@ +/* + * 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 . + */ + +// +// Menu element types +// XXX Upon separation, all OME would be renamed to CME_ or similar. +// + +#pragma once + +//type of elements + +typedef enum +{ + OME_Label, + OME_Back, + OME_OSD_Exit, + OME_Submenu, + OME_Funcall, + OME_Bool, + OME_INT8, + OME_UINT8, + OME_UINT16, + OME_INT16, + OME_String, + OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's + //wlasciwosci elementow +#ifdef OSD + OME_VISIBLE, +#endif + OME_TAB, + OME_END, + + // Debug aid + OME_MENU, + + OME_MAX = OME_MENU +} OSD_MenuElement; + +typedef long (*CMSEntryFuncPtr)(displayPort_t *displayPort, const void *ptr); + +typedef struct +{ + const char *text; + const OSD_MenuElement type; + const CMSEntryFuncPtr func; + void *data; + uint8_t flags; +} OSD_Entry; + +// Bits in flags +#define PRINT_VALUE 0x01 // Value has been changed, need to redraw +#define PRINT_LABEL 0x02 // Text label should be printed +#define DYNAMIC 0x04 // Value should be updated dynamically + +#define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE) +#define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; } +#define CLR_PRINTVALUE(p) { (p)->flags &= ~PRINT_VALUE; } + +#define IS_PRINTLABEL(p) ((p)->flags & PRINT_LABEL) +#define SET_PRINTLABEL(p) { (p)->flags |= PRINT_LABEL; } +#define CLR_PRINTLABEL(p) { (p)->flags &= ~PRINT_LABEL; } + +#define IS_DYNAMIC(p) ((p)->flags & DYNAMIC) + + +typedef long (*CMSMenuFuncPtr)(void); + +/* +onExit function is called with self: +(1) Pointer to an OSD_Entry when cmsMenuBack() was called. + Point to an OSD_Entry with type == OME_Back if BACK was selected. +(2) NULL if called from menu exit (forced exit at top level). +*/ + +typedef long (*CMSMenuOnExitPtr)(const OSD_Entry *self); + +typedef struct +{ + // These two are debug aids for menu content creators. + const char *GUARD_text; + const OSD_MenuElement GUARD_type; + + const CMSMenuFuncPtr onEnter; + const CMSMenuOnExitPtr onExit; + const CMSMenuFuncPtr onGlobalExit; + OSD_Entry *entries; +} CMS_Menu; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; +} OSD_UINT8_t; + +typedef struct +{ + int8_t *val; + int8_t min; + int8_t max; + int8_t step; +} OSD_INT8_t; + +typedef struct +{ + int16_t *val; + int16_t min; + int16_t max; + int16_t step; +} OSD_INT16_t; + +typedef struct +{ + uint16_t *val; + uint16_t min; + uint16_t max; + uint16_t step; +} OSD_UINT16_t; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; + uint16_t multipler; +} OSD_FLOAT_t; + +typedef struct +{ + uint8_t *val; + uint8_t max; + const char * const *names; +} OSD_TAB_t; + +typedef struct +{ + char *val; +} OSD_String_t; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 98d82919e4..6fdf6e5c59 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -75,7 +75,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh const float cs = cosf(omega); const float alpha = sn / (2 * Q); - float b0, b1, b2, a0, a1, a2; + float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0; switch (filterType) { case FILTER_LPF: diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index d9ceba8828..63477173c3 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -76,7 +76,7 @@ #if defined(STM32F40_41xxx) #define FLASH_PAGE_COUNT 4 // just to make calculations work #elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 4 // just to make calculations work +#define FLASH_PAGE_COUNT 3 // just to make calculations work #elif defined (STM32F745xx) #define FLASH_PAGE_COUNT 4 // just to make calculations work #else diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 2e74441969..60a2f35548 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -21,6 +21,8 @@ #include "config/config_profile.h" +#include "cms/cms.h" + #include "drivers/pwm_rx.h" #include "drivers/sound_beeper.h" #include "drivers/sonar_hcsr04.h" @@ -161,11 +163,7 @@ typedef struct master_s { #endif #ifdef LED_STRIP - ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; - hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; - modeColorIndexes_t modeColors[LED_MODE_COUNT]; - specialColorIndexes_t specialColors; - uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on + ledStripConfig_t ledStripConfig; #endif #ifdef TRANSPONDER diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 32d0cefcc5..f761c835d3 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -15,9 +15,12 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define MPU6500_WHO_AM_I_CONST (0x70) #define MPU9250_WHO_AM_I_CONST (0x71) #define ICM20608G_WHO_AM_I_CONST (0xAF) +#define ICM20602_WHO_AM_I_CONST (0x12) #define MPU6500_BIT_RESET (0x80) #define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4) @@ -25,8 +28,6 @@ #define MPU6500_BIT_I2C_IF_DIS (1 << 4) #define MPU6500_BIT_RAW_RDY_EN (0x01) -#pragma once - bool mpu6500AccDetect(acc_t *acc); bool mpu6500GyroDetect(gyro_t *gyro); diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 648d3bd939..6458d2f510 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -73,7 +73,7 @@ static void icm20689SpiInit(void) IOInit(icmSpi20689CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0); IOConfigGPIO(icmSpi20689CsPin, SPI_IO_CS_CFG); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); hardwareInitialised = true; } @@ -101,7 +101,7 @@ bool icm20689SpiDetect(void) } } while (attemptsRemaining--); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); return true; @@ -175,6 +175,6 @@ void icm20689GyroInit(uint8_t lpf) mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 9c2b7a45df..bfbba6dce5 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -84,7 +84,10 @@ bool mpu6500SpiDetect(void) mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST || tmp == ICM20608G_WHO_AM_I_CONST) { + if (tmp == MPU6500_WHO_AM_I_CONST || + tmp == MPU9250_WHO_AM_I_CONST || + tmp == ICM20608G_WHO_AM_I_CONST || + tmp == ICM20602_WHO_AM_I_CONST) { return true; } diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 66e2f887c9..f908440a1e 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -51,7 +51,6 @@ typedef struct adcTagMap_s { typedef struct adcDevice_s { ADC_TypeDef* ADCx; rccPeriphTag_t rccADC; - rccPeriphTag_t rccDMA; #if defined(STM32F4) || defined(STM32F7) DMA_Stream_TypeDef* DMAy_Streamx; uint32_t channel; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 19d7edf6f9..00fd32bcb0 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -32,13 +32,14 @@ #include "adc_impl.h" #include "io.h" #include "rcc.h" +#include "dma.h" #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Channelx = DMA1_Channel1 } }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) @@ -131,7 +132,8 @@ void adcInit(drv_adc_config_t *init) RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) RCC_ClockCmd(adc.rccADC, ENABLE); - RCC_ClockCmd(adc.rccDMA, ENABLE); + + dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, RESOURCE_INDEX(device)); DMA_DeInit(adc.DMAy_Channelx); DMA_InitTypeDef DMA_InitStructure; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index f58a8086d6..f17fb892f1 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -30,6 +30,7 @@ #include "adc_impl.h" #include "io.h" #include "rcc.h" +#include "dma.h" #include "common/utils.h" @@ -38,8 +39,12 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, - { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } + { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 }, +#ifdef ADC24_DMA_REMAP + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 } +#else + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 } +#endif }; const adcTagMap_t adcTagMap[] = { @@ -133,6 +138,9 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; +#ifdef ADC24_DMA_REMAP + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_ADC2ADC4, ENABLE); +#endif adcDevice_t adc = adcHardware[device]; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { @@ -149,7 +157,8 @@ void adcInit(drv_adc_config_t *init) RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_ClockCmd(adc.rccADC, ENABLE); - RCC_ClockCmd(adc.rccDMA, ENABLE); + + dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, RESOURCE_INDEX(device)); DMA_DeInit(adc.DMAy_Channelx); diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 968f05a67e..9655381977 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -25,6 +25,7 @@ #include "io.h" #include "io_impl.h" #include "rcc.h" +#include "dma.h" #include "sensor.h" #include "accgyro.h" @@ -37,8 +38,8 @@ #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 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ @@ -140,9 +141,10 @@ void adcInit(drv_adc_config_t *init) adcConfig[i].enabled = true; } - RCC_ClockCmd(adc.rccDMA, ENABLE); RCC_ClockCmd(adc.rccADC, ENABLE); + dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, RESOURCE_INDEX(device)); + DMA_DeInit(adc.DMAy_Streamx); DMA_StructInit(&DMA_InitStructure); diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index 7bdba67f52..b6c84fbaad 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -25,6 +25,7 @@ #include "io.h" #include "io_impl.h" #include "rcc.h" +#include "dma.h" #include "sensor.h" #include "accgyro.h" @@ -37,8 +38,8 @@ #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 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ @@ -138,8 +139,9 @@ void adcInit(drv_adc_config_t *init) adcConfig[i].enabled = true; } - RCC_ClockCmd(adc.rccDMA, ENABLE); + RCC_ClockCmd(adc.rccADC, ENABLE); + dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, RESOURCE_INDEX(device)); ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; ADCHandle.Init.ContinuousConvMode = ENABLE; diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c new file mode 100644 index 0000000000..6589a7e930 --- /dev/null +++ b/src/main/drivers/display.c @@ -0,0 +1,72 @@ +/* + * 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 "common/utils.h" + +#include "display.h" + +void displayClear(displayPort_t *instance) +{ + instance->vTable->clear(instance); + instance->cleared = true; + instance->cursorRow = -1; +} + +void displayGrab(displayPort_t *instance) +{ + instance->vTable->grab(instance); + instance->vTable->clear(instance); + instance->isGrabbed = true; +} + +void displayRelease(displayPort_t *instance) +{ + instance->vTable->release(instance); + instance->isGrabbed = false; +} + +bool displayIsGrabbed(const displayPort_t *instance) +{ + // can be called before initialised + return (instance && instance->isGrabbed); +} + +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s) +{ + return instance->vTable->write(instance, x, y, s); +} + +void displayHeartbeat(displayPort_t *instance) +{ + instance->vTable->heartbeat(instance); +} + +void displayResync(displayPort_t *instance) +{ + instance->vTable->resync(instance); +} + +uint16_t displayTxBytesFree(const displayPort_t *instance) +{ + return instance->vTable->txBytesFree(instance); +} + diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h new file mode 100644 index 0000000000..1605237736 --- /dev/null +++ b/src/main/drivers/display.h @@ -0,0 +1,49 @@ +/* + * 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 + +struct displayPortVTable_s; +typedef struct displayPort_s { + const struct displayPortVTable_s *vTable; + uint8_t rows; + uint8_t cols; + + // CMS state + bool cleared; + int8_t cursorRow; + bool isGrabbed; +} displayPort_t; + +typedef struct displayPortVTable_s { + int (*grab)(displayPort_t *displayPort); + int (*release)(displayPort_t *displayPort); + int (*clear)(displayPort_t *displayPort); + int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text); + int (*heartbeat)(displayPort_t *displayPort); + void (*resync)(displayPort_t *displayPort); + uint32_t (*txBytesFree)(const displayPort_t *displayPort); +} displayPortVTable_t; + +void displayGrab(displayPort_t *instance); +void displayRelease(displayPort_t *instance); +bool displayIsGrabbed(const displayPort_t *instance); +void displayClear(displayPort_t *instance); +int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s); +void displayHeartbeat(displayPort_t *instance); +void displayResync(displayPort_t *instance); +uint16_t displayTxBytesFree(const displayPort_t *instance); diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 7e4942fbe1..ac46ff4f2b 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -63,16 +63,18 @@ DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_CH4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_CH5_HANDLER) #endif - -void dmaInit(void) +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) { - // TODO: Do we need this? + RCC_AHBPeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); + dmaDescriptors[identifier].owner = owner; + dmaDescriptors[identifier].resourceIndex = resourceIndex; } -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) { NVIC_InitTypeDef NVIC_InitStructure; + /* TODO: remove this - enforce the init */ RCC_AHBPeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); dmaDescriptors[identifier].irqHandlerCallback = callback; dmaDescriptors[identifier].userParam = userParam; @@ -84,3 +86,22 @@ void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr NVIC_Init(&NVIC_InitStructure); } +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].owner; +} + +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].resourceIndex; +} + +dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].channel == channel) { + return i; + } + } + return 0; +} \ No newline at end of file diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index ea2c3484f5..e34233e570 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -15,10 +15,29 @@ * along with Cleanflight. If not, see . */ +#pragma once + +#include "resource.h" struct dmaChannelDescriptor_s; typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channelDescriptor); +typedef struct dmaChannelDescriptor_s { + DMA_TypeDef* dma; +#if defined(STM32F4) || defined(STM32F7) + DMA_Stream_TypeDef* stream; +#else + DMA_Channel_TypeDef* channel; +#endif + dmaCallbackHandlerFuncPtr irqHandlerCallback; + uint8_t flagsShift; + IRQn_Type irqN; + uint32_t rcc; + uint32_t userParam; + resourceOwner_e owner; + uint8_t resourceIndex; +} dmaChannelDescriptor_t; + #if defined(STM32F4) || defined(STM32F7) uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream); @@ -40,19 +59,15 @@ typedef enum { DMA2_ST5_HANDLER, DMA2_ST6_HANDLER, DMA2_ST7_HANDLER, -} dmaHandlerIdentifier_e; + DMA_MAX_DESCRIPTORS +} dmaIdentifier_e; -typedef struct dmaChannelDescriptor_s { - DMA_TypeDef* dma; - DMA_Stream_TypeDef* stream; - dmaCallbackHandlerFuncPtr irqHandlerCallback; - uint8_t flagsShift; - IRQn_Type irqN; - uint32_t rcc; - uint32_t userParam; -} dmaChannelDescriptor_t; +#define DMA_MOD_VALUE 8 +#define DMA_MOD_OFFSET 0 +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Stream %d:" -#define DEFINE_DMA_CHANNEL(d, s, f, i, r) {.dma = d, .stream = s, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0} +#define DEFINE_DMA_CHANNEL(d, s, f, i, r) {.dma = d, .stream = s, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0, .owner = 0, .resourceIndex = 0 } #define DEFINE_DMA_IRQ_HANDLER(d, s, i) void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\ if (dmaDescriptors[i].irqHandlerCallback)\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ @@ -62,11 +77,13 @@ typedef struct dmaChannelDescriptor_s { #define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) -#define DMA_IT_TCIF ((uint32_t)0x00000020) -#define DMA_IT_HTIF ((uint32_t)0x00000010) -#define DMA_IT_TEIF ((uint32_t)0x00000008) -#define DMA_IT_DMEIF ((uint32_t)0x00000004) -#define DMA_IT_FEIF ((uint32_t)0x00000001) +#define DMA_IT_TCIF ((uint32_t)0x00000020) +#define DMA_IT_HTIF ((uint32_t)0x00000010) +#define DMA_IT_TEIF ((uint32_t)0x00000008) +#define DMA_IT_DMEIF ((uint32_t)0x00000004) +#define DMA_IT_FEIF ((uint32_t)0x00000001) + +dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream); #else @@ -78,24 +95,22 @@ typedef enum { DMA1_CH5_HANDLER, DMA1_CH6_HANDLER, DMA1_CH7_HANDLER, +#if defined(STM32F3) || defined(STM32F10X_CL) DMA2_CH1_HANDLER, DMA2_CH2_HANDLER, DMA2_CH3_HANDLER, DMA2_CH4_HANDLER, DMA2_CH5_HANDLER, -} dmaHandlerIdentifier_e; +#endif + DMA_MAX_DESCRIPTORS +} dmaIdentifier_e; -typedef struct dmaChannelDescriptor_s { - DMA_TypeDef* dma; - DMA_Channel_TypeDef* channel; - dmaCallbackHandlerFuncPtr irqHandlerCallback; - uint8_t flagsShift; - IRQn_Type irqN; - uint32_t rcc; - uint32_t userParam; -} dmaChannelDescriptor_t; +#define DMA_MOD_VALUE 7 +#define DMA_MOD_OFFSET 1 +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Channel %d:" -#define DEFINE_DMA_CHANNEL(d, c, f, i, r) {.dma = d, .channel = c, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0} +#define DEFINE_DMA_CHANNEL(d, c, f, i, r) {.dma = d, .channel = c, .irqHandlerCallback = NULL, .flagsShift = f, .irqN = i, .rcc = r, .userParam = 0, .owner = 0, .resourceIndex = 0 } #define DEFINE_DMA_IRQ_HANDLER(d, c, i) void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\ if (dmaDescriptors[i].irqHandlerCallback)\ dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ @@ -104,12 +119,16 @@ typedef struct dmaChannelDescriptor_s { #define DMA_CLEAR_FLAG(d, flag) d->dma->IFCR = (flag << d->flagsShift) #define DMA_GET_FLAG_STATUS(d, flag) (d->dma->ISR & (flag << d->flagsShift)) -#define DMA_IT_TCIF ((uint32_t)0x00000002) -#define DMA_IT_HTIF ((uint32_t)0x00000004) -#define DMA_IT_TEIF ((uint32_t)0x00000008) +#define DMA_IT_TCIF ((uint32_t)0x00000002) +#define DMA_IT_HTIF ((uint32_t)0x00000004) +#define DMA_IT_TEIF ((uint32_t)0x00000008) + +dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel); #endif -void dmaInit(void); -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex); +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam); +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier); +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier); diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index 4147f29c07..afcd144286 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -23,11 +23,12 @@ #include "nvic.h" #include "dma.h" +#include "resource.h" /* * DMA descriptors. */ -static dmaChannelDescriptor_t dmaDescriptors[] = { +static dmaChannelDescriptor_t dmaDescriptors[DMA_MAX_DESCRIPTORS] = { DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHB1Periph_DMA1), DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 6, DMA1_Stream1_IRQn, RCC_AHB1Periph_DMA1), DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 16, DMA1_Stream2_IRQn, RCC_AHB1Periph_DMA1), @@ -67,12 +68,14 @@ 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) +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) { - // TODO: Do we need this? + RCC_AHB1PeriphClockCmd(dmaDescriptors[identifier].rcc, ENABLE); + dmaDescriptors[identifier].owner = owner; + dmaDescriptors[identifier].resourceIndex = resourceIndex; } -void dmaSetHandler(dmaHandlerIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) { NVIC_InitTypeDef NVIC_InitStructure; @@ -100,4 +103,24 @@ uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream) RETURN_TCIF_FLAG(stream, 6); RETURN_TCIF_FLAG(stream, 7); return 0; +} + +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].owner; +} + +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].resourceIndex; +} + +dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].stream == stream) { + return i; + } + } + return 0; } \ No newline at end of file diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c index f13c3a0619..998f5abe73 100644 --- a/src/main/drivers/dma_stm32f7xx.c +++ b/src/main/drivers/dma_stm32f7xx.c @@ -23,11 +23,12 @@ #include "drivers/nvic.h" #include "drivers/dma.h" +#include "resource.h" /* * DMA descriptors. */ -static dmaChannelDescriptor_t dmaDescriptors[] = { +static dmaChannelDescriptor_t dmaDescriptors[DMA_MAX_DESCRIPTORS] = { 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), @@ -68,30 +69,50 @@ 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) +static void enableDmaClock(uint32_t rcc) { - // 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); + SET_BIT(RCC->AHB1ENR, rcc); /* Delay after an RCC peripheral clock enabling */ - tmpreg = READ_BIT(RCC->AHB1ENR, dmaDescriptors[identifier].rcc); + tmpreg = READ_BIT(RCC->AHB1ENR, rcc); UNUSED(tmpreg); } while(0); +} +void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex) +{ + enableDmaClock(dmaDescriptors[identifier].rcc); + dmaDescriptors[identifier].owner = owner; + dmaDescriptors[identifier].resourceIndex = resourceIndex; +} + +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +{ + enableDmaClock(dmaDescriptors[identifier].rcc); 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); } +resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].owner; +} + +uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier) +{ + return dmaDescriptors[identifier].resourceIndex; +} + +dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].stream == stream) { + return i; + } + } + return 0; +} \ No newline at end of file diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index b4d4cfaa49..db41a01c7a 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -56,6 +56,12 @@ void EXTIInit(void) #if defined(STM32F3) || defined(STM32F4) /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); +#ifdef REMAP_TIM16_DMA + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM16, ENABLE); +#endif +#ifdef REMAP_TIM17_DMA + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM17, ENABLE); +#endif #endif memset(extiChannelRecs, 0, sizeof(extiChannelRecs)); memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority)); diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 5104e5cf72..de5be5f68d 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -1,4 +1,21 @@ -#include "common/utils.h" +/* + * 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 "common/utils.h" #include "io.h" #include "io_impl.h" @@ -65,7 +82,7 @@ const struct ioPortDef_s ioPortDefs[] = { const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", "SONAR_TRIGGER", "SONAR_ECHO", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", - "BARO", "MPU", "INVERTER", "LED_STRIP", "LED", "RX", "TX", "SOFT_SPI", "RX_SPI" + "BARO", "MPU", "INVERTER", "LED_STRIP", "LED", "RX", "TX", "SOFT_SPI", "RX_SPI", "MAX7456" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { @@ -231,7 +248,7 @@ void IOToggle(IO_t io) } // claim IO pin, set owner and resources -void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resource, uint8_t index) +void IOInit(IO_t io, resourceOwner_e owner, resourceType_e resource, uint8_t index) { ioRec_t *ioRec = IO_Rec(io); ioRec->owner = owner; @@ -245,13 +262,13 @@ void IORelease(IO_t io) ioRec->owner = OWNER_FREE; } -resourceOwner_t IOGetOwner(IO_t io) +resourceOwner_e IOGetOwner(IO_t io) { ioRec_t *ioRec = IO_Rec(io); return ioRec->owner; } -resourceType_t IOGetResource(IO_t io) +resourceType_e IOGetResource(IO_t io) { ioRec_t *ioRec = IO_Rec(io); return ioRec->resource; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d3f85eaf77..2326db0288 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -1,4 +1,21 @@ -#pragma once +/* + * 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 #include #include @@ -84,10 +101,10 @@ void IOHi(IO_t io); void IOLo(IO_t io); void IOToggle(IO_t io); -void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resource, uint8_t index); +void IOInit(IO_t io, resourceOwner_e owner, resourceType_e resource, uint8_t index); void IORelease(IO_t io); // unimplemented -resourceOwner_t IOGetOwner(IO_t io); -resourceType_t IOGetResources(IO_t io); +resourceOwner_e IOGetOwner(IO_t io); +resourceType_e IOGetResources(IO_t io); IO_t IOGetByTag(ioTag_t tag); void IOConfigGPIO(IO_t io, ioConfig_t cfg); diff --git a/src/main/drivers/io_def.h b/src/main/drivers/io_def.h index 7600054ece..5360dfbc96 100644 --- a/src/main/drivers/io_def.h +++ b/src/main/drivers/io_def.h @@ -1,3 +1,20 @@ +/* + * 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 #include "common/utils.h" diff --git a/src/main/drivers/io_def_generated.h b/src/main/drivers/io_def_generated.h index a1f0fc3114..975f6721c3 100644 --- a/src/main/drivers/io_def_generated.h +++ b/src/main/drivers/io_def_generated.h @@ -1,4 +1,21 @@ -#pragma once +/* + * 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 // this file is automatically generated by def_generated.pl script // do not modify this file directly, your changes will be lost diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 0c880fd78d..9f7a8f01c3 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -1,4 +1,21 @@ -#pragma once +/* + * 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 // TODO - GPIO_TypeDef include #include "io.h" @@ -11,8 +28,8 @@ typedef struct ioDef_s { typedef struct ioRec_s { GPIO_TypeDef *gpio; uint16_t pin; - resourceOwner_t owner; - resourceType_t resource; + resourceOwner_e owner; + resourceType_e resource; uint8_t index; } ioRec_t; diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h index 78a3158e34..ebf2be7ce8 100644 --- a/src/main/drivers/io_types.h +++ b/src/main/drivers/io_types.h @@ -1,4 +1,21 @@ -#pragma once +/* + * 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 #include @@ -8,7 +25,7 @@ typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change // NONE initializer for ioTag_t variables -#define IO_TAG_NONE ((ioTag_t)0) +#define IO_TAG_NONE IO_TAG(NONE) // NONE initializer for IO_t variable #define IO_NONE ((IO_t)0) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index c7110799c9..b61ea977a1 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -37,12 +37,13 @@ #include "common/color.h" #include "common/colorconversion.h" #include "dma.h" +#include "io.h" #include "light_ws2811strip.h" -#if defined(STM32F4) || defined(STM32F7) -uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else +#if defined(STM32F1) uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#else +uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #endif volatile uint8_t ws2811LedDataTransferInProgress = 0; @@ -84,10 +85,13 @@ void setStripColors(const hsvColor_t *colors) } } -void ws2811LedStripInit(void) +void ws2811LedStripInit(ioTag_t ioTag) { memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); - ws2811LedStripHardwareInit(); + ws2811LedStripHardwareInit(ioTag); + + const hsvColor_t hsv_white = { 0, 255, 255}; + setStripColor(&hsv_white); ws2811UpdateStrip(); } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index c980353abb..1f65a58d09 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -17,28 +17,41 @@ #pragma once -#define WS2811_LED_STRIP_LENGTH 32 -#define WS2811_BITS_PER_LED 24 -#define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay +#include "io_types.h" + +#define WS2811_LED_STRIP_LENGTH 32 +#define WS2811_BITS_PER_LED 24 +// for 50us delay +#define WS2811_DELAY_BUFFER_LENGTH 42 #define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH) - -#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) #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 +#define WS2811_TIMER_HZ 84000000 +#define WS2811_TIMER_PERIOD 104 +// timer compare value for logical 1 +#define BIT_COMPARE_1 67 +// timer compare value for logical 0 +#define BIT_COMPARE_0 33 #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 +// timer compare value for logical 1 +#define BIT_COMPARE_1 76 +// timer compare value for logical 0 +#define BIT_COMPARE_0 38 #else -#define BIT_COMPARE_1 17 // timer compare value for logical 1 -#define BIT_COMPARE_0 9 // timer compare value for logical 0 +#define WS2811_TIMER_HZ 24000000 +#define WS2811_TIMER_PERIOD 29 +// timer compare value for logical 1 +#define BIT_COMPARE_1 17 +// timer compare value for logical 0 +#define BIT_COMPARE_0 9 #endif -void ws2811LedStripInit(void); +void ws2811LedStripInit(ioTag_t ioTag); -void ws2811LedStripHardwareInit(void); +void ws2811LedStripHardwareInit(ioTag_t ioTag); void ws2811LedStripDMAEnable(void); void ws2811UpdateStrip(void); @@ -54,9 +67,9 @@ void setStripColors(const hsvColor_t *colors); bool isWS2811LedStripReady(void); -#if defined(STM32F4) || defined(STM32F7) -extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else +#if defined(STM32F1) extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#else +extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #endif extern volatile uint8_t ws2811LedDataTransferInProgress; diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index a9e6061bba..8de162c280 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -31,26 +31,15 @@ #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; +static uint16_t timerChannel = 0; void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) { - if(htim->Instance==WS2811_TIMER) + if(htim->Instance == TimHandle.Instance) { //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); ws2811LedDataTransferInProgress = 0; @@ -62,9 +51,20 @@ void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]); } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { - TimHandle.Instance = WS2811_TIMER; + if (!ioTag) { + return; + } + + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + TIM_TypeDef *timer = timerHardware->tim; + timerChannel = timerHardware->channel; + + if (timerHardware->dmaStream == NULL) { + return; + } + TimHandle.Instance = timer; TimHandle.Init.Prescaler = 1; TimHandle.Init.Period = 135; // 800kHz @@ -78,16 +78,14 @@ void ws2811LedStripHardwareInit(void) static DMA_HandleTypeDef hdma_tim; - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); - /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + ws2811IO = IOGetByTag(ioTag); 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); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); - /* Set the parameters to be configured */ - hdma_tim.Init.Channel = WS2811_DMA_CHANNEL; + 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; @@ -101,35 +99,18 @@ void ws2811LedStripHardwareInit(void) hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; /* Set hdma_tim instance */ - hdma_tim.Instance = WS2811_DMA_STREAM; + hdma_tim.Instance = timerHardware->dmaStream; - 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; - } + uint16_t dmaSource = timerDmaSource(timerChannel); /* Link hdma_tim to hdma[x] (channelx) */ - __HAL_LINKDMA(&TimHandle, hdma[timDMASource], hdma_tim); + __HAL_LINKDMA(&TimHandle, hdma[dmaSource], hdma_tim); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, timDMASource); + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaSource); /* Initialize TIMx DMA handle */ - if(HAL_DMA_Init(TimHandle.hdma[timDMASource]) != HAL_OK) + if(HAL_DMA_Init(TimHandle.hdma[dmaSource]) != HAL_OK) { /* Initialization Error */ return; @@ -145,15 +126,13 @@ void ws2811LedStripHardwareInit(void) 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) + if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, timerChannel) != HAL_OK) { /* Configuration Error */ return; } - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); } @@ -165,9 +144,9 @@ void ws2811LedStripDMAEnable(void) return; } - if( HAL_TIM_PWM_Start_DMA(&TimHandle, WS2811_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) + if (HAL_TIM_PWM_Start_DMA(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { - /* Starting PWM generation Error */ + /* Starting PWM generation Error */ ws2811LedDataTransferInProgress = 0; return; } diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 911b7cdc66..f8aef69ff3 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -32,8 +32,11 @@ static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; +static DMA_Channel_TypeDef *dmaChannel = NULL; +static TIM_TypeDef *timer = NULL; -static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { +static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->channel, DISABLE); @@ -41,32 +44,38 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { } } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { + if (!ioTag) { + return; + } + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - uint16_t prescalerValue; + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + if (timerHardware->dmaChannel == NULL) { + return; + } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); -/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP)); - RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + RCC_ClockCmd(timerRCC(timer), ENABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; + uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1; /* Time base configuration */ TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ TIM_OCStructInit(&TIM_OCInitStructure); @@ -74,20 +83,18 @@ void ws2811LedStripHardwareInit(void) TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(TIM3, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); + TIM_OC1Init(timer, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); - TIM_CtrlPWMOutputs(TIM3, ENABLE); + TIM_CtrlPWMOutputs(timer, ENABLE); /* configure DMA */ - /* DMA clock enable */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); - /* DMA1 Channel6 Config */ - DMA_DeInit(DMA1_Channel6); + dmaChannel = timerHardware->dmaChannel; + DMA_DeInit(dmaChannel); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR1; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -99,17 +106,17 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel6, &DMA_InitStructure); + DMA_Init(dmaChannel, &DMA_InitStructure); /* TIM3 CC1 DMA Request enable */ - TIM_DMACmd(TIM3, TIM_DMA_CC1, ENABLE); + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); + DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); + + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); - ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) @@ -117,10 +124,10 @@ void ws2811LedStripDMAEnable(void) if (!ws2811Initialised) return; - DMA_SetCurrDataCounter(DMA1_Channel6, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(TIM3, 0); - TIM_Cmd(TIM3, ENABLE); - DMA_Cmd(DMA1_Channel6, ENABLE); + DMA_SetCurrDataCounter(dmaChannel, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(dmaChannel, ENABLE); } #endif diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8a0b073c78..854cc171ce 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -31,18 +31,13 @@ #include "rcc.h" #include "timer.h" -#ifndef WS2811_PIN -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 -#endif - static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; +static DMA_Channel_TypeDef *dmaChannel = NULL; +static TIM_TypeDef *timer = NULL; -static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { +static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->channel, DISABLE); @@ -50,72 +45,85 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { } } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { + if (!ioTag) { + return; + } + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - uint16_t prescalerValue; + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + if (timerHardware->dmaChannel == NULL) { + return; + } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); - /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), WS2811_TIMER_GPIO_AF); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); - RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + RCC_ClockCmd(timerRCC(timer), ENABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; + uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1; + /* Time base configuration */ TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); /* PWM1 Mode configuration */ TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + } + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; TIM_OCInitStructure.TIM_Pulse = 0; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + TIM_OC1Init(timer, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); - - TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); + TIM_CtrlPWMOutputs(timer, ENABLE); /* configure DMA */ /* DMA1 Channel Config */ - DMA_DeInit(WS2811_DMA_CHANNEL); + dmaChannel = timerHardware->dmaChannel; + DMA_DeInit(dmaChannel); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&WS2811_TIMER->CCR1; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(WS2811_DMA_CHANNEL, &DMA_InitStructure); + DMA_Init(dmaChannel, &DMA_InitStructure); - TIM_DMACmd(WS2811_TIMER, TIM_DMA_CC1, ENABLE); + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); + DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); + + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); - ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) @@ -123,10 +131,10 @@ void ws2811LedStripDMAEnable(void) if (!ws2811Initialised) return; - DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(WS2811_TIMER, 0); - TIM_Cmd(WS2811_TIMER, ENABLE); - DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE); + DMA_SetCurrDataCounter(dmaChannel, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(dmaChannel, ENABLE); } #endif diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 46bcdd0e46..46671ef256 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -31,85 +31,89 @@ #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_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 -#endif +#include "io.h" static IO_t ws2811IO = IO_NONE; -static uint16_t timDMASource = 0; bool ws2811Initialised = false; +static DMA_Stream_TypeDef *stream = NULL; +static TIM_TypeDef *timer = NULL; static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->stream, DISABLE); - TIM_DMACmd(WS2811_TIMER, timDMASource, DISABLE); DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } } -void ws2811LedStripHardwareInit(void) +void ws2811LedStripHardwareInit(ioTag_t ioTag) { + if (!ioTag) { + return; + } + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - uint16_t prescalerValue; + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; - RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + if (timerHardware->dmaStream == NULL) { + return; + } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + RCC_ClockCmd(timerRCC(timer), ENABLE); + + ws2811IO = IOGetByTag(ioTag); /* 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, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), WS2811_TIMER_GPIO_AF); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); // Stop timer - TIM_Cmd(WS2811_TIMER, DISABLE); + TIM_Cmd(timer, DISABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t)(SystemCoreClock / 2 / 84000000) - 1; + uint16_t prescalerValue = (uint16_t)(SystemCoreClock / timerClockDivisor(timer) / WS2811_TIMER_HZ) - 1; /* Time base configuration */ - TIM_TimeBaseStructure.TIM_Period = 104; // 800kHz + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ + TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + } + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; TIM_OCInitStructure.TIM_Pulse = 0; - timerOCInit(WS2811_TIMER, WS2811_TIMER_CHANNEL, &TIM_OCInitStructure); - timerOCPreloadConfig(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_OCPreload_Enable); - timDMASource = timerDmaSource(WS2811_TIMER_CHANNEL); + timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); + timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); - TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); - TIM_ARRPreloadConfig(WS2811_TIMER, ENABLE); + TIM_CtrlPWMOutputs(timer, ENABLE); + TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_CCx_Enable); - TIM_Cmd(WS2811_TIMER, ENABLE); + TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + TIM_Cmd(timer, ENABLE); + stream = timerHardware->dmaStream; /* configure DMA */ - DMA_Cmd(WS2811_DMA_STREAM, DISABLE); - DMA_DeInit(WS2811_DMA_STREAM); + DMA_Cmd(stream, DISABLE); + DMA_DeInit(stream); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL; - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(WS2811_TIMER, WS2811_TIMER_CHANNEL); + DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -124,17 +128,16 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(WS2811_DMA_STREAM, &DMA_InitStructure); + DMA_Init(stream, &DMA_InitStructure); + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - DMA_ITConfig(WS2811_DMA_STREAM, DMA_IT_TC, ENABLE); - DMA_ClearITPendingBit(WS2811_DMA_STREAM, dmaFlag_IT_TCIF(WS2811_DMA_STREAM)); + DMA_ITConfig(stream, DMA_IT_TC, ENABLE); + DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(stream)); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; - setStripColor(&hsv_white); - ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) @@ -142,10 +145,10 @@ void ws2811LedStripDMAEnable(void) if (!ws2811Initialised) return; - DMA_SetCurrDataCounter(WS2811_DMA_STREAM, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(WS2811_TIMER, 0); - DMA_Cmd(WS2811_DMA_STREAM, ENABLE); - TIM_DMACmd(WS2811_TIMER, timDMASource, ENABLE); + DMA_SetCurrDataCounter(stream, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(stream, ENABLE); } #endif diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index a552673556..10cc071411 100755 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -294,7 +294,7 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c) screenBuffer[y*30+x] = c; } -void max7456Write(uint8_t x, uint8_t y, char *buff) +void max7456Write(uint8_t x, uint8_t y, const char *buff) { uint8_t i = 0; for (i = 0; *(buff+i); i++) @@ -387,7 +387,7 @@ void max7456RefreshAll(void) } } -void max7456WriteNvm(uint8_t char_address, uint8_t *font_data) +void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) { uint8_t x; diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 6663b12934..a4ebe0ab4e 100755 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -146,9 +146,9 @@ extern uint16_t maxScreenSize; void max7456Init(uint8_t system); void max7456DrawScreen(void); -void max7456WriteNvm(uint8_t char_address, uint8_t *font_data); +void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); uint8_t max7456GetRowsCount(void); -void max7456Write(uint8_t x, uint8_t y, char *buff); +void max7456Write(uint8_t x, uint8_t y, const char *buff); void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c); void max7456ClearScreen(void); void max7456RefreshAll(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 46f082a1dc..e4e04cbaa8 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -42,17 +42,18 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + if (output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = value; - TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; timerOCInit(tim, channel, &TIM_OCInitStructure); timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable); @@ -156,7 +157,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { - uint32_t timerMhzCounter; + uint32_t timerMhzCounter = 0; pwmWriteFuncPtr pwmWritePtr; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool isDigital = false; @@ -208,7 +209,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; } - const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY); if (timerHardware == NULL) { /* flag failure and disable ability to arm */ @@ -271,7 +272,7 @@ void servoInit(const servoConfig_t *servoConfig) 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); + const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY); if (timer == NULL) { /* flag failure and disable ability to arm */ diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 53a0a7fc4b..4aa48905ae 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -71,7 +71,7 @@ typedef struct { #endif #if defined(STM32F7) TIM_HandleTypeDef TimHandle; - uint32_t Channel; + DMA_HandleTypeDef hdma_tim; #endif } motorDmaOutput_t; diff --git a/src/main/drivers/pwm_output_hal.c b/src/main/drivers/pwm_output_hal.c index 290b386007..5e3dd4a05c 100644 --- a/src/main/drivers/pwm_output_hal.c +++ b/src/main/drivers/pwm_output_hal.c @@ -202,6 +202,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; #ifdef USE_DSHOT case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 80903ce19f..72fc09043f 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -20,6 +20,8 @@ #include "platform.h" +#include "build/debug.h" + #include "io.h" #include "timer.h" #include "pwm_output.h" @@ -84,7 +86,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { + for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); } @@ -119,26 +121,28 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (configureTimer) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz; - switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): - hz = MOTOR_DSHOT600_MHZ * 1000000; - break; - case(PWM_TYPE_DSHOT300): - hz = MOTOR_DSHOT300_MHZ * 1000000; - break; - default: - case(PWM_TYPE_DSHOT150): - hz = MOTOR_DSHOT150_MHZ * 1000000; - } + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } @@ -146,19 +150,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_Low; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; @@ -177,6 +175,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_Channel_TypeDef *channel = timerHardware->dmaChannel; + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(channel, DISABLE); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 7e60c62f1d..10e841f853 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -85,7 +85,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { + for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); } @@ -120,37 +120,43 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t if (configureTimer) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz; - switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): - hz = MOTOR_DSHOT600_MHZ * 1000000; - break; - case(PWM_TYPE_DSHOT300): - hz = MOTOR_DSHOT300_MHZ * 1000000; - break; - default: - case(PWM_TYPE_DSHOT150): - hz = MOTOR_DSHOT150_MHZ * 1000000; - } + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1; TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } + TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + } TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); @@ -167,9 +173,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } DMA_Stream_TypeDef *stream = timerHardware->dmaStream; + + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); + dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(stream, DISABLE); DMA_DeInit(stream); + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel; DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); @@ -191,8 +201,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_ITConfig(stream, DMA_IT_TC, ENABLE); DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(timerHardware->dmaStream)); - - dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); } #endif diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 523b993df2..e7db17f738 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -145,6 +145,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motor->TimHandle.Instance = timerHardware->tim; motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;; motor->TimHandle.Init.Period = MOTOR_BITLENGTH; + motor->TimHandle.Init.RepetitionCounter = 0; motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; if(HAL_TIM_PWM_Init(&motor->TimHandle) != HAL_OK) @@ -175,23 +176,20 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } 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; + motor->hdma_tim.Init.Channel = timerHardware->dmaChannel; + motor->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; + motor->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; + motor->hdma_tim.Init.MemInc = DMA_MINC_ENABLE; + motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + motor->hdma_tim.Init.Mode = DMA_NORMAL; + motor->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; + motor->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + motor->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + motor->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; + motor->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; /* Set hdma_tim instance */ if(timerHardware->dmaStream == NULL) @@ -199,11 +197,12 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* Initialization Error */ return; } - hdma_tim.Instance = timerHardware->dmaStream; + motor->hdma_tim.Instance = timerHardware->dmaStream; /* Link hdma_tim to hdma[x] (channelx) */ - __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], hdma_tim); + __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim); + dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); /* Initialize TIMx DMA handle */ @@ -219,7 +218,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t 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.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.Pulse = 0; diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index f055683976..13f074fad1 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -393,7 +393,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) pwmInputPort_t *port = &pwmInputPorts[channel]; - const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIMER_INPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM); if (!timer) { /* TODO: maybe fail here if not enough channels? */ @@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IO_t io = IOGetByTag(pwmConfig->ioTags[channel]); IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); - IOConfigGPIO(io, timer->ioMode); + IOConfigGPIO(io, IOCFG_IPD); #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); @@ -459,7 +459,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT]; - const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIMER_INPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_PPM); if (!timer) { /* TODO: fail here? */ return; @@ -472,7 +472,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) IO_t io = IOGetByTag(ppmConfig->ioTag); IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); - IOConfigGPIO(io, timer->ioMode); + IOConfigGPIO(io, IOCFG_IPD); #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 7d1649c5f6..7eb040e7d7 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -33,8 +33,9 @@ typedef enum { OWNER_TX, OWNER_SOFTSPI, OWNER_RX_SPI, + OWNER_MAX7456, OWNER_TOTAL_COUNT -} resourceOwner_t; +} resourceOwner_e; extern const char * const ownerNames[OWNER_TOTAL_COUNT]; @@ -51,6 +52,6 @@ typedef enum { RESOURCE_ADC_BATTERY, RESOURCE_ADC_RSSI, RESOURCE_ADC_EXTERNAL1, RESOURCE_ADC_CURRENT, RESOURCE_RX_CE, RESOURCE_TOTAL_COUNT -} resourceType_t; +} resourceType_e; extern const char * const resourceNames[RESOURCE_TOTAL_COUNT]; diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 577d305d52..1ae53a3659 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -17,6 +17,7 @@ #include #include +#include #include "platform.h" @@ -25,6 +26,13 @@ typedef enum { BAUDRATE_KISS = 38400 } escBaudRate_e; +typedef enum { + PROTOCOL_SIMONK = 0, + PROTOCOL_BLHELI = 1, + PROTOCOL_KISS = 2, + PROTOCOL_KISSALL = 3 +} escProtocol_e; + #if defined(USE_ESCSERIAL) #include "build/build_config.h" @@ -80,11 +88,19 @@ typedef struct escSerial_s { uint8_t escSerialPortIndex; uint8_t mode; + uint8_t outputCount; timerCCHandlerRec_t timerCb; timerCCHandlerRec_t edgeCb; } escSerial_t; +typedef struct { + IO_t io; + uint8_t inverted; +} escOutputs_t; + +escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS]; + extern timerHardware_t* serialTimerHardware; extern escSerial_t escSerialPorts[]; @@ -97,14 +113,36 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture); -static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); +static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) { - if (state) { - IOHi(escSerial->txIO); - } else { - IOLo(escSerial->txIO); + if((escSerial->mode = PROTOCOL_KISSALL)) + { + for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) { + uint8_t state_temp = state; + if(escOutputs[i].inverted) { + state_temp ^= ENABLE; + } + + if (state_temp) { + IOHi(escOutputs[i].io); + } else { + IOLo(escOutputs[i].io); + } + } + } + else + { + if(escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) { + state ^= ENABLE; + } + + if (state) { + IOHi(escSerial->txIO); + } else { + IOLo(escSerial->txIO); + } } } @@ -118,7 +156,7 @@ static void escSerialGPIOConfig(ioTag_t tag, ioConfig_t cfg) IOConfigGPIO(IOGetByTag(tag), cfg); } -void serialInputPortConfigEsc(const timerHardware_t *timerHardwarePtr) +void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) { #ifdef STM32F10X escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_IPU); @@ -164,12 +202,12 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 uint8_t mhz = SystemCoreClock / 2000000; TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, mhz); - serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } -static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) +static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { uint32_t timerPeriod=34; TIM_DeInit(timerHardwarePtr->tim); @@ -178,7 +216,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); } -static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { TIM_ICInitTypeDef TIM_ICInitStructure; @@ -192,17 +230,17 @@ static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) TIM_ICInit(tim, &TIM_ICInitStructure); } -static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) +static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); timerConfigure(timerHardwarePtr, 0xFFFF, 1); - serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); + escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); } -static void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr) +static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr) { escSerialGPIOConfig(timerHardwarePtr->tag, IOCFG_OUT_PP); timerChITConfig(timerHardwarePtr,DISABLE); @@ -225,7 +263,11 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac { escSerial_t *escSerial = &(escSerialPorts[portIndex]); - escSerial->rxTimerHardware = &(timerHardware[output]); + if(mode != PROTOCOL_KISSALL){ + escSerial->rxTimerHardware = &(timerHardware[output]); + } + + escSerial->mode = mode; escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); escSerial->port.vTable = escSerialVTable; @@ -247,30 +289,56 @@ serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbac escSerial->escSerialPortIndex = portIndex; - escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); - serialInputPortConfigEsc(escSerial->rxTimerHardware); - - setTxSignalEsc(escSerial, ENABLE); + if(mode != PROTOCOL_KISSALL) + { + escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag); + escSerialInputPortConfig(escSerial->rxTimerHardware); + setTxSignalEsc(escSerial, ENABLE); + } delay(50); - if(mode==0){ - serialTimerTxConfig(escSerial->txTimerHardware, portIndex); - serialTimerRxConfig(escSerial->rxTimerHardware, portIndex); + if(mode==PROTOCOL_SIMONK){ + escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex); + escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex); } - else if(mode==1){ + else if(mode==PROTOCOL_BLHELI){ serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options); } - else if(mode==2) { - serialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used + else if(mode==PROTOCOL_KISS) { + escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used + serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); + } + else if(mode==PROTOCOL_KISSALL) { + escSerial->outputCount = 0; + memset(&escOutputs, 0, sizeof(escOutputs)); + pwmOutputPort_t *pwmMotors = pwmGetMotors(); + for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (pwmMotors[i].enabled) { + if (pwmMotors[i].io != IO_NONE) { + for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) { + if(pwmMotors[i].io == IOGetByTag(timerHardware[j].tag)) + { + escSerialOutputPortConfig(&timerHardware[j]); + if(timerHardware[j].output & TIMER_OUTPUT_INVERTED) { + escOutputs[escSerial->outputCount].inverted = 1; + } + break; + } + } + escOutputs[escSerial->outputCount].io = pwmMotors[i].io; + escSerial->outputCount++; + } + } + } + setTxSignalEsc(escSerial, ENABLE); serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud); } - escSerial->mode = mode; return &escSerial->port; } -void serialInputPortDeConfig(const timerHardware_t *timerHardwarePtr) +void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr) { timerChClearCCFlag(timerHardwarePtr); timerChITConfig(timerHardwarePtr,DISABLE); @@ -284,7 +352,7 @@ void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output) escSerial->rxTimerHardware = &(timerHardware[output]); escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]); - serialInputPortDeConfig(escSerial->rxTimerHardware); + escSerialInputPortDeConfig(escSerial->rxTimerHardware); timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL); timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); TIM_DeInit(escSerial->txTimerHardware->tim); @@ -339,7 +407,7 @@ reload: escSerial->isTransmittingData = true; //set output - serialOutputPortConfig(escSerial->rxTimerHardware); + escSerialOutputPortConfig(escSerial->rxTimerHardware); return; } @@ -383,7 +451,7 @@ reload: if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { escSerial->isTransmittingData = false; - serialInputPortConfigEsc(escSerial->rxTimerHardware); + escSerialInputPortConfig(escSerial->rxTimerHardware); } } @@ -417,7 +485,9 @@ void processTxStateBL(escSerial_t *escSerial) //set output - serialOutputPortConfig(escSerial->rxTimerHardware); + if(escSerial->mode==PROTOCOL_BLHELI) { + escSerialOutputPortConfig(escSerial->rxTimerHardware); + } return; } @@ -432,9 +502,9 @@ void processTxStateBL(escSerial_t *escSerial) escSerial->isTransmittingData = false; if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) { - if(escSerial->mode==1) + if(escSerial->mode==PROTOCOL_BLHELI) { - serialInputPortConfigEsc(escSerial->rxTimerHardware); + escSerialInputPortConfig(escSerial->rxTimerHardware); } } } @@ -463,7 +533,7 @@ void prepareForNextRxByteBL(escSerial_t *escSerial) escSerial->isSearchingForStartBit = true; if (escSerial->rxEdge == LEADING) { escSerial->rxEdge = TRAILING; - serialICConfig( + escSerialICConfig( escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, (escSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling @@ -551,7 +621,7 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) escSerial->transmissionErrors++; } - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); escSerial->rxEdge = LEADING; escSerial->rxBitIndex = 0; @@ -569,10 +639,10 @@ void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if (escSerial->rxEdge == TRAILING) { escSerial->rxEdge = LEADING; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); } else { escSerial->rxEdge = TRAILING; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); } } /*-------------------------BL*/ @@ -605,7 +675,7 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture) { escSerial->isReceivingData=0; escSerial->receiveTimeout=0; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling); } } @@ -655,7 +725,7 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture bits=1; escSerial->internalRxBuffer = 0x80; - serialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); + escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising); } } escSerial->receiveTimeout = 0; @@ -763,7 +833,7 @@ void escSerialInitialize() for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { // set outputs to pullup - if(timerHardware[i].output==1) + if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) { escSerialGPIOConfig(timerHardware[i].tag, IOCFG_IPU); //GPIO_Mode_IPU } @@ -844,27 +914,34 @@ static bool ProcessExitCommand(uint8_t c) void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode) { bool exitEsc = false; + uint8_t motor_output = 0; LED0_OFF; LED1_OFF; //StopPwmAllMotors(); pwmDisableMotors(); passPort = escPassthroughPort; - uint8_t first_output = 0; - for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if(timerHardware[i].output==1) - { - first_output=i; - break; - } + uint32_t escBaudrate = (mode == PROTOCOL_KISS) ? BAUDRATE_KISS : BAUDRATE_NORMAL; + + if((mode == PROTOCOL_KISS) && (output == 255)){ + motor_output = 255; + mode = PROTOCOL_KISSALL; } + else { + uint8_t first_output = 0; + for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if(timerHardware[i].output & TIMER_OUTPUT_ENABLED) + { + first_output=i; + break; + } + } - //doesn't work with messy timertable - uint8_t motor_output=first_output+output-1; - if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) - return; - - uint32_t escBaudrate = (mode == 2) ? BAUDRATE_KISS : BAUDRATE_NORMAL; + //doesn't work with messy timertable + motor_output=first_output+output-1; + if(motor_output >=USABLE_TIMER_CHANNEL_COUNT) + return; + } escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode); uint8_t ch; @@ -898,7 +975,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin closeEscSerial(ESCSERIAL1, output); return; } - if(mode==1){ + if(mode==PROTOCOL_BLHELI){ serialWrite(escPassthroughPort, ch); // blheli loopback } serialWrite(escPort, ch); diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 8fc2573293..0a00d2aacc 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -57,7 +57,6 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { 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; @@ -90,11 +89,11 @@ static void uartReconfigure(uartPort_t *uartPort) if(uartPort->port.options & SERIAL_BIDIR) { - status = HAL_HalfDuplex_Init(&uartPort->Handle); + HAL_HalfDuplex_Init(&uartPort->Handle); } else { - status = HAL_UART_Init(&uartPort->Handle); + HAL_UART_Init(&uartPort->Handle); } // Receive DMA or IRQ @@ -216,7 +215,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, 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; @@ -252,7 +251,7 @@ void uartStartTxDMA(uartPort_t *s) 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; @@ -387,4 +386,3 @@ const struct serialPortVTable uartVTable[] = { .endWrite = NULL, } }; - diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 8b422df008..b4602ae020 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -109,8 +109,8 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART1; - #ifdef USE_UART1_RX_DMA + dmaInit(DMA1_CH5_HANDLER, OWNER_SERIAL, 1); s->rxDMAChannel = DMA1_Channel5; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; #endif @@ -118,7 +118,6 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; RCC_ClockCmd(RCC_APB2(USART1), ENABLE); - RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); // UART1_TX PA9 // UART1_RX PA10 @@ -138,6 +137,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option } // DMA TX Interrupt + dmaInit(DMA1_CH4_HANDLER, OWNER_SERIAL, 1); dmaSetHandler(DMA1_CH4_HANDLER, uart_tx_dma_IRQHandler, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); #ifndef USE_UART1_RX_DMA @@ -189,7 +189,6 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; RCC_ClockCmd(RCC_APB1(USART2), ENABLE); - RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); // UART2_TX PA2 // UART2_RX PA3 diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 304c702d51..f37f85790c 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -99,6 +99,7 @@ static uartPort_t uartPort4; static uartPort_t uartPort5; #endif +#if defined(USE_UART1_TX_DMA) || defined(USE_UART2_TX_DMA) || defined(USE_UART3_TX_DMA) static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) { uartPort_t *s = (uartPort_t*)(descriptor->userParam); @@ -110,6 +111,7 @@ static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) else s->txDMAEmpty = true; } +#endif void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index) { @@ -150,27 +152,35 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option s->port.baudRate = baudRate; - s->port.rxBuffer = rx1Buffer; - s->port.txBuffer = tx1Buffer; s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE; - -#ifdef USE_UART1_RX_DMA - s->rxDMAChannel = DMA1_Channel5; -#endif - s->txDMAChannel = DMA1_Channel4; + s->port.rxBuffer = rx1Buffer; + s->port.txBuffer = tx1Buffer; s->USARTx = USART1; +#ifdef USE_UART1_RX_DMA + dmaInit(DMA1_CH5_HANDLER, OWNER_SERIAL, 1); + s->rxDMAChannel = DMA1_Channel5; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; +#endif +#ifdef USE_UART1_TX_DMA + s->txDMAChannel = DMA1_Channel4; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; +#endif RCC_ClockCmd(RCC_APB2(USART1), ENABLE); + +#if defined(USE_UART1_TX_DMA) || defined(USE_UART1_RX_DMA) RCC_ClockCmd(RCC_AHB(DMA1), ENABLE); +#endif serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1); +#ifdef USE_UART1_TX_DMA + dmaInit(DMA1_CH4_HANDLER, OWNER_SERIAL, 1); dmaSetHandler(DMA1_CH4_HANDLER, handleUsartTxDma, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); +#endif #ifndef USE_UART1_RX_DMA NVIC_InitTypeDef NVIC_InitStructure; @@ -206,10 +216,12 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART2; #ifdef USE_UART2_RX_DMA + dmaInit(DMA1_CH6_HANDLER, OWNER_SERIAL, 2); s->rxDMAChannel = DMA1_Channel6; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; #endif #ifdef USE_UART2_TX_DMA + dmaInit(DMA1_CH7_HANDLER, OWNER_SERIAL, 2); s->txDMAChannel = DMA1_Channel7; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; #endif @@ -261,10 +273,12 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option s->USARTx = USART3; #ifdef USE_UART3_RX_DMA + dmaInit(DMA1_CH3_HANDLER, OWNER_SERIAL, 3); s->rxDMAChannel = DMA1_Channel3; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; #endif #ifdef USE_UART3_TX_DMA + dmaInit(DMA1_CH2_HANDLER, OWNER_SERIAL, 3); s->txDMAChannel = DMA1_Channel2; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; #endif diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index eb20fc8ec8..c0aed43f73 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -315,9 +315,13 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po if (uart->rxDMAStream) { s->rxDMAChannel = uart->DMAChannel; s->rxDMAStream = uart->rxDMAStream; + dmaInit(dmaGetIdentifier(uart->rxDMAStream), OWNER_SERIAL, RESOURCE_INDEX(device)); + } + if (uart->txDMAStream) { + s->txDMAChannel = uart->DMAChannel; + s->txDMAStream = uart->txDMAStream; + dmaInit(dmaGetIdentifier(uart->txDMAStream), OWNER_SERIAL, RESOURCE_INDEX(device)); } - s->txDMAChannel = uart->DMAChannel; - s->txDMAStream = uart->txDMAStream; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 7dfef2033f..2ffa962ebc 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -199,7 +199,7 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { - for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; } @@ -207,6 +207,16 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } +uint8_t timerInputIrq(TIM_TypeDef *tim) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].inputIrq; + } + } + return 0; +} + void timerNVICConfigure(uint8_t irq) { NVIC_InitTypeDef NVIC_InitStructure; @@ -239,9 +249,11 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui { configTimeBase(timerHardwarePtr->tim, period, mhz); TIM_Cmd(timerHardwarePtr->tim, ENABLE); - timerNVICConfigure(timerHardwarePtr->irq); + + uint8_t irq = timerInputIrq(timerHardwarePtr->tim); + timerNVICConfigure(irq); // HACK - enable second IRQ on timers that need it - switch(timerHardwarePtr->irq) { + switch(irq) { #if defined(STM32F10X) case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_IRQn); @@ -271,7 +283,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui } // 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) +void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq) { unsigned channel = timHw - timerHardware; if(channel >= USABLE_TIMER_CHANNEL_COUNT) @@ -288,7 +300,7 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = timHw->irq; + NVIC_InitStructure.NVIC_IRQChannel = irq; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; @@ -686,14 +698,14 @@ void timerInit(void) #endif /* enable the timer peripherals */ - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); } #if defined(STM32F3) || defined(STM32F4) - for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { + for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif @@ -755,14 +767,11 @@ void timerForceOverflow(TIM_TypeDef *tim) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int 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 + if (timerHardware[i].usageFlags & flag || flag == 0) { return &timerHardware[i]; } } @@ -813,6 +822,7 @@ volatile timCCR_t* timerCCR(TIM_TypeDef *tim, uint8_t channel) return (volatile timCCR_t*)((volatile char*)&tim->CCR1 + channel); } +#ifndef USE_HAL_DRIVER uint16_t timerDmaSource(uint8_t channel) { switch (channel) { @@ -826,4 +836,5 @@ uint16_t timerDmaSource(uint8_t channel) return TIM_DMA_CC4; } return 0; -} \ No newline at end of file +} +#endif diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index cf4d12b220..bb7a6c5bc1 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -54,6 +54,14 @@ typedef uint32_t timCNT_t; #error "Unknown CPU defined" #endif +typedef enum { + TIM_USE_ANY = 0x0, + TIM_USE_PPM = 0x1, + TIM_USE_PWM = 0x2, + TIM_USE_MOTOR = 0x4, + TIM_USE_SERVO = 0x8, + TIM_USE_LED = 0x10 +} timerUsageFlag_e; // use different types from capture and overflow - multiple overflow handlers are implemented as linked list struct timerCCHandlerRec_s; @@ -73,23 +81,23 @@ typedef struct timerOvrHandlerRec_s { typedef struct timerDef_s { TIM_TypeDef *TIMx; rccPeriphTag_t rcc; + uint8_t inputIrq; } timerDef_t; typedef struct timerHardware_s { TIM_TypeDef *tim; ioTag_t tag; uint8_t channel; - uint8_t irq; + timerUsageFlag_e usageFlags; uint8_t output; - ioConfig_t ioMode; #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) uint8_t alternateFunction; #endif -#if defined(USE_DSHOT) +#if defined(USE_DSHOT) || defined(LED_STRIP) #if defined(STM32F4) || defined(STM32F7) DMA_Stream_TypeDef *dmaStream; uint32_t dmaChannel; -#elif defined(STM32F3) +#elif defined(STM32F3) || defined(STM32F1) DMA_Channel_TypeDef *dmaChannel; #endif uint8_t dmaIrqHandler; @@ -97,6 +105,7 @@ typedef struct timerHardware_s { } timerHardware_t; typedef enum { + TIMER_OUTPUT_NONE = 0x00, TIMER_INPUT_ENABLED = 0x00, TIMER_OUTPUT_ENABLED = 0x01, TIMER_OUTPUT_INVERTED = 0x02, @@ -113,6 +122,8 @@ typedef enum { #endif #elif defined(STM32F3) #define HARDWARE_TIMER_DEFINITION_COUNT 10 +#elif defined(STM32F411xE) +#define HARDWARE_TIMER_DEFINITION_COUNT 10 #elif defined(STM32F4) #define HARDWARE_TIMER_DEFINITION_COUNT 14 #elif defined(STM32F7) @@ -159,7 +170,7 @@ void timerChITConfigDualLo(const timerHardware_t* timHw, FunctionalState newStat void timerChITConfig(const timerHardware_t* timHw, FunctionalState newState); void timerChClearCCFlag(const timerHardware_t* timHw); -void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority); +void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq); void timerInit(void); void timerStart(void); @@ -170,8 +181,9 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration rccPeriphTag_t timerRCC(TIM_TypeDef *tim); +uint8_t timerInputIrq(TIM_TypeDef *tim); -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag); +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim); diff --git a/src/main/drivers/timer_def.h b/src/main/drivers/timer_def.h new file mode 100644 index 0000000000..71ebb89972 --- /dev/null +++ b/src/main/drivers/timer_def.h @@ -0,0 +1,395 @@ +/* + * 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 + +#include +#include "common/utils.h" + +#if defined(STM32F3) + +#define DEF_TIM(tim, chan, pin, flags, out) {\ + tim,\ + IO_TAG(pin),\ + EXPAND(DEF_CHAN_ ## chan),\ + flags,\ + (DEF_CHAN_ ## chan ## _OUTPUT | out),\ + EXPAND(GPIO_AF__ ## pin ## _ ## tim ## _ ## chan),\ + CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _CHANNEL),\ + CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _HANDLER)\ + } + +#define DEF_DMA_CHANNEL(tim, chan) CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _CHANNEL) +#define DEF_DMA_HANDLER(tim, chan) CONCAT(EXPAND(DEF_TIM_DMA__ ## tim ## _ ## chan), _HANDLER) + +/* add the DMA mappings here */ +#define DEF_TIM_DMA__TIM1_CH1 DMA1_CH2 +#define DEF_TIM_DMA__TIM1_CH2 DMA1_CH3 +#define DEF_TIM_DMA__TIM1_CH4 DMA1_CH4 +#define DEF_TIM_DMA__TIM1_CH1N DMA1_CH2 +#define DEF_TIM_DMA__TIM1_CH2N DMA1_CH3 +#define DEF_TIM_DMA__TIM1_TRIG DMA1_CH4 +#define DEF_TIM_DMA__TIM1_COM DMA1_CH4 +#define DEF_TIM_DMA__TIM1_UP DMA1_CH5 +#define DEF_TIM_DMA__TIM1_CH3 DMA1_CH6 + +#define DEF_TIM_DMA__TIM2_CH3 DMA1_CH1 +#define DEF_TIM_DMA__TIM2_UP DMA1_CH2 +#define DEF_TIM_DMA__TIM2_CH1 DMA1_CH5 +#define DEF_TIM_DMA__TIM2_CH2 DMA1_CH7 +#define DEF_TIM_DMA__TIM2_CH4 DMA1_CH7 + +#define DEF_TIM_DMA__TIM3_CH2 DMA_NONE +#define DEF_TIM_DMA__TIM3_CH3 DMA1_CH2 +#define DEF_TIM_DMA__TIM3_CH4 DMA1_CH3 +#define DEF_TIM_DMA__TIM3_UP DMA1_CH3 +#define DEF_TIM_DMA__TIM3_CH1 DMA1_CH6 +#define DEF_TIM_DMA__TIM3_TRIG DMA1_CH6 + +#define DEF_TIM_DMA__TIM4_CH1 DMA1_CH1 +#define DEF_TIM_DMA__TIM4_CH2 DMA1_CH4 +#define DEF_TIM_DMA__TIM4_CH3 DMA1_CH5 +#define DEF_TIM_DMA__TIM4_UP DMA1_CH7 +#define DEF_TIM_DMA__TIM4_CH4 DMA_NONE + +#define DEF_TIM_DMA__TIM15_CH1 DMA1_CH5 +#define DEF_TIM_DMA__TIM15_CH2 DMA_NONE +#define DEF_TIM_DMA__TIM15_UP DMA1_CH5 +#define DEF_TIM_DMA__TIM15_TRIG DMA1_CH5 +#define DEF_TIM_DMA__TIM15_COM DMA1_CH5 +#define DEF_TIM_DMA__TIM15_CH1N DMA1_CH5 + +#ifdef REMAP_TIM16_DMA +#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6 +#define DEF_TIM_DMA__TIM16_UP DMA1_CH6 +#else +#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH3 +#define DEF_TIM_DMA__TIM16_UP DMA1_CH3 +#endif + +#ifdef REMAP_TIM17_DMA +#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH7 +#define DEF_TIM_DMA__TIM17_UP DMA1_CH7 +#else +#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH1 +#define DEF_TIM_DMA__TIM17_UP DMA1_CH1 +#endif + +#define DEF_TIM_DMA__TIM8_CH3 DMA2_CH1 +#define DEF_TIM_DMA__TIM8_UP DMA2_CH1 +#define DEF_TIM_DMA__TIM8_CH4 DMA2_CH2 +#define DEF_TIM_DMA__TIM8_TRIG DMA2_CH2 +#define DEF_TIM_DMA__TIM8_COM DMA2_CH2 +#define DEF_TIM_DMA__TIM8_CH1 DMA2_CH3 +#define DEF_TIM_DMA__TIM8_CH1N DMA2_CH3 +#define DEF_TIM_DMA__TIM8_CH2 DMA2_CH5 +#define DEF_TIM_DMA__TIM8_CH2N DMA2_CH5 + + +#define DMA1_CH1_CHANNEL DMA1_Channel1 +#define DMA1_CH2_CHANNEL DMA1_Channel2 +#define DMA1_CH3_CHANNEL DMA1_Channel3 +#define DMA1_CH4_CHANNEL DMA1_Channel4 +#define DMA1_CH5_CHANNEL DMA1_Channel5 +#define DMA1_CH6_CHANNEL DMA1_Channel6 +#define DMA1_CH7_CHANNEL DMA1_Channel7 +#define DMA2_CH1_CHANNEL DMA2_Channel1 +#define DMA2_CH2_CHANNEL DMA2_Channel2 +#define DMA2_CH3_CHANNEL DMA2_Channel3 +#define DMA2_CH4_CHANNEL DMA2_Channel4 +#define DMA2_CH5_CHANNEL DMA2_Channel5 +#define DMA2_CH6_CHANNEL DMA2_Channel6 +#define DMA2_CH7_CHANNEL DMA2_Channel7 + +#define GPIO_AF(p, t) CONCAT(GPIO_AF__, p, _, t) + +#define GPIO_AF__PA0_TIM2_CH1 GPIO_AF_1 +#define GPIO_AF__PA1_TIM2_CH2 GPIO_AF_1 +#define GPIO_AF__PA2_TIM2_CH3 GPIO_AF_1 +#define GPIO_AF__PA3_TIM2_CH3 GPIO_AF_1 +#define GPIO_AF__PA5_TIM2_CH1 GPIO_AF_1 +#define GPIO_AF__PA6_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PA7_TIM17_CH1 GPIO_AF_1 +#define GPIO_AF__PA12_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PA13_TIM16_CH1N GPIO_AF_1 +#define GPIO_AF__PA15_TIM2_CH1 GPIO_AF_1 + +#define GPIO_AF__PA4_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PA6_TIM3_CH1 GPIO_AF_2 +#define GPIO_AF__PA7_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PA15_TIM8_CH1 GPIO_AF_2 + +#define GPIO_AF__PA7_TIM8_CH1N GPIO_AF_4 + +#define GPIO_AF__PA14_TIM4_CH2 GPIO_AF_5 + +#define GPIO_AF__PA7_TIM1_CH1N GPIO_AF_6 +#define GPIO_AF__PA8_TIM1_CH1 GPIO_AF_6 +#define GPIO_AF__PA9_TIM1_CH2 GPIO_AF_6 +#define GPIO_AF__PA10_TIM1_CH3 GPIO_AF_6 +#define GPIO_AF__PA11_TIM1_CH1N GPIO_AF_6 +#define GPIO_AF__PA12_TIM1_CH2N GPIO_AF_6 + +#define GPIO_AF__PA1_TIM15_CH1N GPIO_AF_9 +#define GPIO_AF__PA2_TIM15_CH1 GPIO_AF_9 +#define GPIO_AF__PA3_TIM15_CH2 GPIO_AF_9 + +#define GPIO_AF__PA9_TIM2_CH3 GPIO_AF_10 +#define GPIO_AF__PA10_TIM2_CH4 GPIO_AF_10 +#define GPIO_AF__PA11_TIM4_CH1 GPIO_AF_10 +#define GPIO_AF__PA12_TIM4_CH2 GPIO_AF_10 +#define GPIO_AF__PA13_TIM4_CH3 GPIO_AF_10 +#define GPIO_AF__PA11_TIM1_CH4 GPIO_AF_11 + +#define GPIO_AF__PB3_TIM2_CH2 GPIO_AF_1 +#define GPIO_AF__PB4_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PB6_TIM16_CH1N GPIO_AF_1 +#define GPIO_AF__PB7_TIM17_CH1N GPIO_AF_1 +#define GPIO_AF__PB8_TIM16_CH1 GPIO_AF_1 +#define GPIO_AF__PB9_TIM17_CH1 GPIO_AF_1 +#define GPIO_AF__PB10_TIM2_CH3 GPIO_AF_1 +#define GPIO_AF__PB11_TIM2_CH4 GPIO_AF_1 +#define GPIO_AF__PB14_TIM15_CH1 GPIO_AF_1 +#define GPIO_AF__PB15_TIM15_CH2 GPIO_AF_1 + +#define GPIO_AF__PB0_TIM3_CH3 GPIO_AF_2 +#define GPIO_AF__PB1_TIM3_CH4 GPIO_AF_2 +#define GPIO_AF__PB4_TIM3_CH1 GPIO_AF_2 +#define GPIO_AF__PB5_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PB6_TIM4_CH1 GPIO_AF_2 +#define GPIO_AF__PB7_TIM4_CH2 GPIO_AF_2 +#define GPIO_AF__PB8_TIM4_CH3 GPIO_AF_2 +#define GPIO_AF__PB9_TIM4_CH4 GPIO_AF_2 +#define GPIO_AF__PB15_TIM15_CH1N GPIO_AF_2 + +#define GPIO_AF__PB0_TIM8_CH2N GPIO_AF_4 +#define GPIO_AF__PB1_TIM8_CH3N GPIO_AF_4 +#define GPIO_AF__PB3_TIM8_CH1N GPIO_AF_4 +#define GPIO_AF__PB4_TIM8_CH2N GPIO_AF_4 +#define GPIO_AF__PB15_TIM1_CH3N GPIO_AF_4 + +#define GPIO_AF__PB6_TIM8_CH1 GPIO_AF_5 + +#define GPIO_AF__PB0_TIM1_CH2N GPIO_AF_6 +#define GPIO_AF__PB1_TIM1_CH3N GPIO_AF_6 +#define GPIO_AF__PB13_TIM1_CH1N GPIO_AF_6 +#define GPIO_AF__PB14_TIM1_CH2N GPIO_AF_6 + +#define GPIO_AF__PB5_TIM17_CH1 GPIO_AF_10 +#define GPIO_AF__PB7_TIM3_CH4 GPIO_AF_10 +#define GPIO_AF__PB8_TIM8_CH2 GPIO_AF_10 +#define GPIO_AF__PB9_TIM8_CH3 GPIO_AF_10 + +#define GPIO_AF__PC6_TIM3_CH1 GPIO_AF_2 +#define GPIO_AF__PC7_TIM3_CH2 GPIO_AF_2 +#define GPIO_AF__PC8_TIM3_CH3 GPIO_AF_2 +#define GPIO_AF__PC9_TIM3_CH4 GPIO_AF_2 + +#define GPIO_AF__PC6_TIM8_CH1 GPIO_AF_4 +#define GPIO_AF__PC7_TIM8_CH2 GPIO_AF_4 +#define GPIO_AF__PC8_TIM8_CH3 GPIO_AF_4 +#define GPIO_AF__PC9_TIM8_CH4 GPIO_AF_4 + +#define GPIO_AF__PC10_TIM8_CH1N GPIO_AF_4 +#define GPIO_AF__PC11_TIM8_CH2N GPIO_AF_4 +#define GPIO_AF__PC12_TIM8_CH3N GPIO_AF_4 +#define GPIO_AF__PC13_TIM8_CH1N GPIO_AF_4 + +#define GPIO_AF__PD3_TIM2_CH1 GPIO_AF_2 +#define GPIO_AF__PD4_TIM2_CH2 GPIO_AF_2 +#define GPIO_AF__PD6_TIM2_CH4 GPIO_AF_2 +#define GPIO_AF__PD7_TIM2_CH3 GPIO_AF_2 + +#define GPIO_AF__PD12_TIM4_CH1 GPIO_AF_2 +#define GPIO_AF__PD13_TIM4_CH2 GPIO_AF_2 +#define GPIO_AF__PD14_TIM4_CH3 GPIO_AF_2 +#define GPIO_AF__PD15_TIM4_CH4 GPIO_AF_2 + +#define GPIO_AF__PD1_TIM8_CH4 GPIO_AF_4 + +#elif defined(STM32F4) + +#define DEF_TIM(tim, chan, pin, flags, out, dmaopt) {\ + tim,\ + IO_TAG(pin),\ + EXPAND(DEF_CHAN_ ## chan),\ + flags,\ + (DEF_CHAN_ ## chan ## _OUTPUT | out),\ + EXPAND(GPIO_AF_## tim),\ + CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _STREAM),\ + EXPAND(DEF_TIM_DMA_CHN_ ## dmaopt ## __ ## tim ## _ ## chan),\ + CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _HANDLER)\ + } + +#define DEF_DMA_CHANNEL(tim, chan, dmaopt) EXPAND(DEF_TIM_DMA_CHN_ ## dmaopt ## __ ## tim ## _ ## chan) +#define DEF_DMA_STREAM(tim, chan, dmaopt) CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _STREAM) +#define DEF_DMA_HANDLER(tim, chan, dmaopt) CONCAT(EXPAND(DEF_TIM_DMA_STR_ ## dmaopt ## __ ## tim ## _ ## chan), _HANDLER) + +/* F4 Stream Mappings */ + +#define DEF_TIM_DMA_STR_0__TIM1_CH1 DMA2_ST6 +#define DEF_TIM_DMA_STR_1__TIM1_CH1 DMA2_ST1 +#define DEF_TIM_DMA_STR_2__TIM1_CH1 DMA2_ST3 +#define DEF_TIM_DMA_STR_0__TIM1_CH2 DMA2_ST6 +#define DEF_TIM_DMA_STR_1__TIM1_CH2 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM1_CH3 DMA2_ST6 +#define DEF_TIM_DMA_STR_1__TIM1_CH3 DMA2_ST6 +#define DEF_TIM_DMA_STR_0__TIM1_CH4 DMA2_ST4 + +#define DEF_TIM_DMA_STR_0__TIM2_CH1 DMA1_ST5 +#define DEF_TIM_DMA_STR_0__TIM2_CH2 DMA1_ST6 +#define DEF_TIM_DMA_STR_0__TIM2_CH3 DMA1_ST1 +#define DEF_TIM_DMA_STR_0__TIM2_CH4 DMA1_ST7 +#define DEF_TIM_DMA_STR_1__TIM2_CH4 DMA1_ST6 + +#define DEF_TIM_DMA_STR_0__TIM3_CH1 DMA1_ST4 +#define DEF_TIM_DMA_STR_0__TIM3_CH2 DMA1_ST5 +#define DEF_TIM_DMA_STR_0__TIM3_CH3 DMA1_ST7 +#define DEF_TIM_DMA_STR_0__TIM3_CH4 DMA1_ST2 + +#define DEF_TIM_DMA_STR_0__TIM4_CH1 DMA1_ST0 +#define DEF_TIM_DMA_STR_0__TIM4_CH2 DMA1_ST4 +#define DEF_TIM_DMA_STR_0__TIM4_CH3 DMA1_ST7 +#define DEF_TIM_DMA_STR_0__TIM4_CH4 DMA1_ST3 + +#define DEF_TIM_DMA_STR_0__TIM5_CH1 DMA1_ST2 +#define DEF_TIM_DMA_STR_0__TIM5_CH2 DMA1_ST4 +#define DEF_TIM_DMA_STR_0__TIM5_CH3 DMA1_ST0 +#define DEF_TIM_DMA_STR_0__TIM5_CH4 DMA1_ST1 +#define DEF_TIM_DMA_STR_1__TIM5_CH4 DMA1_ST3 + +#define DEF_TIM_DMA_STR_0__TIM8_CH1 DMA2_ST2 +#define DEF_TIM_DMA_STR_1__TIM8_CH1 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM8_CH2 DMA2_ST3 +#define DEF_TIM_DMA_STR_1__TIM8_CH2 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM8_CH3 DMA2_ST2 +#define DEF_TIM_DMA_STR_0__TIM8_CH4 DMA2_ST7 + +#define DEF_TIM_DMA_STR_0__TIM9_CH1 DMA_NONE +#define DEF_TIM_DMA_STR_0__TIM9_CH2 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM10_CH1 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM11_CH1 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM12_CH1 DMA_NONE +#define DEF_TIM_DMA_STR_0__TIM12_CH2 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM13_CH1 DMA_NONE + +#define DEF_TIM_DMA_STR_0__TIM14_CH1 DMA_NONE + +/* F4 Channel Mappings */ + +#define DEF_TIM_DMA_CHN_0__TIM1_CH1 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM1_CH1 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_2__TIM1_CH1 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH2 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM1_CH2 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH3 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM1_CH3 DMA_Channel_6 +#define DEF_TIM_DMA_CHN_0__TIM1_CH4 DMA_Channel_6 + +#define DEF_TIM_DMA_CHN_0__TIM2_CH1 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH2 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH3 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM2_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_1__TIM2_CH4 DMA_Channel_3 + +#define DEF_TIM_DMA_CHN_0__TIM3_CH1 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH2 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH3 DMA_Channel_5 +#define DEF_TIM_DMA_CHN_0__TIM3_CH4 DMA_Channel_5 + +#define DEF_TIM_DMA_CHN_0__TIM4_CH1 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH2 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH3 DMA_Channel_2 +#define DEF_TIM_DMA_CHN_0__TIM4_CH4 DMA_Channel_2 + +#define DEF_TIM_DMA_CHN_0__TIM5_CH1 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH2 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH3 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_0__TIM5_CH4 DMA_Channel_3 +#define DEF_TIM_DMA_CHN_1__TIM5_CH4 DMA_Channel_3 + +#define DEF_TIM_DMA_CHN_0__TIM8_CH1 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM8_CH1 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0__TIM8_CH2 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_1__TIM8_CH2 DMA_Channel_7 +#define DEF_TIM_DMA_CHN_0__TIM8_CH3 DMA_Channel_0 +#define DEF_TIM_DMA_CHN_0__TIM8_CH4 DMA_Channel_7 + +#define DEF_TIM_DMA_CHN_0__TIM9_CH1 0 +#define DEF_TIM_DMA_CHN_0__TIM9_CH2 0 + +#define DEF_TIM_DMA_CHN_0__TIM10_CH1 0 + +#define DEF_TIM_DMA_CHN_0__TIM11_CH1 0 + +#define DEF_TIM_DMA_CHN_0__TIM12_CH1 0 +#define DEF_TIM_DMA_CHN_0__TIM12_CH2 0 + +#define DEF_TIM_DMA_CHN_0__TIM13_CH1 0 + +#define DEF_TIM_DMA_CHN_0__TIM14_CH1 0 + +#define DMA1_ST0_STREAM DMA1_Stream0 +#define DMA1_ST1_STREAM DMA1_Stream1 +#define DMA1_ST2_STREAM DMA1_Stream2 +#define DMA1_ST3_STREAM DMA1_Stream3 +#define DMA1_ST4_STREAM DMA1_Stream4 +#define DMA1_ST5_STREAM DMA1_Stream5 +#define DMA1_ST6_STREAM DMA1_Stream6 +#define DMA1_ST7_STREAM DMA1_Stream7 +#define DMA2_ST0_STREAM DMA2_Stream0 +#define DMA2_ST1_STREAM DMA2_Stream1 +#define DMA2_ST2_STREAM DMA2_Stream2 +#define DMA2_ST3_STREAM DMA2_Stream3 +#define DMA2_ST4_STREAM DMA2_Stream4 +#define DMA2_ST5_STREAM DMA2_Stream5 +#define DMA2_ST6_STREAM DMA2_Stream6 +#define DMA2_ST7_STREAM DMA2_Stream7 + +#endif + +/**** Common Defines across all targets ****/ +#define DMA_NONE_CHANNEL NULL +#define DMA_NONE_STREAM NULL + + +#define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan +#define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out ) + +#define DMA_NONE_HANDLER 0 + +#define DEF_CHAN_CH1 TIM_Channel_1 +#define DEF_CHAN_CH2 TIM_Channel_2 +#define DEF_CHAN_CH3 TIM_Channel_3 +#define DEF_CHAN_CH4 TIM_Channel_4 +#define DEF_CHAN_CH1N TIM_Channel_1 +#define DEF_CHAN_CH2N TIM_Channel_2 +#define DEF_CHAN_CH3N TIM_Channel_3 +#define DEF_CHAN_CH4N TIM_Channel_4 + +#define DEF_CHAN_CH1_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH2_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH3_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH4_OUTPUT TIMER_OUTPUT_NONE +#define DEF_CHAN_CH1N_OUTPUT TIMER_OUTPUT_N_CHANNEL +#define DEF_CHAN_CH2N_OUTPUT TIMER_OUTPUT_N_CHANNEL +#define DEF_CHAN_CH3N_OUTPUT TIMER_OUTPUT_N_CHANNEL +#define DEF_CHAN_CH4N_OUTPUT TIMER_OUTPUT_N_CHANNEL diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 05c01a5210..d643ed1285 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -208,7 +208,7 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { - for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; } @@ -216,6 +216,16 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } +uint8_t timerInputIrq(TIM_TypeDef *tim) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].inputIrq; + } + } + return 0; +} + void timerNVICConfigure(uint8_t irq) { HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER)); @@ -285,9 +295,11 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui configTimeBase(timerHardwarePtr->tim, period, mhz); HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle); - timerNVICConfigure(timerHardwarePtr->irq); + + uint8_t irq = timerInputIrq(timerHardwarePtr->tim); + timerNVICConfigure(irq); // HACK - enable second IRQ on timers that need it - switch(timerHardwarePtr->irq) { + switch(irq) { case TIM1_CC_IRQn: timerNVICConfigure(TIM1_UP_TIM10_IRQn); @@ -300,7 +312,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui } // 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) +void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq) { uint8_t timerIndex = lookupTimerIndex(timHw->tim); if (timerIndex >= USED_TIMER_COUNT) { @@ -320,8 +332,8 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori 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); + HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); + HAL_NVIC_EnableIRQ(irq); timerInfo[timer].priority = irqPriority; } @@ -787,14 +799,14 @@ void timerInit(void) #endif /* enable the timer peripherals */ - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int 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++) { + for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif @@ -856,17 +868,29 @@ void timerForceOverflow(TIM_TypeDef *tim) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (flag && (timerHardware[i].output & flag) == flag) { + if (timerHardware[i].output & 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; } + +uint16_t timerDmaSource(uint8_t channel) +{ + switch (channel) { + case TIM_CHANNEL_1: + return TIM_DMA_ID_CC1; + case TIM_CHANNEL_2: + return TIM_DMA_ID_CC2; + case TIM_CHANNEL_3: + return TIM_DMA_ID_CC3; + case TIM_CHANNEL_4: + return TIM_DMA_ID_CC4; + } + return 0; +} diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 6265d7fd77..ec3e2ed5ba 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -17,16 +17,16 @@ #include "timer.h" 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 = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM15, .rcc = RCC_APB2(TIM15) }, - { .TIMx = TIM16, .rcc = RCC_APB2(TIM16) }, - { .TIMx = TIM17, .rcc = RCC_APB2(TIM17) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn }, + { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), .inputIrq = TIM1_BRK_TIM15_IRQn }, + { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), .inputIrq = TIM1_UP_TIM16_IRQn }, + { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), .inputIrq = TIM1_TRG_COM_TIM17_IRQn }, }; uint8_t timerClockDivisor(TIM_TypeDef *tim) diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index da13ae77f3..6e0de79c44 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -42,20 +42,24 @@ #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) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn}, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn}, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn}, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn}, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), .inputIrq = TIM5_IRQn}, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0}, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0}, +#ifndef STM32F411xE + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn}, +#endif + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), .inputIrq = TIM1_BRK_TIM9_IRQn}, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), .inputIrq = TIM1_UP_TIM10_IRQn}, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), .inputIrq = TIM1_TRG_COM_TIM11_IRQn}, +#ifndef STM32F411xE + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn}, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn}, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn}, +#endif }; /* diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c index 180116c0fd..7f0077dca0 100644 --- a/src/main/drivers/timer_stm32f7xx.c +++ b/src/main/drivers/timer_stm32f7xx.c @@ -42,20 +42,20 @@ #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) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn}, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn}, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn}, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn}, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), .inputIrq = TIM5_IRQn}, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0}, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0}, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn}, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), .inputIrq = TIM1_BRK_TIM9_IRQn}, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), .inputIrq = TIM1_UP_TIM10_IRQn}, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), .inputIrq = TIM1_TRG_COM_TIM11_IRQn}, + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn}, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn}, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn}, }; /* @@ -96,6 +96,7 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { uint8_t timerClockDivisor(TIM_TypeDef *tim) { + UNUSED(tim); return 1; } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c6a77b96a3..bfad9682e7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -26,6 +26,8 @@ #include "blackbox/blackbox_io.h" +#include "cms/cms.h" + #include "common/color.h" #include "common/axis.h" #include "common/maths.h" @@ -42,6 +44,7 @@ #include "drivers/pwm_output.h" #include "drivers/max7456.h" #include "drivers/sound_beeper.h" +#include "drivers/light_ws2811strip.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -238,11 +241,39 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; } +#ifdef LED_STRIP +void resetLedStripConfig(ledStripConfig_t *ledStripConfig) +{ + applyDefaultColors(ledStripConfig->colors); + applyDefaultLedStripConfig(ledStripConfig->ledConfigs); + applyDefaultModeColors(ledStripConfig->modeColors); + applyDefaultSpecialColors(&(ledStripConfig->specialColors)); + ledStripConfig->ledstrip_visual_beeper = 0; + ledStripConfig->ledstrip_aux_channel = THROTTLE; + + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if (timerHardware[i].usageFlags & TIM_USE_LED) { + ledStripConfig->ioTag = timerHardware[i].tag; + return; + } + } + ledStripConfig->ioTag = IO_TAG_NONE; +} +#endif + #ifdef USE_SERVOS void resetServoConfig(servoConfig_t *servoConfig) { servoConfig->servoCenterPulse = 1500; servoConfig->servoPwmRate = 50; + + int servoIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_SERVO) { + servoConfig->ioTags[servoIndex] = timerHardware[i].tag; + servoIndex++; + } + } } #endif @@ -262,9 +293,9 @@ void resetMotorConfig(motorConfig_t *motorConfig) motorConfig->mincommand = 1000; motorConfig->digitalIdleOffset = 40; - uint8_t motorIndex = 0; - for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { - if ((timerHardware[i].output & TIMER_OUTPUT_ENABLED) == TIMER_OUTPUT_ENABLED) { + int motorIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { motorConfig->ioTags[motorIndex] = timerHardware[i].tag; motorIndex++; } @@ -304,7 +335,7 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) ppmConfig->ioTag = IO_TAG(PPM_PIN); #else for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + if (timerHardware[i].usageFlags & TIM_USE_PPM) { ppmConfig->ioTag = timerHardware[i].tag; return; } @@ -316,9 +347,9 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) void resetPwmConfig(pwmConfig_t *pwmConfig) { - uint8_t inputIndex = 0; + int inputIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && inputIndex < PWM_INPUT_PORT_COUNT; i++) { - if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + if (timerHardware[i].usageFlags & TIM_USE_PWM) { pwmConfig->ioTags[inputIndex] = timerHardware[i].tag; inputIndex++; } @@ -467,7 +498,7 @@ void createDefaultConfig(master_t *config) #ifdef OSD intFeatureSet(FEATURE_OSD, config); - resetOsdConfig(&config->osdProfile); + osdResetConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER @@ -589,6 +620,10 @@ void createDefaultConfig(master_t *config) #endif resetFlight3DConfig(&config->flight3DConfig); +#ifdef LED_STRIP + resetLedStripConfig(&config->ledStripConfig); +#endif + #ifdef GPS // gps/nav stuff config->gpsConfig.provider = GPS_NMEA; @@ -658,14 +693,6 @@ void createDefaultConfig(master_t *config) config->customMotorMixer[i].throttle = 0.0f; } -#ifdef LED_STRIP - applyDefaultColors(config->colors); - applyDefaultLedStripConfig(config->ledConfigs); - applyDefaultModeColors(config->modeColors); - applyDefaultSpecialColors(&(config->specialColors)); - config->ledstrip_visual_beeper = 0; -#endif - #ifdef VTX config->vtx_band = 4; //Fatshark/Airwaves config->vtx_channel = 1; //CH1 @@ -867,21 +894,6 @@ void validateAndFixConfig(void) } #endif -#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) - if (featureConfigured(FEATURE_SOFTSERIAL) && ( - 0 -#ifdef USE_SOFTSERIAL1 - || (WS2811_TIMER == SOFTSERIAL_1_TIMER) -#endif -#ifdef USE_SOFTSERIAL2 - || (WS2811_TIMER == SOFTSERIAL_2_TIMER) -#endif - )) { - // led strip needs the same timer as softserial - featureClear(FEATURE_LED_STRIP); - } -#endif - #if defined(NAZE) && defined(SONAR) if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); @@ -895,18 +907,11 @@ void validateAndFixConfig(void) #endif #if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3) - if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) { - featureClear(FEATURE_DISPLAY); + if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) { + featureClear(FEATURE_DASHBOARD); } #endif -/*#if defined(LED_STRIP) && defined(TRANSPONDER) // TODO - Add transponder feature - if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) { - featureClear(FEATURE_LED_STRIP); - } -#endif -*/ - #if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO) // shared pin if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) { diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 1432b6882b..e6cc491229 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -45,7 +45,7 @@ typedef enum { FEATURE_RX_MSP = 1 << 14, FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, - FEATURE_DISPLAY = 1 << 17, + FEATURE_DASHBOARD = 1 << 17, FEATURE_OSD = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bfeaf319ff..d5aedd7a15 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -62,9 +62,7 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/osd.h" #include "io/serial_4way.h" -#include "io/vtx.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -648,10 +646,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, 0); continue; } - if (isMotorProtocolDshot()) - sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator - else - sbufWriteU16(dst, motor[i]); + + sbufWriteU16(dst, convertMotorToExternal(motor[i])); } break; case MSP_RC: @@ -934,7 +930,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #ifdef LED_STRIP case MSP_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; sbufWriteU16(dst, color->h); sbufWriteU8(dst, color->s); sbufWriteU8(dst, color->v); @@ -943,7 +939,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_LED_STRIP_CONFIG: for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; sbufWriteU32(dst, *ledConfig); } break; @@ -953,15 +949,20 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn for (int j = 0; j < LED_DIRECTION_COUNT; j++) { sbufWriteU8(dst, i); sbufWriteU8(dst, j); - sbufWriteU8(dst, masterConfig.modeColors[i].color[j]); + sbufWriteU8(dst, masterConfig.ledStripConfig.modeColors[i].color[j]); } } for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { sbufWriteU8(dst, LED_MODE_COUNT); sbufWriteU8(dst, j); - sbufWriteU8(dst, masterConfig.specialColors.color[j]); + sbufWriteU8(dst, masterConfig.ledStripConfig.specialColors.color[j]); } + + sbufWriteU8(dst, LED_AUX_CHANNEL); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, masterConfig.ledStripConfig.ledstrip_aux_channel); + break; #endif @@ -1009,7 +1010,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, masterConfig.osdProfile.time_alarm); sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm); - for (i = 0; i < OSD_MAX_ITEMS; i++) { + for (i = 0; i < OSD_ITEM_COUNT; i++) { sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]); } #else @@ -1320,8 +1321,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) 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] = sbufReadU16(src); + for (i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 + motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src)); + } break; case MSP_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS @@ -1652,7 +1654,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef LED_STRIP case MSP_SET_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; color->h = sbufReadU16(src); color->s = sbufReadU8(src); color->v = sbufReadU8(src); @@ -1666,7 +1668,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; } - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; *ledConfig = sbufReadU32(src); reevaluateLedConfig(); } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b81d36ce01..2ffbdc91ec 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -21,6 +21,8 @@ #include +#include "cms/cms.h" + #include "common/axis.h" #include "common/color.h" #include "common/utils.h" @@ -41,7 +43,7 @@ #include "flight/altitudehold.h" #include "io/beeper.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" @@ -222,11 +224,11 @@ static void taskCalculateAltitude(uint32_t currentTime) }} #endif -#ifdef DISPLAY -static void taskUpdateDisplay(uint32_t currentTime) +#ifdef USE_DASHBOARD +static void taskUpdateDashboard(uint32_t currentTime) { - if (feature(FEATURE_DISPLAY)) { - displayUpdate(currentTime); + if (feature(FEATURE_DASHBOARD)) { + dashboardUpdate(currentTime); } } #endif @@ -264,7 +266,7 @@ static void taskTransponder(uint32_t currentTime) static void taskUpdateOsd(uint32_t currentTime) { if (feature(FEATURE_OSD)) { - updateOsd(currentTime); + osdUpdate(currentTime); } } #endif @@ -307,8 +309,8 @@ void fcTasksInit(void) #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif -#ifdef DISPLAY - setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); +#ifdef USE_DASHBOARD + setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); @@ -329,6 +331,13 @@ void fcTasksInit(void) #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +#ifdef CMS +#ifdef USE_MSP_DISPLAYPORT + setTaskEnabled(TASK_CMS, true); +#else + setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); +#endif +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -446,10 +455,10 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef DISPLAY - [TASK_DISPLAY] = { - .taskName = "DISPLAY", - .taskFunc = taskUpdateDisplay, +#ifdef USE_DASHBOARD + [TASK_DASHBOARD] = { + .taskName = "DASHBOARD", + .taskFunc = taskUpdateDashboard, .desiredPeriod = 1000000 / 10, .staticPriority = TASK_PRIORITY_LOW, }, @@ -488,4 +497,13 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif + +#ifdef CMS + [TASK_CMS] = { + .taskName = "CMS", + .taskFunc = cmsHandler, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif }; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 316d5178b2..91123768e3 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -44,7 +44,7 @@ #include "io/beeper.h" #include "io/motors.h" #include "io/vtx.h" -#include "io/display.h" +#include "io/dashboard.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -291,13 +291,13 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } -#ifdef DISPLAY +#ifdef USE_DASHBOARD if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { - displayDisablePageCycling(); + dashboardDisablePageCycling(); } if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { - displayEnablePageCycling(); + dashboardEnablePageCycling(); } #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 7846b91714..e023eed8af 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -47,6 +47,13 @@ #include "fc/runtime_config.h" #include "config/feature.h" +#include "config/config_master.h" + +#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 +// (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) +#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 +#define EXTERNAL_CONVERSION_MIN_VALUE 1000 +#define EXTERNAL_CONVERSION_MAX_VALUE 2000 uint8_t motorCount; @@ -234,21 +241,26 @@ static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, d static float rcCommandThrottleRange; bool isMotorProtocolDshot(void) { +#ifdef USE_DSHOT if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) return true; else +#endif return false; } // Add here scaled ESC outputs for digital protol void initEscEndpoints(void) { +#ifdef USE_DSHOT if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; maxMotorOutputNormal = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH; - deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; - } else { + deadbandMotor3dHigh = DSHOT_3D_MIN_NEGATIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dLow = DSHOT_3D_MAX_POSITIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes + } else +#endif + { disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; minMotorOutputNormal = motorConfig->minthrottle; maxMotorOutputNormal = motorConfig->maxthrottle; @@ -359,7 +371,7 @@ void mixerResetDisarmedMotors(void) int i; // set disarmed motor values for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) - motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand; + motor_disarmed[i] = disarmMotorOutput; } void writeMotors(void) @@ -373,7 +385,7 @@ void writeMotors(void) } } -void writeAllMotors(int16_t mc) +static void writeAllMotors(int16_t mc) { // Sends commands to all motors for (uint8_t i = 0; i < motorCount; i++) { @@ -385,7 +397,7 @@ void writeAllMotors(int16_t mc) void stopMotors(void) { - writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand); + writeAllMotors(disarmMotorOutput); delay(50); // give the timers and ESCs a chance to react. } @@ -440,9 +452,10 @@ void mixTable(pidProfile_t *pidProfile) throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f); throttleRange = motorOutputMax - motorOutputMin; + float scaledAxisPIDf[3]; // Limit the PIDsum for (int axis = 0; axis < 3; axis++) - axisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); // Calculate voltage compensation if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); @@ -450,9 +463,9 @@ void mixTable(pidProfile_t *pidProfile) // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { motorMix[i] = - axisPIDf[PITCH] * currentMixer[i].pitch + - axisPIDf[ROLL] * currentMixer[i].roll + - -mixerConfig->yaw_motor_direction * axisPIDf[YAW] * currentMixer[i].yaw; + scaledAxisPIDf[PITCH] * currentMixer[i].pitch + + scaledAxisPIDf[ROLL] * currentMixer[i].roll + + -mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw; if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation @@ -511,14 +524,31 @@ void mixTable(pidProfile_t *pidProfile) // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { - if (isMotorProtocolDshot()) { - motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range - - if (motor_disarmed[i] != disarmMotorOutput) - motor[i] = (motor_disarmed[i] - 1000) * 2; // TODO - Perhaps needs rescaling as it will never reach 2047 during motor tests - } else { - motor[i] = motor_disarmed[i]; - } + motor[i] = motor_disarmed[i]; } } } + +uint16_t convertExternalToMotor(uint16_t externalValue) +{ + uint16_t motorValue = externalValue; +#ifdef USE_DSHOT + if (isMotorProtocolDshot()) { + motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); + } +#endif + + return motorValue; +} + +uint16_t convertMotorToExternal(uint16_t motorValue) +{ + uint16_t externalValue = motorValue; +#ifdef USE_DSHOT + if (isMotorProtocolDshot()) { + externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain((motorValue / EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE); + } +#endif + + return externalValue; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1a4949a3cc..b329128b80 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -21,12 +21,30 @@ #define QUAD_MOTOR_COUNT 4 +/* + DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings + 0 = stop + 1-5: beep + 6: ESC info request (FW Version and SN sent over the tlm wire) + 7: spin direction 1 + 8: spin direction 2 + 9: 3d mode off + 10: 3d mode on + 11: ESC settings request (saved settings over the TLM wire) + 12: save Settings + + 3D Mode: + 0 = stop + 48 (low) - 1047 (high) -> positive direction + 1048 (low) - 2047 (high) -> negative direction +*/ + // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 #define DSHOT_MAX_THROTTLE 2047 -#define DSHOT_3D_DEADBAND_LOW 900 // TODO - not agreed yet -#define DSHOT_3D_DEADBAND_HIGH 1100 // TODO - not agreed yet +#define DSHOT_3D_MAX_POSITIVE 1047 // TODO - Not working yet!! Mixer requires some throttle rescaling changes +#define DSHOT_3D_MIN_NEGATIVE 1048// TODO - Not working yet!! Mixer requires some throttle rescaling changes // Note: this is called MultiType/MULTITYPE_* in baseflight. typedef enum mixerMode @@ -104,7 +122,6 @@ void mixerUseConfigs( airplaneConfig_t *airplaneConfigToUse, struct rxConfig_s *rxConfigToUse); -void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); @@ -117,4 +134,7 @@ void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); void stopPwmAllMotors(void); + bool isMotorProtocolDshot(void); +uint16_t convertExternalToMotor(uint16_t externalValue); +uint16_t convertMotorToExternal(uint16_t motorValue); diff --git a/src/main/io/display.c b/src/main/io/dashboard.c similarity index 94% rename from src/main/io/display.c rename to src/main/io/dashboard.c index 1d7c88199a..932af2e859 100644 --- a/src/main/io/display.c +++ b/src/main/io/dashboard.c @@ -21,7 +21,9 @@ #include "platform.h" -#ifdef DISPLAY +#ifdef USE_DASHBOARD + +#include "common/utils.h" #include "build/version.h" #include "build/debug.h" @@ -29,8 +31,11 @@ #include "build/build_config.h" #include "drivers/system.h" +#include "drivers/display.h" #include "drivers/display_ug2864hsweg01.h" +#include "cms/cms.h" + #include "common/printf.h" #include "common/maths.h" #include "common/axis.h" @@ -50,6 +55,8 @@ #include "flight/imu.h" #include "flight/failsafe.h" +#include "io/displayport_oled.h" + #ifdef GPS #include "io/gps.h" #include "flight/navigation.h" @@ -58,7 +65,7 @@ #include "config/feature.h" #include "config/config_profile.h" -#include "io/display.h" +#include "io/dashboard.h" #include "rx/rx.h" @@ -74,9 +81,10 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex); #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5) static uint32_t nextDisplayUpdateAt = 0; -static bool displayPresent = false; +static bool dashboardPresent = false; static rxConfig_t *rxConfig; +static displayPort_t *displayPort; #define PAGE_TITLE_LINE_COUNT 1 @@ -98,7 +106,7 @@ static const char* const pageTitles[] = { #ifdef GPS ,"GPS" #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE ,"DEBUG" #endif }; @@ -116,7 +124,7 @@ const pageId_e cyclePageIds[] = { #ifndef SKIP_TASK_STATISTICS ,PAGE_TASKS #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE ,PAGE_DEBUG, #endif }; @@ -144,7 +152,7 @@ typedef struct pageState_s { static pageState_t pageState; void resetDisplay(void) { - displayPresent = ug2864hsweg01InitI2C(); + dashboardPresent = ug2864hsweg01InitI2C(); } void LCDprint(uint8_t i) { @@ -562,7 +570,7 @@ void showTasksPage(void) } #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE void showDebugPage(void) { @@ -577,10 +585,16 @@ void showDebugPage(void) } #endif -void displayUpdate(uint32_t currentTime) +void dashboardUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; +#ifdef CMS + if (displayIsGrabbed(displayPort)) { + return; + } +#endif + const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; if (!updateNow) { return; @@ -623,13 +637,13 @@ void displayUpdate(uint32_t currentTime) // user to power off/on the display or connect it while powered. resetDisplay(); - if (!displayPresent) { + if (!dashboardPresent) { return; } handlePageChange(); } - if (!displayPresent) { + if (!dashboardPresent) { return; } @@ -666,7 +680,7 @@ void displayUpdate(uint32_t currentTime) } break; #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE case PAGE_DEBUG: showDebugPage(); break; @@ -680,53 +694,57 @@ void displayUpdate(uint32_t currentTime) } -void displaySetPage(pageId_e pageId) +void dashboardSetPage(pageId_e pageId) { pageState.pageId = pageId; pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; } -void displayInit(rxConfig_t *rxConfigToUse) +void dashboardInit(rxConfig_t *rxConfigToUse) { delay(200); resetDisplay(); delay(200); + displayPort = displayPortOledInit(); +#if defined(CMS) + cmsDisplayPortRegister(displayPort); +#endif + rxConfig = rxConfigToUse; memset(&pageState, 0, sizeof(pageState)); - displaySetPage(PAGE_WELCOME); + dashboardSetPage(PAGE_WELCOME); - displayUpdate(micros()); + dashboardUpdate(micros()); - displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5)); + dashboardSetNextPageChangeAt(micros() + (1000 * 1000 * 5)); } -void displayShowFixedPage(pageId_e pageId) +void dashboardShowFixedPage(pageId_e pageId) { - displaySetPage(pageId); - displayDisablePageCycling(); + dashboardSetPage(pageId); + dashboardDisablePageCycling(); } -void displaySetNextPageChangeAt(uint32_t futureMicros) +void dashboardSetNextPageChangeAt(uint32_t futureMicros) { pageState.nextPageAt = futureMicros; } -void displayEnablePageCycling(void) +void dashboardEnablePageCycling(void) { pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED; } -void displayResetPageCycling(void) +void dashboardResetPageCycling(void) { pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page } -void displayDisablePageCycling(void) +void dashboardDisablePageCycling(void) { pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } - -#endif +#endif // USE_DASHBOARD diff --git a/src/main/io/display.h b/src/main/io/dashboard.h similarity index 70% rename from src/main/io/display.h rename to src/main/io/dashboard.h index 75abffbe3c..cca02effdf 100644 --- a/src/main/io/display.h +++ b/src/main/io/dashboard.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define ENABLE_DEBUG_OLED_PAGE +#define ENABLE_DEBUG_DASHBOARD_PAGE typedef enum { PAGE_WELCOME, @@ -30,18 +30,18 @@ typedef enum { #ifdef GPS PAGE_GPS, #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE PAGE_DEBUG, #endif } pageId_e; struct rxConfig_s; -void displayInit(struct rxConfig_s *intialRxConfig); -void displayUpdate(uint32_t currentTime); +void dashboardInit(struct rxConfig_s *intialRxConfig); +void dashboardUpdate(uint32_t currentTime); -void displayShowFixedPage(pageId_e pageId); +void dashboardShowFixedPage(pageId_e pageId); -void displayEnablePageCycling(void); -void displayDisablePageCycling(void); -void displayResetPageCycling(void); -void displaySetNextPageChangeAt(uint32_t futureMicros); +void dashboardEnablePageCycling(void); +void dashboardDisablePageCycling(void); +void dashboardResetPageCycling(void); +void dashboardSetNextPageChangeAt(uint32_t futureMicros); diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c new file mode 100644 index 0000000000..0bfbb4231c --- /dev/null +++ b/src/main/io/displayport_max7456.c @@ -0,0 +1,107 @@ +/* + * 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 OSD + +#include "common/utils.h" + +#include "config/config_master.h" + +#include "drivers/display.h" +#include "drivers/max7456.h" + +displayPort_t max7456DisplayPort; // Referenced from osd.c + +extern uint16_t refreshTimeout; + +static int grab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + osdResetAlarms(); + displayPort->isGrabbed = true; + refreshTimeout = 0; + + return 0; +} + +static int release(displayPort_t *displayPort) +{ + UNUSED(displayPort); + displayPort->isGrabbed = false; + + return 0; +} + +static int clearScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + max7456ClearScreen(); + + return 0; +} + +static int write(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + max7456Write(x, y, s); + + return 0; +} + +static void resync(displayPort_t *displayPort) +{ + UNUSED(displayPort); + max7456RefreshAll(); + displayPort->rows = max7456GetRowsCount(); + displayPort->cols = 30; +} + +static int heartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static displayPortVTable_t max7456VTable = { + .grab = grab, + .release = release, + .clear = clearScreen, + .write = write, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree, +}; + +displayPort_t *max7456DisplayPortInit(void) +{ + max7456DisplayPort.vTable = &max7456VTable; + max7456DisplayPort.isGrabbed = false; + resync(&max7456DisplayPort); + return &max7456DisplayPort; +} +#endif // OSD diff --git a/src/main/io/displayport_max7456.h b/src/main/io/displayport_max7456.h new file mode 100644 index 0000000000..bbb4aced35 --- /dev/null +++ b/src/main/io/displayport_max7456.h @@ -0,0 +1,20 @@ +/* + * 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 + +displayPort_t *max7456DisplayPortInit(void); diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c new file mode 100644 index 0000000000..ff16c04c8f --- /dev/null +++ b/src/main/io/displayport_msp.c @@ -0,0 +1,119 @@ +/* + * 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" + +#ifdef USE_MSP_DISPLAYPORT + +#include "common/utils.h" + +#include "drivers/display.h" +#include "drivers/system.h" + +#include "fc/fc_msp.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +static displayPort_t mspDisplayPort; + +static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len) +{ + UNUSED(displayPort); + return mspSerialPush(cmd, buf, len); +} + +static int grab(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 0 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int heartbeat(displayPort_t *displayPort) +{ + return grab(displayPort); // ensure display is not released by MW OSD software +} + +static int release(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 1 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int clear(displayPort_t *displayPort) +{ + const uint8_t subcmd[] = { 2 }; + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string) +{ +#define MSP_OSD_MAX_STRING_LENGTH 30 + uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4]; + + int len = strlen(string); + if (len >= MSP_OSD_MAX_STRING_LENGTH) { + len = MSP_OSD_MAX_STRING_LENGTH; + } + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy(&buf[4], string, len); + + return output(displayPort, MSP_DISPLAYPORT, buf, len + 4); +} + +static void resync(displayPort_t *displayPort) +{ + displayPort->rows = 13; // XXX Will reflect NTSC/PAL in the future + displayPort->cols = 30; +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return mspSerialTxBytesFree(); +} + +static const displayPortVTable_t mspDisplayPortVTable = { + .grab = grab, + .release = release, + .clear = clear, + .write = write, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree +}; + +displayPort_t *displayPortMspInit(void) +{ + mspDisplayPort.vTable = &mspDisplayPortVTable; + mspDisplayPort.isGrabbed = false; + resync(&mspDisplayPort); + return &mspDisplayPort; +} +#endif // USE_MSP_DISPLAYPORT diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h new file mode 100644 index 0000000000..955d0b852c --- /dev/null +++ b/src/main/io/displayport_msp.h @@ -0,0 +1,21 @@ +/* + * 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 + +struct displayPort_s; +struct displayPort_s *displayPortMspInit(void); diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c new file mode 100644 index 0000000000..8bb20329fc --- /dev/null +++ b/src/main/io/displayport_oled.c @@ -0,0 +1,91 @@ +/* + * 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 "common/utils.h" + +#include "drivers/display.h" +#include "drivers/display_ug2864hsweg01.h" + +static displayPort_t oledDisplayPort; + +static int oledGrab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int oledRelease(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int oledClear(displayPort_t *displayPort) +{ + UNUSED(displayPort); + i2c_OLED_clear_display_quick(); + return 0; +} + +static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + i2c_OLED_set_xy(x, y); + i2c_OLED_send_string(s); + return 0; +} + +static int oledHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void oledResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t oledTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static const displayPortVTable_t oledVTable = { + .grab = oledGrab, + .release = oledRelease, + .clear = oledClear, + .write = oledWrite, + .heartbeat = oledHeartbeat, + .resync = oledResync, + .txBytesFree = oledTxBytesFree +}; + +displayPort_t *displayPortOledInit(void) +{ + oledDisplayPort.vTable = &oledVTable; + oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT; + oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT; + oledDisplayPort.isGrabbed = false; + return &oledDisplayPort; +} diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h new file mode 100644 index 0000000000..4daa6de1cd --- /dev/null +++ b/src/main/io/displayport_oled.h @@ -0,0 +1,20 @@ +/* + * 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 + +displayPort_t *displayPortOledInit(void); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index f8e9d880f0..2cd9873baf 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -39,7 +39,7 @@ #include "sensors/sensors.h" #include "io/serial.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/gps.h" #include "flight/gps_conversion.h" @@ -1072,9 +1072,9 @@ static bool gpsNewFrameUBLOX(uint8_t data) static void gpsHandlePassthrough(uint8_t data) { gpsNewData(data); - #ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayUpdate(micros()); + #ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardUpdate(micros()); } #endif @@ -1088,9 +1088,9 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) if(!(gpsPort->mode & MODE_TX)) serialSetMode(gpsPort, gpsPort->mode | MODE_TX); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayShowFixedPage(PAGE_GPS); +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardShowFixedPage(PAGE_GPS); } #endif diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index baeb80c957..9bef7f9cf6 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -62,8 +62,6 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" -#include "io/osd.h" -#include "io/vtx.h" #include "flight/failsafe.h" #include "flight/mixer.h" @@ -88,6 +86,7 @@ PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR static bool ledStripInitialised = false; static bool ledStripEnabled = true; +static ledStripConfig_t * currentLedStripConfig; static void ledStripDisable(void); @@ -170,6 +169,7 @@ static const specialColorIndexes_t defaultSpecialColors[] = { }; static int scaledThrottle; +static int scaledAux; static void updateLedRingCounts(void); @@ -179,7 +179,7 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void) int maxY = 0; for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; maxX = MAX(ledGetX(ledConfig), maxX); maxY = MAX(ledGetY(ledConfig), maxY); @@ -201,7 +201,7 @@ STATIC_UNIT_TESTED void updateLedCount(void) int count = 0, countRing = 0, countScanner= 0; for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (!(*ledConfig)) break; @@ -231,7 +231,7 @@ void reevaluateLedConfig(void) // get specialColor by index static hsvColor_t* getSC(ledSpecialColorIds_e index) { - return &masterConfig.colors[masterConfig.specialColors.color[index]]; + return ¤tLedStripConfig->colors[currentLedStripConfig->specialColors.color[index]]; } static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; @@ -253,9 +253,9 @@ bool parseLedStripConfig(int ledIndex, const char *config) RING_COLORS, PARSE_STATE_COUNT }; - static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; + static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', ':', '\0'}; - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); int x = 0, y = 0, color = 0; // initialize to prevent warnings @@ -374,7 +374,7 @@ typedef enum { static quadrant_e getLedQuadrant(const int ledIndex) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; int x = ledGetX(ledConfig); int y = ledGetY(ledConfig); @@ -416,7 +416,7 @@ static const struct { static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; quadrant_e quad = getLedQuadrant(ledIndex); for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { @@ -424,7 +424,7 @@ static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIn quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) - return &masterConfig.colors[modeColors->color[dir]]; + return ¤tLedStripConfig->colors[modeColors->color[dir]]; } return NULL; } @@ -450,7 +450,7 @@ static const struct { static void applyLedFixedLayers() { for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); int fn = ledGetFunction(ledConfig); @@ -458,13 +458,13 @@ static void applyLedFixedLayers() switch (fn) { case LED_FUNCTION_COLOR: - color = masterConfig.colors[ledGetColor(ledConfig)]; + color = currentLedStripConfig->colors[ledGetColor(ledConfig)]; break; case LED_FUNCTION_FLIGHT_MODE: for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, ¤tLedStripConfig->modeColors[flightModeToLed[i].ledMode]); if (directionalColor) { color = *directionalColor; } @@ -492,7 +492,7 @@ static void applyLedFixedLayers() } if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { - hOffset += ((scaledThrottle - 10) * 4) / 3; + hOffset += scaledAux; } color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); @@ -505,7 +505,7 @@ static void applyLedFixedLayers() static void applyLedHsv(uint32_t mask, const hsvColor_t *color) { for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if ((*ledConfig & mask) == mask) setLedHsv(ledIndex, color); } @@ -701,7 +701,7 @@ static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) } for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { if (getLedQuadrant(ledIndex) & quadrants) setLedHsv(ledIndex, flashColor); @@ -742,7 +742,7 @@ static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) } for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { bool applyColor; @@ -753,7 +753,7 @@ static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) } if (applyColor) { - const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; + const hsvColor_t *ringColor = ¤tLedStripConfig->colors[ledGetColor(ledConfig)]; setLedHsv(ledIndex, ringColor); } @@ -869,7 +869,7 @@ static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig = ¤tLedStripConfig->ledConfigs[ledIndex]; if (ledGetY(ledConfig) == previousRow) { setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); @@ -931,7 +931,7 @@ void ledStripUpdate(uint32_t currentTime) return; } - if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(masterConfig.ledstrip_visual_beeper && isBeeperOn())) { + if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(currentLedStripConfig->ledstrip_visual_beeper && isBeeperOn())) { if (ledStripEnabled) { ledStripDisable(); ledStripEnabled = false; @@ -962,6 +962,7 @@ void ledStripUpdate(uint32_t currentTime) // apply all layers; triggered timed functions has to update timers scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + scaledAux = scaleRange(rcData[currentLedStripConfig->ledstrip_aux_channel], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); applyLedFixedLayers(); @@ -977,7 +978,7 @@ bool parseColor(int index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &masterConfig.colors[index]; + hsvColor_t *color = ¤tLedStripConfig->colors[index]; bool result = true; static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { @@ -1030,11 +1031,15 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) return false; - masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; + currentLedStripConfig->modeColors[modeIndex].color[modeColorIndex] = colorIndex; } else if (modeIndex == LED_SPECIAL) { if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) return false; - masterConfig.specialColors.color[modeColorIndex] = colorIndex; + currentLedStripConfig->specialColors.color[modeColorIndex] = colorIndex; + } else if (modeIndex == LED_AUX_CHANNEL) { + if (modeColorIndex < 0 || modeColorIndex >= 1) + return false; + currentLedStripConfig->ledstrip_aux_channel = colorIndex; } else { return false; } @@ -1092,21 +1097,26 @@ void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) +void ledStripInit(ledStripConfig_t *ledStripConfig) { - ledConfigs = ledConfigsToUse; - colors = colorsToUse; - modeColors = modeColorsToUse; - specialColors = *specialColorsToUse; + currentLedStripConfig = ledStripConfig; + + ledConfigs = currentLedStripConfig->ledConfigs; + colors = currentLedStripConfig->colors; + modeColors = currentLedStripConfig->modeColors; + specialColors = currentLedStripConfig->specialColors; ledStripInitialised = false; } void ledStripEnable(void) { + if (currentLedStripConfig == NULL) { + return; + } reevaluateLedConfig(); ledStripInitialised = true; - ws2811LedStripInit(); + ws2811LedStripInit(currentLedStripConfig->ioTag); } static void ledStripDisable(void) diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 351708abcb..a417509075 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -18,6 +18,7 @@ #pragma once #include "common/color.h" +#include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 #define LED_CONFIGURABLE_COLOR_COUNT 16 @@ -75,7 +76,8 @@ typedef enum { LED_MODE_ANGLE, LED_MODE_MAG, LED_MODE_BARO, - LED_SPECIAL + LED_SPECIAL, + LED_AUX_CHANNEL } ledModeIndex_e; typedef enum { @@ -134,6 +136,15 @@ typedef struct ledCounts_s { uint8_t ringSeqLen; } ledCounts_t; +typedef struct ledStripConfig_s { + ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; + hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; + modeColorIndexes_t modeColors[LED_MODE_COUNT]; + specialColorIndexes_t specialColors; + uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on + uint8_t ledstrip_aux_channel; + ioTag_t ioTag; +} ledStripConfig_t; ledConfig_t *ledConfigs; hsvColor_t *colors; @@ -165,7 +176,7 @@ bool parseLedStripConfig(int ledIndex, const char *config); void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize); void reevaluateLedConfig(void); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripInit(ledStripConfig_t *ledStripConfig); void ledStripEnable(void); void ledStripUpdate(uint32_t currentTime); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c9ad4c1d58..9c677ccf20 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -18,6 +18,8 @@ /* Created by Marcin Baliniak some functions based on MinimOSD + + OSD-CMS separation by jflyper */ #include @@ -33,8 +35,14 @@ #include "common/utils.h" +#include "drivers/display.h" #include "drivers/system.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + +#include "io/displayport_max7456.h" #include "io/flashfs.h" #include "io/osd.h" @@ -42,8 +50,6 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/pid.h" - #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" @@ -55,582 +61,56 @@ #include "drivers/max7456.h" #include "drivers/max7456_symbols.h" -#ifdef USE_RTC6705 -#include "drivers/vtx_soft_spi_rtc6705.h" -#endif - #include "common/printf.h" +#include "build/debug.h" + +// Character coordinate and attributes + +#define OSD_POS(x,y) (x | (y << 5)) +#define OSD_X(x) (x & 0x001F) +#define OSD_Y(x) ((x >> 5) & 0x001F) + +// Things in both OSD and CMS + #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -//key definiotion because API provide menu navigation over MSP/GUI app - not used NOW -#define KEY_ENTER 0 -#define KEY_UP 1 -#define KEY_DOWN 2 -#define KEY_LEFT 3 -#define KEY_RIGHT 4 -#define KEY_ESC 5 - -//osd current screen - to reduce long lines ;-) -#define OSD_cfg masterConfig.osdProfile -#define curr_profile masterConfig.profile[masterConfig.current_profile_index] - -uint16_t refreshTimeout = 0; - -#define VISIBLE_FLAG 0x0800 -#define BLINK_FLAG 0x0400 bool blinkState = true; -#define OSD_POS(x,y) (x | (y << 5)) -#define OSD_X(x) (x & 0x001F) -#define OSD_Y(x) ((x >> 5) & 0x001F) -#define VISIBLE(x) (x & VISIBLE_FLAG) -#define BLINK(x) ((x & BLINK_FLAG) && blinkState) -#define BLINK_OFF(x) (x & ~BLINK_FLAG) - -extern uint8_t RSSI; // TODO: not used? +//extern uint8_t RSSI; // TODO: not used? static uint16_t flyTime = 0; -uint8_t statRssi; +static uint8_t statRssi; -statistic_t stats; +typedef struct statistic_s { + int16_t max_speed; + int16_t min_voltage; // /10 + int16_t max_current; // /10 + int16_t min_rssi; + int16_t max_altitude; +} statistic_t; -#define BUTTON_TIME 2 -#define BUTTON_PAUSE 5 +static statistic_t stats; + +uint16_t refreshTimeout = 0; #define REFRESH_1S 12 -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN 23 -#define MAX_MENU_ITEMS (max7456GetRowsCount() - 2) +static uint8_t armState; -uint8_t osdRows; +static displayPort_t *osd7456DisplayPort; -//type of elements -typedef enum -{ - OME_Label, - OME_Back, - OME_OSD_Exit, - OME_Submenu, - OME_Bool, - OME_INT8, - OME_UINT8, - OME_UINT16, - OME_INT16, - OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's - //wlasciwosci elementow - OME_VISIBLE, - OME_POS, - OME_TAB, - OME_END, -} OSD_MenuElement; -//local variable to detect arm/disarm and show statistic etc -uint8_t armState; -uint8_t featureBlackbox = 0; -uint8_t featureLedstrip = 0; - -#if defined(VTX) || defined(USE_RTC6705) -uint8_t featureVtx = 0, vtxBand, vtxChannel; -#endif // VTX || USE_RTC6705 - -// We are in menu flag -bool inMenu = false; - -typedef void (* OSDMenuFuncPtr)(void *data); - -void osdUpdate(uint32_t currentTime); -char osdGetAltitudeSymbol(); -int32_t osdGetAltitude(int32_t alt); -void osdOpenMenu(void); -void osdExitMenu(void * ptr); -void osdMenuBack(void); -void osdEditElement(void *ptr); -void osdEraseFlash(void *ptr); -void osdUpdateMaxRows(void); -void osdChangeScreen(void * ptr); -void osdDrawElements(void); -void osdDrawSingleElement(uint8_t item); - -typedef struct -{ - char *text; - OSD_MenuElement type; - OSDMenuFuncPtr func; - void *data; -} OSD_Entry; - -typedef struct -{ - uint8_t *val; - uint8_t min; - uint8_t max; - uint8_t step; -} OSD_UINT8_t; - -typedef struct -{ - int8_t *val; - int8_t min; - int8_t max; - int8_t step; -} OSD_INT8_t; - -typedef struct -{ - int16_t *val; - int16_t min; - int16_t max; - int16_t step; -} OSD_INT16_t; - -typedef struct -{ - uint16_t *val; - uint16_t min; - uint16_t max; - uint16_t step; -} OSD_UINT16_t; - -typedef struct -{ - uint8_t *val; - uint8_t min; - uint8_t max; - uint8_t step; - uint16_t multipler; -} OSD_FLOAT_t; - -typedef struct -{ - uint8_t *val; - uint8_t max; - const char * const *names; -} OSD_TAB_t; - -OSD_Entry *menuStack[10]; //tab to save menu stack -uint8_t menuStackHistory[10]; //current position in menu stack -uint8_t menuStackIdx = 0; - -OSD_Entry *currentMenu; -OSD_Entry *nextPage = NULL; - -int8_t currentMenuPos = 0; -uint8_t currentMenuIdx = 0; -uint16_t *currentElement = NULL; - -OSD_Entry menuAlarms[]; -OSD_Entry menuOsdLayout[]; -OSD_Entry menuOsdActiveElems[]; -OSD_Entry menuOsdElemsPositions[]; -OSD_Entry menuFeatures[]; -OSD_Entry menuBlackbox[]; -#ifdef LED_STRIP -OSD_Entry menuLedstrip[]; -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) -OSD_Entry menu_vtx[]; -#endif // VTX || USE_RTC6705 -OSD_Entry menuImu[]; -OSD_Entry menuPid[]; -OSD_Entry menuRc[]; -OSD_Entry menuRateExpo[]; -OSD_Entry menuMisc[]; - -OSD_Entry menuMain[] = -{ - {"----MAIN MENU----", OME_Label, NULL, NULL}, - {"SCREEN LAYOUT", OME_Submenu, osdChangeScreen, &menuOsdLayout[0]}, - {"ALARMS", OME_Submenu, osdChangeScreen, &menuAlarms[0]}, - {"CFG. IMU", OME_Submenu, osdChangeScreen, &menuImu[0]}, - {"FEATURES", OME_Submenu, osdChangeScreen, &menuFeatures[0]}, - {"SAVE & EXIT", OME_OSD_Exit, osdExitMenu, (void*)1}, - {"EXIT", OME_OSD_Exit, osdExitMenu, (void*)0}, - {NULL,OME_END, NULL, NULL} -}; - -OSD_Entry menuFeatures[] = -{ - {"----- FEATURES -----", OME_Label, NULL, NULL}, - {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, -#endif // VTX || USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, osdEraseFlash, NULL}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -#ifdef LED_STRIP -//local variable to keep color value -uint8_t ledColor; - -static const char * const LED_COLOR_NAMES[] = { - " BLACK ", - " WHITE ", - " RED ", - " ORANGE ", - " YELLOW ", - " LIME GREEN", - " GREEN ", - " MINT GREEN", - " CYAN ", - " LIGHT BLUE", - " BLUE ", - "DARK VIOLET", - " MAGENTA ", - " DEEP PINK" -}; - -//find first led with color flag and restore color index -//after saving all leds with flags color will have color set in OSD -void getLedColor(void) -{ - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - - int fn = ledGetFunction(ledConfig); - - if (fn == LED_FUNCTION_COLOR) { - ledColor = ledGetColor(ledConfig); - break; - } - } -} - -//udate all leds with flag color -void applyLedColor(void * ptr) -{ - UNUSED(ptr); - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) - *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); - } -} - -OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; - -OSD_Entry menuLedstrip[] = -{ - {"--- LED TRIP ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; -#endif // LED_STRIP - -#if defined(VTX) || defined(USE_RTC6705) -static const char * const vtxBandNames[] = { - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -OSD_TAB_t entryVtxBand = {&vtxBand,4,&vtxBandNames[0]}; -OSD_UINT8_t entryVtxChannel = {&vtxChannel, 1, 8, 1}; - -#ifdef VTX -OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; -OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; -#endif // VTX - -OSD_Entry menu_vtx[] = -{ - {"--- VTX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureVtx}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; -#endif // VTX || USE_RTC6705 - -OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; -OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; -OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; -OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; -OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; - -OSD_Entry menuMisc[]= -{ - {"----- MISC -----", OME_Label, NULL, NULL}, - {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf}, - {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit}, - {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale}, - {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - -OSD_Entry menuImu[] = -{ - {"-----CFG. IMU-----", OME_Label, NULL, NULL}, - {"PID", OME_Submenu, osdChangeScreen, &menuPid[0]}, - {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile}, - {"RATE & RXPO", OME_Submenu, osdChangeScreen, &menuRateExpo[0]}, - {"RC PREVIEW", OME_Submenu, osdChangeScreen, &menuRc[0]}, - {"MISC", OME_Submenu, osdChangeScreen, &menuMisc[0]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -uint8_t tempPid[4][3]; - -static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1}; -static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1}; -static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1}; - -static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1}; -static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1}; -static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1}; - -static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1}; -static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1}; -static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; - -OSD_Entry menuPid[] = -{ - {"------- PID -------", OME_Label, NULL, NULL}, - {"ROLL P", OME_UINT8, NULL, &entryRollP}, - {"ROLL I", OME_UINT8, NULL, &entryRollI}, - {"ROLL D", OME_UINT8, NULL, &entryRollD}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD}, - - {"YAW P", OME_UINT8, NULL, &entryYawP}, - {"YAW I", OME_UINT8, NULL, &entryYawI}, - {"YAW D", OME_UINT8, NULL, &entryYawD}, - - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -controlRateConfig_t rateProfile; - -static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10}; -static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10}; -static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10}; -static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10}; -static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; -static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; -static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10}; -static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; - -OSD_Entry menuRateExpo[] = -{ - {"----RATE & EXPO----", OME_Label, NULL, NULL}, - {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate}, - {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate}, - {"YAW RATE", OME_FLOAT, NULL, &entryYawRate}, - {"RC RATE", OME_FLOAT, NULL, &entryRcRate}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw}, - {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry}, - {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak}, - {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint}, - {"D SETPOINT TRANSITION", OME_FLOAT, NULL, &entryPSetpoint}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; -static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; -static OSD_INT16_t entryRcThrottle = {&rcData[THROTTLE], 1, 2500, 0}; -static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0}; -static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0}; -static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0}; -static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0}; -static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; - -OSD_Entry menuRc[] = -{ - {"---- RC PREVIEW ----", OME_Label, NULL, NULL}, - {"ROLL", OME_INT16, NULL, &entryRcRoll}, - {"PITCH", OME_INT16, NULL, &entryRcPitch}, - {"THROTTLE", OME_INT16, NULL, &entryRcThrottle}, - {"YAW", OME_INT16, NULL, &entryRcYaw}, - {"AUX1", OME_INT16, NULL, &entryRcAux1}, - {"AUX2", OME_INT16, NULL, &entryRcAux2}, - {"AUX3", OME_INT16, NULL, &entryRcAux3}, - {"AUX4", OME_INT16, NULL, &entryRcAux4}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdLayout[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL}, - {"ACTIVE ELEM.", OME_Submenu, osdChangeScreen, &menuOsdActiveElems[0]}, - {"POSITION", OME_Submenu, osdChangeScreen, &menuOsdElemsPositions[0]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryAlarmRssi = {&OSD_cfg.rssi_alarm, 5, 90, 5}; -OSD_UINT16_t entryAlarmCapacity = {&OSD_cfg.cap_alarm, 50, 30000, 50}; -OSD_UINT16_t enryAlarmFlyTime = {&OSD_cfg.time_alarm, 1, 200, 1}; -OSD_UINT16_t entryAlarmAltitude = {&OSD_cfg.alt_alarm, 1, 200, 1}; - -OSD_Entry menuAlarms[] = -{ - {"------ ALARMS ------", OME_Label, NULL, NULL}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi}, - {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime}, - {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdElemsPositions[] = -{ - {"---POSITION---", OME_Label, NULL, NULL}, - {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, - {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, - {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, - {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, - {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, - {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, - {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, -#ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, - {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, -#ifdef GPS - {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, - {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, -#endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdActiveElems[] = -{ - {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS]}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, - {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, -#ifdef VTX - {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -void resetOsdConfig(osd_profile_t *osdProfile) -{ - osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); - osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); - osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); - osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); - osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); - osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); - osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); - osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); - - osdProfile->rssi_alarm = 20; - osdProfile->cap_alarm = 2200; - osdProfile->time_alarm = 10; // in minutes - osdProfile->alt_alarm = 100; // meters or feet depend on configuration - - osdProfile->video_system = 0; -} - -void osdInit(void) -{ - char x, string_buffer[30]; - - armState = ARMING_FLAG(ARMED); - - max7456Init(masterConfig.osdProfile.video_system); - - max7456ClearScreen(); - - // display logo and help - x = 160; - for (int i = 1; i < 5; i++) { - for (int j = 3; j < 27; j++) { - if (x != 255) - max7456WriteChar(j, i, x++); - } - } - - sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); - max7456Write(5, 6, string_buffer); - max7456Write(7, 7, "MENU: THRT MID"); - max7456Write(13, 8, "YAW RIGHT"); - max7456Write(13, 9, "PITCH UP"); - max7456RefreshAll(); - - refreshTimeout = 4 * REFRESH_1S; -} +#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees +#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees +#define AH_SIDEBAR_WIDTH_POS 7 +#define AH_SIDEBAR_HEIGHT_POS 3 /** * Gets the correct altitude symbol for the current unit system */ -char osdGetAltitudeSymbol() +static char osdGetAltitudeSymbol() { switch (masterConfig.osdProfile.units) { case OSD_UNIT_IMPERIAL: @@ -644,7 +124,7 @@ char osdGetAltitudeSymbol() * Converts altitude based on the current unit system. * @param alt Raw altitude (i.e. as taken from BaroAlt) */ -int32_t osdGetAltitude(int32_t alt) +static int32_t osdGetAltitude(int32_t alt) { switch (masterConfig.osdProfile.units) { case OSD_UNIT_IMPERIAL: @@ -654,831 +134,13 @@ int32_t osdGetAltitude(int32_t alt) } } -void osdUpdateAlarms(void) +static void osdDrawSingleElement(uint8_t item) { - int32_t alt = osdGetAltitude(BaroAlt) / 100; - statRssi = rssi * 100 / 1024; - - if (statRssi < OSD_cfg.rssi_alarm) - OSD_cfg.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - - if (vbat <= (batteryWarningVoltage - 1)) - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - - if (STATE(GPS_FIX) == 0) - OSD_cfg.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - - if (flyTime / 60 >= OSD_cfg.time_alarm && ARMING_FLAG(ARMED)) - OSD_cfg.item_pos[OSD_FLYTIME] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - - if (mAhDrawn >= OSD_cfg.cap_alarm) - OSD_cfg.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; - - if (alt >= OSD_cfg.alt_alarm) - OSD_cfg.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; -} - -void osdResetAlarms(void) -{ - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; -} - -uint8_t osdHandleKey(uint8_t key) -{ - uint8_t res = BUTTON_TIME; - OSD_Entry *p; - - if (!currentMenu) - return res; - - if (key == KEY_ESC) { - osdMenuBack(); - return BUTTON_PAUSE; - } - - if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) - currentMenuPos++; - else { - if (nextPage) // we have more pages - { - max7456ClearScreen(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - osdUpdateMaxRows(); - } - currentMenuPos = 0; - } - } - - if (key == KEY_UP) { - currentMenuPos--; - - if ((currentMenu + currentMenuPos)->type == OME_Label && currentMenuPos > 0) - currentMenuPos--; - - if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { - if (nextPage) { - max7456ClearScreen(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - osdUpdateMaxRows(); - } - currentMenuPos = currentMenuIdx; - } - } - - if (key == KEY_DOWN || key == KEY_UP) - return res; - - p = currentMenu + currentMenuPos; - - switch (p->type) { - case OME_POS: - if (key == KEY_RIGHT) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) // no submenu for hidden elements - break; - } - case OME_Submenu: - case OME_OSD_Exit: - if (p->func && key == KEY_RIGHT) { - p->func(p->data); - res = BUTTON_PAUSE; - } - break; - case OME_Back: - osdMenuBack(); - res = BUTTON_PAUSE; - break; - case OME_Bool: - if (p->data) { - uint8_t *val = p->data; - if (key == KEY_RIGHT) - *val = 1; - else - *val = 0; - } - break; - case OME_VISIBLE: - if (p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (key == KEY_RIGHT) - *val |= VISIBLE_FLAG; - else - *val %= ~VISIBLE_FLAG; - } - break; - case OME_UINT8: - case OME_FLOAT: - if (p->data) { - OSD_UINT8_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_TAB: - if (p->type == OME_TAB) { - OSD_TAB_t *ptr = p->data; - - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += 1; - } - else { - if (*ptr->val > 0) - *ptr->val -= 1; - } - if (p->func) - p->func(p->data); - } - break; - case OME_INT8: - if (p->data) { - OSD_INT8_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_UINT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_INT16: - if (p->data) { - OSD_INT16_t *ptr = p->data; - if (key == KEY_RIGHT) { - if (*ptr->val < ptr->max) - *ptr->val += ptr->step; - } - else { - if (*ptr->val > ptr->min) - *ptr->val -= ptr->step; - } - } - break; - case OME_Label: - case OME_END: - break; - } - return res; -} - -void osdUpdateMaxRows(void) -{ - OSD_Entry *ptr; - - currentMenuIdx = 0; - for (ptr = currentMenu; ptr->type != OME_END; ptr++) - currentMenuIdx++; - - if (currentMenuIdx > MAX_MENU_ITEMS) - currentMenuIdx = MAX_MENU_ITEMS; - - currentMenuIdx--; -} - -void osdMenuBack(void) -{ - uint8_t i; - - // becasue pids and rates meybe stored in profiles we need some thicks to manipulate it - // hack to save pid profile - if (currentMenu == &menuPid[0]) { - for (i = 0; i < 3; i++) { - curr_profile.pidProfile.P8[i] = tempPid[i][0]; - curr_profile.pidProfile.I8[i] = tempPid[i][1]; - curr_profile.pidProfile.D8[i] = tempPid[i][2]; - } - - curr_profile.pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - curr_profile.pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - curr_profile.pidProfile.D8[PIDLEVEL] = tempPid[3][2]; - } - - // hack - save rate config for current profile - if (currentMenu == &menuRateExpo[0]) - memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); - - if (menuStackIdx) { - max7456ClearScreen(); - menuStackIdx--; - nextPage = NULL; - currentMenu = menuStack[menuStackIdx]; - currentMenuPos = menuStackHistory[menuStackIdx]; - - osdUpdateMaxRows(); - } - else - osdOpenMenu(); -} - -void simple_ftoa(int32_t value, char *floatString) -{ - uint8_t k; - // np. 3450 - - itoa(100000 + value, floatString, 10); // Create string from abs of integer value - - // 103450 - - floatString[0] = floatString[1]; - floatString[1] = floatString[2]; - floatString[2] = '.'; - - // 03.450 - // usuwam koncowe zera i kropke - for (k = 5; k > 1; k--) - if (floatString[k] == '0' || floatString[k] == '.') - floatString[k] = 0; - else - break; - - // oraz zero wiodonce - if (floatString[0] == '0') - floatString[0] = ' '; -} - -void osdDrawMenu(void) -{ - uint8_t i = 0; - OSD_Entry *p; - char buff[10]; - uint8_t top = (osdRows - currentMenuIdx) / 2 - 1; - if (!currentMenu) + if (!VISIBLE(masterConfig.osdProfile.item_pos[item]) || BLINK(masterConfig.osdProfile.item_pos[item])) return; - if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label - currentMenuPos++; - - for (p = currentMenu; p->type != OME_END; p++) { - if (currentMenuPos == i) - max7456Write(LEFT_MENU_COLUMN, i + top, " >"); - else - max7456Write(LEFT_MENU_COLUMN, i + top, " "); - max7456Write(LEFT_MENU_COLUMN + 2, i + top, p->text); - - switch (p->type) { - case OME_POS: { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) - break; - } - case OME_Submenu: - max7456Write(RIGHT_MENU_COLUMN, i + top, ">"); - break; - case OME_Bool: - if (p->data) { - if (*((uint8_t *)(p->data))) - max7456Write(RIGHT_MENU_COLUMN, i + top, "YES"); - else - max7456Write(RIGHT_MENU_COLUMN, i + top, "NO "); - } - break; - case OME_TAB: { - OSD_TAB_t *ptr = p->data; - max7456Write(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); - break; - } - case OME_VISIBLE: - if (p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (VISIBLE(*val)) - max7456Write(RIGHT_MENU_COLUMN, i + top, "YES"); - else - max7456Write(RIGHT_MENU_COLUMN, i + top, "NO "); - } - break; - case OME_UINT8: - if (p->data) { - OSD_UINT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_INT8: - if (p->data) { - OSD_INT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_UINT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_INT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_FLOAT: - if (p->data) { - OSD_FLOAT_t *ptr = p->data; - simple_ftoa(*ptr->val * ptr->multipler, buff); - max7456Write(RIGHT_MENU_COLUMN - 1, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN - 1, i + top, buff); - } - break; - case OME_OSD_Exit: - case OME_Label: - case OME_END: - case OME_Back: - break; - } - i++; - - if (i == MAX_MENU_ITEMS) // max per page - { - nextPage = currentMenu + i; - if (nextPage->type == OME_END) - nextPage = NULL; - break; - } - } -} - -void osdResetStats(void) -{ - stats.max_current = 0; - stats.max_speed = 0; - stats.min_voltage = 500; - stats.max_current = 0; - stats.min_rssi = 99; - stats.max_altitude = 0; -} - -void osdUpdateStats(void) -{ - int16_t value; - - value = GPS_speed * 36 / 1000; - if (stats.max_speed < value) - stats.max_speed = value; - - if (stats.min_voltage > vbat) - stats.min_voltage = vbat; - - value = amperage / 100; - if (stats.max_current < value) - stats.max_current = value; - - if (stats.min_rssi > statRssi) - stats.min_rssi = statRssi; - - if (stats.max_altitude < BaroAlt) - stats.max_altitude = BaroAlt; -} - -void osdShowStats(void) -{ - uint8_t top = 2; - char buff[10]; - - max7456ClearScreen(); - max7456Write(2, top++, " --- STATS ---"); - - if (STATE(GPS_FIX)) { - max7456Write(2, top, "MAX SPEED :"); - itoa(stats.max_speed, buff, 10); - max7456Write(22, top++, buff); - } - - max7456Write(2, top, "MIN BATTERY :"); - sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); - max7456Write(22, top++, buff); - - max7456Write(2, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - max7456Write(22, top++, buff); - - if (feature(FEATURE_CURRENT_METER)) { - max7456Write(2, top, "MAX CURRENT :"); - itoa(stats.max_current, buff, 10); - strcat(buff, "A"); - max7456Write(22, top++, buff); - - max7456Write(2, top, "USED MAH :"); - itoa(mAhDrawn, buff, 10); - strcat(buff, "\x07"); - max7456Write(22, top++, buff); - } - - max7456Write(2, top, "MAX ALTITUDE :"); - int32_t alt = osdGetAltitude(stats.max_altitude); - sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); - max7456Write(22, top++, buff); - - refreshTimeout = 60 * REFRESH_1S; -} - -// called when motors armed -void osdArmMotors(void) -{ - max7456ClearScreen(); - max7456Write(12, 7, "ARMED"); - refreshTimeout = REFRESH_1S / 2; - osdResetStats(); -} - -void updateOsd(uint32_t currentTime) -{ - static uint32_t counter; -#ifdef MAX7456_DMA_CHANNEL_TX - // don't touch buffers if DMA transaction is in progress - if (max7456DmaInProgres()) - return; -#endif // MAX7456_DMA_CHANNEL_TX - - // redraw values in buffer - if (counter++ % 5 == 0) - osdUpdate(currentTime); - else // rest of time redraw screen 10 chars per idle to don't lock the main idle - max7456DrawScreen(); - - // do not allow ARM if we are in menu - if (inMenu) - DISABLE_ARMING_FLAG(OK_TO_ARM); -} - -void osdUpdate(uint32_t currentTime) -{ - static uint8_t rcDelay = BUTTON_TIME; - static uint8_t lastSec = 0; - uint8_t key = 0, sec; - - // detect enter to menu - if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) - osdOpenMenu(); - - // detect arm/disarm - if (armState != ARMING_FLAG(ARMED)) { - if (ARMING_FLAG(ARMED)) - osdArmMotors(); // reset statistic etc - else - osdShowStats(); // show statistic - - armState = ARMING_FLAG(ARMED); - } - - osdUpdateStats(); - - sec = currentTime / 1000000; - - if (ARMING_FLAG(ARMED) && sec != lastSec) { - flyTime++; - lastSec = sec; - } - - if (refreshTimeout) { - if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics - refreshTimeout = 1; - refreshTimeout--; - if (!refreshTimeout) - max7456ClearScreen(); - return; - } - - blinkState = (millis() / 200) % 2; - - if (inMenu) { - if (rcDelay) { - rcDelay--; - } - else if (IS_HI(PITCH)) { - key = KEY_UP; - rcDelay = BUTTON_TIME; - } - else if (IS_LO(PITCH)) { - key = KEY_DOWN; - rcDelay = BUTTON_TIME; - } - else if (IS_LO(ROLL)) { - key = KEY_LEFT; - rcDelay = BUTTON_TIME; - } - else if (IS_HI(ROLL)) { - key = KEY_RIGHT; - rcDelay = BUTTON_TIME; - } - else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != menuRc) // this menu is used to check transmitter signals so can exit using YAW - { - key = KEY_ESC; - rcDelay = BUTTON_TIME; - } - - if (key && !currentElement) { - rcDelay = osdHandleKey(key); - return; - } - if (currentElement) // edit position of element - { - if (key) { - if (key == KEY_ESC) { - // exit - osdMenuBack(); - rcDelay = BUTTON_PAUSE; - *currentElement &= ~BLINK_FLAG; - currentElement = NULL; - return; - } - else { - uint8_t x, y; - x = OSD_X(*currentElement); - y = OSD_Y(*currentElement); - switch (key) { - case KEY_UP: - y--; - break; - case KEY_DOWN: - y++; - break; - case KEY_RIGHT: - x++; - break; - case KEY_LEFT: - x--; - break; - } - - *currentElement &= 0xFC00; - *currentElement |= OSD_POS(x, y); - max7456ClearScreen(); - } - } - osdDrawElements(); - } - else - osdDrawMenu(); - } - else { - osdUpdateAlarms(); - osdDrawElements(); - } -} - -void osdChangeScreen(void *ptr) -{ - uint8_t i; - if (ptr) { - max7456ClearScreen(); - // hack - save profile to temp - if (ptr == &menuPid[0]) { - for (i = 0; i < 3; i++) { - tempPid[i][0] = curr_profile.pidProfile.P8[i]; - tempPid[i][1] = curr_profile.pidProfile.I8[i]; - tempPid[i][2] = curr_profile.pidProfile.D8[i]; - } - tempPid[3][0] = curr_profile.pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = curr_profile.pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = curr_profile.pidProfile.D8[PIDLEVEL]; - } - - if (ptr == &menuRateExpo[0]) - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); - - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentMenu = (OSD_Entry *)ptr; - currentMenuPos = 0; - osdUpdateMaxRows(); - } -} - -#ifdef USE_FLASHFS -void osdEraseFlash(void *ptr) -{ - UNUSED(ptr); - - max7456ClearScreen(); - max7456Write(5, 3, "ERASING FLASH..."); - max7456RefreshAll(); - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - max7456ClearScreen(); - max7456RefreshAll(); -} -#endif // USE_FLASHFS - -void osdEditElement(void *ptr) -{ - uint32_t address = (uint32_t)ptr; - - // zsave position on menu stack - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentElement = (uint16_t *)address; - - *currentElement |= BLINK_FLAG; - max7456ClearScreen(); -} - -void osdExitMenu(void *ptr) -{ - max7456ClearScreen(); - max7456Write(5, 3, "RESTARTING IMU..."); - max7456RefreshAll(); - stopMotors(); - stopPwmAllMotors(); - delay(200); - - if (ptr) { - // save local variables to configuration - if (featureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); - - if (featureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); -#if defined(VTX) || defined(USE_RTC6705) - if (featureVtx) - featureSet(FEATURE_VTX); - else - featureClear(FEATURE_VTX); -#endif // VTX || USE_RTC6705 - -#ifdef VTX - masterConfig.vtxBand = vtxBand; - masterConfig.vtx_channel = vtxChannel - 1; -#endif // VTX - -#ifdef USE_RTC6705 - masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1; -#endif // USE_RTC6705 - - saveConfigAndNotify(); - } - - systemReset(); -} - -void osdOpenMenu(void) -{ - if (inMenu) - return; - - if (feature(FEATURE_LED_STRIP)) - featureLedstrip = 1; - - if (feature(FEATURE_BLACKBOX)) - featureBlackbox = 1; - -#if defined(VTX) || defined(USE_RTC6705) - if (feature(FEATURE_VTX)) - featureVtx = 1; -#endif // VTX || USE_RTC6705 - -#ifdef VTX - vtxBand = masterConfig.vtxBand; - vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX - -#ifdef USE_RTC6705 - vtxBand = masterConfig.vtx_channel / 8; - vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 - - osdRows = max7456GetRowsCount(); - inMenu = true; - refreshTimeout = 0; - max7456ClearScreen(); - currentMenu = &menuMain[0]; - osdResetAlarms(); - osdChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP -} - -void osdDrawElementPositioningHelp(void) -{ - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); -} - -void osdDrawElements(void) -{ - max7456ClearScreen(); - - if (currentElement) - osdDrawElementPositioningHelp(); - else if (sensors(SENSOR_ACC) || inMenu) - { - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - osdDrawSingleElement(OSD_CROSSHAIRS); - } - - osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); - osdDrawSingleElement(OSD_RSSI_VALUE); - osdDrawSingleElement(OSD_FLYTIME); - osdDrawSingleElement(OSD_ONTIME); - osdDrawSingleElement(OSD_FLYMODE); - osdDrawSingleElement(OSD_THROTTLE_POS); - osdDrawSingleElement(OSD_VTX_CHANNEL); - osdDrawSingleElement(OSD_CURRENT_DRAW); - osdDrawSingleElement(OSD_MAH_DRAWN); - osdDrawSingleElement(OSD_CRAFT_NAME); - osdDrawSingleElement(OSD_ALTITUDE); - -#ifdef GPS - if (sensors(SENSOR_GPS) || inMenu) { - osdDrawSingleElement(OSD_GPS_SATS); - osdDrawSingleElement(OSD_GPS_SPEED); - } -#endif // GPS -} - -#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees -#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees -#define AH_SIDEBAR_WIDTH_POS 7 -#define AH_SIDEBAR_HEIGHT_POS 3 - - -void osdDrawSingleElement(uint8_t item) -{ - if (!VISIBLE(OSD_cfg.item_pos[item]) || BLINK(OSD_cfg.item_pos[item])) - return; - - uint8_t elemPosX = OSD_X(OSD_cfg.item_pos[item]); - uint8_t elemPosY = OSD_Y(OSD_cfg.item_pos[item]); + uint8_t elemPosX = OSD_X(masterConfig.osdProfile.item_pos[item]); + uint8_t elemPosY = OSD_Y(masterConfig.osdProfile.item_pos[item]); char buff[32]; switch(item) { @@ -1601,30 +263,26 @@ void osdDrawSingleElement(uint8_t item) #endif // VTX case OSD_CROSSHAIRS: - { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; - + elemPosX = 14 - 1; // Offset for 1 char to the left + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; - - screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); - screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); - screenBuffer[position] = (SYM_AH_CENTER); - - return; - } + ++elemPosY; + buff[0] = SYM_AH_CENTER_LINE; + buff[1] = SYM_AH_CENTER; + buff[2] = SYM_AH_CENTER_LINE_RIGHT; + buff[3] = 0; + break; case OSD_ARTIFICIAL_HORIZON: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6 - 4; // Top center of the AH area int rollAngle = attitude.values.roll; int pitchAngle = attitude.values.pitch; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; if (pitchAngle > AH_MAX_PITCH) pitchAngle = AH_MAX_PITCH; @@ -1635,13 +293,15 @@ void osdDrawSingleElement(uint8_t item) if (rollAngle < -AH_MAX_ROLL) rollAngle = -AH_MAX_ROLL; - for (uint8_t x = 0; x <= 8; x++) { - int y = (rollAngle * (4 - x)) / 64; - y -= pitchAngle / 8; - y += 41; + // Convert pitchAngle to y compensation value + pitchAngle = (pitchAngle / 8) - 41; // 41 = 4 * 9 + 5 + + for (int8_t x = -4; x <= 4; x++) { + int y = (rollAngle * x) / 64; + y -= pitchAngle; + // y += 41; // == 4 * 9 + 5 if (y >= 0 && y <= 81) { - uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; - screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); + max7456WriteChar(elemPosX + x, elemPosY + (y / 9), (SYM_AH_BAR9_0 + (y % 9))); } } @@ -1652,23 +312,23 @@ void osdDrawSingleElement(uint8_t item) case OSD_HORIZON_SIDEBARS: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; // Draw AH sides int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; - for (int8_t x = -hudheight; x <= hudheight; x++) { - screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); - screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + for (int8_t y = -hudheight; y <= hudheight; y++) { + max7456WriteChar(elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); + max7456WriteChar(elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators - screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); - screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); + max7456WriteChar(elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); + max7456WriteChar(elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return; } @@ -1680,4 +340,323 @@ void osdDrawSingleElement(uint8_t item) max7456Write(elemPosX, elemPosY, buff); } +void osdDrawElements(void) +{ + max7456ClearScreen(); + +#if 0 + if (currentElement) + osdDrawElementPositioningHelp(); +#else + if (false) + ; +#endif +#ifdef CMS + else if (sensors(SENSOR_ACC) || displayIsGrabbed(osd7456DisplayPort)) +#else + else if (sensors(SENSOR_ACC)) +#endif + { + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + osdDrawSingleElement(OSD_CROSSHAIRS); + } + + osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); + osdDrawSingleElement(OSD_RSSI_VALUE); + osdDrawSingleElement(OSD_FLYTIME); + osdDrawSingleElement(OSD_ONTIME); + osdDrawSingleElement(OSD_FLYMODE); + osdDrawSingleElement(OSD_THROTTLE_POS); + osdDrawSingleElement(OSD_VTX_CHANNEL); + osdDrawSingleElement(OSD_CURRENT_DRAW); + osdDrawSingleElement(OSD_MAH_DRAWN); + osdDrawSingleElement(OSD_CRAFT_NAME); + osdDrawSingleElement(OSD_ALTITUDE); + +#ifdef GPS +#ifdef CMS + if (sensors(SENSOR_GPS) || displayIsGrabbed(osd7456DisplayPort)) +#else + if (sensors(SENSOR_GPS)) +#endif + { + osdDrawSingleElement(OSD_GPS_SATS); + osdDrawSingleElement(OSD_GPS_SPEED); + } +#endif // GPS +} + +void osdResetConfig(osd_profile_t *osdProfile) +{ + osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); + osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); + osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); + osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); + osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); + osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); + osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); + + osdProfile->rssi_alarm = 20; + osdProfile->cap_alarm = 2200; + osdProfile->time_alarm = 10; // in minutes + osdProfile->alt_alarm = 100; // meters or feet depend on configuration + + osdProfile->video_system = 0; +} + +void osdInit(void) +{ + char x, string_buffer[30]; + + armState = ARMING_FLAG(ARMED); + + max7456Init(masterConfig.osdProfile.video_system); + + max7456ClearScreen(); + + // display logo and help + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) { + if (x != 255) + max7456WriteChar(j, i, x++); + } + } + + sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); + max7456Write(5, 6, string_buffer); +#ifdef CMS + max7456Write(7, 7, CMS_STARTUP_HELP_TEXT1); + max7456Write(11, 8, CMS_STARTUP_HELP_TEXT2); + max7456Write(11, 9, CMS_STARTUP_HELP_TEXT3); +#endif + + max7456RefreshAll(); + + refreshTimeout = 4 * REFRESH_1S; + + osd7456DisplayPort = max7456DisplayPortInit(); +#ifdef CMS + cmsDisplayPortRegister(osd7456DisplayPort); +#endif +} + +void osdUpdateAlarms(void) +{ + osd_profile_t *pOsdProfile = &masterConfig.osdProfile; + + // This is overdone? + // uint16_t *itemPos = masterConfig.osdProfile.item_pos; + + int32_t alt = osdGetAltitude(BaroAlt) / 100; + statRssi = rssi * 100 / 1024; + + if (statRssi < pOsdProfile->rssi_alarm) + pOsdProfile->item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + + if (vbat <= (batteryWarningVoltage - 1)) + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + + if (STATE(GPS_FIX) == 0) + pOsdProfile->item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + + if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED)) + pOsdProfile->item_pos[OSD_FLYTIME] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + + if (mAhDrawn >= pOsdProfile->cap_alarm) + pOsdProfile->item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + + if (alt >= pOsdProfile->alt_alarm) + pOsdProfile->item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + else + pOsdProfile->item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; +} + +void osdResetAlarms(void) +{ + osd_profile_t *pOsdProfile = &masterConfig.osdProfile; + + pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; +} + +static void osdResetStats(void) +{ + stats.max_current = 0; + stats.max_speed = 0; + stats.min_voltage = 500; + stats.max_current = 0; + stats.min_rssi = 99; + stats.max_altitude = 0; +} + +static void osdUpdateStats(void) +{ + int16_t value; + + value = GPS_speed * 36 / 1000; + if (stats.max_speed < value) + stats.max_speed = value; + + if (stats.min_voltage > vbat) + stats.min_voltage = vbat; + + value = amperage / 100; + if (stats.max_current < value) + stats.max_current = value; + + if (stats.min_rssi > statRssi) + stats.min_rssi = statRssi; + + if (stats.max_altitude < BaroAlt) + stats.max_altitude = BaroAlt; +} + +static void osdShowStats(void) +{ + uint8_t top = 2; + char buff[10]; + + max7456ClearScreen(); + max7456Write(2, top++, " --- STATS ---"); + + if (STATE(GPS_FIX)) { + max7456Write(2, top, "MAX SPEED :"); + itoa(stats.max_speed, buff, 10); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MIN BATTERY :"); + sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); + max7456Write(22, top++, buff); + + max7456Write(2, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + max7456Write(22, top++, buff); + + if (feature(FEATURE_CURRENT_METER)) { + max7456Write(2, top, "MAX CURRENT :"); + itoa(stats.max_current, buff, 10); + strcat(buff, "A"); + max7456Write(22, top++, buff); + + max7456Write(2, top, "USED MAH :"); + itoa(mAhDrawn, buff, 10); + strcat(buff, "\x07"); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MAX ALTITUDE :"); + int32_t alt = osdGetAltitude(stats.max_altitude); + sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); + max7456Write(22, top++, buff); + + refreshTimeout = 60 * REFRESH_1S; +} + +// called when motors armed +static void osdArmMotors(void) +{ + max7456ClearScreen(); + max7456Write(12, 7, "ARMED"); + refreshTimeout = REFRESH_1S / 2; + osdResetStats(); +} + +static void osdRefresh(uint32_t currentTime) +{ + static uint8_t lastSec = 0; + uint8_t sec; + + // detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) + osdArmMotors(); // reset statistic etc + else + osdShowStats(); // show statistic + + armState = ARMING_FLAG(ARMED); + } + + osdUpdateStats(); + + sec = currentTime / 1000000; + + if (ARMING_FLAG(ARMED) && sec != lastSec) { + flyTime++; + lastSec = sec; + } + + if (refreshTimeout) { + if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics + refreshTimeout = 1; + refreshTimeout--; + if (!refreshTimeout) + max7456ClearScreen(); + return; + } + + blinkState = (currentTime / 200000) % 2; + +#ifdef CMS + if (!displayIsGrabbed(osd7456DisplayPort)) { + osdUpdateAlarms(); + osdDrawElements(); +#ifdef OSD_CALLS_CMS + } else { + cmsUpdate(currentTime); +#endif + } +#endif +} + +/* + * Called periodically by the scheduler + */ +void osdUpdate(uint32_t currentTime) +{ + static uint32_t counter = 0; +#ifdef MAX7456_DMA_CHANNEL_TX + // don't touch buffers if DMA transaction is in progress + if (max7456DmaInProgres()) + return; +#endif // MAX7456_DMA_CHANNEL_TX + + // redraw values in buffer + if (counter++ % 5 == 0) + osdRefresh(currentTime); + else // rest of time redraw screen 10 chars per idle to don't lock the main idle + max7456DrawScreen(); + +#ifdef CMS + // do not allow ARM if we are in menu + if (displayIsGrabbed(osd7456DisplayPort)) { + DISABLE_ARMING_FLAG(OK_TO_ARM); + } +#endif +} + + #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7b5eae891f..e1c542ca77 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -17,7 +17,11 @@ #pragma once -#include +#define VISIBLE_FLAG 0x0800 +#define BLINK_FLAG 0x0400 +#define VISIBLE(x) (x & VISIBLE_FLAG) +#define BLINK(x) ((x & BLINK_FLAG) && blinkState) +#define BLINK_OFF(x) (x & ~BLINK_FLAG) typedef enum { OSD_RSSI_VALUE, @@ -36,16 +40,16 @@ typedef enum { OSD_GPS_SPEED, OSD_GPS_SATS, OSD_ALTITUDE, - OSD_MAX_ITEMS, // MUST BE LAST -} osd_items_t; + OSD_ITEM_COUNT // MUST BE LAST +} osd_items_e; typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC -} osd_unit_t; +} osd_unit_e; -typedef struct { - uint16_t item_pos[OSD_MAX_ITEMS]; +typedef struct osd_profile_s { + uint16_t item_pos[OSD_ITEM_COUNT]; // Alarms uint8_t rssi_alarm; @@ -54,17 +58,12 @@ typedef struct { uint16_t alt_alarm; uint8_t video_system; - osd_unit_t units; + uint8_t row_shiftdown; + + osd_unit_e units; } osd_profile_t; -typedef struct { - int16_t max_speed; - int16_t min_voltage; // /10 - int16_t max_current; // /10 - int16_t min_rssi; - int16_t max_altitude; -} statistic_t; - -void updateOsd(uint32_t currentTime); void osdInit(void); -void resetOsdConfig(osd_profile_t *osdProfile); +void osdResetConfig(osd_profile_t *osdProfile); +void osdResetAlarms(void); +void osdUpdate(uint32_t currentTime); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f024d1ae31..46c5de156d 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -35,6 +35,8 @@ uint8_t cliMode = 0; #include "build/debug.h" #include "build/version.h" +#include "cms/cms.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -50,6 +52,7 @@ uint8_t cliMode = 0; #include "drivers/flash.h" #include "drivers/io.h" #include "drivers/io_impl.h" +#include "drivers/dma.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" @@ -147,7 +150,8 @@ static void cliTasks(char *cmdline); #endif static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) +static void printResource(uint8_t dumpMask, master_t *defaultConfig); static void cliResource(char *cmdline); #endif #ifdef GPS @@ -202,6 +206,7 @@ typedef enum { DUMP_ALL = (1 << 3), DO_DIFF = (1 << 4), SHOW_DEFAULTS = (1 << 5), + HIDE_UNUSED = (1 << 6), } dumpFlags_e; static const char* const emptyName = "-"; @@ -236,7 +241,7 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } }; -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) // sync this with sensors_e static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL @@ -330,7 +335,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), @@ -933,7 +938,7 @@ const clivalue_t valueTable[] = { { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, #endif #ifdef LED_STRIP - { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, + { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef USE_RTC6705 { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, @@ -942,6 +947,7 @@ const clivalue_t valueTable[] = { #ifdef OSD { "osd_video_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.video_system, .config.minmax = { 0, 2 } }, + { "osd_row_shiftdown", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.row_shiftdown, .config.minmax = { 0, 1 } }, { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.osdProfile.units, .config.lookup = { TABLE_UNIT } }, { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.rssi_alarm, .config.minmax = { 0, 100 } }, @@ -1240,12 +1246,12 @@ static void printSerial(uint8_t dumpMask, master_t *defaultConfig) serialConfig_t *serialConfigDefault; bool equalsDefault; for (uint32_t i = 0; i < SERIAL_PORT_COUNT; i++) { - serialConfig = &masterConfig.serialConfig; + serialConfig = &masterConfig.serialConfig; if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { continue; }; - serialConfigDefault = &defaultConfig->serialConfig; - equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + serialConfigDefault = &defaultConfig->serialConfig; + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex @@ -1703,8 +1709,8 @@ static void printLed(uint8_t dumpMask, master_t *defaultConfig) char ledConfigBuffer[20]; char ledConfigDefaultBuffer[20]; for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig = masterConfig.ledConfigs[i]; - ledConfigDefault = defaultConfig->ledConfigs[i]; + ledConfig = masterConfig.ledStripConfig.ledConfigs[i]; + ledConfigDefault = defaultConfig->ledStripConfig.ledConfigs[i]; equalsDefault = ledConfig == ledConfigDefault; generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); generateLedConfig(&ledConfigDefault, ledConfigDefaultBuffer, sizeof(ledConfigDefaultBuffer)); @@ -1741,8 +1747,8 @@ static void printColor(uint8_t dumpMask, master_t *defaultConfig) hsvColor_t *colorDefault; bool equalsDefault; for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - color = &masterConfig.colors[i]; - colorDefault = &defaultConfig->colors[i]; + color = &masterConfig.ledStripConfig.colors[i]; + colorDefault = &defaultConfig->ledStripConfig.colors[i]; equalsDefault = color->h == colorDefault->h && color->s == colorDefault->s && color->v == colorDefault->v; @@ -1787,21 +1793,26 @@ static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) { for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = masterConfig.modeColors[i].color[j]; - int colorIndexDefault = defaultConfig->modeColors[i].color[j]; + int colorIndex = masterConfig.ledStripConfig.modeColors[i].color[j]; + int colorIndexDefault = defaultConfig->ledStripConfig.modeColors[i].color[j]; const char *format = "mode_color %u %u %u\r\n"; cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, i, j, colorIndexDefault); cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, i, j, colorIndex); } } + const char *format = "mode_color %u %u %u\r\n"; for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - int colorIndex = masterConfig.specialColors.color[j]; - int colorIndexDefault = defaultConfig->specialColors.color[j]; - const char *format = "mode_color %u %u %u\r\n"; + int colorIndex = masterConfig.ledStripConfig.specialColors.color[j]; + int colorIndexDefault = defaultConfig->ledStripConfig.specialColors.color[j]; cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndexDefault); cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndex); } + + int ledStripAuxChannel = masterConfig.ledStripConfig.ledstrip_aux_channel; + int ledStripAuxChannelDefault = defaultConfig->ledStripConfig.ledstrip_aux_channel; + cliDefaultPrintf(dumpMask, ledStripAuxChannel == ledStripAuxChannelDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannelDefault); + cliDumpPrintf(dumpMask, ledStripAuxChannel == ledStripAuxChannelDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannel); } static void cliModeColor(char *cmdline) @@ -1954,11 +1965,11 @@ static void cliServo(char *cmdline) static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) { for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - servoMixer_t customServoMixer = masterConfig.customServoMixer[i]; - servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; + servoMixer_t customServoMixer = masterConfig.customServoMixer[i]; + servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; if (customServoMixer.rate == 0) { break; - } + } bool equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel && customServoMixer.inputSource == customServoMixerDefault.inputSource @@ -1968,7 +1979,7 @@ static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) && customServoMixer.max == customServoMixerDefault.max && customServoMixer.box == customServoMixerDefault.box; - const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; + const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; cliDefaultPrintf(dumpMask, equalsDefault, format, i, customServoMixerDefault.targetChannel, @@ -1991,7 +2002,7 @@ static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) ); } - cliPrint("\r\n"); + cliPrint("\r\n"); // print servo directions for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -2019,7 +2030,7 @@ static void cliServoMix(char *cmdline) len = strlen(cmdline); if (len == 0) { - printServoMix(DUMP_MASTER, NULL); + printServoMix(DUMP_MASTER, NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer)); @@ -2364,7 +2375,7 @@ static void cliVtx(char *cmdline) static void printName(uint8_t dumpMask) { bool equalsDefault = strlen(masterConfig.name) == 0; - cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : masterConfig.name); + cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : masterConfig.name); } static void cliName(char *cmdline) @@ -2592,7 +2603,7 @@ static void cliMap(char *cmdline) parseRcChannels(cmdline, &masterConfig.rxConfig); } cliPrint("Map: "); - uint32_t i; + uint32_t i; for (i = 0; i < 8; i++) out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; out[i] = '\0'; @@ -2738,6 +2749,11 @@ static void printConfig(char *cmdline, bool doDiff) #endif printName(dumpMask); +#ifndef CLI_MINIMAL_VERBOSITY + cliPrint("\r\n# resources\r\n"); +#endif + printResource(dumpMask, &defaultConfig); + #ifndef USE_QUAD_MIXER_ONLY #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# mixer\r\n"); @@ -2988,12 +3004,18 @@ static void cliEscPassthrough(char *cmdline) break; case 1: index = atoi(pch); - if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { - printf("passthru at pwm output %d enabled\r\n", index); + if(mode == 2 && index == 255) + { + printf("passthru on all pwm outputs enabled\r\n"); } - else { - printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); - return; + else{ + if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { + printf("passthru at pwm output %d enabled\r\n", index); + } + else { + printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); + return; + } } break; } @@ -3095,11 +3117,11 @@ static void cliMotor(char *cmdline) cliShowArgumentRangeError("value", 1000, 2000); return; } else { - motor_disarmed[motor_index] = motor_value; + motor_disarmed[motor_index] = convertExternalToMotor(motor_value); } } - cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); + cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); } static void cliPlaySound(char *cmdline) @@ -3601,7 +3623,8 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintf("# BetaFlight/%s %s %s / %s (%s)\r\n", + cliPrintf("# %s/%s %s %s / %s (%s)\r\n", + FC_FIRMWARE_NAME, targetName, FC_VERSION_STRING, buildDate, @@ -3720,7 +3743,7 @@ void cliProcess(void) } } -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) typedef struct { const uint8_t owner; @@ -3730,30 +3753,92 @@ typedef struct { const cliResourceValue_t resourceTable[] = { #ifdef BEEPER - { OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 }, + { OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 }, #endif - { OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS }, + { OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS }, #ifdef USE_SERVOS - { OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS }, + { OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS }, #endif #ifndef SKIP_RX_PWM_PPM - { OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 }, - { OWNER_PWMINPUT, &masterConfig.pwmConfig.ioTags[0], PWM_INPUT_PORT_COUNT }, + { OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 }, + { OWNER_PWMINPUT, &masterConfig.pwmConfig.ioTags[0], PWM_INPUT_PORT_COUNT }, #endif #ifdef SONAR { OWNER_SONAR_TRIGGER, &masterConfig.sonarConfig.triggerTag, 0 }, - { OWNER_SONAR_ECHO, &masterConfig.sonarConfig.echoTag, 0 }, + { OWNER_SONAR_ECHO, &masterConfig.sonarConfig.echoTag, 0 }, +#endif +#ifdef LED_STRIP + { OWNER_LED_STRIP, &masterConfig.ledStripConfig.ioTag, 0 }, #endif }; +static void printResource(uint8_t dumpMask, master_t *defaultConfig) +{ + for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { + const char* owner; + owner = ownerNames[resourceTable[i].owner]; + + if (resourceTable[i].maxIndex > 0) { + for (int index = 0; index < resourceTable[i].maxIndex; index++) { + ioTag_t ioPtr = *(resourceTable[i].ptr + index); + ioTag_t ioPtrDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + + IO_t io = IOGetByTag(ioPtr); + IO_t ioDefault = IOGetByTag(ioPtrDefault); + bool equalsDefault = io == ioDefault; + const char *format = "resource %s %d %c%02d\r\n"; + const char *formatUnassigned = "resource %s %d NONE\r\n"; + if (DEFIO_TAG_ISEMPTY(ioDefault)) { + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } else { + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + } + if (DEFIO_TAG_ISEMPTY(io)) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } + } else { + cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + } else { + ioTag_t ioPtr = *resourceTable[i].ptr; + ioTag_t ioPtrDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + + IO_t io = IOGetByTag(ioPtr); + IO_t ioDefault = IOGetByTag(ioPtrDefault); + bool equalsDefault = io == ioDefault; + const char *format = "resource %s %c%02d\r\n"; + const char *formatUnassigned = "resource %s NONE\r\n"; + if (DEFIO_TAG_ISEMPTY(ioDefault)) { + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + } else { + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + } + if (DEFIO_TAG_ISEMPTY(io)) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + } + } else { + cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + } +} + static void cliResource(char *cmdline) { - int len; - len = strlen(cmdline); + int len = strlen(cmdline); if (len == 0) { - cliPrintf("IO:\r\n----------------------\r\n"); - for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { + printResource(DUMP_MASTER | HIDE_UNUSED, NULL); + + return; + } else if (strncasecmp(cmdline, "list", len) == 0) { +#ifndef CLI_MINIMAL_VERBOSITY + cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n----------------------\r\n"); +#endif + for (int i = 0; i < DEFIO_IO_USED_COUNT; i++) { const char* owner; owner = ownerNames[ioRecs[i].owner]; @@ -3766,34 +3851,25 @@ static void cliResource(char *cmdline) cliPrintf("%c%02d: %s %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, resource); } } - cliPrintf("\r\nUse: 'resource list' to see how to change resources.\r\n"); - return; - } else if (strncasecmp(cmdline, "list", len) == 0) { - for (uint8_t i = 0; i < ARRAYLEN(resourceTable); i++) { - const char* owner; - owner = ownerNames[resourceTable[i].owner]; - if (resourceTable[i].maxIndex > 0) { - for (int index = 0; index < resourceTable[i].maxIndex; index++) { - - if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr + index))) { - continue; - } - - IO_t io = IOGetByTag(*(resourceTable[i].ptr + index)); - if (!io) { - continue; - } - cliPrintf("resource %s %d %c%02d\r\n", owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); - } + cliPrintf("\r\n\r\nCurrently active DMA:\r\n"); + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + const char* owner; + owner = ownerNames[dmaGetOwner(i)]; + + cliPrintf(DMA_OUTPUT_STRING, i / DMA_MOD_VALUE + 1, (i % DMA_MOD_VALUE) + DMA_MOD_OFFSET); + uint8_t resourceIndex = dmaGetResourceIndex(i); + if (resourceIndex > 0) { + cliPrintf(" %s%d\r\n", owner, resourceIndex); } else { - if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr))) { - continue; - } - IO_t io = IOGetByTag(*resourceTable[i].ptr); - cliPrintf("resource %s %c%02d\r\n", owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); - } + cliPrintf(" %s\r\n", owner); + } } + +#ifndef CLI_MINIMAL_VERBOSITY + cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n"); +#endif + return; } @@ -3834,7 +3910,11 @@ static void cliResource(char *cmdline) cliPrintf("Resource is freed!"); return; } else { - uint8_t port = (*pch)-'A'; + uint8_t port = (*pch) - 'A'; + if (port >= 8) { + port = (*pch) - 'a'; + } + if (port < 8) { pch++; pin = atoi(pch); @@ -3859,7 +3939,9 @@ static void cliResource(char *cmdline) void cliDfu(char *cmdLine) { UNUSED(cmdLine); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\nRestarting in DFU mode"); +#endif cliRebootEx(true); } diff --git a/src/main/main.c b/src/main/main.c index 442964e9c1..fb0b7d8b81 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -29,8 +29,9 @@ #include "common/maths.h" #include "common/printf.h" -#include "drivers/nvic.h" +#include "cms/cms.h" +#include "drivers/nvic.h" #include "drivers/sensor.h" #include "drivers/system.h" #include "drivers/dma.h" @@ -82,11 +83,12 @@ #include "io/servos.h" #include "io/gimbal.h" #include "io/ledstrip.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" #include "io/osd.h" +#include "io/displayport_msp.h" #include "io/vtx.h" #include "scheduler/scheduler.h" @@ -233,10 +235,6 @@ 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), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); @@ -395,9 +393,13 @@ void init(void) initBoardAlignment(&masterConfig.boardAlignment); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayInit(&masterConfig.rxConfig); +#ifdef CMS + cmsInit(); +#endif + +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardInit(&masterConfig.rxConfig); } #endif @@ -454,6 +456,10 @@ void init(void) mspFcInit(); mspSerialInit(); +#if defined(USE_MSP_DISPLAYPORT) && defined(CMS) + cmsDisplayPortRegister(displayPortMspInit()); +#endif + #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif @@ -482,7 +488,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); + ledStripInit(&masterConfig.ledStripConfig); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); @@ -585,13 +591,13 @@ void init(void) if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY - displayShowFixedPage(PAGE_GPS); + dashboardShowFixedPage(PAGE_GPS); #else - displayResetPageCycling(); - displayEnablePageCycling(); + dashboardResetPageCycling(); + dashboardEnablePageCycling(); #endif } #endif diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 8d14b4d698..84f0a60a1b 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 21 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 22 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -216,6 +216,9 @@ #define MSP_OSD_VIDEO_CONFIG 180 #define MSP_SET_OSD_VIDEO_CONFIG 181 +// External OSD displayport mode messages +#define MSP_DISPLAYPORT 182 + // // Multwii original MSP commands // diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index d3f68b125a..9b38932bfd 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -120,7 +120,7 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l #define JUMBO_FRAME_SIZE_LIMIT 255 -static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) +static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) { serialBeginWrite(msp->port); const int len = sbufBytesRemaining(&packet->buf); @@ -140,6 +140,7 @@ static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) } serialWrite(msp->port, checksum); serialEndWrite(msp->port); + return sizeof(hdr) + len + 1; // header, data, and checksum } static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) @@ -211,3 +212,57 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) +{ + static uint8_t pushBuf[30]; + int ret = 0; + + mspPacket_t push = { + .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), }, + .cmd = cmd, + .result = 0, + }; + + for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + sbufWriteData(&push.buf, data, datalen); + + sbufSwitchToReader(&push.buf, pushBuf); + + ret = mspSerialEncode(mspPort, &push); + } + return ret; // return the number of bytes written +} + +uint32_t mspSerialTxBytesFree() +{ + uint32_t ret = UINT32_MAX; + + for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + const uint32_t bytesFree = serialTxBytesFree(mspPort->port); + if (bytesFree < ret) { + ret = bytesFree; + } + } + + return ret; +} diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index f2f071464b..f6e2954a24 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -66,3 +66,5 @@ void mspSerialInit(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen); +uint32_t mspSerialTxBytesFree(void); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 0ca7f0b32d..9530450208 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -108,20 +108,16 @@ static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT]; uint8_t spektrumFrameStatus(void) { - uint8_t b; - uint16_t fade; - uint32_t current_secs; - if (!rcFrameComplete) { return RX_FRAME_PENDING; } - + rcFrameComplete = false; // Fetch the fade count - fade = (spekFrame[0] << 8) + spekFrame[1]; - current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC); - + const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1]; + const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC); + if (spek_fade_last_sec == 0) { // This is the first frame status received. spek_fade_last_sec_count = fade; @@ -140,12 +136,11 @@ uint8_t spektrumFrameStatus(void) spek_fade_last_sec_count = fade; spek_fade_last_sec = current_secs; } - - - for (b = 3; b < SPEK_FRAME_SIZE; b += 2) { - uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); + + for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) { + const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { - if(rssi_channel != 0 && spekChannel != rssi_channel) { + if(rssi_channel == 0 || spekChannel != rssi_channel) { spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; } } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f44cd497f9..d43b778aa5 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -67,8 +67,8 @@ typedef enum { #if defined(BARO) || defined(SONAR) TASK_ALTITUDE, #endif -#ifdef DISPLAY - TASK_DISPLAY, +#ifdef USE_DASHBOARD + TASK_DASHBOARD, #endif #ifdef TELEMETRY TASK_TELEMETRY, @@ -85,6 +85,9 @@ typedef enum { #ifdef USE_BST TASK_BST_MASTER_PROCESS, #endif +#ifdef CMS + TASK_CMS, +#endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index acb1586cfe..1a9a98d65a 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -21,19 +21,20 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, //LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, //LED_STRIP }; diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index 2698fa6ec5..bf112222f2 100644 --- a/src/main/target/AIORACERF3/target.h +++ b/src/main/target/AIORACERF3/target.h @@ -125,14 +125,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 6ead0d6fee..a6b455f81b 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -21,15 +21,17 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 1b8f6c2111..eb848b4532 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -89,7 +89,6 @@ #define SENSORS_SET (SENSOR_ACC) #undef GPS -#define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -101,34 +100,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#if 1 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 -#endif - -#if 0 -// Alternate LED strip pin -// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 -#define LED_STRIP_TIMER TIM17 -#define USE_LED_STRIP_ON_DMA1_CHANNEL7 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource7 -#define WS2811_TIMER TIM17 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 -#define WS2811_DMA_CHANNEL DMA1_Channel7 -#define WS2811_IRQ DMA1_Channel7_IRQn -#endif - #define SPEKTRUM_BIND // USART2, PB4 @@ -144,6 +115,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index 242a1945f9..0ba72a6bca 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM8 - RC8 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_11}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM | TIM_USE_LED, 0, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0}, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_11, NULL, 0}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0} // PWM14 - OUT6 }; diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index e77e042cad..7e88ffe13d 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -85,12 +85,6 @@ #define VBAT_ADC_PIN PA4 #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_CHANNEL DMA1_Channel6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_2 #define GPS diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index e838c785ba..b7a0fa3288 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.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 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; - diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index d4e7d32467..875d5e5122 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -59,11 +59,6 @@ #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 WS2811_TIMER_GPIO_AF GPIO_AF_2 #undef GPS diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index fc7861550b..71849ec4dc 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -24,18 +24,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // up to 10 Motor Outputs - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - - // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index dcdce3e43c..36d786ff20 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,19 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 - { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 - { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 - { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 + { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 + { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 + { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 }; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index bc899a217d..f9c45d08df 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -147,12 +147,6 @@ // LED strip configuration using RC5 pin. //#define LED_STRIP -//#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -//#define WS2811_PIN PB15 // TIM8_CH3 -//#define WS2811_TIMER TIM8 -//#define WS2811_DMA_CHANNEL DMA1_Channel3 -//#define WS2811_IRQ DMA1_Channel3_IRQn - #define SPEKTRUM_BIND // USART2, PA3 #define BIND_PIN PA3 diff --git a/src/main/target/ANYFCF7/README.md b/src/main/target/ANYFCF7/README.md new file mode 100644 index 0000000000..bf8b4c7406 --- /dev/null +++ b/src/main/target/ANYFCF7/README.md @@ -0,0 +1,21 @@ +# AnyFC-F7 + +* The first F7 board flown with betaflight and inavflight, made by [@sambas](https://github.com/sambas) +* OSHW CC BY-SA 3.0 +* Source: https://github.com/sambas/hw/tree/master/AnyFCF7 +* 1st betaflight: https://www.youtube.com/watch?v=tv7k3A0FG80 +* 1st inavflight: https://www.youtube.com/watch?v=kJvlZAzprBs + +## HW info + +* STM32F745VGT6 100lqfp 216MHz +* MPU6000 SPI +* MS5611 baro +* All 8 uarts available + VCP +* 10 pwm outputs + 6 inputs +* external I2C +* external SPI (shared with U4/5) +* support for CAN +* SD card logging (SPI) +* 3 AD channels, one with 10k/1k divider, two with 1k series resistor + diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index d3b944ee70..f05f3ceeae 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -23,48 +23,50 @@ #include "drivers/timer.h" +#if defined(USE_DSHOT) // 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 + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_USE_PWM, 0, 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 + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; - -/* STANDARD LAYOUT +#else +// 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 + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // 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 + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S6_OUT 2 + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, 0 }, // S2_OUT + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, NULL, 0, 0 }, // S7_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; -*/ +#endif + // ALTERNATE LAYOUT //const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index c36c5b529a..f1c5546a53 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -142,18 +142,6 @@ #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) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 26c8c44a73..7a1a904534 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -14,26 +14,26 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - //Target code By Hector "Hectech FPV" Hind + //Target code By BorisB and Hector "Hectech FPV" Hind #include #include #include "drivers/io.h" + #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM IN + DEF_TIM(TIM16,CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM1 + DEF_TIM(TIM1, CH1N,PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM2 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 + DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM7 + DEF_TIM(TIM15,CH1, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM8 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP --requires resource remap with dshot (remap to motor 5??)-- }; - diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 9be40e8489..7c911b3669 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -14,28 +14,22 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - //Target code By Hector "Hectech FPV" Hind + //Target code By BorisB and Hector "Hectech FPV" Hind #pragma once -#define TARGET_BOARD_IDENTIFIER "BFFC" +#define TARGET_BOARD_IDENTIFIER "BFF3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE - -//#define LED0 PC14 #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USABLE_TIMER_CHANNEL_COUNT 10 -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - -#define MPU6000_CS_PIN PC13 +#define MPU6000_CS_PIN PA15 #define MPU6000_SPI_INSTANCE SPI1 - #define GYRO #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW180_DEG @@ -46,14 +40,15 @@ // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define EXTI_CALLBACK_HANDLER_COUNT 1 #define MPU_INT_EXTI PC13 #define USE_EXTI -#define USB_IO +#define USE_DSHOT +#define REMAP_TIM16_DMA +#define REMAP_TIM17_DMA -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 +#define USB_IO #define USE_VCP #define USE_UART1 @@ -68,11 +63,11 @@ #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_TX_PIN PA2 #define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 5 @@ -83,7 +78,6 @@ #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 -//GPIO_AF_1 #define SPI1_NSS_PIN PA15 #define SPI1_SCK_PIN PB3 @@ -109,7 +103,6 @@ #define SDCARD_DETECT_PIN PC14 #define SDCARD_SPI_INSTANCE SPI2 -//#define SDCARD_SPI_CS_GPIO SPI2_GPIO #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 @@ -117,9 +110,6 @@ #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 -//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -//#define SDCARD_DMA_CHANNEL DMA_Channel_0 - #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC @@ -130,15 +120,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -156,4 +137,4 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index c82953b221..8bbbe806a9 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -22,13 +22,27 @@ #include "drivers/timer.h" #include "drivers/dma.h" +#include "drivers/timer_def.h" + +/* const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT +}; +*/ + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // S1_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 0 ), // S4_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR | TIM_USE_LED, 0, 0 ), // S5_OUT + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S6_OUT }; diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 50b0d87ddd..5341a536a3 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -145,16 +145,6 @@ #define USE_DSHOT #define LED_STRIP -// LED Strip can run off Pin 6 (PB1) of the ESC outputs. -#define WS2811_PIN PB1 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_4 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM3 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn - #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index c0a1dd07c2..9d34d96dc1 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -23,17 +23,17 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // S1_IN - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // S4_IN - Current - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // S5_IN - Vbattery - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // S6_IN - PPM IN - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP }, // S1_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP }, // S2_OUT - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP }, // S3_OUT - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP }, // S4_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP } // S6_OUT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_PWM, 0, }, // S1_IN + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, }, // S4_IN - Current + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM, 0, }, // S5_IN - Vbattery + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM | TIM_USE_PPM, 0, }, // S6_IN - PPM IN + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, }, // S1_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, }, // S2_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, }, // S3_OUT + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, }, // S4_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, } // S6_OUT }; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 7dc877cd1d..e57b5cbfa8 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -96,12 +96,6 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -//#define LED_STRIP -//#define WS2811_PIN PB4 -//#define WS2811_TIMER TIM3 -//#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER - #define SPEKTRUM_BIND // USART3, PB11 (Flexport) #define BIND_PIN PB11 diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 8867ca53e6..6484ee4cc6 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -24,23 +24,23 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM6 - PC8 - { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM7 - PF9 - { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM8 - PF10 - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM14 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM15 - PA3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM16 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM17 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 } // PWM18 - PA4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM1 - PA8 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM2 - PB8 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM3 - PB9 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM4 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM5 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM6 - PC8 + { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM7 - PF9 + { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM8 - PF10 + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PD12 + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM10 - PD13 + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM11 - PD14 + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM12 - PD15 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM13 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM14 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM15 - PA3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM16 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM17 - PB1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 } // PWM18 - PA4 }; diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index d784dfcb9f..2478c668d4 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -23,19 +23,19 @@ #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 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index b9cb1b1704..0c273a64e6 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -22,23 +22,24 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S8_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM10 }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM11 }, // S8_OUT + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1, NULL, 0, 0 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8 , NULL, 0, 0 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM2 , NULL, 0, 0 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM5 , NULL, 0, 0 }, // S8_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 , NULL, 0, 0 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM12, NULL, 0, 0 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM10, NULL, 0, 0 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM11, NULL, 0, 0 }, // S8_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_LED , 0, GPIO_AF_TIM11, DMA1_Stream3, DMA_Channel_2, DMA1_ST3_HANDLER }, // S8_OUT }; diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index d30ab8e774..702f04c740 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -122,16 +122,6 @@ #define SENSORS_SET (SENSOR_ACC) #define LED_STRIP -#define WS2811_PIN PB7 // Shared UART1 -#define WS2811_TIMER TIM4 -#define WS2811_TIMER_CHANNEL TIM_Channel_2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST3_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream3 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF3 -#define WS2811_DMA_IT DMA_IT_TCIF3 -#define WS2811_DMA_CHANNEL DMA_Channel_2 -#define WS2811_DMA_IRQ DMA1_Stream3_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM4 // alternative defaults for Colibri/Gemini target #define TARGET_CONFIG @@ -148,5 +138,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define USABLE_TIMER_CHANNEL_COUNT 16 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 980bde07d6..1981cfd144 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -28,8 +28,10 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "rx/rx.h" -#include "rx/msp.h" +#include "fc/config.h" +#include "fc/mw.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" #include "io/motors.h" #include "io/servos.h" @@ -40,7 +42,8 @@ #include "io/flashfs.h" #include "io/beeper.h" -#include "telemetry/telemetry.h" +#include "rx/rx.h" +#include "rx/msp.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -51,6 +54,8 @@ #include "sensors/compass.h" #include "sensors/gyro.h" +#include "telemetry/telemetry.h" + #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" @@ -58,11 +63,6 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" -#include "fc/config.h" -#include "fc/mw.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - #include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" @@ -935,7 +935,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) #ifdef LED_STRIP case BST_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; bstWrite16(color->h); bstWrite8(color->s); bstWrite8(color->v); @@ -944,7 +944,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) case BST_LED_STRIP_CONFIG: for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; bstWrite32(*ledConfig); } break; @@ -1141,7 +1141,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) break; case BST_SET_MOTOR: for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 - motor_disarmed[i] = bstRead16(); + motor_disarmed[i] = convertExternalToMotor(bstRead16()); break; case BST_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS @@ -1367,7 +1367,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { { i = bstRead8(); - hsvColor_t *color = &masterConfig.colors[i]; + hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; color->h = bstRead16(); color->s = bstRead8(); color->v = bstRead8(); @@ -1380,7 +1380,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; break; } - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; *ledConfig = bstRead32(); reevaluateLedConfig(); } diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 9bf7e986c0..8cfeb6edbb 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -18,21 +18,22 @@ #include #include + #include "drivers/io.h" - +#include "drivers/dma.h" #include "drivers/timer.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 -}; +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15 +}; diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 2c4fd4c334..1959a3e952 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -116,14 +116,6 @@ #define LED_STRIP -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 - #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -138,6 +130,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index b75f0e7dd8..19c10eefd4 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -21,19 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" +#include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB9 - - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM9 - PB0 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0), // PWM1 - PA8 + DEF_TIM(TIM8, CH2, PB8, TIM_USE_MOTOR, 1), // PWM2 - PB8, DMA2ch5 + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, 1), // PWM3 - PB9, DMA2ch1 + DEF_TIM(TIM2, CH4, PA10, TIM_USE_MOTOR, 1), // PMW4 - PA10, DMA1ch7 + DEF_TIM(TIM2, CH3, PA9, TIM_USE_MOTOR, 1), // PWM5 - PA9, DMA1ch1 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1), // PWM6 - PA0 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM7 - PA1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM8 - PB1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM9 - PB0 + DEF_TIM(TIM16, CH1, PA6, TIM_USE_LED, 1), // PWM9 - PB0 }; - diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index dc31a1640a..fd2ea81494 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -113,6 +113,7 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 +#define ADC24_DMA_REMAP // moves ADC2 DMA from DMA2ch1 to DMA2ch3. #define VBAT_ADC_PIN PA4 #define CURRENT_METER_ADC_PIN PA5 @@ -124,16 +125,7 @@ #define ENSURE_MPU_DATA_READY_IS_LOW #define LED_STRIP - -// tqfp48 pin 16 -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 +#define USE_DSHOT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -152,6 +144,6 @@ // timer definitions in drivers/timer.c // channel mapping in drivers/pwm_mapping.c // only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 42608606e9..249e4dede6 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,24 +6,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT - - { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S8_IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S8_OUT + { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM_USE_MOTOR, 0, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 504e3b6c2d..a897bf38a0 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -25,16 +25,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 - { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 - { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, (1 | TIMER_OUTPUT_N_CHANNEL), IOCFG_AF_PP, GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel4, DMA1_CH4_HANDLER }, // PWM4 - S1 + { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 + { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM6 - S3 + { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM7 - S4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO TIMER - LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO TIMER - LED_STRIP }; - diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index c7292349b6..52c673af86 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -148,15 +148,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define SONAR #define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN PB0 @@ -171,12 +162,12 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define REMAP_TIM17_DMA #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 8 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index af6feb53c6..a57e0e3351 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -24,12 +24,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN - - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT // { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip }; diff --git a/src/main/target/FURYF7/README.md b/src/main/target/FURYF7/README.md new file mode 100644 index 0000000000..9b6255a618 --- /dev/null +++ b/src/main/target/FURYF7/README.md @@ -0,0 +1,18 @@ +# FURYF7 + +* STM32F745VGT6 +* ICM20689 SPI Gyro +* MS5611 baro - SPI (Not Currently Woerking. Need to write driver for MS5611 SPI) +* VCP +* 3 UARTS (UART1, UART3, UART6) +* 4 PWM outputs +* No PWM inputs (SBUS, PPM, Spek Sat) +* SD card (Not currently working, Fatal Error) +* 16MB Flash (Not currently working, no chip detected) +* ADC (RSSI, CURR, VBAT) +* I2C +* LED Strip +* Built in 5v 2A Regulator +* Spek Sat Connector +* SWD Port +* Beeper output \ No newline at end of file diff --git a/src/main/target/FURYF7/stm32f7xx_hal_conf.h b/src/main/target/FURYF7/stm32f7xx_hal_conf.h new file mode 100644 index 0000000000..20a305def5 --- /dev/null +++ b/src/main/target/FURYF7/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****/ \ No newline at end of file diff --git a/src/main/target/FURYF7/target.c b/src/main/target/FURYF7/target.c new file mode 100644 index 0000000000..eb48b71181 --- /dev/null +++ b/src/main/target/FURYF7/target.c @@ -0,0 +1,36 @@ + +/* + * 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" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_USE_PPM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // PPM_IN + + { TIM3, IO_TAG(PB0), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream7, DMA_CHANNEL_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream2, DMA_CHANNEL_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S4_OUT + +// { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip +}; \ No newline at end of file diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h new file mode 100644 index 0000000000..ae25088955 --- /dev/null +++ b/src/main/target/FURYF7/target.h @@ -0,0 +1,155 @@ +/* + * 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 "FYF7" + +#define CONFIG_START_FLASH_ADDRESS (0x080C0000) + +#define USBD_PRODUCT_STRING "FuryF7" + +#define USE_DSHOT + +#define LED0 PB5 +#define LED1 PB4 + +#define BEEPER PD10 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define ICM20689_CS_PIN PA4 +#define ICM20689_SPI_INSTANCE SPI1 + +#define GYRO +#define USE_GYRO_SPI_ICM20689 +#define GYRO_ICM20689_ALIGN CW180_DEG + +#define ACC +#define USE_ACC_SPI_ICM20689 +#define ACC_ICM20689_ALIGN CW180_DEG + +//#define BARO +//#define USE_BARO_MS5611 +//#define MS5611_I2C_INSTANCE I2CDEV_1 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +//#define USE_SPI_DEVICE_2 +//#define SPI2_NSS_PIN PB12 +//#define SPI2_SCK_PIN PB13 +//#define SPI2_MISO_PIN PB14 +//#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#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 PD2 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD +#define SDCARD_DETECT_EXTI_IRQn EXTI2_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 // 13.5MHz + +#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_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PA15 +#define M25P16_SPI_INSTANCE SPI3 + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +#define USE_I2C_PULLUP +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define SENSORS_SET (SENSOR_ACC) + +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define SPEKTRUM_BIND +// USART3 Rx, PB11 +#define BIND_PIN PB11 + +#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 USABLE_TIMER_CHANNEL_COUNT 5 +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8)) \ No newline at end of file diff --git a/src/main/target/FURYF7/target.mk b/src/main/target/FURYF7/target.mk new file mode 100644 index 0000000000..77cfc3e5f7 --- /dev/null +++ b/src/main/target/FURYF7/target.mk @@ -0,0 +1,7 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += SDCARD VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_icm20689.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c \ No newline at end of file diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index 04200bc0b4..d60b119917 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -19,16 +19,18 @@ #include #include "drivers/io.h" + #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // LED_STRIP + DEF_TIM(TIM2, CH1,PA15, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM IN + DEF_TIM(TIM8,CH2N, PB4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), // PWM1 + DEF_TIM(TIM17,CH1, PB5, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM4 + DEF_TIM(TIM16,CH1, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM5 + DEF_TIM(TIM17,CH1, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM6 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP }; - diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 41b18c9bf6..291605e0c4 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -46,6 +46,9 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 +#define USE_DSHOT +#define REMAP_TIM17_DMA + #define USE_VCP #define USE_UART1 #define USE_UART2 @@ -81,15 +84,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 370f9eb358..b0eb9d70b2 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -23,22 +23,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c index d998cdba92..03015e22ce 100644 --- a/src/main/target/ISHAPEDF3/target.c +++ b/src/main/target/ISHAPEDF3/target.c @@ -21,27 +21,27 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 , NULL, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, NULL, 0 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index d36fa9903a..a48fc93a02 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -19,23 +19,20 @@ #include #include "drivers/io.h" -#include "drivers/dma.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_6, DMA1_Channel5, DMA1_CH5_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_2, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, - - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_10, NULL, 0}, - { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_5, NULL, 0}, + DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM17,CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED), }; - diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index f8a6062776..fcb2c38a03 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -84,5 +84,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/LUXV2_RACE.mk b/src/main/target/LUX_RACE/LUXV2_RACE.mk new file mode 100755 index 0000000000..63550affa6 --- /dev/null +++ b/src/main/target/LUX_RACE/LUXV2_RACE.mk @@ -0,0 +1 @@ +# LUXR_RACE is new version diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 07c5716f90..d94c5a6869 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -21,18 +21,22 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, // PMW4 - PC8 + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM5 - PC9 +#ifndef LUXV2_RACE + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 +#endif + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, }; diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index e2491c73d6..4a3f30cbc7 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -17,16 +17,26 @@ #pragma once +#ifdef LUXV2_RACE +#define TARGET_BOARD_IDENTIFIER "LUXR" +#else #define TARGET_BOARD_IDENTIFIER "LUX" +#endif #define BOARD_HAS_VOLTAGE_DIVIDER #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define LED0 PC15 #define LED1 PC14 +#ifndef LUXV2_RACE #define LED2 PC13 +#endif +#ifdef LUXV2_RACE +#define BEEPER PB9 +#else #define BEEPER PB13 +#endif #define BEEPER_INVERTED // MPU6500 interrupt @@ -36,26 +46,72 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW +#define USE_DSHOT + #define USE_SPI #define USE_SPI_DEVICE_1 +#ifdef LUXV2_RACE +#define USE_SPI_DEVICE_2 +#endif #define SPI1_SCK_PIN PB3 #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 +//#ifndef LUXV2_RACE #define SPI1_NSS_PIN PA4 +//#endif +#ifdef LUXV2_RACE +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC13 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#endif + +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_INSTANCE SPI1 #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 #define GYRO +#ifdef LUXV2_RACE +#define USE_GYRO_MPU6000 +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG +#else #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG +#endif #define ACC +#ifdef LUXV2_RACE +#define USE_ACC_MPU6000 +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG +#else #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG +#endif #define USB_IO @@ -63,7 +119,13 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 +#ifdef LUXV2_RACE +#define USE_UART4 +#define USE_UART5 +#define SERIAL_PORT_COUNT 6 +#else #define SERIAL_PORT_COUNT 4 +#endif #define UART1_TX_PIN PC4 #define UART1_RX_PIN PC5 @@ -74,8 +136,7 @@ #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#undef USE_I2C #define USE_ADC #define ADC_INSTANCE ADC1 @@ -85,16 +146,14 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#ifdef LUXV2_RACE +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define DEFAULT_FEATURES FEATURE_BLACKBOX +#endif + #define SPEKTRUM_BIND // USART1, PC5 #define BIND_PIN PC5 @@ -108,6 +167,10 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 11 -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#ifdef LUXV2_RACE +#define USABLE_TIMER_CHANNEL_COUNT 6 +#else +#define USABLE_TIMER_CHANNEL_COUNT 12 +#endif +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/LUX_RACE/target.mk b/src/main/target/LUX_RACE/target.mk index 4833b16e4d..ca59497685 100644 --- a/src/main/target/LUX_RACE/target.mk +++ b/src/main/target/LUX_RACE/target.mk @@ -1,9 +1,9 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP +FEATURES = VCP SDCARD TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c - + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ No newline at end of file diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index e838c785ba..fd9678c799 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.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 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; - diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 2628264522..e29948be63 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -71,11 +71,6 @@ #define I2C_DEVICE (I2CDEV_2) #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 WS2811_TIMER_GPIO_AF GPIO_AF_2 #define SPEKTRUM_BIND // USART2, PA3 @@ -91,7 +86,6 @@ #undef GPS #undef USE_SERVOS #define USE_QUAD_MIXER_ONLY -#define DISPLAY // IO - assuming all IOs on 48pin package diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 43db149492..c9adecf16f 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -21,16 +21,19 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1 ), // PWM1 - PA4 - *TIM3_CH2 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1 ), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1 ), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0 ), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0 ), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 6a7ae42a0b..a2739251a7 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -91,7 +91,6 @@ #define SENSORS_SET (SENSOR_ACC) #undef GPS -#define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -103,35 +102,7 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#if 1 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 -#endif - -#if 0 -// Alternate LED strip pin -// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 -#define LED_STRIP_TIMER TIM17 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL7 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource7 -#define WS2811_TIMER TIM17 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 -#define WS2811_DMA_CHANNEL DMA1_Channel7 -#define WS2811_IRQ DMA1_Channel7_IRQn -#endif - +#define USE_DSHOT #define SPEKTRUM_BIND // USART2, PB4 @@ -147,6 +118,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 9efa2c5830..b47fe55744 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -46,6 +46,7 @@ void targetConfiguration(master_t *config) config->gyro_lpf = 1; config->gyro_soft_lpf_hz = 100; config->gyro_soft_notch_hz_1 = 0; + config->gyro_soft_notch_hz_2 = 0; /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { config->rxConfig.channelRanges[channel].min = 1180; @@ -53,12 +54,12 @@ void targetConfiguration(master_t *config) }*/ for (int profileId = 0; profileId < 2; profileId++) { - config->profile[profileId].pidProfile.P8[ROLL] = 55; - config->profile[profileId].pidProfile.I8[ROLL] = 50; - config->profile[profileId].pidProfile.D8[ROLL] = 25; - config->profile[profileId].pidProfile.P8[PITCH] = 65; - config->profile[profileId].pidProfile.I8[PITCH] = 60; - config->profile[profileId].pidProfile.D8[PITCH] = 28; + config->profile[profileId].pidProfile.P8[ROLL] = 70; + config->profile[profileId].pidProfile.I8[ROLL] = 70; + config->profile[profileId].pidProfile.D8[ROLL] = 30; + config->profile[profileId].pidProfile.P8[PITCH] = 80; + config->profile[profileId].pidProfile.I8[PITCH] = 80; + config->profile[profileId].pidProfile.D8[PITCH] = 30; config->profile[profileId].pidProfile.P8[YAW] = 180; config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; @@ -66,12 +67,14 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.levelSensitivity = 1.0f; for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { - config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 100; config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rcExpo8 = 20; config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80; + config->profile[profileId].pidProfile.dtermSetpointWeight = 254; config->profile[profileId].pidProfile.setpointRelaxRatio = 100; } } diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index af48f988d7..12ce8d40ed 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -21,21 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.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, 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 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PPM | TIM_USE_PWM, 0, NULL, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM | TIM_USE_LED, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6 }; - diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index eb8ab5503a..f048215e40 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -118,8 +118,6 @@ //#define SONAR_TRIGGER_PIN_PWM PB8 //#define SONAR_ECHO_PIN_PWM PB9 -//#define DISPLAY - #define USE_UART1 #define USE_UART2 /* only 2 uarts available on the NAZE, add ifdef here if present on other boards */ @@ -152,10 +150,6 @@ #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 #undef GPS @@ -163,7 +157,9 @@ // USART2, PA3 #define BIND_PIN PA3 +#if !defined(BRUSHED_MOTORS) #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index c194fc9752..58be31b640 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -25,18 +25,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 // UART3 RX/TX - //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) - //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 - //{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) + //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 05676f0046..beadee7a0d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -91,10 +91,20 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#define USE_DASHBOARD + +// Configuratoin Menu System +#define CMS +#define CMS_MAX_DEVICE 4 + +// Use external display connected by MSP to run CMS +#define USE_MSP_DISPLAYPORT + // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code #define OSD + // include the max7456 driver #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 @@ -127,9 +137,13 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 -// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define USE_DSHOT + +// DSHOT output 4 uses DMA1_Channel5, so don't use it for the SDCARD until we find an alternative +#ifndef USE_DSHOT #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +#endif // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING @@ -143,16 +157,7 @@ //#define RSSI_ADC_PIN PB1 //#define ADC_INSTANCE ADC3 -#define USE_DSHOT - #define LED_STRIP -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA @@ -196,5 +201,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 7 // PPM + 6 Outputs (2 shared with UART3) +#define USABLE_TIMER_CHANNEL_COUNT 8 // PPM + 6 Outputs (2 shared with UART3) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 67a0132080..5878246a44 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -9,9 +9,8 @@ TARGET_SRC = \ drivers/compass_ak8963.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ + drivers/max7456.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c \ - drivers/max7456.c \ - io/osd.c + io/transponder_ir.c diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 5e63b90590..eaaba9beec 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -24,17 +24,17 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT }; diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index d7137502d8..0fba1225a4 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -70,6 +70,9 @@ //#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER +#define CMS +#define USE_MSP_DISPLAYPORT + //#define PITOT //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT @@ -122,19 +125,9 @@ #define USE_DSHOT #define LED_STRIP -// LED Strip can run off Pin 5 (PA1) of the MOTOR 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_CHANNEL DMA_Channel_6 -#define WS2811_DMA_IRQ DMA1_Stream4_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 #define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 18034c1332..333e4cdcb9 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -5,6 +5,5 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c \ - io/osd.c + drivers/max7456.c diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 43db149492..4a9603edec 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -21,16 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 2b08b62b2e..9e8984bae3 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -81,14 +81,6 @@ #define VBAT_ADC_PIN PA5 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA @@ -117,6 +109,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c index cc2c688932..ff5ec100c7 100755 --- a/src/main/target/RACEBASE/target.c +++ b/src/main/target/RACEBASE/target.c @@ -20,15 +20,13 @@ #include #include "drivers/io.h" #include "drivers/timer.h" - +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, - - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - PC6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - PC7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PMW4 - PC8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PPM, 1, GPIO_AF_1, NULL, 0 }, + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PC9 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_2, DMA1_Channel2, DMA1_CH2_HANDLER }, // PWM5 - PC9 }; - - diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index fd44bd0415..470347b9a8 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -98,14 +98,6 @@ #define RSSI_ADC_PIN PA6 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define OSD @@ -124,9 +116,8 @@ #define TARGET_IO_PORTC (BIT(5)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USABLE_TIMER_CHANNEL_COUNT 6 #define USED_TIMERS (TIM_N(2) | TIM_N(4)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM4) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 8ac51e6713..63ac464d5d 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -22,12 +22,14 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - PA4 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - PA7 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM3 - PA8 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - PPM + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PPM + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER}, // PWM6 - PPM }; diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index a1f5f8527d..f0133e68f1 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -27,9 +27,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 6 - - #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL #define MPU_INT_EXTI PA15 @@ -107,15 +104,6 @@ #define LED_STRIP // LED strip configuration using PWM motor output pin 5. -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 - #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -130,7 +118,6 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE -#define DISPLAY #define USE_SERVOS #define USE_CLI @@ -145,5 +132,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USABLE_TIMER_CHANNEL_COUNT 7 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(17)) diff --git a/src/main/target/SPARKY2/SPARKY_OPBL.mk b/src/main/target/REVO/AIRBOTF4.mk similarity index 100% rename from src/main/target/SPARKY2/SPARKY_OPBL.mk rename to src/main/target/REVO/AIRBOTF4.mk diff --git a/src/main/target/REVO/REVOLT.mk b/src/main/target/REVO/REVOLT.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index f8be6e2eab..b9164d7b81 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -24,16 +24,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT +#ifdef REVOLT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_TIM4, DMA1_Stream0, DMA_Channel_2, DMA1_ST0_HANDLER }, // LED for REVOLT +#else + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT / LED for REVO + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT +#endif }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 4d73c99355..02080b4f52 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -17,43 +17,71 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "REVO" - #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) +#if defined(AIRBOTF4) +#define TARGET_BOARD_IDENTIFIER "AIR4" +#define USBD_PRODUCT_STRING "AirbotF4" + +#elif defined(REVOLT) +#define TARGET_BOARD_IDENTIFIER "RVLT" +#define USBD_PRODUCT_STRING "Revolt" + +#else +#define TARGET_BOARD_IDENTIFIER "REVO" #define USBD_PRODUCT_STRING "Revolution" + #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" #endif +#endif + #define USE_DSHOT #define LED0 PB5 -// Disable LED1, conflicts with AirbotF4/Flip32F4 beeper -//#define LED1 PB4 - +// Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper +#if defined(AIRBOTF4) || defined(REVOLT) #define BEEPER PB4 #define BEEPER_INVERTED +#else +#define LED1 PB4 +// Leave beeper here but with none as io - so disabled unless mapped. +#define BEEPER NONE +#endif -#define INVERTER PC0 // PC0 used as inverter select GPIO +// PC0 used as inverter select GPIO +#define INVERTER PC0 #define INVERTER_USART USART1 #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 + #define ACC #define USE_ACC_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW270_DEG +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG + #define GYRO #define USE_GYRO_SPI_MPU6000 #define ACC_MPU6000_ALIGN CW270_DEG +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU9250_ALIGN CW270_DEG + // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL +#if !defined(AIRBOTF4) && !defined(REVOLT) #define MAG #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW90_DEG @@ -67,6 +95,7 @@ //#define PITOT //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT +#endif #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 @@ -78,8 +107,8 @@ #define VBUS_SENSING_PIN PC5 #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 @@ -106,7 +135,7 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 @@ -114,19 +143,8 @@ //#define RSSI_ADC_PIN PA0 #define LED_STRIP -// LED Strip can run off Pin 5 (PA1) of the MOTOR outputs. -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 -#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_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 SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_BLACKBOX) @@ -140,7 +158,15 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#ifdef REVOLT +#define USABLE_TIMER_CHANNEL_COUNT 11 +#else #define USABLE_TIMER_CHANNEL_COUNT 12 +#endif + #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) + +#define CMS +#define USE_MSP_DISPLAYPORT diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 2711b19dac..3d8b57a8a6 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -3,5 +3,7 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9349b40100..44a60162f3 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -23,18 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_IN - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_IN - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_IN - { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_OUT - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S3_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S5_IN + { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S1_OUT + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S3_OUT + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 }, // S5_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index 61603c7b4e..6e71f3cd0d 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -21,24 +21,25 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0 ), // RC_CH1 - PA0 - *TIM2_CH1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0 ), // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0 ), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0 ), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 0 ), // RC_CH5 - PB4 - *TIM3_CH1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0 ), // RC_CH6 - PB5 - *TIM3_CH2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0 ), // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0 ), // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM16, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1 ), // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM4, CH1, PA11, TIM_USE_MOTOR, 1 ), // PWM3 - PA11 + DEF_TIM(TIM4, CH2, PA12, TIM_USE_MOTOR, 1 ), // PWM4 - PA12 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1 ), // PWM5 - PB8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1 ), // PWM6 - PB9 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1 ), // PWM7 - PA2 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM8 - PA3 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, 1 ), // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index e7c5744bd2..658f6528e9 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -95,15 +95,7 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP - -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 +#define USE_DSHOT #undef GPS diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 0926431f19..88b9213abc 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -21,17 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 TX - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 RX (NC) + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // SOFTSERIAL1 TX + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // LED_STRIP }; diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 75895fc12a..315e3e4a5b 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -88,15 +88,6 @@ #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index be26986a86..920ca000c5 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -24,15 +24,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB6 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 - - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PB6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB6 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y }; diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 8062644aa9..8f311405d7 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -137,8 +137,16 @@ //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_DASHBOARD + #define OSD +// Configuratoin Menu System +#define CMS + +// Use external display connected by MSP to run CMS +#define USE_MSP_DISPLAYPORT + #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_SERVOS diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index eac7fbb6ec..23a8b46be3 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -11,5 +11,4 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ drivers/vtx_soft_spi_rtc6705.c \ - drivers/max7456.c \ - io/osd.c + drivers/max7456.c diff --git a/src/main/target/SOULF4/target.c b/src/main/target/SOULF4/target.c index f0673d116c..3428d2e6a4 100644 --- a/src/main/target/SOULF4/target.c +++ b/src/main/target/SOULF4/target.c @@ -20,18 +20,20 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM (5th pin on FlexiIO port) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0 ), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0 ), // S2_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 0 ), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0 ), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR | TIM_USE_LED, 1, 0 ), // S6_OUT }; diff --git a/src/main/target/SOULF4/target.h b/src/main/target/SOULF4/target.h index aa8f6fa676..410841e747 100644 --- a/src/main/target/SOULF4/target.h +++ b/src/main/target/SOULF4/target.h @@ -104,13 +104,7 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define LED_STRIP -#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_CHANNEL DMA_Channel_6 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM5 +#define USE_DSHOT #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 44aa07090a..cfd1441285 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -21,21 +21,22 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 0, GPIO_AF_2, NULL, 0 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 0, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PWM, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index e98ef0aea8..d24276c1b4 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -86,14 +86,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_PIN PA6 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index 87abe2834b..1bf8f23511 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -21,19 +21,20 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM, 0, 0 ), // S2_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S3_IN - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S4_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0 ), // S1_OUT + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0 ), // S2_OUT + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 0 ), // S3_OUT + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0 ), // S4_OUT + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0 ), // S5_OUT - GPIO_PartialRemap_TIM3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 1, 0 ), // S6_OUT }; diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 28f480e4bd..69a120a06b 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -32,10 +32,11 @@ #define BEEPER PC9 #define BEEPER_INVERTED - #define INVERTER PC6 #define INVERTER_USART USART6 +#define USE_DSHOT + // MPU9250 interrupt #define USE_EXTI #define MPU_INT_EXTI PC5 @@ -118,9 +119,6 @@ #define VBAT_ADC_PIN PC3 #define CURRENT_METER_ADC_PIN PC2 -#define LED_STRIP -#define LED_STRIP_TIMER TIM5 - #define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index b96e424066..b09058af7c 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -24,25 +24,24 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easierTIM_USE_MOTOR, to using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 634444f94c..6a4e297877 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -103,18 +103,15 @@ #define RSSI_ADC_PIN PB2 #define USE_DSHOT +#define REMAP_TIM17_DMA + +// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 +#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) +#undef USE_UART1_TX_DMA +#endif #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -135,3 +132,10 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USE_DASHBOARD + +// Configuratoin Menu System +#define CMS + +// Use external display connected by MSP to run CMS +#define USE_MSP_DISPLAYPORT diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 16541c9aeb..526fa029e9 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -21,20 +21,21 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + DEF_TIM(TIM8, CH1, PA15, TIM_USE_PPM, 0 ), // PPM + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MOTOR, 1 ), // PWM1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1 ), // PWM2 + DEF_TIM(TIM15, CH1, PA2, TIM_USE_MOTOR, 1 ), // PWM3 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM4 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1 ), // PWM5 + DEF_TIM(TIM3, CH2, PA7, TIM_USE_MOTOR, 1 ), // PWM6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // PWM7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // PWM8 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 1 ), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 1 ), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 1 ), // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 6e3991b631..516d023dba 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -35,6 +35,7 @@ #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH +#define USE_DSHOT #define GYRO #define USE_GYRO_SPI_MPU6500 @@ -122,14 +123,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 13e7ee222c..7147c4a1fd 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -21,31 +21,32 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, 0, 1, GPIO_AF_1, NULL, 0 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 8cd9a3584f..bc29d23c14 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -24,19 +24,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, NULL, 0 }, - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, NULL, 0 }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, NULL, 0 }, - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 } + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_PPM | TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM17, IO_TAG(PB9), TIM_Channel_1, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, + { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM3, IO_TAG(PA4), TIM_Channel_2, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD12), TIM_Channel_1, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD13), TIM_Channel_2, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD14), TIM_Channel_3, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD15), TIM_Channel_4, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM2, IO_TAG(PA1), TIM_Channel_2, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM2, IO_TAG(PA2), TIM_Channel_3, 0, 0, GPIO_AF_1, NULL, 0 } }; diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 766447a09e..017b143372 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -120,6 +120,8 @@ #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define CMS + //#define USE_SDCARD //#define USE_SDCARD_SPI2 // @@ -170,14 +172,6 @@ #define USE_DSHOT #define LED_STRIP -#define WS2811_PIN PB8 // TIM16_CH1 -#define WS2811_TIMER TIM16 -#define WS2811_DMA_CHANNEL DMA1_Channel3 -#define WS2811_IRQ DMA1_Channel3_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define LED_STRIP_TIMER TIM16 -#define WS2811_TIMER_GPIO_AF GPIO_AF_1 #define SPEKTRUM_BIND #define BIND_PIN PA3 // USART2, PA3 diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 971ddf04a2..cef064b7e3 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -25,5 +25,4 @@ TARGET_SRC = \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ - drivers/max7456.c \ - io/osd.c + drivers/max7456.c diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index af85c900d0..aff64adfb4 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PPM - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S2_IN - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S3_IN - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S4_IN - { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S5_IN - { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S6_IN - - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S1_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PPM + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S2_IN + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S3_IN + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S4_IN + { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S5_IN + { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S6_IN + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S4_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index b59d0ba6a3..045633276a 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -6,24 +6,24 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 , NULL, 0}, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 , NULL, 0}, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 , NULL, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 , NULL, 0}, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_6 , DMA1_Channel2, DMA1_CH2_HANDLER}, // GPIO_TIMER / LED_STRIP }; - diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7e3bee0a54..1b812cc8ba 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -101,16 +101,14 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define LED_STRIP +#define USE_DSHOT +#define REMAP_TIM17_DMA +// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 +#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) +#undef USE_UART1_TX_DMA +#endif -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 +#define LED_STRIP #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 951a147791..74a21fdb61 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -20,13 +20,15 @@ #include #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // MS5 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // MS6 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6 }; + diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 55c60a4c3c..07d6236f2d 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -38,18 +38,16 @@ #define MPU_INT_EXTI PC4 //MPU6500 -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define ICM20689_CS_PIN PA4 +#define ICM20689_SPI_INSTANCE SPI1 #define ACC -#define USE_ACC_MPU6500 -#define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define USE_ACC_SPI_ICM20689 +#define ACC_ICM20689_ALIGN CW90_DEG #define GYRO -#define USE_GYRO_MPU6500 -#define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define USE_GYRO_SPI_ICM20689 +#define GYRO_ICM20689_ALIGN CW90_DEG #define USE_VCP //#define VBUS_SENSING_PIN PA8 @@ -127,16 +125,6 @@ #define LED_STRIP -// LED Strip can run off Pin 6 (PB1) -#define WS2811_PIN PB1 -#define WS2811_TIMER TIM3 -#define WS2811_TIMER_CHANNEL TIM_Channel_4 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_CHANNEL DMA_Channel_5 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn -#define WS2811_TIMER_GPIO_AF GPIO_AF_TIM3 - // Default configuration #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/YUPIF4/target.mk b/src/main/target/YUPIF4/target.mk index a5e7683b42..61d26037c4 100644 --- a/src/main/target/YUPIF4/target.mk +++ b/src/main/target/YUPIF4/target.mk @@ -2,6 +2,5 @@ F405_TARGETS += $(TARGET) FEATURES += SDCARD VCP TARGET_SRC = \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c + drivers/accgyro_spi_icm20689.c diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index d6197448da..72d860c3f0 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -8,24 +8,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/common.h b/src/main/target/common.h index 9072918f4f..e5de703ab8 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -40,6 +40,7 @@ #ifdef STM32F1 // Using RX DMA disables the use of receive callbacks #define USE_UART1_RX_DMA +#define USE_UART1_TX_DMA #endif @@ -50,11 +51,19 @@ #define BLACKBOX #define GPS #define TELEMETRY +#define TELEMETRY_FRSKY +#define TELEMETRY_HOTT +#define TELEMETRY_IBUS +#define TELEMETRY_LTM +#define TELEMETRY_SMARTPORT #define USE_SERVOS #endif #if (FLASH_SIZE > 128) -#define DISPLAY +#define CMS +#define USE_DASHBOARD +#define USE_MSP_DISPLAYPORT +#define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK #else #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 879acff784..43b21f83ea 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -346,21 +346,6 @@ uint32_t hse_value = HSE_VALUE; /* #define DATA_IN_ExtSDRAM */ #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ -#if defined(STM32F410xx) || defined(STM32F411xE) -/*!< Uncomment the following line if you need to clock the STM32F410xx/STM32F411xE by HSE Bypass - through STLINK MCO pin of STM32F103 microcontroller. The frequency cannot be changed - and is fixed at 8 MHz. - Hardware configuration needed for Nucleo Board: - – SB54, SB55 OFF - – R35 removed - – SB16, SB50 ON */ -/* #define USE_HSE_BYPASS */ - -#if defined(USE_HSE_BYPASS) -#define HSE_BYPASS_INPUT_FREQUENCY 8000000 -#endif /* USE_HSE_BYPASS */ -#endif /* STM32F410xx || STM32F411xE */ - /*!< Uncomment the following line if you need to relocate your vector Table in Internal SRAM. */ /* #define VECT_TAB_SRAM */ @@ -379,17 +364,10 @@ uint32_t hse_value = HSE_VALUE; #elif defined (STM32F446xx) #define PLL_M 8 #elif defined (STM32F410xx) || defined (STM32F411xE) - #if defined(USE_HSE_BYPASS) - #define PLL_M 8 - #else /* !USE_HSE_BYPASS */ - #define PLL_M 8 - #endif /* USE_HSE_BYPASS */ + #define PLL_M 8 #else #endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */ -/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ -#define PLL_Q 7 - #if defined(STM32F446xx) /* PLL division factor for I2S, SAI, SYSTEM and SPDIF: Clock = PLL_VCO / PLLR */ #define PLL_R 7 @@ -399,24 +377,32 @@ uint32_t hse_value = HSE_VALUE; #define PLL_N 360 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 2 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ #if defined (STM32F40_41xxx) #define PLL_N 336 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 2 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 #endif /* STM32F40_41xxx */ #if defined(STM32F401xx) #define PLL_N 336 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 4 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 #endif /* STM32F401xx */ #if defined(STM32F410xx) || defined(STM32F411xE) -#define PLL_N 400 +#define PLL_N 384 /* SYSCLK = PLL_VCO / PLL_P */ #define PLL_P 4 +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 8 #endif /* STM32F410xx || STM32F411xE */ /******************************************************************************/ @@ -450,7 +436,7 @@ uint32_t hse_value = HSE_VALUE; #endif /* STM32F401xx */ #if defined(STM32F410xx) || defined(STM32F411xE) - uint32_t SystemCoreClock = 100000000; + uint32_t SystemCoreClock = 96000000; #endif /* STM32F410xx || STM32F401xE */ __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; @@ -585,7 +571,6 @@ void SystemCoreClockUpdate(void) pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; -#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx) || defined(STM32F469_479xx) if (pllsource != 0) { /* HSE used as PLL clock source */ @@ -596,21 +581,7 @@ void SystemCoreClockUpdate(void) /* HSI used as PLL clock source */ pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); } -#elif defined(STM32F410xx) || defined(STM32F411xE) -#if defined(USE_HSE_BYPASS) - if (pllsource != 0) - { - /* HSE used as PLL clock source */ - pllvco = (HSE_BYPASS_INPUT_FREQUENCY / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } -#else - if (pllsource == 0) - { - /* HSI used as PLL clock source */ - pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); - } -#endif /* USE_HSE_BYPASS */ -#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F446xx || STM32F469_479xx */ + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; SystemCoreClock = pllvco/pllp; break; @@ -657,7 +628,6 @@ void SystemCoreClockUpdate(void) */ void SetSysClock(void) { -#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx)|| defined(STM32F469_479xx) /******************************************************************************/ /* PLL (clocked by HSE) used as System clock source */ /******************************************************************************/ @@ -707,16 +677,22 @@ void SetSysClock(void) RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; #endif /* STM32F401xx */ -#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); -#endif /* STM32F40_41xxx || STM32F401xx || STM32F427_437x || STM32F429_439xx || STM32F469_479xx */ +#if defined(STM32F410xx) || defined(STM32F411xE) + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; +#endif /* STM32F410xx || STM32F411xE */ #if defined(STM32F446xx) /* Configure the main PLL */ RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28); +#else + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); #endif /* STM32F446xx */ /* Enable the main PLL */ @@ -737,19 +713,17 @@ void SetSysClock(void) while((PWR->CSR & PWR_CSR_ODSWRDY) == 0) { } - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ -#if defined(STM32F40_41xxx) +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx) /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; -#endif /* STM32F40_41xxx */ +#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ -#if defined(STM32F401xx) +#if defined(STM32F401xx) || defined(STM32F410xx) || defined(STM32F411xE) /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; -#endif /* STM32F401xx */ +#endif /* STM32F401xx || STM32F410xx || STM32F411xE*/ /* Select the main PLL as system clock source */ RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); @@ -764,113 +738,6 @@ void SetSysClock(void) { /* If HSE fails to start-up, the application will have wrong clock configuration. User can add here some code to deal with this error */ } -#elif defined(STM32F410xx) || defined(STM32F411xE) -#if defined(USE_HSE_BYPASS) -/******************************************************************************/ -/* PLL (clocked by HSE) used as System clock source */ -/******************************************************************************/ - __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - - /* Enable HSE and HSE BYPASS */ - RCC->CR |= ((uint32_t)RCC_CR_HSEON | RCC_CR_HSEBYP); - - /* Wait till HSE is ready and if Time out is reached exit */ - do - { - HSEStatus = RCC->CR & RCC_CR_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - - if ((RCC->CR & RCC_CR_HSERDY) != RESET) - { - HSEStatus = (uint32_t)0x01; - } - else - { - HSEStatus = (uint32_t)0x00; - } - - if (HSEStatus == (uint32_t)0x01) - { - /* Select regulator voltage output Scale 1 mode */ - RCC->APB1ENR |= RCC_APB1ENR_PWREN; - PWR->CR |= PWR_CR_VOS; - - /* HCLK = SYSCLK / 1*/ - RCC->CFGR |= RCC_CFGR_HPRE_DIV1; - - /* PCLK2 = HCLK / 2*/ - RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; - - /* PCLK1 = HCLK / 4*/ - RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; - - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); - - /* Enable the main PLL */ - RCC->CR |= RCC_CR_PLLON; - - /* Wait till the main PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) - { - } - - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; - - /* Select the main PLL as system clock source */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); - RCC->CFGR |= RCC_CFGR_SW_PLL; - - /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); - { - } - } - else - { /* If HSE fails to start-up, the application will have wrong clock - configuration. User can add here some code to deal with this error */ - } -#else /* HSI will be used as PLL clock source */ - /* Select regulator voltage output Scale 1 mode */ - RCC->APB1ENR |= RCC_APB1ENR_PWREN; - PWR->CR |= PWR_CR_VOS; - - /* HCLK = SYSCLK / 1*/ - RCC->CFGR |= RCC_CFGR_HPRE_DIV1; - - /* PCLK2 = HCLK / 2*/ - RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; - - /* PCLK1 = HCLK / 4*/ - RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; - - /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | (PLL_Q << 24); - - /* Enable the main PLL */ - RCC->CR |= RCC_CR_PLLON; - - /* Wait till the main PLL is ready */ - while((RCC->CR & RCC_CR_PLLRDY) == 0) - { - } - - /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ - FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; - - /* Select the main PLL as system clock source */ - RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); - RCC->CFGR |= RCC_CFGR_SW_PLL; - - /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); - { - } -#endif /* USE_HSE_BYPASS */ -#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */ } /** diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 15030e1517..b4cd33fb02 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -61,8 +61,6 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" -#include "io/osd.h" -#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 4be785d479..b8c446721d 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -14,6 +14,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/maths.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -23,14 +24,12 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/fc_msp.h" #include "io/beeper.h" #include "io/motors.h" #include "io/gps.h" #include "io/serial.h" -#include "io/ledstrip.h" -#include "io/osd.h" -#include "io/vtx.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -56,6 +55,8 @@ #include "config/config_profile.h" #include "config/feature.h" +#include "msp/msp.h" + extern profile_t *currentProfile; extern controlRateConfig_t *currentControlRateProfile; @@ -70,7 +71,13 @@ enum enum { FSSP_START_STOP = 0x7E, + + FSSP_DLE = 0x7D, + FSSP_DLE_XOR = 0x20, + FSSP_DATA_FRAME = 0x10, + FSSP_MSPC_FRAME = 0x30, // MSP client frame + FSSP_MSPS_FRAME = 0x32, // MSP server frame // ID of sensor. Must be something that is polled by FrSky RX FSSP_SENSOR_ID1 = 0x1B, @@ -152,33 +159,116 @@ static uint8_t smartPortHasRequest = 0; static uint8_t smartPortIdCnt = 0; static uint32_t smartPortLastRequestTime = 0; +typedef struct smartPortFrame_s { + uint8_t sensorId; + uint8_t frameId; + uint16_t valueId; + uint32_t data; + uint8_t crc; +} __attribute__((packed)) smartPortFrame_t; + +#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) +#define SMARTPORT_TX_BUF_SIZE 256 + +#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId) +#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1) + +static smartPortFrame_t smartPortRxBuffer; +static uint8_t smartPortRxBytes = 0; +static bool smartPortFrameReceived = false; + +#define SMARTPORT_MSP_VERSION 1 +#define SMARTPORT_MSP_VER_SHIFT 5 +#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT) +#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) + +#define SMARTPORT_MSP_ERROR_FLAG (1 << 5) +#define SMARTPORT_MSP_START_FLAG (1 << 4) +#define SMARTPORT_MSP_SEQ_MASK 0x0F + +#define SMARTPORT_MSP_RX_BUF_SIZE 64 + +static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; +static mspPacket_t smartPortMspReply; +static bool smartPortMspReplyPending = false; + +#define SMARTPORT_MSP_RES_ERROR (-10) + +enum { + SMARTPORT_MSP_VER_MISMATCH=0, + SMARTPORT_MSP_CRC_ERROR=1, + SMARTPORT_MSP_ERROR=2 +}; + static void smartPortDataReceive(uint16_t c) { + static bool skipUntilStart = true; + static bool byteStuffing = false; + static uint16_t checksum = 0; + uint32_t now = millis(); - // look for a valid request sequence - static uint8_t lastChar; - if (lastChar == FSSP_START_STOP) { - smartPortState = SPSTATE_WORKING; - if (c == FSSP_SENSOR_ID1 && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + if (c == FSSP_START_STOP) { + smartPortRxBytes = 0; + smartPortHasRequest = 0; + skipUntilStart = false; + return; + } else if (skipUntilStart) { + return; + } + + uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer; + if (smartPortRxBytes == 0) { + if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + + // our slot is starting... smartPortLastRequestTime = now; smartPortHasRequest = 1; - // we only responde to these IDs - // the X4R-SB does send other IDs, we ignore them, but take note of the time + } else if (c == FSSP_SENSOR_ID2) { + rxBuffer[smartPortRxBytes++] = c; + checksum = 0; + } + else { + skipUntilStart = true; + } + } + else { + + if (c == FSSP_DLE) { + byteStuffing = true; + return; + } + + if (byteStuffing) { + c ^= FSSP_DLE_XOR; + byteStuffing = false; + } + + rxBuffer[smartPortRxBytes++] = c; + + if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { + if (c == (0xFF - checksum)) { + smartPortFrameReceived = true; + } + skipUntilStart = true; + } else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) { + checksum += c; + checksum += checksum >> 8; + checksum &= 0x00FF; } } - lastChar = c; } static void smartPortSendByte(uint8_t c, uint16_t *crcp) { // smart port escape sequence - if (c == 0x7D || c == 0x7E) { - serialWrite(smartPortSerialPort, 0x7D); - c ^= 0x20; + if (c == FSSP_DLE || c == FSSP_START_STOP) { + serialWrite(smartPortSerialPort, FSSP_DLE); + serialWrite(smartPortSerialPort, c ^ FSSP_DLE_XOR); + } + else { + serialWrite(smartPortSerialPort, c); } - - serialWrite(smartPortSerialPort, c); if (crcp == NULL) return; @@ -190,21 +280,30 @@ static void smartPortSendByte(uint8_t c, uint16_t *crcp) *crcp = crc; } -static void smartPortSendPackage(uint16_t id, uint32_t val) +static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data) { uint16_t crc = 0; - smartPortSendByte(FSSP_DATA_FRAME, &crc); - uint8_t *u8p = (uint8_t*)&id; - smartPortSendByte(u8p[0], &crc); - smartPortSendByte(u8p[1], &crc); - u8p = (uint8_t*)&val; - smartPortSendByte(u8p[0], &crc); - smartPortSendByte(u8p[1], &crc); - smartPortSendByte(u8p[2], &crc); - smartPortSendByte(u8p[3], &crc); + smartPortSendByte(frameId, &crc); + for(unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) { + smartPortSendByte(*data++, &crc); + } smartPortSendByte(0xFF - (uint8_t)crc, NULL); } +static void smartPortSendPackage(uint16_t id, uint32_t val) +{ + uint8_t payload[SMARTPORT_PAYLOAD_SIZE]; + uint8_t *dst = payload; + *dst++ = id & 0xFF; + *dst++ = id >> 8; + *dst++ = val & 0xFF; + *dst++ = (val >> 8) & 0xFF; + *dst++ = (val >> 16) & 0xFF; + *dst++ = (val >> 24) & 0xFF; + + smartPortSendPackageEx(FSSP_DATA_FRAME,payload); +} + void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) { telemetryConfig = initialTelemetryConfig; @@ -270,6 +369,196 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } +static void initSmartPortMspReply(int16_t cmd) +{ + smartPortMspReply.buf.ptr = smartPortMspTxBuffer; + smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer); + + smartPortMspReply.cmd = cmd; + smartPortMspReply.result = 0; +} + +static void processMspPacket(mspPacket_t* packet) +{ + initSmartPortMspReply(0); + + if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR) { + sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR); + } + + // change streambuf direction + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + smartPortMspReplyPending = true; +} + +/** + * Request frame format: + * - Header: 1 byte + * - Reserved: 2 bits (future use) + * - Error-flag: 1 bit + * - Start-flag: 1 bit + * - CSeq: 4 bits + * + * - MSP payload: + * - if Error-flag == 0: + * - size: 1 byte + * - payload + * - CRC (request type included) + * - if Error-flag == 1: + * - size: 1 byte (== 1) + * - error: 1 Byte + * - 0: Version mismatch (type=0) + * - 1: Sequence number error + * - 2: MSP error + * - CRC (request type included) + */ +bool smartPortSendMspReply() +{ + static uint8_t checksum = 0; + static uint8_t seq = 0; + + uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; + uint8_t* p = packet; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; + + sbuf_t* txBuf = &smartPortMspReply.buf; + + // detect first reply packet + if (txBuf->ptr == smartPortMspTxBuffer) { + + // header + uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); + if (smartPortMspReply.result < 0) { + head |= SMARTPORT_MSP_ERROR_FLAG; + } + *p++ = head; + + uint8_t size = sbufBytesRemaining(txBuf); + *p++ = size; + + checksum = size ^ smartPortMspReply.cmd; + } + else { + // header + *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK); + } + + while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { + *p = sbufReadU8(txBuf); + checksum ^= *p++; // MSP checksum + } + + // to be continued... + if (p == end) { + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return true; + } + + // nothing left in txBuf, + // append the MSP checksum + *p++ = checksum; + + // pad with zeros + while (p < end) + *p++ = 0; + + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return false; +} + +void smartPortSendErrorReply(uint8_t error, int16_t cmd) +{ + initSmartPortMspReply(cmd); + sbufWriteU8(&smartPortMspReply.buf,error); + smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR; + + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + smartPortMspReplyPending = true; +} + +/** + * Request frame format: + * - Header: 1 byte + * - Version: 3 bits + * - Start-flag: 1 bit + * - CSeq: 4 bits + * + * - MSP payload: + * - Size: 1 Byte + * - Type: 1 Byte + * - payload... + * - CRC + */ +void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) +{ + static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; + static uint8_t mspStarted = 0; + static uint8_t lastSeq = 0; + static uint8_t checksum = 0; + static mspPacket_t cmd; + + // re-assemble MSP frame & forward to MSP port when complete + uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; + + uint8_t head = *p++; + uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK; + uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT; + + if (version != SMARTPORT_MSP_VERSION) { + mspStarted = 0; + smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0); + return; + } + + // check start-flag + if (head & SMARTPORT_MSP_START_FLAG) { + + //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error! + uint8_t p_size = *p++; + cmd.cmd = *p++; + cmd.result = 0; + + cmd.buf.ptr = mspBuffer; + cmd.buf.end = mspBuffer + p_size; + + checksum = p_size ^ cmd.cmd; + mspStarted = 1; + } else if (!mspStarted) { + // no start packet yet, throw this one away + return; + } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { + // packet loss detected! + mspStarted = 0; + return; + } + + // copy payload bytes + while ((p < end) && sbufBytesRemaining(&cmd.buf)) { + checksum ^= *p; + sbufWriteU8(&cmd.buf,*p++); + } + + // reached end of smart port frame + if (p == end) { + lastSeq = seq; + return; + } + + // last byte must be the checksum + if (checksum != *p) { + mspStarted = 0; + smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd); + return; + } + + // end of MSP packet reached + mspStarted = 0; + sbufSwitchToReader(&cmd.buf,mspBuffer); + + processMspPacket(&cmd); +} + void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); @@ -295,6 +584,17 @@ void handleSmartPortTelemetry(void) return; } + if(smartPortFrameReceived) { + smartPortFrameReceived = false; + // do not check the physical ID here again + // unless we start receiving other sensors' packets + if(smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { + + // Pass only the payload: skip sensorId & frameId + handleSmartPortMspFrame(&smartPortRxBuffer); + } + } + while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { @@ -302,6 +602,12 @@ void handleSmartPortTelemetry(void) return; } + if(smartPortMspReplyPending) { + smartPortMspReplyPending = smartPortSendMspReply(); + smartPortHasRequest = 0; + return; + } + // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back @@ -311,7 +617,7 @@ void handleSmartPortTelemetry(void) smartPortIdCnt++; int32_t tmpi; - uint32_t tmp2; + uint32_t tmp2 = 0; static uint8_t t1Cnt = 0; static uint8_t t2Cnt = 0; @@ -319,8 +625,10 @@ void handleSmartPortTelemetry(void) #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; - smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H + //convert to knots: 1cm/s = 0.0194384449 knots + //Speed should be sent in knots/1000 (GPS speed is in cm/s) + uint32_t tmpui = GPS_speed * 1944 / 100; + smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } break; @@ -458,13 +766,10 @@ void handleSmartPortTelemetry(void) smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif - } - else if (feature(FEATURE_GPS)) { + } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } - - else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d4a1519825..305d4df229 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -23,6 +23,8 @@ #ifdef TELEMETRY +#include "common/utils.h" + #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -42,7 +44,6 @@ #include "telemetry/ltm.h" #include "telemetry/jetiexbus.h" #include "telemetry/mavlink.h" -#include "rx/jetiexbus.h" static telemetryConfig_t *telemetryConfig; @@ -53,13 +54,22 @@ void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) void telemetryInit(void) { +#ifdef TELEMETRY_FRSKY initFrSkyTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_HOTT initHoTTTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_SMARTPORT initSmartPortTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_LTM initLtmTelemetry(telemetryConfig); +#endif +#ifdef TELEMETRY_JETIEXBUS initJetiExBusTelemetry(telemetryConfig); - -#if defined(TELEMETRY_MAVLINK) +#endif +#ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); #endif @@ -89,26 +99,49 @@ serialPort_t *telemetrySharedPort = NULL; void telemetryCheckState(void) { +#ifdef TELEMETRY_FRSKY checkFrSkyTelemetryState(); +#endif +#ifdef TELEMETRY_HOTT checkHoTTTelemetryState(); +#endif +#ifdef TELEMETRY_SMARTPORT checkSmartPortTelemetryState(); +#endif +#ifdef TELEMETRY_LTM checkLtmTelemetryState(); +#endif +#ifdef TELEMETRY_JETIEXBUS checkJetiExBusTelemetryState(); - -#if defined(TELEMETRY_MAVLINK) +#endif +#ifdef TELEMETRY_MAVLINK checkMAVLinkTelemetryState(); #endif } void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { +#ifdef TELEMETRY_FRSKY handleFrSkyTelemetry(rxConfig, deadband3d_throttle); +#else + UNUSED(rxConfig); + UNUSED(deadband3d_throttle); +#endif +#ifdef TELEMETRY_HOTT handleHoTTTelemetry(currentTime); +#else + UNUSED(currentTime); +#endif +#ifdef TELEMETRY_SMARTPORT handleSmartPortTelemetry(); +#endif +#ifdef TELEMETRY_LTM handleLtmTelemetry(); +#endif +#ifdef TELEMETRY_JETIEXBUS handleJetiExBusTelemetry(); - -#if defined(TELEMETRY_MAVLINK) +#endif +#ifdef TELEMETRY_MAVLINK handleMAVLinkTelemetry(); #endif } diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index dbddd6850c..3723a7bc65 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -48,7 +48,6 @@ extern uint32_t APP_Rx_ptr_in; APP TX is the circular buffer for data that is transmitted from the APP (host) to the USB device (flight controller). */ -#define APP_TX_DATA_SIZE 1024 static uint8_t APP_Tx_Buffer[APP_TX_DATA_SIZE]; static uint32_t APP_Tx_ptr_out = 0; static uint32_t APP_Tx_ptr_in = 0; @@ -195,7 +194,9 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; - while (CDC_Send_FreeBytes() <= 0); + while (CDC_Send_FreeBytes() == 0) { + delay(1); + } } return USBD_OK; @@ -247,15 +248,11 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) return USBD_FAIL; } - __disable_irq(); - for (uint32_t i = 0; i < Len; i++) { APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; } - __enable_irq(); - return USBD_OK; } diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index 87481c2ef1..d062c1e431 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -42,19 +42,21 @@ /* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ #ifdef USE_USB_OTG_HS - #define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ - #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ +#define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ - #define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */ - #define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: +#define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */ +#define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL*8 */ +#define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #else - #define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ - #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ +#define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ - #define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ - #define APP_RX_DATA_SIZE 1024 /* Total size of IN buffer: +#define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ +#define APP_RX_DATA_SIZE 2048 /* Total size of IN (outbound from FC) buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */ +#define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #endif /* USE_USB_OTG_HS */ #define APP_FOPS VCP_fops diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index cdf6c4fe7a..c7ac34fd60 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -26,6 +26,7 @@ #include "usbd_conf.h" #include "usb_regs.h" #include "platform.h" +#include "build/version.h" /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY * @{ @@ -57,7 +58,7 @@ * @{ */ #define USBD_LANGID_STRING 0x409 -#define USBD_MANUFACTURER_STRING "BetaFlight" +#define USBD_MANUFACTURER_STRING FC_FIRMWARE_NAME #ifdef USBD_PRODUCT_STRING #define USBD_PRODUCT_HS_STRING USBD_PRODUCT_STRING diff --git a/src/test/Makefile b/src/test/Makefile index 6581e7ae2b..d8eb183f34 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -110,6 +110,14 @@ $(OBJECT_DIR)/common/maths.o : \ @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ +$(OBJECT_DIR)/common/typeconversion.o : \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/common/typeconversion.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/typeconversion.c -o $@ + $(OBJECT_DIR)/common/filter.o : \ $(USER_DIR)/common/filter.c \ $(USER_DIR)/common/filter.h \ @@ -574,6 +582,39 @@ $(OBJECT_DIR)/alignsensor_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/drivers/display.o : \ + $(USER_DIR)/drivers/display.c \ + $(USER_DIR)/drivers/display.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/display.c -o $@ + +$(OBJECT_DIR)/cms/cms.o : \ + $(USER_DIR)/cms/cms.c \ + $(USER_DIR)/cms/cms.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/cms/cms.c -o $@ + +$(OBJECT_DIR)/cms_unittest.o : \ + $(TEST_DIR)/cms_unittest.cc \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/cms_unittest.cc -o $@ + +$(OBJECT_DIR)/cms_unittest : \ + $(OBJECT_DIR)/cms_unittest.o \ + $(OBJECT_DIR)/common/typeconversion.o \ + $(OBJECT_DIR)/drivers/display.o \ + $(OBJECT_DIR)/cms/cms.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + + ## test : Build and run the Unit Tests test: $(TESTS:%=test-%) diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua new file mode 100644 index 0000000000..40d49d6cce --- /dev/null +++ b/src/test/SpMsp.lua @@ -0,0 +1,207 @@ +-- +-- Test script for the MSP/SPORT bridge +-- + +-- Protocol version +SPORT_MSP_VERSION = 1 + +-- Sensor ID used by the local LUA script +LOCAL_SENSOR_ID = 0x0D + +-- Sensor ID used by the MSP server (BF, CF, MW, etc...) +REMOTE_SENSOR_ID = 0x1B + +REQUEST_FRAME_ID = 0x30 +REPLY_FRAME_ID = 0x32 + +-- Sequence number for next MSP/SPORT packet +local sportMspSeq = 0 +local sportMspRemoteSeq = 0 + +local mspRxBuf = {} +local mspRxIdx = 1 +local mspRxCRC = 0 +local mspStarted = false +local mspLastReq = 0 + +-- Stats +mspRequestsSent = 0 +mspRepliesReceived = 0 +mspPkRxed = 0 +mspErrorPk = 0 +mspStartPk = 0 +mspOutOfOrder = 0 +mspCRCErrors = 0 + +local function mspResetStats() + mspRequestsSent = 0 + mspRepliesReceived = 0 + mspPkRxed = 0 + mspErrorPk = 0 + mspStartPk = 0 + mspOutOfOrderPk = 0 + mspCRCErrors = 0 +end + +local function mspSendRequest(cmd) + + local dataId = 0 + dataId = sportMspSeq -- sequence number + dataId = dataId + bit32.lshift(1,4) -- start flag + dataId = dataId + bit32.lshift(SPORT_MSP_VERSION,5) -- MSP/SPORT version + -- size is 0 for now, no need to add it to dataId + -- dataId = dataId + bit32.lshift(0,8) + sportMspSeq = bit32.band(sportMspSeq + 1, 0x0F) + + local value = 0 + value = bit32.band(cmd,0xFF) -- MSP command + value = value + bit32.lshift(cmd,8) -- CRC + + mspLastReq = cmd + mspRequestsSent = mspRequestsSent + 1 + return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) +end + +local function mspReceivedReply(payload) + + mspPkRxed = mspPkRxed + 1 + + local idx = 1 + local head = payload[idx] + local err_flag = (bit32.band(head,0x20) ~= 0) + idx = idx + 1 + + if err_flag then + -- error flag set + mspStarted = false + + mspErrorPk = mspErrorPk + 1 + + -- return error + -- CRC checking missing + + --return payload[idx] + return nil + end + + local start = (bit32.band(head,0x10) ~= 0) + local seq = bit32.band(head,0x0F) + + if start then + -- start flag set + mspRxIdx = 1 + mspRxBuf = {} + + mspRxSize = payload[idx] + mspRxCRC = bit32.bxor(mspRxSize,mspLastReq) + idx = idx + 1 + mspStarted = true + + mspStartPk = mspStartPk + 1 + + elseif not mspStarted then + mspOutOfOrder = mspOutOfOrder + 1 + return nil + + elseif bit32.band(sportMspRemoteSeq + 1, 0x0F) ~= seq then + mspOutOfOrder = mspOutOfOrder + 1 + mspStarted = false + return nil + end + + while (idx <= 6) and (mspRxIdx <= mspRxSize) do + mspRxBuf[mspRxIdx] = payload[idx] + mspRxCRC = bit32.bxor(mspRxCRC,payload[idx]) + mspRxIdx = mspRxIdx + 1 + idx = idx + 1 + end + + if idx > 6 then + sportMspRemoteSeq = seq + return + end + + -- check CRC + if mspRxCRC ~= payload[idx] then + mspStarted = false + mspCRCErrors = mspCRCErrors + 1 + return nil + end + + mspRepliesReceived = mspRepliesReceived + 1 + mspStarted = false + return mspRxBuf +end + +local function mspPollReply() + local sensorId, frameId, dataId, value = sportTelemetryPop() + if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then + + local payload = {} + payload[1] = bit32.band(dataId,0xFF) + dataId = bit32.rshift(dataId,8) + payload[2] = bit32.band(dataId,0xFF) + + payload[3] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[4] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[5] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[6] = bit32.band(value,0xFF) + + return mspReceivedReply(payload) + end +end + +local lastReqTS = 0 + +local function run(event) + + local now = getTime() + + if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then + mspResetStats() + end + + lcd.clear() + lcd.drawText(41,1,"MSP/SPORT test script",INVERS) + + -- do we have valid telemetry data? + if getValue("RSSI") > 0 then + + -- draw screen + lcd.drawText(1,11,"Requests:",0) + lcd.drawNumber(60,11,mspRequestsSent) + + lcd.drawText(1,21,"Replies:",0) + lcd.drawNumber(60,21,mspRepliesReceived) + + lcd.drawText(1,31,"PkRxed:",0) + lcd.drawNumber(60,31,mspPkRxed) + + lcd.drawText(1,41,"ErrorPk:",0) + lcd.drawNumber(60,41,mspErrorPk) + + lcd.drawText(91,31,"StartPk:",0) + lcd.drawNumber(160,31,mspStartPk) + + lcd.drawText(91,41,"OutOfOrder:",0) + lcd.drawNumber(160,41,mspOutOfOrder) + + lcd.drawText(1,51,"CRCErrors:",0) + lcd.drawNumber(60,51,mspCRCErrors) + + -- last request is at least 2s old + if lastReqTS + 200 <= now then + mspSendRequest(117) -- MSP_PIDNAMES + lastReqTS = now + end + else + lcd.drawText(15,20,"No telemetry signal", BLINK + DBLSIZE) + end + + mspPollReply() +end + +return {run=run} diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc new file mode 100644 index 0000000000..a417e8b7ee --- /dev/null +++ b/src/test/unit/cms_unittest.cc @@ -0,0 +1,207 @@ +/* + * 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 + +#define BARO + +extern "C" { + #include "target.h" + #include "drivers/display.h" + #include "cms/cms.h" + #include "cms/cms_types.h" + void cmsMenuOpen(void); + long cmsMenuBack(displayPort_t *pDisplay); + uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key); + extern CMS_Menu *currentMenu; // Points to top entry of the current page +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +static displayPort_t testDisplayPort; +static int displayPortTestGrab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestRelease(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestClear(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s) +{ + UNUSED(displayPort); + UNUSED(x); + UNUSED(y); + UNUSED(s); + return 0; +} + +static int displayPortTestHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void displayPortTestResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static const displayPortVTable_t testDisplayPortVTable = { + .grab = displayPortTestGrab, + .release = displayPortTestRelease, + .clear = displayPortTestClear, + .write = displayPortTestWrite, + .heartbeat = displayPortTestHeartbeat, + .resync = displayPortTestResync, + .txBytesFree = displayPortTestTxBytesFree +}; + +displayPort_t *displayPortTestInit(void) +{ + testDisplayPort.vTable = &testDisplayPortVTable; + testDisplayPort.rows = 10; + testDisplayPort.cols = 40; + testDisplayPort.isGrabbed = false; + return &testDisplayPort; +} + +TEST(CMSUnittest, TestCmsDisplayPortRegister) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + for (int ii = 0; ii < CMS_MAX_DEVICE; ++ii) { + const bool registered = cmsDisplayPortRegister(displayPort); + EXPECT_EQ(true, registered); + } + const bool registered = cmsDisplayPortRegister(displayPort); + EXPECT_EQ(false, registered); +} + +TEST(CMSUnittest, TestCmsMenuOpen) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + displayGrab(displayPort); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); +} + +TEST(CMSUnittest, TestCmsMenuExit0) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuExit(displayPort, (void*)0); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuExit1) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuExit(displayPort, (void*)0); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuBack) +{ + cmsInit(); + displayPort_t *displayPort = displayPortTestInit(); + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + long exit = cmsMenuBack(displayPort); + EXPECT_EQ(0, exit); +} + +TEST(CMSUnittest, TestCmsMenuKey) +{ +#define KEY_ENTER 0 +#define KEY_UP 1 +#define KEY_DOWN 2 +#define KEY_LEFT 3 +#define KEY_RIGHT 4 +#define KEY_ESC 5 +#define BUTTON_TIME 250 // msec +#define BUTTON_PAUSE 500 // msec + cmsInit(); + displayPort_t *displayPort = &testDisplayPort; + cmsDisplayPortRegister(displayPort); + + cmsMenuOpen(); + uint16_t result = cmsHandleKey(displayPort, KEY_ESC); + EXPECT_EQ(BUTTON_PAUSE, result); +} +// STUBS + +extern "C" { +static OSD_Entry menuMainEntries[] = +{ + {"-- MAIN MENU --", OME_Label, NULL, NULL, 0}, + {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, + {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +CMS_Menu menuMain = { + "MENUMAIN", + OME_MENU, + NULL, + NULL, + NULL, + menuMainEntries, +}; +uint8_t armingFlags; +int16_t debug[4]; +int16_t rcData[18]; +void delay(uint32_t) {} +uint32_t micros(void) { return 0; } +uint32_t millis(void) { return 0; } +void saveConfigAndNotify(void) {} +void stopMotors(void) {} +void stopPwmAllMotors(void) {} +void systemReset(void) {} +} \ No newline at end of file diff --git a/src/test/unit/target.h b/src/test/unit/target.h index b7899e43a2..c2ebf479ed 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -17,6 +17,8 @@ #pragma once +#define CMS +#define CMS_MAX_DEVICE 4 #define MAG #define BARO #define GPS