From dd49ef0166cc5fd48e2b7f901a4e32c027035eb5 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 19 Apr 2017 15:55:36 +0200 Subject: [PATCH 01/20] LP-480 work in progress: revo, nano & sparky2 targets ok. --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 628 ++++----- flight/make/apps-defs.mk | 2 + flight/modules/Osd/osdoutput/osdoutput.c | 5 + flight/pios/common/pios_board_io.c | 798 ++++++++++++ flight/pios/common/pios_com.c | 37 +- flight/pios/{stm32f4xx => common}/pios_dsm.c | 68 +- flight/pios/common/pios_exbus.c | 12 +- flight/pios/common/pios_hott.c | 10 + flight/pios/common/pios_ibus.c | 12 +- flight/pios/common/pios_sbus.c | 19 +- flight/pios/common/pios_srxl.c | 12 +- flight/pios/common/pios_usb_desc_hid_cdc.c | 16 + flight/pios/common/pios_usb_desc_hid_only.c | 6 + flight/pios/inc/pios_board_hw.h | 56 + flight/pios/inc/pios_board_io.h | 221 ++++ flight/pios/inc/pios_com.h | 36 +- flight/pios/inc/pios_dsm_priv.h | 6 - flight/pios/inc/pios_sbus_priv.h | 11 +- flight/pios/inc/pios_stm32.h | 8 +- flight/pios/inc/pios_usart.h | 19 + flight/pios/inc/pios_usart_priv.h | 8 +- flight/pios/inc/pios_usb_desc_hid_cdc_priv.h | 5 + flight/pios/inc/pios_usb_desc_hid_only_priv.h | 4 + flight/pios/stm32f10x/pios_dsm.c | 438 ------- flight/pios/stm32f4xx/pios_usart.c | 69 +- .../firmware/inc/pios_config.h | 1 + .../gpsplatinum/firmware/inc/pios_config.h | 1 + .../targets/boards/revolution/board_hw_defs.c | 1136 ++++------------- .../boards/revolution/bootloader/pios_board.c | 10 +- .../revolution/firmware/inc/pios_config.h | 1 + .../boards/revolution/firmware/pios_board.c | 1102 ++-------------- .../targets/boards/revonano/board_hw_defs.c | 709 +++------- .../boards/revonano/bootloader/pios_board.c | 9 +- .../boards/revonano/firmware/pios_board.c | 760 ++--------- .../revoproto/firmware/inc/pios_config.h | 1 + flight/targets/boards/sparky2/board_hw_defs.c | 848 +++--------- .../boards/sparky2/bootloader/pios_board.c | 8 +- .../boards/sparky2/firmware/pios_board.c | 1098 ++-------------- shared/uavobjectdefinition/hwsettings.xml | 1 + 39 files changed, 2625 insertions(+), 5566 deletions(-) create mode 100644 flight/pios/common/pios_board_io.c rename flight/pios/{stm32f4xx => common}/pios_dsm.c (90%) create mode 100644 flight/pios/inc/pios_board_hw.h create mode 100644 flight/pios/inc/pios_board_io.h delete mode 100644 flight/pios/stm32f10x/pios_dsm.c diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 14b7c9ae2..da24ebb79 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -1,298 +1,330 @@ -// !$*UTF8*$! -{ - archiveVersion = 1; - classes = { - }; - objectVersion = 45; - objects = { - -/* Begin PBXFileReference section */ - 4354B66314FED9FE004BA3B4 /* flight */ = {isa = PBXFileReference; lastKnownFileType = folder; name = flight; path = ../..; sourceTree = SOURCE_ROOT; }; - 65173C9F12EBFD1700D6A7CB /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../../Makefile; sourceTree = SOURCE_ROOT; }; - 65904F1814632C1700FD9482 /* firmware-defs.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "firmware-defs.mk"; sourceTree = ""; }; - 65904F2214632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2314632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2414632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2514632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2614632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2714632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2814632C1700FD9482 /* version-info.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = "version-info.py"; sourceTree = ""; }; - 65904F2914632C1700FD9482 /* firmware_info.c.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmware_info.c.template; sourceTree = ""; }; - 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcs_version_info.h.template; sourceTree = ""; }; - 65904F2D14632C1700FD9482 /* README.txt */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = README.txt; sourceTree = ""; }; - 65904F2E14632C1700FD9482 /* shell_script.reg */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = shell_script.reg; sourceTree = ""; }; - 65904F2F14632C1700FD9482 /* install */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = install; sourceTree = ""; }; - 65904F3014632C1700FD9482 /* make */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make; sourceTree = ""; }; - 65904F3114632C1700FD9482 /* make.sh */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make.sh; sourceTree = ""; }; - 65904F3214632C1700FD9482 /* sh.cmd */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = sh.cmd; sourceTree = ""; }; - 65904F34146362F300FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65E466BC14E244020075459C /* uavobjectdefinition */ = {isa = PBXFileReference; lastKnownFileType = folder; path = uavobjectdefinition; sourceTree = ""; }; -/* End PBXFileReference section */ - -/* Begin PBXGroup section */ - 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */ = { - isa = PBXGroup; - children = ( - 4354B66314FED9FE004BA3B4 /* flight */, - 65904F1614632C1700FD9482 /* make */, - 65C35E4E12EFB2F3004811C2 /* shared */, - 65173C9F12EBFD1700D6A7CB /* Makefile */, - ); - name = OpenPilotOSX; - sourceTree = ""; - }; - 65904F1614632C1700FD9482 /* make */ = { - isa = PBXGroup; - children = ( - 65904F1714632C1700FD9482 /* boards */, - 65904F1814632C1700FD9482 /* firmware-defs.mk */, - 65904F1914632C1700FD9482 /* scripts */, - 65904F1A14632C1700FD9482 /* templates */, - 65904F1B14632C1700FD9482 /* winx86 */, - ); - name = make; - path = OpenPilotOSX.xcodeproj/../../../../make; - sourceTree = ""; - }; - 65904F1714632C1700FD9482 /* boards */ = { - isa = PBXGroup; - children = ( - 65904F33146362F300FD9482 /* revolution */, - 65904F1C14632C1700FD9482 /* ahrs */, - 65904F1D14632C1700FD9482 /* coptercontrol */, - 65904F1E14632C1700FD9482 /* esc */, - 65904F1F14632C1700FD9482 /* ins */, - 65904F2014632C1700FD9482 /* openpilot */, - 65904F2114632C1700FD9482 /* oplink */, - ); - path = boards; - sourceTree = ""; - }; - 65904F1914632C1700FD9482 /* scripts */ = { - isa = PBXGroup; - children = ( - 65904F2814632C1700FD9482 /* version-info.py */, - ); - path = scripts; - sourceTree = ""; - }; - 65904F1A14632C1700FD9482 /* templates */ = { - isa = PBXGroup; - children = ( - 65904F2914632C1700FD9482 /* firmware_info.c.template */, - 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */, - ); - path = templates; - sourceTree = ""; - }; - 65904F1B14632C1700FD9482 /* winx86 */ = { - isa = PBXGroup; - children = ( - 65904F2B14632C1700FD9482 /* bin */, - 65904F2C14632C1700FD9482 /* cmd */, - 65904F2D14632C1700FD9482 /* README.txt */, - 65904F2E14632C1700FD9482 /* shell_script.reg */, - ); - path = winx86; - sourceTree = ""; - }; - 65904F1C14632C1700FD9482 /* ahrs */ = { - isa = PBXGroup; - children = ( - 65904F2214632C1700FD9482 /* board-info.mk */, - ); - path = ahrs; - sourceTree = ""; - }; - 65904F1D14632C1700FD9482 /* coptercontrol */ = { - isa = PBXGroup; - children = ( - 65904F2314632C1700FD9482 /* board-info.mk */, - ); - path = coptercontrol; - sourceTree = ""; - }; - 65904F1E14632C1700FD9482 /* esc */ = { - isa = PBXGroup; - children = ( - 65904F2414632C1700FD9482 /* board-info.mk */, - ); - path = esc; - sourceTree = ""; - }; - 65904F1F14632C1700FD9482 /* ins */ = { - isa = PBXGroup; - children = ( - 65904F2514632C1700FD9482 /* board-info.mk */, - ); - path = ins; - sourceTree = ""; - }; - 65904F2014632C1700FD9482 /* openpilot */ = { - isa = PBXGroup; - children = ( - 65904F2614632C1700FD9482 /* board-info.mk */, - ); - path = openpilot; - sourceTree = ""; - }; - 65904F2114632C1700FD9482 /* oplink */ = { - isa = PBXGroup; - children = ( - 65904F2714632C1700FD9482 /* board-info.mk */, - ); - path = oplink; - sourceTree = ""; - }; - 65904F2B14632C1700FD9482 /* bin */ = { - isa = PBXGroup; - children = ( - 65904F2F14632C1700FD9482 /* install */, - 65904F3014632C1700FD9482 /* make */, - ); - path = bin; - sourceTree = ""; - }; - 65904F2C14632C1700FD9482 /* cmd */ = { - isa = PBXGroup; - children = ( - 65904F3114632C1700FD9482 /* make.sh */, - 65904F3214632C1700FD9482 /* sh.cmd */, - ); - path = cmd; - sourceTree = ""; - }; - 65904F33146362F300FD9482 /* revolution */ = { - isa = PBXGroup; - children = ( - 65904F34146362F300FD9482 /* board-info.mk */, - ); - path = revolution; - sourceTree = ""; - }; - 65C35E4E12EFB2F3004811C2 /* shared */ = { - isa = PBXGroup; - children = ( - 65E466BC14E244020075459C /* uavobjectdefinition */, - ); - name = shared; - path = ../../../shared; - sourceTree = SOURCE_ROOT; - }; -/* End PBXGroup section */ - -/* Begin PBXLegacyTarget section */ - 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { - isa = PBXLegacyTarget; - buildArgumentsString = "$(ACTION) -f Makefile.posix"; - buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; - buildPhases = ( - ); - buildToolPath = /usr/bin/make; - buildWorkingDirectory = ../../OpenPilot; - dependencies = ( - ); - name = OpenPilotOSX; - passBuildSettingsInEnvironment = 1; - productName = OpenPilotOSX; - }; -/* End PBXLegacyTarget section */ - -/* Begin PBXProject section */ - 08FB7793FE84155DC02AAC07 /* Project object */ = { - isa = PBXProject; - attributes = { - ORGANIZATIONNAME = OpenPilot; - }; - buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */; - compatibilityVersion = "Xcode 3.1"; - developmentRegion = English; - hasScannedForEncodings = 1; - knownRegions = ( - English, - Japanese, - French, - German, - ); - mainGroup = 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */; - projectDirPath = ""; - projectRoot = ../../..; - targets = ( - 6581071511DE809D0049FB12 /* OpenPilotOSX */, - ); - }; -/* End PBXProject section */ - -/* Begin XCBuildConfiguration section */ - 1DEB928A08733DD80010E9CD /* Debug */ = { - isa = XCBuildConfiguration; - buildSettings = { - ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; - GCC_C_LANGUAGE_STANDARD = gnu99; - GCC_OPTIMIZATION_LEVEL = 0; - GCC_WARN_ABOUT_RETURN_TYPE = YES; - GCC_WARN_UNUSED_VARIABLE = YES; - ONLY_ACTIVE_ARCH = YES; - PREBINDING = NO; - SDKROOT = macosx10.6; - }; - name = Debug; - }; - 1DEB928B08733DD80010E9CD /* Release */ = { - isa = XCBuildConfiguration; - buildSettings = { - ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; - GCC_C_LANGUAGE_STANDARD = gnu99; - GCC_WARN_ABOUT_RETURN_TYPE = YES; - GCC_WARN_UNUSED_VARIABLE = YES; - PREBINDING = NO; - SDKROOT = macosx10.6; - }; - name = Release; - }; - 6581071611DE809D0049FB12 /* Debug */ = { - isa = XCBuildConfiguration; - buildSettings = { - COPY_PHASE_STRIP = NO; - GCC_DYNAMIC_NO_PIC = NO; - GCC_OPTIMIZATION_LEVEL = 0; - PRODUCT_NAME = OpenPilotOSX; - }; - name = Debug; - }; - 6581071711DE809D0049FB12 /* Release */ = { - isa = XCBuildConfiguration; - buildSettings = { - COPY_PHASE_STRIP = YES; - DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; - GCC_ENABLE_FIX_AND_CONTINUE = NO; - PRODUCT_NAME = OpenPilotOSX; - ZERO_LINK = NO; - }; - name = Release; - }; -/* End XCBuildConfiguration section */ - -/* Begin XCConfigurationList section */ - 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */ = { - isa = XCConfigurationList; - buildConfigurations = ( - 1DEB928A08733DD80010E9CD /* Debug */, - 1DEB928B08733DD80010E9CD /* Release */, - ); - defaultConfigurationIsVisible = 0; - defaultConfigurationName = Release; - }; - 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */ = { - isa = XCConfigurationList; - buildConfigurations = ( - 6581071611DE809D0049FB12 /* Debug */, - 6581071711DE809D0049FB12 /* Release */, - ); - defaultConfigurationIsVisible = 0; - defaultConfigurationName = Release; - }; -/* End XCConfigurationList section */ - }; - rootObject = 08FB7793FE84155DC02AAC07 /* Project object */; -} +// !$*UTF8*$! +{ + archiveVersion = 1; + classes = { + }; + objectVersion = 46; + objects = { + +/* Begin PBXFileReference section */ + 4354B66314FED9FE004BA3B4 /* flight */ = {isa = PBXFileReference; lastKnownFileType = folder; name = flight; path = ../..; sourceTree = SOURCE_ROOT; }; + 65173C9F12EBFD1700D6A7CB /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../../Makefile; sourceTree = SOURCE_ROOT; }; + 65904F1814632C1700FD9482 /* firmware-defs.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "firmware-defs.mk"; sourceTree = ""; }; + 65904F2214632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2314632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2414632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2514632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2614632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2714632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2814632C1700FD9482 /* version-info.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = "version-info.py"; sourceTree = ""; }; + 65904F2914632C1700FD9482 /* firmware_info.c.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmware_info.c.template; sourceTree = ""; }; + 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcs_version_info.h.template; sourceTree = ""; }; + 65904F2D14632C1700FD9482 /* README.txt */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = README.txt; sourceTree = ""; }; + 65904F2E14632C1700FD9482 /* shell_script.reg */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = shell_script.reg; sourceTree = ""; }; + 65904F2F14632C1700FD9482 /* install */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = install; sourceTree = ""; }; + 65904F3014632C1700FD9482 /* make */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make; sourceTree = ""; }; + 65904F3114632C1700FD9482 /* make.sh */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make.sh; sourceTree = ""; }; + 65904F3214632C1700FD9482 /* sh.cmd */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = sh.cmd; sourceTree = ""; }; + 65904F34146362F300FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65E466BC14E244020075459C /* uavobjectdefinition */ = {isa = PBXFileReference; lastKnownFileType = folder; path = uavobjectdefinition; sourceTree = ""; }; +/* End PBXFileReference section */ + +/* Begin PBXGroup section */ + 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */ = { + isa = PBXGroup; + children = ( + 4354B66314FED9FE004BA3B4 /* flight */, + 65904F1614632C1700FD9482 /* make */, + 65C35E4E12EFB2F3004811C2 /* shared */, + 65173C9F12EBFD1700D6A7CB /* Makefile */, + ); + name = OpenPilotOSX; + sourceTree = ""; + }; + 65904F1614632C1700FD9482 /* make */ = { + isa = PBXGroup; + children = ( + 65904F1714632C1700FD9482 /* boards */, + 65904F1814632C1700FD9482 /* firmware-defs.mk */, + 65904F1914632C1700FD9482 /* scripts */, + 65904F1A14632C1700FD9482 /* templates */, + 65904F1B14632C1700FD9482 /* winx86 */, + ); + name = make; + path = OpenPilotOSX.xcodeproj/../../../../make; + sourceTree = ""; + }; + 65904F1714632C1700FD9482 /* boards */ = { + isa = PBXGroup; + children = ( + 65904F33146362F300FD9482 /* revolution */, + 65904F1C14632C1700FD9482 /* ahrs */, + 65904F1D14632C1700FD9482 /* coptercontrol */, + 65904F1E14632C1700FD9482 /* esc */, + 65904F1F14632C1700FD9482 /* ins */, + 65904F2014632C1700FD9482 /* openpilot */, + 65904F2114632C1700FD9482 /* oplink */, + ); + path = boards; + sourceTree = ""; + }; + 65904F1914632C1700FD9482 /* scripts */ = { + isa = PBXGroup; + children = ( + 65904F2814632C1700FD9482 /* version-info.py */, + ); + path = scripts; + sourceTree = ""; + }; + 65904F1A14632C1700FD9482 /* templates */ = { + isa = PBXGroup; + children = ( + 65904F2914632C1700FD9482 /* firmware_info.c.template */, + 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */, + ); + path = templates; + sourceTree = ""; + }; + 65904F1B14632C1700FD9482 /* winx86 */ = { + isa = PBXGroup; + children = ( + 65904F2B14632C1700FD9482 /* bin */, + 65904F2C14632C1700FD9482 /* cmd */, + 65904F2D14632C1700FD9482 /* README.txt */, + 65904F2E14632C1700FD9482 /* shell_script.reg */, + ); + path = winx86; + sourceTree = ""; + }; + 65904F1C14632C1700FD9482 /* ahrs */ = { + isa = PBXGroup; + children = ( + 65904F2214632C1700FD9482 /* board-info.mk */, + ); + path = ahrs; + sourceTree = ""; + }; + 65904F1D14632C1700FD9482 /* coptercontrol */ = { + isa = PBXGroup; + children = ( + 65904F2314632C1700FD9482 /* board-info.mk */, + ); + path = coptercontrol; + sourceTree = ""; + }; + 65904F1E14632C1700FD9482 /* esc */ = { + isa = PBXGroup; + children = ( + 65904F2414632C1700FD9482 /* board-info.mk */, + ); + path = esc; + sourceTree = ""; + }; + 65904F1F14632C1700FD9482 /* ins */ = { + isa = PBXGroup; + children = ( + 65904F2514632C1700FD9482 /* board-info.mk */, + ); + path = ins; + sourceTree = ""; + }; + 65904F2014632C1700FD9482 /* openpilot */ = { + isa = PBXGroup; + children = ( + 65904F2614632C1700FD9482 /* board-info.mk */, + ); + path = openpilot; + sourceTree = ""; + }; + 65904F2114632C1700FD9482 /* oplink */ = { + isa = PBXGroup; + children = ( + 65904F2714632C1700FD9482 /* board-info.mk */, + ); + path = oplink; + sourceTree = ""; + }; + 65904F2B14632C1700FD9482 /* bin */ = { + isa = PBXGroup; + children = ( + 65904F2F14632C1700FD9482 /* install */, + 65904F3014632C1700FD9482 /* make */, + ); + path = bin; + sourceTree = ""; + }; + 65904F2C14632C1700FD9482 /* cmd */ = { + isa = PBXGroup; + children = ( + 65904F3114632C1700FD9482 /* make.sh */, + 65904F3214632C1700FD9482 /* sh.cmd */, + ); + path = cmd; + sourceTree = ""; + }; + 65904F33146362F300FD9482 /* revolution */ = { + isa = PBXGroup; + children = ( + 65904F34146362F300FD9482 /* board-info.mk */, + ); + path = revolution; + sourceTree = ""; + }; + 65C35E4E12EFB2F3004811C2 /* shared */ = { + isa = PBXGroup; + children = ( + 65E466BC14E244020075459C /* uavobjectdefinition */, + ); + name = shared; + path = ../../../shared; + sourceTree = SOURCE_ROOT; + }; +/* End PBXGroup section */ + +/* Begin PBXLegacyTarget section */ + 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { + isa = PBXLegacyTarget; + buildArgumentsString = ef_sparky2; + buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; + buildPhases = ( + ); + buildToolPath = /usr/bin/make; + buildWorkingDirectory = ../../..; + dependencies = ( + ); + name = OpenPilotOSX; + passBuildSettingsInEnvironment = 1; + productName = OpenPilotOSX; + }; +/* End PBXLegacyTarget section */ + +/* Begin PBXProject section */ + 08FB7793FE84155DC02AAC07 /* Project object */ = { + isa = PBXProject; + attributes = { + LastUpgradeCheck = 0820; + ORGANIZATIONNAME = OpenPilot; + }; + buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */; + compatibilityVersion = "Xcode 3.2"; + developmentRegion = English; + hasScannedForEncodings = 1; + knownRegions = ( + English, + Japanese, + French, + German, + ); + mainGroup = 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */; + projectDirPath = ""; + projectRoot = ../../..; + targets = ( + 6581071511DE809D0049FB12 /* OpenPilotOSX */, + ); + }; +/* End PBXProject section */ + +/* Begin XCBuildConfiguration section */ + 1DEB928A08733DD80010E9CD /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + CLANG_ANALYZER_LOCALIZABILITY_NONLOCALIZED = YES; + CLANG_WARN_BOOL_CONVERSION = YES; + CLANG_WARN_CONSTANT_CONVERSION = YES; + CLANG_WARN_EMPTY_BODY = YES; + CLANG_WARN_ENUM_CONVERSION = YES; + CLANG_WARN_INFINITE_RECURSION = YES; + CLANG_WARN_INT_CONVERSION = YES; + CLANG_WARN_SUSPICIOUS_MOVE = YES; + CLANG_WARN_UNREACHABLE_CODE = YES; + CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; + ENABLE_STRICT_OBJC_MSGSEND = YES; + ENABLE_TESTABILITY = YES; + GCC_C_LANGUAGE_STANDARD = gnu99; + GCC_NO_COMMON_BLOCKS = YES; + GCC_OPTIMIZATION_LEVEL = 0; + GCC_WARN_64_TO_32_BIT_CONVERSION = YES; + GCC_WARN_ABOUT_RETURN_TYPE = YES; + GCC_WARN_UNDECLARED_SELECTOR = YES; + GCC_WARN_UNINITIALIZED_AUTOS = YES; + GCC_WARN_UNUSED_FUNCTION = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + ONLY_ACTIVE_ARCH = YES; + PREBINDING = NO; + SDKROOT = macosx; + }; + name = Debug; + }; + 1DEB928B08733DD80010E9CD /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + CLANG_ANALYZER_LOCALIZABILITY_NONLOCALIZED = YES; + CLANG_WARN_BOOL_CONVERSION = YES; + CLANG_WARN_CONSTANT_CONVERSION = YES; + CLANG_WARN_EMPTY_BODY = YES; + CLANG_WARN_ENUM_CONVERSION = YES; + CLANG_WARN_INFINITE_RECURSION = YES; + CLANG_WARN_INT_CONVERSION = YES; + CLANG_WARN_SUSPICIOUS_MOVE = YES; + CLANG_WARN_UNREACHABLE_CODE = YES; + CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; + ENABLE_STRICT_OBJC_MSGSEND = YES; + GCC_C_LANGUAGE_STANDARD = gnu99; + GCC_NO_COMMON_BLOCKS = YES; + GCC_WARN_64_TO_32_BIT_CONVERSION = YES; + GCC_WARN_ABOUT_RETURN_TYPE = YES; + GCC_WARN_UNDECLARED_SELECTOR = YES; + GCC_WARN_UNINITIALIZED_AUTOS = YES; + GCC_WARN_UNUSED_FUNCTION = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + PREBINDING = NO; + SDKROOT = macosx; + }; + name = Release; + }; + 6581071611DE809D0049FB12 /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + COPY_PHASE_STRIP = NO; + GCC_DYNAMIC_NO_PIC = NO; + GCC_OPTIMIZATION_LEVEL = 0; + PRODUCT_NAME = OpenPilotOSX; + }; + name = Debug; + }; + 6581071711DE809D0049FB12 /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + COPY_PHASE_STRIP = YES; + DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; + GCC_ENABLE_FIX_AND_CONTINUE = NO; + PRODUCT_NAME = OpenPilotOSX; + ZERO_LINK = NO; + }; + name = Release; + }; +/* End XCBuildConfiguration section */ + +/* Begin XCConfigurationList section */ + 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 1DEB928A08733DD80010E9CD /* Debug */, + 1DEB928B08733DD80010E9CD /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 6581071611DE809D0049FB12 /* Debug */, + 6581071711DE809D0049FB12 /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; +/* End XCConfigurationList section */ + }; + rootObject = 08FB7793FE84155DC02AAC07 /* Project object */; +} diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index 475609c03..32623ef69 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -89,6 +89,7 @@ SRC += $(PIOSCOMMON)/pios_wavplay.c SRC += $(PIOSCOMMON)/pios_rfm22b.c SRC += $(PIOSCOMMON)/pios_rfm22b_com.c SRC += $(PIOSCOMMON)/pios_rcvr.c +SRC += $(PIOSCOMMON)/pios_dsm.c SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_hott.c SRC += $(PIOSCOMMON)/pios_srxl.c @@ -99,6 +100,7 @@ SRC += $(PIOSCOMMON)/pios_sensors.c SRC += $(PIOSCOMMON)/pios_servo.c SRC += $(PIOSCOMMON)/pios_openlrs.c SRC += $(PIOSCOMMON)/pios_openlrs_rcvr.c +SRC += $(PIOSCOMMON)/pios_board_io.c ## Misc library functions SRC += $(FLIGHTLIB)/sanitycheck.c diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 88f84d8f7..7f18b4720 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -284,6 +284,11 @@ static int32_t osdoutputInitialize(void) osdoutputEnabled = 0; } #endif + + if (osdoutputEnabled && osd_hk_com_id) { + PIOS_COM_ChangeBaud(osd_hk_com_id, 57600); + } + return 0; } MODULE_INITCALL(osdoutputInitialize, osdoutputStart); diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c new file mode 100644 index 000000000..835ca272a --- /dev/null +++ b/flight/pios/common/pios_board_io.c @@ -0,0 +1,798 @@ +/** + ****************************************************************************** + * + * @file pios_board_io.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief board io setup + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include +#include +#include + +#include "uavobjectmanager.h" + +#include + +#ifdef PIOS_INCLUDE_RCVR +# ifdef PIOS_INCLUDE_HOTT +# include +# endif +# ifdef PIOS_INCLUDE_DSM +# include +# endif +# ifdef PIOS_INCLUDE_SBUS +# include +# endif +# ifdef PIOS_INCLUDE_IBUS +# include +# endif +# ifdef PIOS_INCLUDE_EXBUS +# include +# endif +# ifdef PIOS_INCLUDE_SRXL +# include +# endif +# include +# ifdef PIOS_INCLUDE_GCSRCVR +# include +# endif +#endif /* PIOS_INCLUDE_RCVR */ + +#ifdef PIOS_INCLUDE_RFM22B +# include +# include +# ifdef PIOS_INCLUDE_RCVR +# include +# include +# include +# include +# endif /* PIOS_INCLUDE_RCVR */ +#endif /* PIOS_INCLUDE_RFM22B */ + +#ifdef PIOS_INCLUDE_ADC +# include +#endif + +#ifdef PIOS_INCLUDE_MS5611 +# include +#endif + +#ifdef PIOS_INCLUDE_RCVR +# include "manualcontrolsettings.h" +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; /* Receivers */ +#endif /* PIOS_INCLUDE_RCVR */ + +#include "hwsettings.h" +#include "auxmagsettings.h" +#include "gcsreceiver.h" + +#ifdef PIOS_INCLUDE_GPS +uint32_t pios_com_gps_id; /* GPS */ +#endif /* PIOS_INCLUDE_GPS */ + +uint32_t pios_com_bridge_id; /* ComUsbBridge */ +uint32_t pios_com_telem_rf_id; /* Serial port telemetry */ + +#ifdef PIOS_INCLUDE_RFM22B +uint32_t pios_rfm22b_id; /* RFM22B handle */ +uint32_t pios_com_rf_id; /* RFM22B telemetry */ +#endif + +uint32_t pios_com_hkosd_id; /* HK OSD ?? */ +uint32_t pios_com_msp_id; /* MSP */ +uint32_t pios_com_mavlink_id; /* MAVLink */ +uint32_t pios_com_vcp_id; /* USB VCP */ + +#ifdef PIOS_INCLUDE_DEBUG_CONSOLE +uint32_t pios_com_debug_id; /* DebugConsole */ +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + +#ifdef PIOS_INCLUDE_USB + +uint32_t pios_com_telem_usb_id; /* USB telemetry */ + +#ifdef PIOS_INCLUDE_USB_RCTX +uint32_t pios_usb_rctx_id; +#endif + +#if defined(PIOS_INCLUDE_HMC5X83) +# include "pios_hmc5x83.h" +# if defined(PIOS_HMC5X83_HAS_GPIOS) +pios_hmc5x83_dev_t pios_hmc5x83_internal_id; +# endif +#endif + + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#include "pios_usbhook.h" + + +#if defined(PIOS_INCLUDE_COM_MSG) +/* for bootloader? */ +#include + +#endif /* PIOS_INCLUDE_COM_MSG */ + +void PIOS_BOARD_IO_Configure_USB() +{ + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; + +#if defined(PIOS_INCLUDE_USB_CDC) + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; +#else + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; +#endif + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(pios_board_info_blob.board_rev)); + + uint8_t hwsettings_usb_vcpport; + uint8_t hwsettings_usb_hidport; + + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + +#if defined(PIOS_INCLUDE_USB_CDC) + + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } + + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } + + /* Why do we need to init these if function is *NONE* ?? */ +#if defined(PIOS_INCLUDE_USB_CDC) + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } +#else + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { + PIOS_Assert(0); + } +#endif + + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: +#if defined(PIOS_INCLUDE_COM) + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + 0, PIOS_COM_TELEM_USB_RX_BUF_LEN, /* Let Init() allocate this buffer */ + 0, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: +#if defined(PIOS_INCLUDE_COM) + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + 0, PIOS_COM_BRIDGE_RX_BUF_LEN, /* Let Init() allocate this buffer */ + 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + 0, 0, /* No RX buffer */ + 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_MAVLINK: + if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + 0, PIOS_COM_MAVLINK_RX_BUF_LEN, /* Let Init() allocate this buffer */ + 0, PIOS_COM_MAVLINK_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ + +#if defined(PIOS_INCLUDE_USB_HID) + /* Configure the usb HID port */ + + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: +#if defined(PIOS_INCLUDE_COM) + { + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + 0, PIOS_COM_TELEM_USB_RX_BUF_LEN, /* Let Init() allocate this buffer */ + 0, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: +#if defined(PIOS_INCLUDE_USB_RCTX) + { + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { /* this will reinstall endpoint callbacks */ + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + break; + } + +#endif /* PIOS_INCLUDE_USB_HID */ + + if (usb_hid_present || usb_cdc_present) { + PIOS_USBHOOK_Activate(); + } +} + +#endif /* PIOS_INCLUDE_USB */ + +#ifdef PIOS_INCLUDE_RCVR +# ifdef PIOS_INCLUDE_HOTT +static int32_t PIOS_HOTT_Init_SUMD(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id) +{ + return PIOS_HOTT_Init(id, driver, lower_id, PIOS_HOTT_PROTO_SUMD); +} + +static int32_t PIOS_HOTT_Init_SUMH(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id) +{ + return PIOS_HOTT_Init(id, driver, lower_id, PIOS_HOTT_PROTO_SUMH); +} +# endif /* PIOS_INCLUDE_HOTT */ +# ifdef PIOS_INCLUDE_DSM +static int32_t PIOS_DSM_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id) +{ + // DSM Bind stuff + uint8_t hwsettings_DSMxBind; + + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); + + return PIOS_DSM_Init(id, driver, lower_id, hwsettings_DSMxBind); +} +# endif +# ifdef PIOS_INCLUDE_SBUS +static int32_t PIOS_SBus_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id) +{ + // sbus-noninverted + + HwSettingsSBusModeOptions hwsettings_SBusMode; + + HwSettingsSBusModeGet(&hwsettings_SBusMode); + + struct pios_sbus_cfg sbus_cfg = { + .non_inverted = (hwsettings_SBusMode == HWSETTINGS_SBUSMODE_NONINVERTED), + }; + + return PIOS_SBus_Init(id, &sbus_cfg, driver, lower_id); +} +# endif +#endif /* ifdef PIOS_INCLUDE_RCVR */ + +struct uart_function { + uint32_t *com_id; + uint16_t com_rx_buf_len; + uint16_t com_tx_buf_len; + int32_t (*rcvr_init)(uint32_t *target, const struct pios_com_driver *driver, uint32_t lower_id); + const struct pios_rcvr_driver *rcvr_driver; + ManualControlSettingsChannelGroupsOptions rcvr_group; +}; + +static const struct uart_function uart_function_map[] = { + [PIOS_BOARD_IO_UART_TELEMETRY] = { + .com_id = &pios_com_telem_rf_id, + .com_rx_buf_len = PIOS_COM_TELEM_RF_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_TELEM_RF_TX_BUF_LEN, + }, + + [PIOS_BOARD_IO_UART_MAVLINK] = { + .com_id = &pios_com_mavlink_id, + .com_rx_buf_len = PIOS_COM_MAVLINK_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_MAVLINK_TX_BUF_LEN, + }, + + [PIOS_BOARD_IO_UART_MSP] = { + .com_id = &pios_com_msp_id, + .com_rx_buf_len = PIOS_COM_MSP_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_MSP_TX_BUF_LEN, + }, + + [PIOS_BOARD_IO_UART_GPS] = { + .com_id = &pios_com_gps_id, + .com_rx_buf_len = PIOS_COM_GPS_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_GPS_TX_BUF_LEN, + }, + + [PIOS_BOARD_IO_UART_OSDHK] = { + .com_id = &pios_com_hkosd_id, + .com_rx_buf_len = PIOS_COM_HKOSD_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_HKOSD_TX_BUF_LEN, + }, +#ifdef PIOS_INCLUDE_RCVR +# ifdef PIOS_INCLUDE_IBUS + [PIOS_BOARD_IO_UART_IBUS] = { + .rcvr_init = &PIOS_IBUS_Init, + .rcvr_driver = &pios_ibus_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS, + }, +# endif /* PIOS_INCLUDE_IBUS */ +# ifdef PIOS_INCLUDE_EXBUS + [PIOS_BOARD_IO_UART_EXBUS] = { + .rcvr_init = &PIOS_EXBUS_Init, + .rcvr_driver = &pios_exbus_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS, + }, +# endif /* PIOS_INCLUDE_EXBUS */ +# ifdef PIOS_INCLUDE_SRXL + [PIOS_BOARD_IO_UART_SRXL] = { + .rcvr_init = &PIOS_SRXL_Init, + .rcvr_driver = &pios_srxl_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL, + }, +# endif /* PIOS_INCLUDE_SRXL */ +# ifdef PIOS_INCLUDE_HOTT + [PIOS_BOARD_IO_UART_HOTT_SUMD] = { + .rcvr_init = &PIOS_HOTT_Init_SUMD, + .rcvr_driver = &pios_hott_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT, + }, + + [PIOS_BOARD_IO_UART_HOTT_SUMH] = { + .rcvr_init = &PIOS_HOTT_Init_SUMH, + .rcvr_driver = &pios_hott_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT, + }, +# endif /* PIOS_INCLUDE_HOTT */ +# ifdef PIOS_INCLUDE_DSM + [PIOS_BOARD_IO_UART_DSM_MAIN] = { + .rcvr_init = &PIOS_DSM_Init_Helper, + .rcvr_driver = &pios_dsm_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, + }, + + [PIOS_BOARD_IO_UART_DSM_FLEXI] = { + .rcvr_init = &PIOS_DSM_Init_Helper, + .rcvr_driver = &pios_dsm_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, + }, + + [PIOS_BOARD_IO_UART_DSM_RCVR] = { + .rcvr_init = &PIOS_DSM_Init_Helper, + .rcvr_driver = &pios_dsm_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT, + }, +# endif /* PIOS_INCLUDE_DSM */ +# ifdef PIOS_INCLUDE_SBUS + [PIOS_BOARD_IO_UART_SBUS] = { + .rcvr_init = &PIOS_SBus_Init_Helper, + .rcvr_driver = &pios_sbus_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS, + }, +# endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_RCVR */ +}; + +void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *hw_config, PIOS_BOARD_IO_UART_Function function) +{ + if (function >= NELEMENTS(uart_function_map)) { + return; + } + + if (uart_function_map[function].com_id) { + uint32_t usart_id; + + if (PIOS_USART_Init(&usart_id, hw_config)) { + PIOS_Assert(0); + } + + if (PIOS_COM_Init(uart_function_map[function].com_id, &pios_usart_com_driver, usart_id, + 0, uart_function_map[function].com_rx_buf_len, + 0, uart_function_map[function].com_tx_buf_len)) { + PIOS_Assert(0); + } + } else if (uart_function_map[function].rcvr_init) { + uint32_t usart_id; + + if (PIOS_USART_Init(&usart_id, hw_config)) { + PIOS_Assert(0); + } + + uint32_t rcvr_driver_id; + + if (uart_function_map[function].rcvr_init(&rcvr_driver_id, &pios_usart_com_driver, usart_id)) { + PIOS_Assert(0); + } + + uint32_t rcvr_id; + if (PIOS_RCVR_Init(&rcvr_id, uart_function_map[function].rcvr_driver, rcvr_driver_id)) { + PIOS_Assert(0); + } + + pios_rcvr_group_map[uart_function_map[function].rcvr_group] = rcvr_id; + } +} + +#ifdef PIOS_INCLUDE_PWM +void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg) +{ + /* Set up the receiver port. Later this should be optional */ + uint32_t pios_pwm_id; + + PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; +} +#endif /* PIOS_INCLUDE_PWM */ + +#ifdef PIOS_INCLUDE_PPM +void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg) +{ + uint32_t pios_ppm_id; + + PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; +} +#endif /* PIOS_INCLUDE_PPM */ + +#ifdef PIOS_INCLUDE_GCSRCVR +void PIOS_BOARD_IO_Configure_GCSRCVR() +{ + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +} +#endif /* PIOS_INCLUDE_GCSRCVR */ + +#ifdef PIOS_INCLUDE_RFM22B + +#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) +static void PIOS_Board_PPM_callback(const int16_t *channels) +{ + OPLinkReceiverData opl_rcvr; + + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { + opl_rcvr.Channel[i] = (i < RFM22B_PPM_NUM_CHANNELS) ? channels[i] : PIOS_RCVR_TIMEOUT; + } + OPLinkReceiverSet(&opl_rcvr); +} +#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */ + +void PIOS_BOARD_IO_Configure_RFM22B() +{ +#if defined(PIOS_INCLUDE_RFM22B) + OPLinkSettingsInitialize(); + OPLinkStatusInitialize(); +#endif /* PIOS_INCLUDE_RFM22B */ +#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) + OPLinkReceiverInitialize(); +#endif + /* Initialize the RFM22B radio COM device. */ + + /* Fetch the OPLinkSettings object. */ + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + // Initialize out status object. + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + oplinkStatus.BoardType = pios_board_info_blob.board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = pios_board_info_blob.board_rev; + + /* Is the radio turned on? */ + bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR); + bool openlrs = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPENLRS); + bool data_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATA) || + (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); + bool ppm_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) || + (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); + bool ppm_only = (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL); + bool is_enabled = ((oplinkSettings.Protocol != OPLINKSETTINGS_PROTOCOL_DISABLED) && + ((oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) || openlrs)); + if (is_enabled) { + if (openlrs) { +#if defined(PIOS_INCLUDE_OPENLRS_RCVR) && defined(PIOS_INCLUDE_RCVR) + uint32_t openlrs_id; + const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(pios_board_info_blob.board_rev); + + PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg); + { + uint32_t openlrsrcvr_id; + PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); + uint32_t openlrsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS] = openlrsrcvr_rcvr_id; + } +#endif /* PIOS_INCLUDE_OPENLRS_RCVR && PIOS_INCLUDE_RCVR */ + } else { + /* Configure the RFM22B device. */ + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(pios_board_info_blob.board_rev); + + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { + PIOS_Assert(0); + } + + /* Configure the radio com interface */ + if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + 0, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + 0, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; + + // Set the modem (over the air) datarate. + enum rfm22b_datarate datarate = RFM22_datarate_64000; + switch (oplinkSettings.AirDataRate) { + case OPLINKSETTINGS_AIRDATARATE_9600: + datarate = RFM22_datarate_9600; + break; + case OPLINKSETTINGS_AIRDATARATE_19200: + datarate = RFM22_datarate_19200; + break; + case OPLINKSETTINGS_AIRDATARATE_32000: + datarate = RFM22_datarate_32000; + break; + case OPLINKSETTINGS_AIRDATARATE_57600: + datarate = RFM22_datarate_57600; + break; + case OPLINKSETTINGS_AIRDATARATE_64000: + datarate = RFM22_datarate_64000; + break; + case OPLINKSETTINGS_AIRDATARATE_100000: + datarate = RFM22_datarate_100000; + break; + case OPLINKSETTINGS_AIRDATARATE_128000: + datarate = RFM22_datarate_128000; + break; + case OPLINKSETTINGS_AIRDATARATE_192000: + datarate = RFM22_datarate_192000; + break; + case OPLINKSETTINGS_AIRDATARATE_256000: + datarate = RFM22_datarate_256000; + break; + } + + /* Set the radio configuration parameters. */ + PIOS_RFM22B_SetDeviceID(pios_rfm22b_id, oplinkSettings.CustomDeviceID); + PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap); + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode); + +#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) + /* Set the PPM callback if we should be receiving PPM. */ + if (ppm_mode || (ppm_only && !is_coordinator)) { + PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback); + } +#endif + /* Set the modem Tx power level */ + switch (oplinkSettings.MaxRFPower) { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + default: + // do nothing + break; + } + + /* Reinitialize the modem. */ + PIOS_RFM22B_Reinit(pios_rfm22b_id); + + // TODO: this is in preparation for full mavlink support and is used by LP-368 + uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN; + + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); + + switch (hwsettings_radioaux) { + case HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE: + case HWSETTINGS_RADIOAUXSTREAM_DISABLED: + break; + case HWSETTINGS_RADIOAUXSTREAM_MAVLINK: + { + if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, + 0, mavlink_rx_size, + 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + } + } + } else { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + } + + OPLinkStatusSet(&oplinkStatus); + +#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) + uint32_t pios_oplinkrcvr_id; + PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id); + uint32_t pios_oplinkrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */ +} +#endif /* ifdef PIOS_INCLUDE_RFM22B */ + +#ifdef PIOS_INCLUDE_I2C +void PIOS_BOARD_IO_Configure_I2C(uint32_t i2c_internal_id, uint32_t i2c_external_id) +{ + // internal HMC5x83 mag +# ifdef PIOS_INCLUDE_HMC5X83_INTERNAL + const struct pios_hmc5x83_cfg *hmc5x83_internal_cfg = PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(pios_board_info_blob.board_rev); + + if (hmc5x83_internal_cfg) { + // attach the 5x83 mag to internal i2c bus + pios_hmc5x83_internal_id = PIOS_HMC5x83_Init(hmc5x83_internal_cfg, i2c_internal_id, 0); +# ifdef PIOS_INCLUDE_WDG + // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed + // this is not in a loop, so it is safe + PIOS_WDG_Clear(); +# endif /* PIOS_INCLUDE_WDG */ + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(pios_hmc5x83_internal_id, PIOS_SENSORS_TYPE_3AXIS_MAG); + } + +# endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */ + + // internal ms5611 baro +#if defined(PIOS_INCLUDE_MS5611) + PIOS_MS5611_Init(PIOS_BOARD_HW_DEFS_GetMS5611Cfg(pios_board_info_blob.board_rev), i2c_internal_id); + PIOS_MS5611_Register(); +#endif + + // internal bmp280 baro + // internal MPU6000 imu (i2c) + // internal eeprom (revo nano) - NOT HERE, should be initialized earlier + + + /* Initialize external sensors */ + + // aux mag +# ifdef PIOS_INCLUDE_HMC5X83 + AuxMagSettingsInitialize(); + + AuxMagSettingsTypeOptions option; + AuxMagSettingsTypeGet(&option); + + const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev); + + if (hmc5x83_external_cfg) { + uint32_t i2c_id = 0; + + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // i2c_external + i2c_id = i2c_external_id; + } else if (option == AUXMAGSETTINGS_TYPE_I2C) { + // i2c_internal (or Sparky2/F3 dedicated I2C port) + i2c_id = i2c_internal_id; + } + + if (i2c_id) { + uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, i2c_id, 0); +# ifdef PIOS_INCLUDE_WDG + // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed + // this is not in a loop, so it is safe + PIOS_WDG_Clear(); +# endif /* PIOS_INCLUDE_WDG */ + // add this sensor to the sensor task's list + // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor + // and before registering some other fast and important sensor + // as that would cause delay and time jitter for the second fast sensor + PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); + // mag alarm is cleared later, so use I2C + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); + } + } +# endif /* PIOS_INCLUDE_HMC5X83 */ + + // external ETASV3 Eagletree Airspeed v3 + // external MS4525D PixHawk Airpeed based on MS4525DO + + // BMA180 accelerometer? + // bmp085/bmp180 baro + + // hmc5843 mag + // i2c esc (?) + // UBX DCC +} +#endif /* ifdef PIOS_INCLUDE_I2C */ + +#ifdef PIOS_INCLUDE_ADC +void PIOS_BOARD_IO_Configure_ADC() +{ + PIOS_ADC_Init(PIOS_BOARD_HW_DEFS_GetAdcCfg(pios_board_info_blob.board_rev)); + uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingArrayGet(adc_config); + for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { + PIOS_ADC_PinSetup(i); + } + } +} +#endif /* PIOS_INCLUDE_ADC */ diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index fe5f89061..26794095f 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -118,8 +118,18 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui PIOS_Assert(com_id); PIOS_Assert(driver); - bool has_rx = (rx_buffer && rx_buffer_len > 0); - bool has_tx = (tx_buffer && tx_buffer_len > 0); + if ((rx_buffer_len > 0) && !rx_buffer) { + rx_buffer = (uint8_t *)pios_malloc(rx_buffer_len); + PIOS_Assert(rx_buffer); + } + + if ((tx_buffer_len > 0) && !tx_buffer) { + tx_buffer = (uint8_t *)pios_malloc(tx_buffer_len); + PIOS_Assert(tx_buffer); + } + + bool has_rx = (rx_buffer_len > 0); + bool has_tx = (tx_buffer_len > 0); PIOS_Assert(driver->bind_tx_cb || !has_tx); PIOS_Assert(driver->bind_rx_cb || !has_rx); @@ -846,6 +856,29 @@ void PIOS_COM_LinkComPair(uint32_t com1_id, uint32_t com2_id, bool link_ctrl_lin } } +/* + * Invoke driver specific control functions + * \param[in] port COM port + * \param[in] ctl control function number + * \param[inout] control function parameter + * \return 0 on success + */ +int32_t PIOS_COM_Ioctl(uint32_t com_id, uint32_t ctl, void *param) +{ + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + if (!com_dev->driver->ioctl) { + return -1; + } + + return com_dev->driver->ioctl(com_dev->lower_id, ctl, param); +} + #endif /* PIOS_INCLUDE_COM */ /** diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/common/pios_dsm.c similarity index 90% rename from flight/pios/stm32f4xx/pios_dsm.c rename to flight/pios/common/pios_dsm.c index bdd8244e1..23d3601e3 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/common/pios_dsm.c @@ -78,9 +78,8 @@ struct pios_dsm_state { #define DSM_FL_WEIGHTED_AVE 18 struct pios_dsm_dev { - enum pios_dsm_dev_magic magic; - const struct pios_dsm_cfg *cfg; - struct pios_dsm_state state; + enum pios_dsm_dev_magic magic; + struct pios_dsm_state state; }; /* Allocate DSM device descriptor */ @@ -122,39 +121,42 @@ static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev) } /* Try to bind DSMx satellite using specified number of pulses */ -static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) +static void PIOS_DSM_Bind(struct stm32_gpio *rxpin, uint8_t bind) { - const struct pios_dsm_cfg *cfg = dsm_dev->cfg; + GPIO_InitTypeDef bindInit = { + .GPIO_Pin = rxpin->init.GPIO_Pin, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }; - GPIO_InitTypeDef GPIO_InitStructure = cfg->bind.init; - - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - - /* just to limit bind pulses */ - if (bind > 10) { - bind = 10; - } - - GPIO_Init(cfg->bind.gpio, &cfg->bind.init); + GPIO_Init(rxpin->gpio, &bindInit); /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_SetBits(rxpin->gpio, rxpin->init.GPIO_Pin); /* Wait until the bind window opens. */ while (PIOS_DELAY_GetuS() < DSM_BIND_MIN_DELAY_US) { ; } + /* just to limit bind pulses */ + if (bind > 10) { + bind = 10; + } + for (int i = 0; i < bind; i++) { /* RX line, drive low for 120us */ - GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_ResetBits(rxpin->gpio, rxpin->init.GPIO_Pin); PIOS_DELAY_WaituS(120); /* RX line, drive high for 120us */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); + GPIO_SetBits(rxpin->gpio, rxpin->init.GPIO_Pin); PIOS_DELAY_WaituS(120); } + /* RX line, set input and wait for data */ - GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); + GPIO_Init(rxpin->gpio, (GPIO_InitTypeDef *)&rxpin->init); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ @@ -292,13 +294,11 @@ static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte) /* Initialise DSM receiver interface */ int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t bind) { PIOS_DEBUG_Assert(dsm_id); - PIOS_DEBUG_Assert(cfg); PIOS_DEBUG_Assert(driver); struct pios_dsm_dev *dsm_dev; @@ -308,20 +308,36 @@ int32_t PIOS_DSM_Init(uint32_t *dsm_id, return -1; } - /* Bind the configuration to the device instance */ - dsm_dev->cfg = cfg; - /* Bind the receiver if requested */ if (bind) { - PIOS_DSM_Bind(dsm_dev, bind); + struct stm32_gpio rxpin; + + PIOS_DEBUG_Assert(driver->ioctl); + + if ((driver->ioctl)(lower_id, PIOS_IOCTL_USART_GET_RXGPIO, &rxpin) < 0) { + return -1; + } + + PIOS_DSM_Bind(&rxpin, bind); } PIOS_DSM_ResetState(dsm_dev); *dsm_id = (uint32_t)dsm_dev; + + /* Set comm driver parameters */ + PIOS_DEBUG_Assert(driver->set_config); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + + /* Set irq priority */ + if (driver->ioctl) { + uint8_t irq_prio = PIOS_IRQ_PRIO_HIGH; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_IRQ_PRIO, &irq_prio); + } + /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); + driver->bind_rx_cb(lower_id, PIOS_DSM_RxInCallback, *dsm_id); if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { PIOS_DEBUG_Assert(0); diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c index a65333ea0..c29564075 100644 --- a/flight/pios/common/pios_exbus.c +++ b/flight/pios/common/pios_exbus.c @@ -307,8 +307,18 @@ int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, *exbus_id = (uint32_t)exbus_dev; + /* Set comm driver parameters */ + PIOS_DEBUG_Assert(driver->set_config); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 125000, PIOS_COM_Mode_Rx); + + /* Set irq priority */ + if (driver->ioctl) { + uint8_t irq_prio = PIOS_IRQ_PRIO_HIGH; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_IRQ_PRIO, &irq_prio); + } + /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_EXBUS_RxInCallback, *exbus_id); + driver->bind_rx_cb(lower_id, PIOS_EXBUS_RxInCallback, *exbus_id); if (!PIOS_RTC_RegisterTickCallback(PIOS_EXBUS_Supervisor, *exbus_id)) { PIOS_DEBUG_Assert(0); diff --git a/flight/pios/common/pios_hott.c b/flight/pios/common/pios_hott.c index a3106edc4..04f2fc940 100644 --- a/flight/pios/common/pios_hott.c +++ b/flight/pios/common/pios_hott.c @@ -325,6 +325,16 @@ int32_t PIOS_HOTT_Init(uint32_t *hott_id, *hott_id = (uint32_t)hott_dev; + /* Set comm driver parameters */ + PIOS_DEBUG_Assert(driver->set_config); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + + /* Set irq priority */ + if (driver->ioctl) { + uint8_t irq_prio = PIOS_IRQ_PRIO_HIGH; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_IRQ_PRIO, &irq_prio); + } + /* Set comm driver callback */ (driver->bind_rx_cb)(lower_id, PIOS_HOTT_RxInCallback, *hott_id); diff --git a/flight/pios/common/pios_ibus.c b/flight/pios/common/pios_ibus.c index e37b43014..953ef8865 100644 --- a/flight/pios/common/pios_ibus.c +++ b/flight/pios/common/pios_ibus.c @@ -174,7 +174,17 @@ int32_t PIOS_IBUS_Init(uint32_t *ibus_id, const struct pios_com_driver *driver, PIOS_Assert(0); } - (driver->bind_rx_cb)(lower_id, PIOS_IBUS_Receive, *ibus_id); + /* Set comm driver parameters */ + PIOS_DEBUG_Assert(driver->set_config); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + + /* Set irq priority */ + if (driver->ioctl) { + uint8_t irq_prio = PIOS_IRQ_PRIO_HIGH; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_IRQ_PRIO, &irq_prio); + } + + driver->bind_rx_cb(lower_id, PIOS_IBUS_Receive, *ibus_id); return 0; } diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 240a7c51c..979c6a856 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -166,13 +166,22 @@ int32_t PIOS_SBus_Init(uint32_t *sbus_id, *sbus_id = (uint32_t)sbus_dev; - /* Enable inverter clock and enable the inverter */ - (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); - GPIO_Init(cfg->inv.gpio, &cfg->inv.init); - GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); + /* Set rest of the parameters */ + if (driver->set_config) { + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_2, PIOS_COM_Parity_Even, 100000, PIOS_COM_Mode_Rx); + } + + /* Set inverted UART and IRQ priority */ + if (driver->ioctl) { + enum PIOS_USART_Inverted param = cfg->non_inverted ? 0 : PIOS_USART_Inverted_Rx; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_INVERTED, ¶m); + + uint8_t irq_prio = PIOS_IRQ_PRIO_HIGH; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_IRQ_PRIO, &irq_prio); + } /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id); + driver->bind_rx_cb(lower_id, PIOS_SBus_RxInCallback, *sbus_id); if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) { PIOS_DEBUG_Assert(0); diff --git a/flight/pios/common/pios_srxl.c b/flight/pios/common/pios_srxl.c index 711679829..b0595ed8e 100644 --- a/flight/pios/common/pios_srxl.c +++ b/flight/pios/common/pios_srxl.c @@ -156,8 +156,18 @@ int32_t PIOS_SRXL_Init(uint32_t *srxl_id, *srxl_id = (uint32_t)srxl_dev; + /* Set comm driver parameters */ + PIOS_DEBUG_Assert(driver->set_config); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + + /* Set irq priority */ + if (driver->ioctl) { + uint8_t irq_prio = PIOS_IRQ_PRIO_HIGH; + driver->ioctl(lower_id, PIOS_IOCTL_USART_SET_IRQ_PRIO, &irq_prio); + } + /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_SRXL_RxInCallback, *srxl_id); + driver->bind_rx_cb(lower_id, PIOS_SRXL_RxInCallback, *srxl_id); if (!PIOS_RTC_RegisterTickCallback(PIOS_SRXL_Supervisor, *srxl_id)) { PIOS_DEBUG_Assert(0); diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index e68e5ab68..d14add637 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -317,6 +317,22 @@ static const struct usb_config_hid_cdc config_hid_cdc = { }, }; +const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { + .ctrl_if = 0, + .ctrl_tx_ep = 2, + + .data_if = 1, + .data_rx_ep = 3, + .data_tx_ep = 3, +}; + +const struct pios_usb_hid_cfg pios_usb_hid_cfg = { + .data_if = 2, + .data_rx_ep = 1, + .data_tx_ep = 1, +}; + + int32_t PIOS_USB_DESC_HID_CDC_Init(void) { PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_cdc, sizeof(config_hid_cdc)); diff --git a/flight/pios/common/pios_usb_desc_hid_only.c b/flight/pios/common/pios_usb_desc_hid_only.c index 89cf9ebe4..ce4501388 100644 --- a/flight/pios/common/pios_usb_desc_hid_only.c +++ b/flight/pios/common/pios_usb_desc_hid_only.c @@ -156,6 +156,12 @@ const struct usb_config_hid_only config_hid_only = { }, }; +const struct pios_usb_hid_cfg pios_usb_hid_only_cfg = { + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, +}; + int32_t PIOS_USB_DESC_HID_ONLY_Init(void) { PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_only, sizeof(config_hid_only)); diff --git a/flight/pios/inc/pios_board_hw.h b/flight/pios/inc/pios_board_hw.h new file mode 100644 index 000000000..ce344b9ce --- /dev/null +++ b/flight/pios/inc/pios_board_hw.h @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * + * @file pios_board_io.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief PIOS_BOARD_HW_DEFS functions + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIOS_BOARD_HW_H +#define PIOS_BOARD_HW_H + +#ifdef PIOS_INCLUDE_LED +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_USB +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_RFM22B +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_OPENLRS +const struct pios_openlrs_cfg *PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(uint32_t board_revision); +#endif +# ifdef PIOS_INCLUDE_HMC5X83_INTERNAL +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(uint32_t board_revision); +# endif +#ifdef PIOS_INCLUDE_HMC5X83 +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_MS5611 +const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_BMP280 +const struct pios_bmp280_cfg *PIOS_BOARD_HW_DEFS_GetBMP280Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_ADC +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(uint32_t board_revision); +#endif +#endif /* PIOS_BOARD_HW_H */ diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h new file mode 100644 index 000000000..bbcd5c2cc --- /dev/null +++ b/flight/pios/inc/pios_board_io.h @@ -0,0 +1,221 @@ +/** + ****************************************************************************** + * + * @file pios_board_io.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief board io setup + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIOS_BOARD_IO_H +#define PIOS_BOARD_IO_H + +#include "pios.h" + + +#ifdef PIOS_INCLUDE_USB +#include +#endif + +#include + +#ifdef PIOS_INCLUDE_PWM +#include +#endif + +#ifdef PIOS_INCLUDE_PPM +#include +#endif + +#ifdef PIOS_INCLUDE_OPENLRS +#include +#endif + +#ifdef PIOS_INCLUDE_RCVR +extern uint32_t pios_rcvr_group_map[]; /* Receivers */ +#endif + +/* GPS */ +#ifdef PIOS_INCLUDE_GPS +extern uint32_t pios_com_gps_id; +# define PIOS_COM_GPS (pios_com_gps_id) +# ifndef PIOS_COM_GPS_RX_BUF_LEN +# define PIOS_COM_GPS_RX_BUF_LEN 128 +# endif +# ifndef PIOS_COM_GPS_TX_BUF_LEN +# define PIOS_COM_GPS_TX_BUF_LEN 32 +# endif +#endif /* PIOS_INCLUDE_GPS */ + + +/* ComUsbBridge */ +extern uint32_t pios_com_bridge_id; +#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#ifndef PIOS_COM_BRIDGE_RX_BUF_LEN +# define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#endif +#ifndef PIOS_COM_BRIDGE_TX_BUF_LEN +# define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#endif + + +/* USB telemetry */ +extern uint32_t pios_com_telem_usb_id; +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#ifndef PIOS_COM_TELEM_USB_RX_BUF_LEN +# define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#endif +#ifndef PIOS_COM_TELEM_USB_TX_BUF_LEN +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 +#endif + +/* Serial port telemetry */ +extern uint32_t pios_com_telem_rf_id; +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#ifndef PIOS_COM_TELEM_RF_RX_BUF_LEN +# define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +#endif +#ifndef PIOS_COM_TELEM_RF_TX_BUF_LEN +# define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +#endif + + +/* RFM22B telemetry */ +#ifdef PIOS_INCLUDE_RFM22B +extern uint32_t pios_com_rf_id; +# define PIOS_COM_RF (pios_com_rf_id) +# ifndef PIOS_COM_RFM22B_RF_RX_BUF_LEN +# define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 +# endif +# ifndef PIOS_COM_RFM22B_RF_TX_BUF_LEN +# define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 +# endif +#endif + + +/* HK OSD ?? */ +extern uint32_t pios_com_hkosd_id; +#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#ifndef PIOS_COM_HKOSD_RX_BUF_LEN +# define PIOS_COM_HKOSD_RX_BUF_LEN 22 +#endif +#ifndef PIOS_COM_HKOSD_TX_BUF_LEN +# define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#endif + +/* MSP */ +extern uint32_t pios_com_msp_id; +#define PIOS_COM_MSP (pios_com_msp_id) +#ifndef PIOS_COM_MSP_TX_BUF_LEN +# define PIOS_COM_MSP_TX_BUF_LEN 128 +#endif +#ifndef PIOS_COM_MSP_RX_BUF_LEN +# define PIOS_COM_MSP_RX_BUF_LEN 64 +#endif + +/* MAVLink */ +extern uint32_t pios_com_mavlink_id; +#define PIOS_COM_MAVLINK (pios_com_mavlink_id) +#ifndef PIOS_COM_MAVLINK_TX_BUF_LEN +# define PIOS_COM_MAVLINK_TX_BUF_LEN 128 +#endif +#ifndef PIOS_COM_MAVLINK_RX_BUF_LEN +# define PIOS_COM_MAVLINK_RX_BUF_LEN 128 +#endif + +/* USB VCP */ +extern uint32_t pios_com_vcp_id; +#define PIOS_COM_VCP (pios_com_vcp_id) + + +#ifdef PIOS_INCLUDE_DEBUG_CONSOLE +extern uint32_t pios_com_debug_id; +#define PIOS_COM_DEBUG (pios_com_debug_id) +#ifndef PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN +# define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 +#endif +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + +#ifdef PIOS_INCLUDE_USB_RCTX +extern uint32_t pios_usb_rctx_id; +#endif /* PIOS_INCLUDE_USB_RCTX */ + +#if defined(PIOS_INCLUDE_HMC5X83) && defined(PIOS_HMC5X83_HAS_GPIOS) +#include +extern pios_hmc5x83_dev_t pios_hmc5x83_internal_id; +#endif + +typedef enum { + PIOS_BOARD_IO_UART_NONE, + PIOS_BOARD_IO_UART_TELEMETRY, /* com */ + PIOS_BOARD_IO_UART_MAVLINK, /* com */ + PIOS_BOARD_IO_UART_MSP, /* com */ + PIOS_BOARD_IO_UART_GPS, /* com */ + PIOS_BOARD_IO_UART_COMBRIDGE, /* com */ + PIOS_BOARD_IO_UART_DEBUGCONSOLE, /* com */ + PIOS_BOARD_IO_UART_OSDHK, /* com */ + PIOS_BOARD_IO_UART_SBUS, /* rcvr */ + PIOS_BOARD_IO_UART_DSM_MAIN, /* rcvr */ + PIOS_BOARD_IO_UART_DSM_FLEXI, /* rcvr */ + PIOS_BOARD_IO_UART_DSM_RCVR, /* rcvr */ + PIOS_BOARD_IO_UART_HOTT_SUMD, /* rcvr */ + PIOS_BOARD_IO_UART_HOTT_SUMH, /* rcvr */ + PIOS_BOARD_IO_UART_SRXL, /* rcvr */ + PIOS_BOARD_IO_UART_IBUS, /* rcvr */ + PIOS_BOARD_IO_UART_EXBUS, /* rcvr */ +} PIOS_BOARD_IO_UART_Function; + +#ifdef PIOS_INCLUDE_USB +void PIOS_BOARD_IO_Configure_USB(); +# if defined(PIOS_INCLUDE_USB_HID) +# include +extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; +# endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB */ +#ifdef PIOS_INCLUDE_PWM +void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); +#endif +#ifdef PIOS_INCLUDE_PPM +void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg); +#endif + +void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *usart_cfg, PIOS_BOARD_IO_UART_Function function); + +#ifdef PIOS_INCLUDE_RFM22B +void PIOS_BOARD_IO_Configure_RFM22B(); +#endif + +#ifdef PIOS_INCLUDE_I2C +void PIOS_BOARD_IO_Configure_I2C(uint32_t i2c_internal_id, uint32_t i2c_external_id); +#endif + +#ifdef PIOS_INCLUDE_GCSRCVR +void PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif + +#ifdef PIOS_INCLUDE_WS2811 +void PIOS_BOARD_IO_Configure_WS2811(); +#endif + +#ifdef PIOS_INCLUDE_ADC +void PIOS_BOARD_IO_Configure_ADC(); +#endif + + +#endif /* PIOS_BOARD_IO_H */ diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index eb4ee37a8..a150691a3 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -70,7 +70,6 @@ enum PIOS_COM_Mode { }; struct pios_com_driver { - void (*init)(uint32_t id); void (*set_baud)(uint32_t id, uint32_t baud); void (*set_halfduplex)(uint32_t id, bool halfduplex); void (*set_config)(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); @@ -83,6 +82,7 @@ struct pios_com_driver { void (*bind_baud_rate_cb)(uint32_t id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context); uint32_t (*available)(uint32_t id); void (*bind_available_cb)(uint32_t id, pios_com_callback_available available_cb, uint32_t context); + int32_t (*ioctl)(uint32_t id, uint32_t ctl, void *param); }; /* Control line definitions */ @@ -117,10 +117,36 @@ extern int32_t PIOS_COM_ASYNC_RegisterTxCallback(uint32_t id, pios_com_callback extern void PIOS_COM_LinkComPair(uint32_t com1_id, uint32_t com2_id, bool link_ctrl_line, bool link_baud_rate); -#define COM_AVAILABLE_NONE (0) -#define COM_AVAILABLE_RX (1 << 0) -#define COM_AVAILABLE_TX (1 << 1) -#define COM_AVAILABLE_RXTX (COM_AVAILABLE_RX | COM_AVAILABLE_TX) +#define COM_AVAILABLE_NONE (0) +#define COM_AVAILABLE_RX (1 << 0) +#define COM_AVAILABLE_TX (1 << 1) +#define COM_AVAILABLE_RXTX (COM_AVAILABLE_RX | COM_AVAILABLE_TX) + +/* ioctl */ +extern int32_t PIOS_COM_Ioctl(uint32_t com_id, uint32_t ctl, void *param); + +#define COM_IOCTL_NRBITS 16 +#define COM_IOCTL_TYPEBITS 16 + +#define COM_IOCTL_NRMASK ((1 << COM_IOCTL_NRBITS) - 1) +#define COM_IOCTL_TYPEMASK ((1 << COM_IOCTL_TYPEBITS) - 1) + +#define COM_IOCTL_NRSHIFT 0 +#define COM_IOCTL_TYPESHIFT (COM_IOCTL_NRSHIFT + COM_IOCTL_NRBITS) + +#define COM_IOCTL(type, nr, pt) \ + (((type) << COM_IOCTL_TYPESHIFT) | \ + ((nr) << COM_IOCTL_NRSHIFT)) + +/* used to decode ioctl numbers.. */ +#define COM_IOCTL_TYPE(nr) (((nr) >> COM_IOCTL_TYPESHIFT) & COM_IOCTL_TYPEMASK) +#define COM_IOCTL_NR(nr) (((nr) >> COM_IOCTL_NRSHIFT) & COM_IOCTL_NRMASK) + +enum pios_com_ioctl_type { + COM_IOCTL_TYPE_USART, + COM_IOCTL_TYPE_USB_CDC, + COM_IOCTL_TYPE_SOFT_UART, +}; #endif /* PIOS_COM_H */ diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index 40607e93e..6e7ce1106 100644 --- a/flight/pios/inc/pios_dsm_priv.h +++ b/flight/pios/inc/pios_dsm_priv.h @@ -109,15 +109,9 @@ */ // #define DSM_LOST_FRAME_COUNTER -/* DSM receiver instance configuration */ -struct pios_dsm_cfg { - struct stm32_gpio bind; -}; - extern const struct pios_rcvr_driver pios_dsm_rcvr_driver; extern int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t bind); diff --git a/flight/pios/inc/pios_sbus_priv.h b/flight/pios/inc/pios_sbus_priv.h index 791683831..e99a4adca 100644 --- a/flight/pios/inc/pios_sbus_priv.h +++ b/flight/pios/inc/pios_sbus_priv.h @@ -84,11 +84,12 @@ * S.Bus configuration programmable invertor */ struct pios_sbus_cfg { - struct stm32_gpio inv; - void (*gpio_clk_func)(uint32_t periph, FunctionalState state); - uint32_t gpio_clk_periph; - BitAction gpio_inv_enable; - BitAction gpio_inv_disable; + bool non_inverted; +// struct stm32_gpio inv; +// void (*gpio_clk_func)(uint32_t periph, FunctionalState state); +// uint32_t gpio_clk_periph; +// BitAction gpio_inv_enable; +// BitAction gpio_inv_disable; }; extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; diff --git a/flight/pios/inc/pios_stm32.h b/flight/pios/inc/pios_stm32.h index 01aefe059..03eaa2699 100644 --- a/flight/pios/inc/pios_stm32.h +++ b/flight/pios/inc/pios_stm32.h @@ -60,7 +60,13 @@ struct stm32_dma { struct stm32_gpio { GPIO_TypeDef *gpio; GPIO_InitTypeDef init; - uint8_t pin_source; + uint8_t pin_source; /* do we really need this, or we can get it from init.GPIO_Pin ? */ +}; + +struct stm32_gpio_action { + struct stm32_gpio pin; + BitAction on; + BitAction off; }; /** diff --git a/flight/pios/inc/pios_usart.h b/flight/pios/inc/pios_usart.h index 379223a6f..49ac3c92a 100644 --- a/flight/pios/inc/pios_usart.h +++ b/flight/pios/inc/pios_usart.h @@ -34,6 +34,25 @@ /* Global Types */ /* Public Functions */ +/* USART Ioctls */ + +enum PIOS_USART_Inverted { + PIOS_USART_Inverted_None = 0, + PIOS_USART_Inverted_Rx = (1 << 0), + PIOS_USART_Inverted_Tx = (1 << 1), + PIOS_USART_Inverted_RxTx = (PIOS_USART_Inverted_Rx | PIOS_USART_Inverted_Tx) +}; + +#define PIOS_IOCTL_USART_SET_INVERTED COM_IOCTL(COM_IOCTL_TYPE_USART, 1, enum PIOS_USART_Inverted) +#define PIOS_IOCTL_USART_SET_SWAPPIN COM_IOCTL(COM_IOCTL_TYPE_USART, 2, bool) +#define PIOS_IOCTL_USART_SET_ONEWIRE COM_IOCTL(COM_IOCTL_TYPE_USART, 3, bool) + +#define PIOS_IOCTL_USART_GET_RXGPIO COM_IOCTL(COM_IOCTL_TYPE_USART, 4, struct stm32_gpio) +#define PIOS_IOCTL_USART_GET_TXGPIO COM_IOCTL(COM_IOCTL_TYPE_USART, 5, struct stm32_gpio) + +/* PIOS_IRQ_PRIO_ values */ +#define PIOS_IOCTL_USART_SET_IRQ_PRIO COM_IOCTL(COM_IOCTL_TYPE_USART, 6, uint8_t) + #endif /* PIOS_USART_H */ /** diff --git a/flight/pios/inc/pios_usart_priv.h b/flight/pios/inc/pios_usart_priv.h index 68a44bc9d..d939e1f44 100644 --- a/flight/pios/inc/pios_usart_priv.h +++ b/flight/pios/inc/pios_usart_priv.h @@ -41,15 +41,17 @@ extern const struct pios_com_driver pios_usart_com_driver; struct pios_usart_cfg { USART_TypeDef *regs; uint32_t remap; /* GPIO_Remap_* */ - USART_InitTypeDef init; struct stm32_gpio rx; struct stm32_gpio tx; struct stm32_gpio dtr; - struct stm32_irq irq; + + /* provide hook for board specific ioctls */ + int32_t (*ioctl)(uint32_t id, uint32_t ctl, void *param); }; extern int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg); -extern const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id); + +const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id); #endif /* PIOS_USART_PRIV_H */ diff --git a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h index a828a1c6f..6a3145a8b 100644 --- a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h @@ -31,9 +31,14 @@ #define PIOS_USB_DESC_HID_CDC_PRIV_H #include +#include +#include extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); +extern const struct pios_usb_cdc_cfg pios_usb_cdc_cfg; +extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; + #endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ /** diff --git a/flight/pios/inc/pios_usb_desc_hid_only_priv.h b/flight/pios/inc/pios_usb_desc_hid_only_priv.h index 6d98c3967..b9c9dcda2 100644 --- a/flight/pios/inc/pios_usb_desc_hid_only_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_only_priv.h @@ -31,9 +31,13 @@ #define PIOS_USB_DESC_HID_ONLY_PRIV_H #include +#include extern int32_t PIOS_USB_DESC_HID_ONLY_Init(void); +extern const struct pios_usb_hid_cfg pios_usb_hid_only_cfg; + + #endif /* PIOS_USB_DESC_HID_ONLY_PRIV_H */ /** diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c deleted file mode 100644 index 44fa484be..000000000 --- a/flight/pios/stm32f10x/pios_dsm.c +++ /dev/null @@ -1,438 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_DSM Spektrum/JR DSMx satellite receiver functions - * @brief Code to bind and read Spektrum/JR DSMx satellite receiver serial stream - * @{ - * - * @file pios_dsm.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @brief Code bind and read Spektrum/JR DSMx satellite receiver serial stream - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "pios.h" - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" - -// *** UNTESTED CODE *** -#undef DSM_LINK_QUALITY - -/* Forward Declarations */ -static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel); -static uint8_t PIOS_DSM_Quality_Get(uint32_t rcvr_id); -static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); -static void PIOS_DSM_Supervisor(uint32_t dsm_id); - -/* Local Variables */ -const struct pios_rcvr_driver pios_dsm_rcvr_driver = { - .read = PIOS_DSM_Get, - .get_quality = PIOS_DSM_Quality_Get -}; - -enum pios_dsm_dev_magic { - PIOS_DSM_DEV_MAGIC = 0x44534d78, -}; - -struct pios_dsm_state { - uint16_t channel_data[PIOS_DSM_NUM_INPUTS]; - uint8_t received_data[DSM_FRAME_LENGTH]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t frame_found; - uint8_t byte_count; - uint8_t frames_lost_last; - float quality; -}; - -/* With an DSM frame rate of 11ms (90Hz) averaging over 18 samples - * gives about a 200ms response. - */ -#define DSM_FL_WEIGHTED_AVE 18 - -struct pios_dsm_dev { - enum pios_dsm_dev_magic magic; - const struct pios_dsm_cfg *cfg; - struct pios_dsm_state state; -}; - -/* Allocate DSM device descriptor */ -#if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_dsm_dev *PIOS_DSM_Alloc(void) -{ - struct pios_dsm_dev *dsm_dev; - - dsm_dev = (struct pios_dsm_dev *)pios_malloc(sizeof(*dsm_dev)); - if (!dsm_dev) { - return NULL; - } - - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - return dsm_dev; -} -#else -static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS]; -static uint8_t pios_dsm_num_devs; -static struct pios_dsm_dev *PIOS_DSM_Alloc(void) -{ - struct pios_dsm_dev *dsm_dev; - - if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) { - return NULL; - } - - dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++]; - dsm_dev->magic = PIOS_DSM_DEV_MAGIC; - - return dsm_dev; -} -#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ - -/* Validate DSM device descriptor */ -static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev) -{ - return dsm_dev->magic == PIOS_DSM_DEV_MAGIC; -} - -/* Try to bind DSMx satellite using specified number of pulses */ -static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) -{ - const struct pios_dsm_cfg *cfg = dsm_dev->cfg; - - GPIO_InitTypeDef GPIO_InitStructure; - - GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; - GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; - - /* just to limit bind pulses */ - if (bind > 10) { - bind = 10; - } - - GPIO_Init(cfg->bind.gpio, &cfg->bind.init); - - /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - - /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ - PIOS_DELAY_WaitmS(20); - - for (int i = 0; i < bind; i++) { - /* RX line, drive low for 120us */ - GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - /* RX line, drive high for 120us */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - PIOS_DELAY_WaituS(120); - } - /* RX line, set input and wait for data */ - GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); -} - -/* Reset channels in case of lost signal or explicit failsafe receiver flag */ -static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev) -{ - struct pios_dsm_state *state = &(dsm_dev->state); - - for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } -} - -/* Reset DSM receiver state */ -static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) -{ - struct pios_dsm_state *state = &(dsm_dev->state); - - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = 0; - state->quality = 0.0f; - state->frames_lost_last = 0; - PIOS_DSM_ResetChannels(dsm_dev); -} - -/** - * Check and unroll complete frame data. - * \output 0 frame data accepted - * \output -1 frame error found - */ -static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) -{ - struct pios_dsm_state *state = &(dsm_dev->state); - /* Fix resolution for detection. */ - static uint8_t resolution = 11; - uint32_t channel_log = 0; - - // *** UNTESTED CODE *** -#ifdef DSM_LINK_QUALITY - /* increment the lost frame counter */ - uint8_t frames_lost = state->received_data[0]; - - /* We only get a lost frame count when the next good frame comes in */ - /* Present quality as a weighted average of good frames */ - /* First consider the bad frames */ - for (int i = 0; i < frames_lost - state->frames_lost_last; i++) { - state->quality = (state->quality * (DSM_FL_WEIGHTED_AVE - 1)) / - DSM_FL_WEIGHTED_AVE; - } - /* And now the good frame */ - state->quality = ((state->quality * (DSM_FL_WEIGHTED_AVE - 1)) + - 100) / DSM_FL_WEIGHTED_AVE; - - state->frames_lost_last = frames_lost; -#endif /* DSM_LINK_QUALITY */ - - /* unroll channels */ - uint8_t *s = &(state->received_data[2]); - uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; - - for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) { - uint16_t word = ((uint16_t)s[0] << 8) | s[1]; - s += 2; - - /* skip empty channel slot */ - if (word == 0xffff) { - continue; - } - - /* minimal data validation */ - if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) { - /* invalid frame data, ignore rest of the frame */ - goto stream_error; - } - - /* extract and save the channel value */ - uint8_t channel_num = (word >> resolution) & 0x0f; - if (channel_num < PIOS_DSM_NUM_INPUTS) { - if (channel_log & (1 << channel_num)) { - /* Found duplicate. This should happen when in 11 bit */ - /* mode and the data is 10 bits */ - if (resolution == 10) { - return -1; - } - resolution = 10; - return PIOS_DSM_UnrollChannels(dsm_dev); - } - - if ((channel_log & 0xFF) == 0x55) { - /* This pattern indicates 10 bit pattern */ - if (resolution == 11) { - return -1; - } - resolution = 11; - return PIOS_DSM_UnrollChannels(dsm_dev); - } - - state->channel_data[channel_num] = (word & mask); - /* keep track of this channel */ - channel_log |= (1 << channel_num); - } - } - - /* all channels processed */ - return 0; - -stream_error: - /* either DSM2 selected with DSMX stream found, or vice-versa */ - return -1; -} - -/* Update decoder state processing input byte from the DSMx stream */ -static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte) -{ - struct pios_dsm_state *state = &(dsm_dev->state); - - if (state->frame_found) { - /* receiving the data frame */ - if (state->byte_count < DSM_FRAME_LENGTH) { - /* store next byte */ - state->received_data[state->byte_count++] = byte; - if (state->byte_count == DSM_FRAME_LENGTH) { - /* full frame received - process and wait for new one */ - if (!PIOS_DSM_UnrollChannels(dsm_dev)) { - /* data looking good */ - state->failsafe_timer = 0; - } - - /* prepare for the next frame */ - state->frame_found = 0; - } - } - } -} - -/* Initialise DSM receiver interface */ -int32_t PIOS_DSM_Init(uint32_t *dsm_id, - const struct pios_dsm_cfg *cfg, - const struct pios_com_driver *driver, - uint32_t lower_id, - uint8_t bind) -{ - PIOS_DEBUG_Assert(dsm_id); - PIOS_DEBUG_Assert(cfg); - PIOS_DEBUG_Assert(driver); - - struct pios_dsm_dev *dsm_dev; - - dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc(); - if (!dsm_dev) { - return -1; - } - - /* Bind the configuration to the device instance */ - dsm_dev->cfg = cfg; - - /* Bind the receiver if requested */ - if (bind) { - PIOS_DSM_Bind(dsm_dev, bind); - } - - PIOS_DSM_ResetState(dsm_dev); - - *dsm_id = (uint32_t)dsm_dev; - - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id); - - if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) { - PIOS_DEBUG_Assert(0); - } - - return 0; -} - -/* Comm byte received callback */ -static uint16_t PIOS_DSM_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) -{ - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context; - - bool valid = PIOS_DSM_Validate(dsm_dev); - - PIOS_Assert(valid); - - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_DSM_UpdateState(dsm_dev, buf[i]); - dsm_dev->state.receive_timer = 0; - } - - /* Always signal that we can accept another byte */ - if (headroom) { - *headroom = DSM_FRAME_LENGTH; - } - - /* We never need a yield */ - *need_yield = false; - - /* Always indicate that all bytes were consumed */ - return buf_len; -} - -/** - * Get the value of an input channel - * \param[in] channel Number of the channel desired (zero based) - * \output PIOS_RCVR_INVALID channel not available - * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver - * \output >=0 channel value - */ -static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel) -{ - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id; - - if (!PIOS_DSM_Validate(dsm_dev)) { - return PIOS_RCVR_INVALID; - } - - /* return error if channel is not available */ - if (channel >= PIOS_DSM_NUM_INPUTS) { - return PIOS_RCVR_INVALID; - } - - /* may also be PIOS_RCVR_TIMEOUT set by other function */ - return dsm_dev->state.channel_data[channel]; -} - -/** - * Input data supervisor is called periodically and provides - * two functions: frame syncing and failsafe triggering. - * - * DSM frames come at 11ms or 22ms rate at 115200bps. - * RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives - * 8ms pause between frames which is good for both DSM frame rates. - * - * Data receive function must clear the receive_timer to confirm new - * data reception. If no new data received in 100ms, we must call the - * failsafe function which clears all channels. - */ -static void PIOS_DSM_Supervisor(uint32_t dsm_id) -{ - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; - - bool valid = PIOS_DSM_Validate(dsm_dev); - - PIOS_Assert(valid); - - struct pios_dsm_state *state = &(dsm_dev->state); - - /* waiting for new frame if no bytes were received in 8ms */ - if (++state->receive_timer > 4) { - state->frame_found = 1; - state->byte_count = 0; - state->receive_timer = 0; - } - - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_DSM_ResetChannels(dsm_dev); - state->failsafe_timer = 0; - } -} - -static uint8_t PIOS_DSM_Quality_Get(uint32_t dsm_id) -{ - struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id; - - bool valid = PIOS_DSM_Validate(dsm_dev); - - PIOS_Assert(valid); - - struct pios_dsm_state *state = &(dsm_dev->state); - - return (uint8_t)(state->quality + 0.5f); -} - -#endif /* PIOS_INCLUDE_DSM */ - -/** - * @} - * @} - */ diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index e594f55e0..950cbac77 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -48,6 +48,7 @@ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback r static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); +static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); const struct pios_com_driver pios_usart_com_driver = { .set_baud = PIOS_USART_ChangeBaud, @@ -58,6 +59,7 @@ const struct pios_com_driver pios_usart_com_driver = { .rx_start = PIOS_USART_RxStart, .bind_tx_cb = PIOS_USART_RegisterTxCallback, .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .ioctl = PIOS_USART_Ioctl, }; enum pios_usart_dev_magic { @@ -72,6 +74,7 @@ struct pios_usart_dev { uint32_t rx_in_context; pios_com_callback tx_out_cb; uint32_t tx_out_context; + uint8_t irq_channel; }; static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) @@ -79,6 +82,30 @@ static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) return usart_dev->magic == PIOS_USART_DEV_MAGIC; } +const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + return usart_dev->cfg; +} + +static int32_t PIOS_USART_SetIrqPrio(struct pios_usart_dev *usart_dev, uint8_t irq_prio) +{ + NVIC_InitTypeDef init = { + .NVIC_IRQChannel = usart_dev->irq_channel, + .NVIC_IRQChannelPreemptionPriority = irq_prio, + .NVIC_IRQChannelCmd = ENABLE, + }; + + NVIC_Init(&init); + + return 0; +} + #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_usart_dev *PIOS_USART_alloc(void) { @@ -179,10 +206,10 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) } /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + usart_dev->cfg = cfg; /* Copy the comm parameter structure */ - usart_dev->init = cfg->init; + USART_StructInit(&usart_dev->init); // 9600 8n1 /* Map pins to USART function */ /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ @@ -214,24 +241,32 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) switch ((uint32_t)usart_dev->cfg->regs) { case (uint32_t)USART1: PIOS_USART_1_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART1_IRQn; break; case (uint32_t)USART2: PIOS_USART_2_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART2_IRQn; break; +#if !defined(STM32F411xE) case (uint32_t)USART3: PIOS_USART_3_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART3_IRQn; break; case (uint32_t)UART4: PIOS_USART_4_id = (uint32_t)usart_dev; + usart_dev->irq_channel = UART4_IRQn; break; case (uint32_t)UART5: PIOS_USART_5_id = (uint32_t)usart_dev; + usart_dev->irq_channel = UART5_IRQn; break; +#endif /* STM32F411xE */ case (uint32_t)USART6: PIOS_USART_6_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART6_IRQn; break; } - NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); @@ -494,6 +529,34 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) #endif /* PIOS_INCLUDE_FREERTOS */ } +static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_IRQ_PRIO: + return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); + + case PIOS_IOCTL_USART_GET_RXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->rx; + break; + case PIOS_IOCTL_USART_GET_TXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->tx; + break; + default: + if (usart_dev->cfg->ioctl) { + return usart_dev->cfg->ioctl(usart_id, ctl, param); + } + return -1; + } + + return 0; +} + #endif /* PIOS_INCLUDE_USART */ /** diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index 47388c988..aae41c922 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -86,6 +86,7 @@ // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83_INTERNAL // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ // #define PIOS_INCLUDE_MS5611 diff --git a/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h b/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h index 5a36646cf..1b557810c 100644 --- a/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h +++ b/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h @@ -83,6 +83,7 @@ // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83_INTERNAL // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ /* #define PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 9e33beeb2..7e040bdb9 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -26,6 +26,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ + #if defined(PIOS_INCLUDE_LED) #include @@ -788,31 +789,22 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN USART */ + +// Inverter for SBUS handling +#define MAIN_USART_INVERTER_GPIO GPIOC +#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET + +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -822,52 +814,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; -#endif /* PIOS_INCLUDE_COM_TELEM */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -877,120 +824,16 @@ static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; -// Because of the inverter on the main port this will not -// work. Notice the mode is set to IN to maintain API -// compatibility but protect the pins -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#include -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -// Need this defined regardless to be able to turn it off -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, -}; - -#ifdef PIOS_INCLUDE_COM_FLEXI /* * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -1000,7 +843,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -1012,400 +855,11 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { }, }; -#endif /* PIOS_INCLUDE_COM_FLEXI */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#if defined(PIOS_INCLUDE_SRXL) -/* - * SRXL USART - */ -#include - -static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_SRXL */ - -#if defined(PIOS_INCLUDE_HOTT) -/* - * HOTT USART - */ -#include - -static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_HOTT */ - -#if defined(PIOS_INCLUDE_IBUS) -/* - * IBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_ibus_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_ibus_rcvr_cfg = { +/* FLEXI-IO (Receiver) port */ +static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_IBUS */ - -#if defined(PIOS_INCLUDE_EXBUS) -/* - * EXBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_EXBUS */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .dtr = { + .dtr = { // FlexIO pin 9 .gpio = GPIOC, .init = { @@ -1416,7 +870,7 @@ static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { }, }, - .tx = { + .tx = { // * 7: PC6 = TIM8 CH1, USART6 TX .gpio = GPIOC, .init = { @@ -1426,10 +880,10 @@ static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, - .pin_source = GPIO_PinSource6, + .pin_source = GPIO_PinSource6, }, - .rx = { + .rx = { // * 8: PC7 = TIM8 CH2, USART6 RX .gpio = GPIOC, .init = { @@ -1439,7 +893,7 @@ static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, - .pin_source = GPIO_PinSource7, + .pin_source = GPIO_PinSource7, } }; @@ -1797,212 +1251,24 @@ static const struct pios_tim_clock_cfg tim_12_cfg = { * Using TIM3, TIM9, TIM2, TIM5 */ #include +#include + static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM9, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource2, - }, - .remap = GPIO_AF_TIM2, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM5, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0), // PWM pins on FlexiIO(receiver) port - { - // * 6: PB15 = SPI2 MOSI, TIM12 CH2 - - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - // * 7: PC6 = TIM8 CH1, USART6 TX - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 8: PC7 = TIM8 CH2, USART6 RX - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 9: PC8 = TIM8 CH3 - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 10: PC9 = TIM8 CH4 - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), // * 6: PB15 = SPI2 MOSI, TIM12 CH2 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), // * 7: PC6 = TIM8 CH1, USART6 TX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), // * 8: PC7 = TIM8 CH2, USART6 RX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), // * 9: PC8 = TIM8 CH3 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), // * 10: PC9 = TIM8 CH4 + TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS }; + #define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6 #define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM 11 #define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12 @@ -2060,102 +1326,12 @@ const struct pios_servo_cfg pios_servo_cfg_out_in = { #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), + TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), }; const struct pios_pwm_cfg pios_pwm_cfg = { @@ -2285,37 +1461,6 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; - -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ - #ifdef PIOS_INCLUDE_WS2811 #include #include @@ -2450,3 +1595,198 @@ void PIOS_WS2811_irq_handler(void) PIOS_WS2811_DMA_irq_handler(); } #endif // PIOS_INCLUDE_WS2811 + +/** + * Sensor configurations + */ +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, +}; + +void PIOS_ADC_DMC_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); +} + +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" + +#ifdef PIOS_HMC5X83_HAS_GPIOS +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(pios_hmc5x83_internal_id); +} + +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = pios_board_internal_mag_handler, + .line = EXTI_Line7, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line7, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_HMC5X83_HAS_GPIOS */ + +static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = &pios_exti_hmc5x83_cfg, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_internal_cfg; +} + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag +}; + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_external_cfg; +} +#endif /* PIOS_INCLUDE_HMC5X83 */ + +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS5611) +#include "pios_ms5611.h" +static const struct pios_ms5611_cfg pios_ms5611_cfg = { + .oversampling = MS5611_OSR_4096, +}; +const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms5611_cfg; +} +#endif /* PIOS_INCLUDE_MS5611 */ + + +/** + * Configuration for the MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +#include "pios_mpu6000_config.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz + .Smpl_rate_div_no_dlp = 0, + // with dlp on output rate is 1000Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 20, +}; +#endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/targets/boards/revolution/bootloader/pios_board.c b/flight/targets/boards/revolution/bootloader/pios_board.c index 5352c7f73..e0065e2a9 100644 --- a/flight/targets/boards/revolution/bootloader/pios_board.c +++ b/flight/targets/boards/revolution/bootloader/pios_board.c @@ -25,6 +25,7 @@ #include #include +#include /* * Pull in the board-specific static HW definitions. @@ -39,6 +40,13 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + +static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) +{ + return -1; +} + + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +76,7 @@ void PIOS_Board_Init() #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index c50d8c598..ead737207 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -86,6 +86,7 @@ #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83_INTERNAL #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 9795c9e97..cc1f93ddf 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -30,21 +30,15 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include -#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -55,344 +49,72 @@ */ #include "../board_hw_defs.c" -/** - * Sensor configurations - */ -#if defined(PIOS_INCLUDE_ADC) -#include "pios_adc_priv.h" -void PIOS_ADC_DMC_irq_handler(void); -void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); - -struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, -}; - -void PIOS_ADC_DMC_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); -} -#endif /* if defined(PIOS_INCLUDE_ADC) */ - -#if defined(PIOS_INCLUDE_HMC5X83) -#include "pios_hmc5x83.h" -pios_hmc5x83_dev_t onboard_mag = 0; -pios_hmc5x83_dev_t external_mag = 0; - -#ifdef PIOS_HMC5X83_HAS_GPIOS -bool pios_board_internal_mag_handler() -{ - return PIOS_HMC5x83_IRQHandler(onboard_mag); -} - -static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = pios_board_internal_mag_handler, - .line = EXTI_Line7, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line7, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_HMC5X83_HAS_GPIOS */ - -static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = &pios_exti_hmc5x83_cfg, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, -}; - -static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = NULL, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag -}; -#endif /* PIOS_INCLUDE_HMC5X83 */ - -/** - * Configuration for the MS5611 chip - */ -#if defined(PIOS_INCLUDE_MS5611) -#include "pios_ms5611.h" -static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_4096, -}; -#endif /* PIOS_INCLUDE_MS5611 */ - - -/** - * Configuration for the MPU6000 chip - */ -#if defined(PIOS_INCLUDE_MPU6000) -#include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" -static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz - .Smpl_rate_div_no_dlp = 0, - // with dlp on output rate is 1000Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 20, -}; -#endif /* PIOS_INCLUDE_MPU6000 */ - -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 - -#define PIOS_COM_GPS_RX_BUF_LEN 128 -#define PIOS_COM_GPS_TX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_HOTT_RX_BUF_LEN 512 -#define PIOS_COM_HOTT_TX_BUF_LEN 512 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 - -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 -#define PIOS_COM_MAVLINK_RX_BUF_LEN 128 - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 -uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_gps_id = 0; -uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_hott_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; -uint32_t pios_com_vcp_id = 0; -uint32_t pios_com_msp_id = 0; -uint32_t pios_com_mavlink_id = 0; - -#if defined(PIOS_INCLUDE_RFM22B) -uint32_t pios_rfm22b_id = 0; -#include -#endif uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size = 0 make the port rx only - * rx size = 0 make the port tx only - * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) +static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = { + [HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_RCVRPORT_PPMMSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_RCVRPORT_PPMMAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_RM_RCVRPORT_PPMGPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_RCVRPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_RCVRPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_RCVRPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_RCVRPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_RM_RCVRPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, +}; + +static const PIOS_BOARD_IO_UART_Function main_function_map[] = { + [HWSETTINGS_RM_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_MAINPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_RM_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_RM_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + +static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { + [HWSETTINGS_RM_FLEXIPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_FLEXIPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + [HWSETTINGS_RM_FLEXIPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_RM_FLEXIPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_RM_FLEXIPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSETTINGS_RM_FLEXIPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_RM_FLEXIPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_FLEXIPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_RM_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + + +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { - uint32_t pios_usart_id; + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; } - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - -static void PIOS_Board_configure_ibus(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_ibus_id; - - if (PIOS_USART_Init(&pios_usart_ibus_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_id; - if (PIOS_IBUS_Init(&pios_ibus_id, &pios_usart_com_driver, pios_usart_ibus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_rcvr_id; - if (PIOS_RCVR_Init(&pios_ibus_rcvr_id, &pios_ibus_rcvr_driver, pios_ibus_id)) { - PIOS_Assert(0); - } - - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS] = pios_ibus_rcvr_id; -} - -static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg) -{ - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - - PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; -} - -static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) -{ - uint32_t pios_ppm_id; - - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + return -1; } /** @@ -400,9 +122,6 @@ static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ - -#include - void PIOS_Board_Init(void) { const struct pios_board_info *bdinfo = &pios_board_info_blob; @@ -473,10 +192,6 @@ void PIOS_Board_Init(void) EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ /* Initialize the alarms library */ AlarmsInitialize(); @@ -503,537 +218,71 @@ void PIOS_Board_Init(void) } /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_FLEXIPORT_I2C: + + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); + } + #if defined(PIOS_INCLUDE_I2C) + /* Set up internal I2C bus */ + if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + PIOS_DELAY_WaitmS(50); + + if (hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } - PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ -#if defined(PIOS_INCLUDE_HMC5X83) - // get auxmag type - AuxMagSettingsTypeOptions option; - AuxMagSettingsInitialize(); - AuxMagSettingsTypeGet(&option); - // if the aux mag type is FlexiPort then set it up - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - // attach the 5x83 mag to the previously inited I2C2 - external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor - // and before registering some other fast and important sensor - // as that would cause delay and time jitter for the second fast sensor - PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } -#endif /* PIOS_INCLUDE_HMC5X83 */ -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_RM_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_FLEXIPORT_DSM: - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; + PIOS_DELAY_WaitmS(50); + } - case HWSETTINGS_RM_FLEXIPORT_SRXL: -#if defined(PIOS_INCLUDE_SRXL) - { - uint32_t pios_usart_srxl_id; - if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) { - PIOS_Assert(0); - } + PIOS_BOARD_IO_Configure_I2C(pios_i2c_mag_pressure_adapter_id, pios_i2c_flexiport_adapter_id); +#endif - uint32_t pios_srxl_id; - if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_rcvr_id; - if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; - } -#endif /* PIOS_INCLUDE_SRXL */ - break; - - case HWSETTINGS_RM_FLEXIPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_flexi_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - - case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD: - case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - case HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY: - PIOS_Board_configure_com(&pios_usart_hott_flexi_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); - break; - - case HWSETTINGS_RM_FLEXIPORT_EXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; - } /* hwsettings_rm_flexiport */ - - /* Moved this here to allow binding on flexiport */ + /* Moved this here to allow DSM binding on flexiport */ #if defined(PIOS_INCLUDE_FLASH) if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_user_cfg, &pios_jedec_flash_driver, flash_id)) { PIOS_DEBUG_Assert(0); } #endif /* if defined(PIOS_INCLUDE_FLASH) */ + #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); - -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ - /* Configure main USART port */ + + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; + + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + MAIN_USART_INVERTER_DISABLE); + } + uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DISABLED: - break; - case HWSETTINGS_RM_MAINPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif - break; - case HWSETTINGS_RM_MAINPORT_DSM: - // Force binding to zero on the main port - hwsettings_DSMxBind = 0; - - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); - break; - case HWSETTINGS_RM_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_MAINPORT_MSP: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_MAINPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_mainport */ - - if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { - GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); - GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - /* Initialize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) - - /* Fetch the OPLinkSettings object. */ - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - - // Initialize out status object. - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); - oplinkStatus.BoardType = bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision = bdinfo->board_rev; - - /* Is the radio turned on? */ - bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR); - bool openlrs = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPENLRS); - bool data_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATA) || - (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); - bool ppm_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) || - (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); - bool is_enabled = ((oplinkSettings.Protocol != OPLINKSETTINGS_PROTOCOL_DISABLED) && - ((oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) || openlrs)); - if (is_enabled) { - if (openlrs) { -#if defined(PIOS_INCLUDE_OPENLRS_RCVR) - const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(bdinfo->board_rev); - uint32_t openlrs_id; - - PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg); - { - uint32_t openlrsrcvr_id; - PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); - uint32_t openlrsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS] = openlrsrcvr_rcvr_id; - } -#endif /* PIOS_INCLUDE_OPENLRS_RCVR */ - } else { - /* Configure the RFM22B device. */ - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { - PIOS_Assert(0); - } - - /* Configure the radio com interface */ - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; - - // Set the modem (over the air) datarate. - enum rfm22b_datarate datarate = RFM22_datarate_64000; - switch (oplinkSettings.AirDataRate) { - case OPLINKSETTINGS_AIRDATARATE_9600: - datarate = RFM22_datarate_9600; - break; - case OPLINKSETTINGS_AIRDATARATE_19200: - datarate = RFM22_datarate_19200; - break; - case OPLINKSETTINGS_AIRDATARATE_32000: - datarate = RFM22_datarate_32000; - break; - case OPLINKSETTINGS_AIRDATARATE_57600: - datarate = RFM22_datarate_57600; - break; - case OPLINKSETTINGS_AIRDATARATE_64000: - datarate = RFM22_datarate_64000; - break; - case OPLINKSETTINGS_AIRDATARATE_100000: - datarate = RFM22_datarate_100000; - break; - case OPLINKSETTINGS_AIRDATARATE_128000: - datarate = RFM22_datarate_128000; - break; - case OPLINKSETTINGS_AIRDATARATE_192000: - datarate = RFM22_datarate_192000; - break; - case OPLINKSETTINGS_AIRDATARATE_256000: - datarate = RFM22_datarate_256000; - break; - } - - /* Set the radio configuration parameters. */ - PIOS_RFM22B_SetDeviceID(pios_rfm22b_id, oplinkSettings.CustomDeviceID); - PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); - PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap); - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode); - - /* Set the modem Tx power level */ - switch (oplinkSettings.MaxRFPower) { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - default: - // do nothing - break; - } - - /* Reinitialize the modem. */ - PIOS_RFM22B_Reinit(pios_rfm22b_id); - - uint8_t hwsettings_radioaux; - HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - // TODO: this is in preparation for full mavlink support and is used by LP-368 - uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN; - - switch (hwsettings_radioaux) { - case HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE: - case HWSETTINGS_RADIOAUXSTREAM_DISABLED: - break; - case HWSETTINGS_RADIOAUXSTREAM_MAVLINK: - { - uint8_t *auxrx_buffer = 0; - if (mavlink_rx_size) { - auxrx_buffer = (uint8_t *)pios_malloc(mavlink_rx_size); - } - uint8_t *auxtx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); - PIOS_Assert(auxrx_buffer); - PIOS_Assert(auxtx_buffer); - if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - auxrx_buffer, mavlink_rx_size, - auxtx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - } - } - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; - } - - OPLinkStatusSet(&oplinkStatus); + PIOS_BOARD_IO_Configure_RFM22B(); #endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) @@ -1046,12 +295,16 @@ void PIOS_Board_Init(void) uint8_t hwsettings_rcvrport; HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); + if (hwsettings_rcvrport < NELEMENTS(flexiio_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexiio_cfg, flexiio_function_map[hwsettings_rcvrport]); + } + // Configure rcvrport PPM/PWM/OUTPUTS switch (hwsettings_rcvrport) { case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) /* Set up the receiver port. Later this should be optional */ - PIOS_Board_configure_pwm(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -1065,7 +318,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMGPS: case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY: #if defined(PIOS_INCLUDE_PPM) - PIOS_Board_configure_ppm(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { // configure servo outputs and the remaining 5 inputs as outputs @@ -1074,7 +327,7 @@ void PIOS_Board_Init(void) // enable pwm on the remaining channels if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { - PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_ppm_cfg); } break; @@ -1084,69 +337,9 @@ void PIOS_Board_Init(void) pios_servo_cfg = &pios_servo_cfg_out_in; break; } - - // Configure rcvrport usart - switch (hwsettings_rcvrport) { - case HWSETTINGS_RM_RCVRPORT_TELEMETRY: - case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE: - case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_RCVRPORT_COMBRIDGE: - case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_RCVRPORT_MSP: - case HWSETTINGS_RM_RCVRPORT_PPMMSP: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_RCVRPORT_MAVLINK: - case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_RCVRPORT_GPS: - case HWSETTINGS_RM_RCVRPORT_PPMGPS: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_RCVRPORT_HOTTTELEMETRY: - case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); - break; - case HWSETTINGS_RM_RCVRPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_rcvr_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - } - -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#if defined(PIOS_INCLUDE_OPLINKRCVR) - { - OPLinkReceiverInitialize(); - uint32_t pios_oplinkrcvr_id; - PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id); - uint32_t pios_oplinkrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; - } -#endif /* PIOS_INCLUDE_OPLINKRCVR */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif #ifndef PIOS_ENABLE_DEBUG_PINS // pios_servo_cfg points to the correct configuration based on input port settings @@ -1155,6 +348,22 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif +#if defined(PIOS_INCLUDE_MPU6000) + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); + PIOS_MPU6000_Register(); +#endif + +#ifdef PIOS_INCLUDE_WS2811 + HwSettingsWS2811LED_OutOptions ws2811_pin_settings; + HwSettingsWS2811LED_OutGet(&ws2811_pin_settings); + + if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED && ws2811_pin_settings < NELEMENTS(pios_ws2811_pin_cfg)) { + PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]); + } +#endif // PIOS_INCLUDE_WS2811 + +#ifdef PIOS_INCLUDE_ADC // Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout GPIO_InitTypeDef gpioA8 = { .GPIO_Speed = GPIO_Speed_2MHz, @@ -1165,66 +374,7 @@ void PIOS_Board_Init(void) }; GPIO_Init(GPIOA, &gpioA8); - // init I2C1 for use with the internal mag and baro - if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } - PIOS_DELAY_WaitmS(50); - -#if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); -#endif - -#if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - PIOS_MPU6000_Register(); -#endif - -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - -#if defined(PIOS_INCLUDE_HMC5X83) - // attach the 5x83 mag to the previously inited I2C1 - onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_pressure_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); -#endif - -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); - PIOS_MS5611_Register(); -#endif - - #ifdef PIOS_INCLUDE_WS2811 -#include - HwSettingsWS2811LED_OutOptions ws2811_pin_settings; - HwSettingsWS2811LED_OutGet(&ws2811_pin_settings); - - if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED && ws2811_pin_settings < NELEMENTS(pios_ws2811_pin_cfg)) { - PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]); - } -#endif // PIOS_INCLUDE_WS2811 -#ifdef PIOS_INCLUDE_ADC - { - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); - } - } - } + PIOS_BOARD_IO_Configure_ADC(); #endif // PIOS_INCLUDE_ADC DEBUG_PRINTF(2, "Board complete\r\n"); diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 63f019913..e84a8cc35 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -52,8 +52,8 @@ // Inverter for SBUS handling #define MAIN_USART_INVERTER_GPIO GPIOC #define MAIN_USART_INVERTER_PIN GPIO_Pin_15 -#define MAIN_USART_INVERTER_CLOCK_FUNC RCC_AHB1PeriphClockCmd -#define MAIN_USART_INVERTER_CLOCK_PERIPH RCC_AHB1Periph_GPIOC +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET #define FLEXI_USART_REGS USART1 #define FLEXI_USART_REMAP GPIO_AF_USART1 @@ -240,75 +240,15 @@ void PIOS_SPI_gyro_irq_handler(void) #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN USART */ + +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = MAIN_USART_REGS, .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = MAIN_USART_TX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; -#endif /* PIOS_INCLUDE_COM_TELEM */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = MAIN_USART_REGS, - .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = MAIN_USART_RX_GPIO, .init = { @@ -329,472 +269,15 @@ static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; -// Because of the inverter on the main port this will not -// work. Notice the mode is set to IN to maintain API -// compatibility but protect the pins -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - - -#include -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = MAIN_USART_REGS, - .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 99999, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = MAIN_USART_TX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -// Need this defined regardless to be able to turn it off -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = MAIN_USART_INVERTER_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = MAIN_USART_INVERTER_CLOCK_FUNC, - .gpio_clk_periph = MAIN_USART_INVERTER_CLOCK_PERIPH, -}; - - -#ifdef PIOS_INCLUDE_COM_FLEXI /* * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = FLEXI_USART_REGS, .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = FLEXI_USART_TX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .dtr = { - .gpio = FLEXI_USART_DTR_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_DTR_PIN, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - }, - }, -}; - -#endif /* PIOS_INCLUDE_COM_FLEXI */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = FLEXI_USART_REGS, - .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = FLEXI_USART_TX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#if defined(PIOS_INCLUDE_HOTT) -/* - * HOTT USART - */ -#include - -static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { - .regs = FLEXI_USART_REGS, - .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = FLEXI_USART_TX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_HOTT */ - -#if defined(PIOS_INCLUDE_SRXL) -/* - * SRXL USART - */ -#include - -static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { - .regs = FLEXI_USART_REGS, - .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = FLEXI_USART_TX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_SRXL */ - -#if defined(PIOS_INCLUDE_IBUS) -/* - * IBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_ibus_flexi_cfg = { - .regs = FLEXI_USART_REGS, - .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = FLEXI_USART_TX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_IBUS */ - -#if defined(PIOS_INCLUDE_EXBUS) -/* - * EXBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { - .regs = FLEXI_USART_REGS, - .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = FLEXI_USART_RX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = FLEXI_USART_TX_GPIO, - .init = { - .GPIO_Pin = FLEXI_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_EXBUS */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = MAIN_USART_REGS, - .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = MAIN_USART_TX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = FLEXI_USART_REGS, - .remap = FLEXI_USART_REMAP, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = FLEXI_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = FLEXI_USART_RX_GPIO, .init = { @@ -815,6 +298,15 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, +// .dtr = { +// .gpio = FLEXI_USART_DTR_GPIO, +// .init = { +// .GPIO_Pin = FLEXI_USART_DTR_PIN, +// .GPIO_Speed = GPIO_Speed_25MHz, +// .GPIO_Mode = GPIO_Mode_OUT, +// .GPIO_OType = GPIO_OType_PP, +// }, +// }, }; #if defined(PIOS_INCLUDE_COM) @@ -1347,37 +839,6 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; - -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ - #ifdef PIOS_INCLUDE_WS2811 #include #include @@ -1470,3 +931,143 @@ struct pios_flash_eeprom_cfg flash_main_chip_cfg = { }; // .i2c_address = 0x50, #endif /* PIOS_INCLUDE_FLASH_EEPROM */ + +/** + * Sensor configurations + */ + + + +#if defined(PIOS_INCLUDE_ADC) + +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, +}; +void PIOS_ADC_DMC_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); +} + +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag +}; + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_external_cfg; +} +#endif /* PIOS_INCLUDE_HMC5X83 */ + +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS5611) +#include "pios_ms5611.h" +static const struct pios_ms5611_cfg pios_ms5611_cfg = { + .oversampling = MS5611_OSR_4096, +}; +const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms5611_cfg; +} +#endif /* PIOS_INCLUDE_MS5611 */ + +/** + * Configuration for the MPU9250 chip + */ +#if defined(PIOS_INCLUDE_MPU9250) +#include "pios_mpu9250.h" +#include "pios_mpu9250_config.h" +static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { + .vector = PIOS_MPU9250_IRQHandler, + .line = EXTI_Line15, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI15_10_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line15, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_mpu9250_cfg pios_mpu9250_cfg = { + .exti_cfg = &pios_exti_mpu9250_cfg, + .Fifo_store = 0, + // Clock at 8 khz + .Smpl_rate_div_no_dlp = 0, + // with dlp on output rate is 1000Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU9250_INT_CLR_ANYRD, // | PIOS_MPU9250_INT_LATCH_EN, + .interrupt_en = PIOS_MPU9250_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU9250_USERCTL_DIS_I2C | PIOS_MPU9250_USERCTL_I2C_MST_EN, + .Pwr_mgmt_clk = PIOS_MPU9250_PWRMGMT_PLL_Z_CLK, + .accel_range = PIOS_MPU9250_ACCEL_8G, + .gyro_range = PIOS_MPU9250_SCALE_2000_DEG, + .filter = PIOS_MPU9250_LOWPASS_256_HZ, + .orientation = PIOS_MPU9250_TOP_180DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 26, +}; +#endif /* PIOS_INCLUDE_MPU9250 */ diff --git a/flight/targets/boards/revonano/bootloader/pios_board.c b/flight/targets/boards/revonano/bootloader/pios_board.c index 5352c7f73..595eac507 100644 --- a/flight/targets/boards/revonano/bootloader/pios_board.c +++ b/flight/targets/boards/revonano/bootloader/pios_board.c @@ -25,6 +25,7 @@ #include #include +#include /* * Pull in the board-specific static HW definitions. @@ -39,6 +40,12 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + +static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) +{ + return -1; +} + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +75,7 @@ void PIOS_Board_Init() #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 0de1b0eed..6d95791f4 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -30,21 +30,18 @@ #include #include #include -#include -#include -#include -#include -#include #include #include -#include -#include -#include + +#include "sanitycheck.h" +#include "actuatorsettings.h" #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -55,286 +52,11 @@ */ #include "../board_hw_defs.c" -static SystemAlarmsExtendedAlarmStatusOptions RevoNanoConfigHook(); -static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); -/** - * Sensor configurations - */ - -#if defined(PIOS_INCLUDE_ADC) - -#include "pios_adc_priv.h" -void PIOS_ADC_DMC_irq_handler(void); -void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); -struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, -}; -void PIOS_ADC_DMC_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); -} -#endif /* if defined(PIOS_INCLUDE_ADC) */ - -#if defined(PIOS_INCLUDE_HMC5X83) -#include "pios_hmc5x83.h" -pios_hmc5x83_dev_t external_mag = 0; - -static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = NULL, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag -}; -#endif /* PIOS_INCLUDE_HMC5X83 */ - -/** - * Configuration for the MS5611 chip - */ -#if defined(PIOS_INCLUDE_MS5611) -#include "pios_ms5611.h" -static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_4096, -}; -#endif /* PIOS_INCLUDE_MS5611 */ - -/** - * Configuration for the MPU9250 chip - */ -#if defined(PIOS_INCLUDE_MPU9250) -#include "pios_mpu9250.h" -#include "pios_mpu9250_config.h" -static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { - .vector = PIOS_MPU9250_IRQHandler, - .line = EXTI_Line15, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI15_10_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line15, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_mpu9250_cfg pios_mpu9250_cfg = { - .exti_cfg = &pios_exti_mpu9250_cfg, - .Fifo_store = 0, - // Clock at 8 khz - .Smpl_rate_div_no_dlp = 0, - // with dlp on output rate is 1000Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU9250_INT_CLR_ANYRD, // | PIOS_MPU9250_INT_LATCH_EN, - .interrupt_en = PIOS_MPU9250_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU9250_USERCTL_DIS_I2C | PIOS_MPU9250_USERCTL_I2C_MST_EN, - .Pwr_mgmt_clk = PIOS_MPU9250_PWRMGMT_PLL_Z_CLK, - .accel_range = PIOS_MPU9250_ACCEL_8G, - .gyro_range = PIOS_MPU9250_SCALE_2000_DEG, - .filter = PIOS_MPU9250_LOWPASS_256_HZ, - .orientation = PIOS_MPU9250_TOP_180DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 26, -}; -#endif /* PIOS_INCLUDE_MPU9250 */ - - -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 - -#define PIOS_COM_GPS_RX_BUF_LEN 128 -#define PIOS_COM_GPS_TX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 - -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 - -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 -uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_gps_id = 0; -uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; -uint32_t pios_com_msp_id = 0; -uint32_t pios_com_vcp_id = 0; -uint32_t pios_com_mavlink_id = 0; - uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size = 0 make the port rx only - * rx size = 0 make the port tx only - * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) -{ - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - -static void PIOS_Board_configure_ibus(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_ibus_id; - - if (PIOS_USART_Init(&pios_usart_ibus_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_id; - if (PIOS_IBUS_Init(&pios_ibus_id, &pios_usart_com_driver, pios_usart_ibus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_rcvr_id; - if (PIOS_RCVR_Init(&pios_ibus_rcvr_id, &pios_ibus_rcvr_driver, pios_ibus_id)) { - PIOS_Assert(0); - } - - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS] = pios_ibus_rcvr_id; -} - -static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg) -{ - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - - PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; -} - -static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) -{ - uint32_t pios_ppm_id; - - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; -} +static SystemAlarmsExtendedAlarmStatusOptions RevoNanoConfigHook(); +static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); /** * PIOS_Board_Init() @@ -344,11 +66,55 @@ static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) #include +static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { + [HWSETTINGS_RM_FLEXIPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_FLEXIPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + [HWSETTINGS_RM_FLEXIPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_RM_FLEXIPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_RM_FLEXIPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSETTINGS_RM_FLEXIPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_RM_FLEXIPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_FLEXIPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_RM_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + +static const PIOS_BOARD_IO_UART_Function main_function_map[] = { + [HWSETTINGS_RM_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_MAINPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_RM_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_RM_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; + } + + return -1; +} + void PIOS_Board_Init(void) { - /* Delay system */ - PIOS_DELAY_Init(); - const struct pios_board_info *bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) @@ -425,10 +191,6 @@ void PIOS_Board_Init(void) EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ /* Initialize the alarms library */ AlarmsInitialize(); @@ -451,365 +213,43 @@ void PIOS_Board_Init(void) HwSettingsSetDefaults(HwSettingsHandle(), 0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } - - - // PIOS_IAP_Init(); - + #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); - -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ - - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - - /* Configure main USART port */ - uint8_t hwsettings_mainport; - HwSettingsRM_MainPortGet(&hwsettings_mainport); - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DISABLED: - break; - case HWSETTINGS_RM_MAINPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif - break; - case HWSETTINGS_RM_MAINPORT_DSM: - { - // Force binding to zero on the main port - hwsettings_DSMxBind = 0; - - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - } - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_MAINPORT_MSP: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_MAINPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_mainport */ - - if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { - GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); - GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); - } - /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_FLEXIPORT_I2C: -#if defined(PIOS_INCLUDE_I2C) - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ -#if defined(PIOS_INCLUDE_HMC5X83) - // get auxmag type - AuxMagSettingsTypeOptions option; - AuxMagSettingsInitialize(); - AuxMagSettingsTypeGet(&option); - // if the aux mag type is FlexiPort then set it up - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - // attach the 5x83 mag to the previously inited I2C2 - external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor - // and before registering some other fast and important sensor - // as that would cause delay and time jitter for the second fast sensor - PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } -#endif /* PIOS_INCLUDE_HMC5X83 */ -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_RM_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_FLEXIPORT_DSM: - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - case HWSETTINGS_RM_FLEXIPORT_SRXL: -#if defined(PIOS_INCLUDE_SRXL) - { - uint32_t pios_usart_srxl_id; - if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) { - PIOS_Assert(0); - } + + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); + } - uint32_t pios_srxl_id; - if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_rcvr_id; - if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; - } -#endif /* PIOS_INCLUDE_SRXL */ - break; - - case HWSETTINGS_RM_FLEXIPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_flexi_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - - case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD: - case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - - case HWSETTINGS_RM_FLEXIPORT_EXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; - } /* hwsettings_rm_flexiport */ + /* Configure main USART port */ + + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + MAIN_USART_INVERTER_DISABLE); + } + + uint8_t hwsettings_mainport; + HwSettingsRM_MainPortGet(&hwsettings_mainport); + + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); + } #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) @@ -828,7 +268,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) /* Set up the receiver port. Later this should be optional */ - PIOS_Board_configure_pwm(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -840,7 +280,7 @@ void PIOS_Board_Init(void) pios_servo_cfg = &pios_servo_cfg_out_in_ppm; } - PIOS_Board_configure_ppm(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); break; #endif /* PIOS_INCLUDE_PPM */ @@ -851,16 +291,9 @@ void PIOS_Board_Init(void) } -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif #ifdef PIOS_INCLUDE_WS2811 #include @@ -885,7 +318,7 @@ void PIOS_Board_Init(void) PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); + PIOS_BOARD_IO_Configure_ADC(); #endif #if defined(PIOS_INCLUDE_MPU9250) @@ -895,21 +328,6 @@ void PIOS_Board_Init(void) PIOS_MPU9250_MagRegister(); #endif -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); - PIOS_MS5611_Register(); -#endif - -#ifdef PIOS_INCLUDE_ADC - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); - } - } -#endif - // Attach the board config check hook SANITYCHECK_AttachHook(&RevoNanoConfigHook); // trigger a config check if actuatorsettings are updated diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 468264855..ceeaf0ea1 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -83,6 +83,7 @@ #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83_INTERNAL #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 diff --git a/flight/targets/boards/sparky2/board_hw_defs.c b/flight/targets/boards/sparky2/board_hw_defs.c index 25afb14e2..3a531ce1e 100644 --- a/flight/targets/boards/sparky2/board_hw_defs.c +++ b/flight/targets/boards/sparky2/board_hw_defs.c @@ -542,75 +542,13 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN PORT */ + static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; -#endif /* PIOS_INCLUDE_COM_TELEM */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOA, .init = { @@ -633,516 +571,12 @@ static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { }, }; -// Because of the inverter on the main port this will not -// work. Notice the mode is set to IN to maintain API -// compatibility but protect the pins -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#ifdef PIOS_INCLUDE_COM_FLEXI /* * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_COM_FLEXI */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#if defined(PIOS_INCLUDE_SRXL) -/* - * SRXL USART - */ -#include - -static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_srxl_rcvr_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_SRXL */ - -#if defined(PIOS_INCLUDE_IBUS) -/* - * IBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_ibus_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_ibus_rcvr_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_IBUS */ - -// these were copied from Revo support -// they might need to be further modified for Sparky2 support -#if defined(PIOS_INCLUDE_HOTT) -/* - * HOTT USART - */ -#include - -static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hott_rcvr_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_HOTT */ - -#if defined(PIOS_INCLUDE_EXBUS) -/* - * EXBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_exbus_rcvr_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_EXBUS */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOB, .init = { @@ -1170,31 +604,18 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { * RCVR PORT */ -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include +// Inverter for SBUS handling +#define RCVR_USART_INVERTER_GPIO GPIOC +#define RCVR_USART_INVERTER_PIN GPIO_Pin_4 +#define RCVR_USART_INVERTER_ENABLE Bit_SET +#define RCVR_USART_INVERTER_DISABLE Bit_RESET -static const struct pios_usart_cfg pios_usart_sbus_rcvr_cfg = { +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + + +static const struct pios_usart_cfg pios_usart_rcvr_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOC, .init = { @@ -1205,83 +626,9 @@ static const struct pios_usart_cfg pios_usart_sbus_rcvr_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; - -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -#ifdef PIOS_INCLUDE_DSM - -// It looks like TL notes originally came from OP's pios_dsm_main_cfg -// (TL note) Because of the inverter on the main port this will not -// (TL note) work. Notice the mode is set to IN to maintain API -// (TL note) compatibility but protect the pins -static const struct pios_dsm_cfg pios_dsm_rcvr_cfg = { - .bind = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - - -static const struct pios_usart_cfg pios_usart_dsm_rcvr_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - - #if defined(PIOS_INCLUDE_COM) #include @@ -1720,7 +1067,7 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { #if defined(PIOS_INCLUDE_USB) #include "pios_usb_priv.h" -static const struct pios_usb_cfg pios_usb_main_rm2_cfg = { +static const struct pios_usb_cfg pios_usb_main_spk2_cfg = { .irq = { .init = { .NVIC_IRQChannel = OTG_FS_IRQn, @@ -1743,7 +1090,7 @@ static const struct pios_usb_cfg pios_usb_main_rm2_cfg = { const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_usb_main_rm2_cfg; + return &pios_usb_main_spk2_cfg; } #include "pios_usb_board_data_priv.h" @@ -1759,37 +1106,6 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; - -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ - #ifdef PIOS_INCLUDE_WS2811 #include #include @@ -1906,3 +1222,141 @@ void PIOS_WS2811_irq_handler(void) PIOS_WS2811_DMA_irq_handler(); } #endif // PIOS_INCLUDE_WS2811 + +/** + * Sensor configurations + */ + +#if defined(PIOS_INCLUDE_ADC) + +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, +}; +void PIOS_ADC_DMC_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); +} + +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif /* PIOS_HMC5X83_HAS_GPIOS */ + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_external_cfg; +} +#endif /* PIOS_INCLUDE_HMC5X83 */ + +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS5611) +#include "pios_ms5611.h" +static const struct pios_ms5611_cfg pios_ms5611_cfg = { + .oversampling = MS5611_OSR_4096, +}; +const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms5611_cfg; +} +#endif /* PIOS_INCLUDE_MS5611 */ + +/** + * Configuration for the MPU9250 chip + */ +#if defined(PIOS_INCLUDE_MPU9250) +#include "pios_mpu9250.h" +#include "pios_mpu9250_config.h" +static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { + .vector = PIOS_MPU9250_IRQHandler, + .line = EXTI_Line5, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line5, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_mpu9250_cfg pios_mpu9250_cfg = { + .exti_cfg = &pios_exti_mpu9250_cfg, + .Fifo_store = 0, + // Clock at 8 khz + .Smpl_rate_div_no_dlp = 0, + // with dlp on output rate is 1000Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU9250_INT_CLR_ANYRD, // | PIOS_MPU9250_INT_LATCH_EN, + .interrupt_en = PIOS_MPU9250_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU9250_USERCTL_DIS_I2C | PIOS_MPU9250_USERCTL_I2C_MST_EN, + .Pwr_mgmt_clk = PIOS_MPU9250_PWRMGMT_PLL_Z_CLK, + .accel_range = PIOS_MPU9250_ACCEL_8G, + .gyro_range = PIOS_MPU9250_SCALE_2000_DEG, + .filter = PIOS_MPU9250_LOWPASS_256_HZ, + .orientation = PIOS_MPU9250_TOP_180DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 26, +}; +#endif /* PIOS_INCLUDE_MPU9250 */ + diff --git a/flight/targets/boards/sparky2/bootloader/pios_board.c b/flight/targets/boards/sparky2/bootloader/pios_board.c index 5352c7f73..f93555938 100644 --- a/flight/targets/boards/sparky2/bootloader/pios_board.c +++ b/flight/targets/boards/sparky2/bootloader/pios_board.c @@ -39,6 +39,12 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + +static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) +{ + return -1; +} + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +74,7 @@ void PIOS_Board_Init() #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/sparky2/firmware/pios_board.c b/flight/targets/boards/sparky2/firmware/pios_board.c index 270f53152..1c0561dcb 100644 --- a/flight/targets/boards/sparky2/firmware/pios_board.c +++ b/flight/targets/boards/sparky2/firmware/pios_board.c @@ -30,22 +30,18 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include -#include -#include -#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif +#ifdef PIOS_INCLUDE_WS2811 +#include +#endif + +#include + /* * Pull in the board-specific static HW definitions. @@ -57,338 +53,63 @@ */ #include "../board_hw_defs.c" -/** - * Sensor configurations - */ - -#if defined(PIOS_INCLUDE_ADC) - -#include "pios_adc_priv.h" -void PIOS_ADC_DMC_irq_handler(void); -void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); -struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, -}; -void PIOS_ADC_DMC_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); -} - -#endif /* if defined(PIOS_INCLUDE_ADC) */ - -#if defined(PIOS_INCLUDE_HMC5X83) -#include "pios_hmc5x83.h" -pios_hmc5x83_dev_t i2c_port_mag = 0; -pios_hmc5x83_dev_t flexi_port_mag = 0; - -static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = NULL, -#endif /* PIOS_HMC5X83_HAS_GPIOS */ - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, -}; -#endif /* PIOS_INCLUDE_HMC5X83 */ - -/** - * Configuration for the MS5611 chip - */ -#if defined(PIOS_INCLUDE_MS5611) -#include "pios_ms5611.h" -static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_4096, -}; -#endif /* PIOS_INCLUDE_MS5611 */ - -/** - * Configuration for the MPU9250 chip - */ -#if defined(PIOS_INCLUDE_MPU9250) -#include "pios_mpu9250.h" -#include "pios_mpu9250_config.h" -static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { - .vector = PIOS_MPU9250_IRQHandler, - .line = EXTI_Line5, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line5, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_mpu9250_cfg pios_mpu9250_cfg = { - .exti_cfg = &pios_exti_mpu9250_cfg, - .Fifo_store = 0, - // Clock at 8 khz - .Smpl_rate_div_no_dlp = 0, - // with dlp on output rate is 1000Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU9250_INT_CLR_ANYRD, // | PIOS_MPU9250_INT_LATCH_EN, - .interrupt_en = PIOS_MPU9250_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU9250_USERCTL_DIS_I2C | PIOS_MPU9250_USERCTL_I2C_MST_EN, - .Pwr_mgmt_clk = PIOS_MPU9250_PWRMGMT_PLL_Z_CLK, - .accel_range = PIOS_MPU9250_ACCEL_8G, - .gyro_range = PIOS_MPU9250_SCALE_2000_DEG, - .filter = PIOS_MPU9250_LOWPASS_256_HZ, - .orientation = PIOS_MPU9250_TOP_180DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 26, -}; -#endif /* PIOS_INCLUDE_MPU9250 */ - - -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 - -#define PIOS_COM_GPS_RX_BUF_LEN 128 -#define PIOS_COM_GPS_TX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 - -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 - -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 -#define PIOS_COM_MAVLINK_RX_BUF_LEN 128 - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 -uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_gps_id = 0; -uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; -uint32_t pios_com_msp_id = 0; -uint32_t pios_com_mavlink_id = 0; -uint32_t pios_com_vcp_id = 0; - -#if defined(PIOS_INCLUDE_RFM22B) -uint32_t pios_rfm22b_id = 0; -#include -#endif - uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size = 0 make the port rx only - * rx size = 0 make the port tx only - * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) +static const PIOS_BOARD_IO_UART_Function rcvr_function_map[] = { + [HWSETTINGS_SPK2_RCVRPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_SPK2_RCVRPORT_DSM] = PIOS_BOARD_IO_UART_DSM_RCVR, + [HWSETTINGS_SPK2_RCVRPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_SPK2_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_SPK2_RCVRPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_SPK2_RCVRPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_SPK2_RCVRPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, +}; + +static const PIOS_BOARD_IO_UART_Function main_function_map[] = { + [HWSETTINGS_SPK2_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_SPK2_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_SPK2_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_SPK2_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_SPK2_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_SPK2_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_SPK2_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_SPK2_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + +static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { + [HWSETTINGS_SPK2_FLEXIPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_SPK2_FLEXIPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_SPK2_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + [HWSETTINGS_SPK2_FLEXIPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSETTINGS_SPK2_FLEXIPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_SPK2_FLEXIPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_SPK2_FLEXIPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_SPK2_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_SPK2_FLEXIPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_SPK2_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_SPK2_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_rcvr_cfg.regs) { /* rcvr port */ + GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, + RCVR_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? RCVR_USART_INVERTER_ENABLE : RCVR_USART_INVERTER_DISABLE); + + return 0; + } + break; } - - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - -static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) -{ - uint32_t pios_ppm_id; - - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; -} - -static void PIOS_Board_configure_srxl(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_srxl_id; - - if (PIOS_USART_Init(&pios_usart_srxl_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_id; - if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_rcvr_id; - if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) { - PIOS_Assert(0); - } - - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; -} - -static void PIOS_Board_configure_ibus(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_ibus_id; - - if (PIOS_USART_Init(&pios_usart_ibus_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_id; - if (PIOS_IBUS_Init(&pios_ibus_id, &pios_usart_com_driver, pios_usart_ibus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_rcvr_id; - if (PIOS_RCVR_Init(&pios_ibus_rcvr_id, &pios_ibus_rcvr_driver, pios_ibus_id)) { - PIOS_Assert(0); - } - - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS] = pios_ibus_rcvr_id; -} - - -static void PIOS_Board_configure_exbus(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_exbus_id; - - if (PIOS_USART_Init(&pios_usart_exbus_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; -} - -static void PIOS_Board_configure_hott(const struct pios_usart_cfg *usart_cfg, enum pios_hott_proto proto) -{ - uint32_t pios_usart_hott_id; - - if (PIOS_USART_Init(&pios_usart_hott_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, proto)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; + + return -1; } /** @@ -397,13 +118,8 @@ static void PIOS_Board_configure_hott(const struct pios_usart_cfg *usart_cfg, en * called from System/openpilot.c */ -#include - void PIOS_Board_Init(void) { - /* Delay system */ - PIOS_DELAY_Init(); - const struct pios_board_info *bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) @@ -427,12 +143,6 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Assert(0); } -#ifdef PIOS_INCLUDE_I2C - if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } -#endif - #if defined(PIOS_INCLUDE_FLASH) /* Connect flash to the appropriate interface and configure it */ uintptr_t flash_id = 0; @@ -477,13 +187,6 @@ void PIOS_Board_Init(void) EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ -#if defined(PIOS_INCLUDE_HMC5X83) - AuxMagSettingsInitialize(); -#endif /* PIOS_INCLUDE_HMC5X83 */ /* Initialize the alarms library */ AlarmsInitialize(); @@ -511,110 +214,31 @@ void PIOS_Board_Init(void) } /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsSPK2_FlexiPortGet(&hwsettings_flexiport); - switch (hwsettings_flexiport) { - case HWSETTINGS_SPK2_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_SPK2_FLEXIPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_SPK2_FLEXIPORT_I2C: + + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); + } + #if defined(PIOS_INCLUDE_I2C) -#if defined(PIOS_INCLUDE_HMC5X83) - { - // get auxmag type - AuxMagSettingsTypeOptions option; - AuxMagSettingsTypeGet(&option); - // the FlexiPort type is I2C, so if the AuxMag type is Flexi(Port) then set it up - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } - PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // attach the 5x83 mag to the previously inited I2C2 - flexi_port_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_flexiport_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor - // and before registering some other fast and important sensor - // as that would cause delay and time jitter for the second fast sensor - PIOS_HMC5x83_Register(flexi_port_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (flexi_port_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } + /* Set up internal I2C bus */ + if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + PIOS_DELAY_WaitmS(50); + + if (hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_I2C) { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); } -#endif /* PIOS_INCLUDE_HMC5X83 */ -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_SPK2_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_SPK2_FLEXIPORT_DSM: - // TODO: Define the various Channelgroup for Sparky2 dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_SPK2_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_SPK2_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_SPK2_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_SPK2_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_SPK2_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - case HWSETTINGS_SPK2_FLEXIPORT_SRXL: -#if defined(PIOS_INCLUDE_SRXL) - PIOS_Board_configure_srxl(&pios_usart_srxl_flexi_cfg); -#endif /* PIOS_INCLUDE_SRXL */ - break; - - case HWSETTINGS_SPK2_FLEXIPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_flexi_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - - case HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMD: - case HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - PIOS_Board_configure_hott(&pios_usart_hott_flexi_cfg, - hwsettings_flexiport == HWSETTINGS_SPK2_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH); -#endif /* PIOS_INCLUDE_HOTT */ - break; - - case HWSETTINGS_SPK2_FLEXIPORT_EXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - PIOS_Board_configure_exbus(&pios_usart_exbus_flexi_cfg); -#endif /* PIOS_INCLUDE_EXBUS */ - break; - } /* hwsettings_spk2_flexiport */ + PIOS_DELAY_WaitmS(50); + } + + PIOS_BOARD_IO_Configure_I2C(pios_i2c_mag_pressure_adapter_id, pios_i2c_flexiport_adapter_id); +#endif /* Moved this here to allow binding on flexiport */ #if defined(PIOS_INCLUDE_FLASH) @@ -624,472 +248,62 @@ void PIOS_Board_Init(void) #endif /* if defined(PIOS_INCLUDE_FLASH) */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); - -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ - - - /* Configure main USART port */ uint8_t hwsettings_mainport; HwSettingsSPK2_MainPortGet(&hwsettings_mainport); - switch (hwsettings_mainport) { - case HWSETTINGS_SPK2_MAINPORT_DISABLED: - break; - case HWSETTINGS_SPK2_MAINPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_SPK2_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_SPK2_MAINPORT_DSM: - // Force binding to zero on the main port - hwsettings_DSMxBind = 0; - - // TODO: Define the various Channelgroup for Sparky2 dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_SPK2_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_SPK2_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_SPK2_MAINPORT_MSP: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_SPK2_MAINPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_SPK2_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_spk2_mainport */ - - - /* Initialize the RFM22B radio COM device. */ -#if defined(PIOS_INCLUDE_RFM22B) - - /* Fetch the OPLinkSettings object. */ - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - - // Initialize out status object. - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); - oplinkStatus.BoardType = bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision = bdinfo->board_rev; - - /* Is the radio turned on? */ - bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR); - bool openlrs = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPENLRS); - bool data_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATA) || - (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); - bool ppm_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) || - (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); - bool is_enabled = ((oplinkSettings.Protocol != OPLINKSETTINGS_PROTOCOL_DISABLED) && - ((oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) || openlrs)); - if (is_enabled) { - if (openlrs) { -#if defined(PIOS_INCLUDE_OPENLRS_RCVR) - const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(); - uint32_t openlrs_id; - - PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg); - { - uint32_t openlrsrcvr_id; - PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); - uint32_t openlrsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS] = openlrsrcvr_rcvr_id; - } -#endif /* PIOS_INCLUDE_OPENLRS_RCVR */ - } else { - /* Configure the RFM22B device. */ - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { - PIOS_Assert(0); - } - - /* Configure the radio com interface */ - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; - - // Set the modem (over the air) datarate. - enum rfm22b_datarate datarate = RFM22_datarate_64000; - switch (oplinkSettings.AirDataRate) { - case OPLINKSETTINGS_AIRDATARATE_9600: - datarate = RFM22_datarate_9600; - break; - case OPLINKSETTINGS_AIRDATARATE_19200: - datarate = RFM22_datarate_19200; - break; - case OPLINKSETTINGS_AIRDATARATE_32000: - datarate = RFM22_datarate_32000; - break; - case OPLINKSETTINGS_AIRDATARATE_57600: - datarate = RFM22_datarate_57600; - break; - case OPLINKSETTINGS_AIRDATARATE_64000: - datarate = RFM22_datarate_64000; - break; - case OPLINKSETTINGS_AIRDATARATE_100000: - datarate = RFM22_datarate_100000; - break; - case OPLINKSETTINGS_AIRDATARATE_128000: - datarate = RFM22_datarate_128000; - break; - case OPLINKSETTINGS_AIRDATARATE_192000: - datarate = RFM22_datarate_192000; - break; - case OPLINKSETTINGS_AIRDATARATE_256000: - datarate = RFM22_datarate_256000; - break; - } - - /* Set the radio configuration parameters. */ - PIOS_RFM22B_SetDeviceID(pios_rfm22b_id, oplinkSettings.CustomDeviceID); - PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); - PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap); - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode); - - /* Set the modem Tx poer level */ - switch (oplinkSettings.MaxRFPower) { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - default: - // do nothing - break; - } - - /* Reinitialize the modem. */ - PIOS_RFM22B_Reinit(pios_rfm22b_id); - // TODO: this is in preparation for full mavlink support and is used by LP-368 - uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN; - - uint8_t hwsettings_radioaux; - HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - - switch (hwsettings_radioaux) { - case HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE: - case HWSETTINGS_RADIOAUXSTREAM_DISABLED: - break; - case HWSETTINGS_RADIOAUXSTREAM_MAVLINK: - { - uint8_t *auxrx_buffer = 0; - if (mavlink_rx_size) { - auxrx_buffer = (uint8_t *)pios_malloc(mavlink_rx_size); - } - uint8_t *auxtx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); - PIOS_Assert(auxrx_buffer); - PIOS_Assert(auxtx_buffer); - if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - auxrx_buffer, mavlink_rx_size, - auxtx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } - } - } - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - - OPLinkStatusSet(&oplinkStatus); + +#if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); #endif /* PIOS_INCLUDE_RFM22B */ -#if 1 -#if defined(PIOS_INCLUDE_PPM) - const struct pios_servo_cfg *pios_servo_cfg; - // default to servo outputs only - pios_servo_cfg = &pios_servo_cfg_out; -#endif -#endif + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = RCVR_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; + + GPIO_Init(RCVR_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, + RCVR_USART_INVERTER_PIN, + RCVR_USART_INVERTER_DISABLE); + } // Configure the receiver port // Sparky2 receiver input on PC7 TIM8 CH2 // include PPM,S.Bus,DSM,SRXL,IBus,EX.Bus,HoTT SUMD,HoTT SUMH + /* Configure the receiver port*/ + uint8_t hwsettings_rcvrport; HwSettingsSPK2_RcvrPortGet(&hwsettings_rcvrport); + + if (hwsettings_rcvrport < NELEMENTS(rcvr_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_rcvr_cfg, rcvr_function_map[hwsettings_rcvrport]); + } - switch (hwsettings_rcvrport) { - case HWSETTINGS_SPK2_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) - PIOS_Board_configure_ppm(&pios_ppm_cfg); -#endif /* PIOS_INCLUDE_PPM */ - break; - case HWSETTINGS_SPK2_RCVRPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } + if(hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_PPM) { + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + } #endif - break; - case HWSETTINGS_SPK2_RCVRPORT_SRXL: -#if defined(PIOS_INCLUDE_SRXL) - PIOS_Board_configure_srxl(&pios_usart_srxl_rcvr_cfg); -#endif /* PIOS_INCLUDE_SRXL */ - break; - case HWSETTINGS_SPK2_RCVRPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_rcvr_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - case HWSETTINGS_SPK2_RCVRPORT_HOTTSUMD: - case HWSETTINGS_SPK2_RCVRPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - PIOS_Board_configure_hott(&pios_usart_hott_rcvr_cfg, - hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH); -#endif /* PIOS_INCLUDE_HOTT */ - break; - case HWSETTINGS_SPK2_RCVRPORT_EXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - PIOS_Board_configure_exbus(&pios_usart_exbus_rcvr_cfg); -#endif /* PIOS_INCLUDE_EXBUS */ - break; - case HWSETTINGS_SPK2_RCVRPORT_DSM: - // TODO: Define the various Channelgroup for Sparky2 dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_rcvr_cfg, &pios_dsm_rcvr_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT, &hwsettings_DSMxBind); - break; - default: - break; - } +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif - if (hwsettings_rcvrport != HWSETTINGS_SPK2_RCVRPORT_SBUS) { - GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); - GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); - } - -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#if defined(PIOS_INCLUDE_OPLINKRCVR) - { - OPLinkReceiverInitialize(); - uint32_t pios_oplinkrcvr_id; - PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id); - uint32_t pios_oplinkrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; - } -#endif /* PIOS_INCLUDE_OPLINKRCVR */ - -#if 1 #ifndef PIOS_ENABLE_DEBUG_PINS - // pios_servo_cfg points to the correct configuration based on input port settings - PIOS_Servo_Init(pios_servo_cfg); + PIOS_Servo_Init(&pios_servo_cfg_out); #else PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif -#endif - - // Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout - GPIO_InitTypeDef gpioA8 = { - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - .GPIO_Pin = GPIO_Pin_8, - .GPIO_OType = GPIO_OType_OD, - }; - GPIO_Init(GPIOA, &gpioA8); - -#if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); -#endif #if defined(PIOS_INCLUDE_MPU9250) PIOS_MPU9250_Init(pios_spi_gyro_id, 0, &pios_mpu9250_cfg); @@ -1098,71 +312,7 @@ void PIOS_Board_Init(void) PIOS_MPU9250_MagRegister(); #endif -#if 0 -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - -#if defined(PIOS_INCLUDE_HMC5X83) - // on board mag is in the MPU9250 - // this hmc5x83 mag is an external mag - i2c_port_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - PIOS_HMC5x83_Register(i2c_port_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); -#endif /* PIOS_INCLUDE_HMC5X83 */ - -#else /* if 0 */ - -#if defined(PIOS_INCLUDE_I2C) -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ -#if defined(PIOS_INCLUDE_HMC5X83) - // get auxmag type - HwSettingsSPK2_I2CPortOptions i2cOption; - AuxMagSettingsTypeOptions option; - HwSettingsSPK2_I2CPortGet(&i2cOption); - AuxMagSettingsTypeGet(&option); - // if the I2CPort type is I2C(Port) and the AuxMag type is I2C(Port) then set it up - if (i2cOption == HWSETTINGS_SPK2_I2CPORT_I2C && option == AUXMAGSETTINGS_TYPE_I2C) { - // attach the 5x83 mag to the previously inited I2C2 - i2c_port_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor - // and before registering some other fast and important sensor - // as that would cause delay and time jitter for the second fast sensor - PIOS_HMC5x83_Register(i2c_port_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (i2c_port_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } -#endif /* PIOS_INCLUDE_HMC5X83 */ -#endif /* PIOS_INCLUDE_I2C */ -#endif /* 0 */ - -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); - PIOS_MS5611_Register(); -#endif /* PIOS_INCLUDE_MS5611 */ - - #ifdef PIOS_INCLUDE_WS2811 -#include +#ifdef PIOS_INCLUDE_WS2811 HwSettingsWS2811LED_OutOptions ws2811_pin_settings; HwSettingsWS2811LED_OutGet(&ws2811_pin_settings); @@ -1170,17 +320,21 @@ void PIOS_Board_Init(void) PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]); } #endif // PIOS_INCLUDE_WS2811 + #ifdef PIOS_INCLUDE_ADC - { - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); - } - } - } -#endif // PIOS_INCLUDE_ADC + // Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout + GPIO_InitTypeDef gpioA8 = { + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + .GPIO_Pin = GPIO_Pin_8, + .GPIO_OType = GPIO_OType_OD, + }; + GPIO_Init(GPIOA, &gpioA8); + + PIOS_BOARD_IO_Configure_ADC(); +#endif /* PIOS_INCLUDE_ADC */ + } /** diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index fed486c90..6f74f8425 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -34,6 +34,7 @@ + From c0adcb904bd15f9680e34a7035859d74c5ca0739 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 19 Apr 2017 15:57:00 +0200 Subject: [PATCH 02/20] LP-480 build discoveryf4bare --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- flight/pios/common/pios_board_io.c | 9 +- .../boards/discoveryf4bare/board_hw_defs.c | 885 ++++----------- .../discoveryf4bare/bootloader/pios_board.c | 10 +- .../discoveryf4bare/firmware/pios_board.c | 1004 +++-------------- .../boards/revolution/bootloader/pios_board.c | 1 - 6 files changed, 405 insertions(+), 1506 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index da24ebb79..bf7efbcf1 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_sparky2; + buildArgumentsString = ef_discoveryf4bare; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 835ca272a..f3b2ac92e 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -708,14 +708,19 @@ void PIOS_BOARD_IO_Configure_I2C(uint32_t i2c_internal_id, uint32_t i2c_external if (hmc5x83_internal_cfg) { // attach the 5x83 mag to internal i2c bus - pios_hmc5x83_internal_id = PIOS_HMC5x83_Init(hmc5x83_internal_cfg, i2c_internal_id, 0); + + pios_hmc5x83_dev_t internal_mag = PIOS_HMC5x83_Init(hmc5x83_internal_cfg, i2c_internal_id, 0); # ifdef PIOS_INCLUDE_WDG // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed // this is not in a loop, so it is safe PIOS_WDG_Clear(); # endif /* PIOS_INCLUDE_WDG */ + +#ifdef PIOS_HMC5X83_HAS_GPIOS + pios_hmc5x83_internal_id = internal_mag; +#endif // add this sensor to the sensor task's list - PIOS_HMC5x83_Register(pios_hmc5x83_internal_id, PIOS_SENSORS_TYPE_3AXIS_MAG); + PIOS_HMC5x83_Register(internal_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); } # endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */ diff --git a/flight/targets/boards/discoveryf4bare/board_hw_defs.c b/flight/targets/boards/discoveryf4bare/board_hw_defs.c index 0c833776b..678a1500a 100644 --- a/flight/targets/boards/discoveryf4bare/board_hw_defs.c +++ b/flight/targets/boards/discoveryf4bare/board_hw_defs.c @@ -602,75 +602,20 @@ static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = { #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN USART */ +// Inverter for SBUS handling +#define MAIN_USART_INVERTER_GPIO GPIOC +#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET + +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; -#endif /* PIOS_INCLUDE_COM_TELEM */ - -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOA, .init = { @@ -691,120 +636,15 @@ static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; -// Because of the inverter on the main port this will not -// work. Notice the mode is set to IN to maintain API -// compatibility but protect the pins -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#include -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -// Need this defined regardless to be able to turn it off -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, -}; - - -#ifdef PIOS_INCLUDE_COM_FLEXI /* * FLEXI PORT */ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOB, .init = { @@ -827,172 +667,10 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { }, }; -#endif /* PIOS_INCLUDE_COM_FLEXI */ -#ifdef PIOS_INCLUDE_DSM - -#include "pios_dsm_priv.h" -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { +static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .dtr = { // FlexIO pin 9 .gpio = GPIOC, @@ -1385,212 +1063,24 @@ static const struct pios_tim_clock_cfg tim_12_cfg = { * Using TIM3, TIM9, TIM2, TIM5 */ #include +#include + static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM3, - }, - { - .timer = TIM9, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource3, - }, - .remap = GPIO_AF_TIM9, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource2, - }, - .remap = GPIO_AF_TIM2, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource1, - }, - .remap = GPIO_AF_TIM5, - }, - { - .timer = TIM5, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource0, - }, - .remap = GPIO_AF_TIM5, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0), // PWM pins on FlexiIO(receiver) port - { - // * 6: PB15 = SPI2 MOSI, TIM12 CH2 - - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - // * 7: PC6 = TIM8 CH1, USART6 TX - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 8: PC7 = TIM8 CH2, USART6 RX - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 9: PC8 = TIM8 CH3 - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 10: PC9 = TIM8 CH4 - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, - - { - // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), // * 6: PB15 = SPI2 MOSI, TIM12 CH2 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), // * 7: PC6 = TIM8 CH1, USART6 TX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), // * 8: PC7 = TIM8 CH2, USART6 RX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), // * 9: PC8 = TIM8 CH3 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), // * 10: PC9 = TIM8 CH4 + TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS }; + #define PIOS_SERVOPORT_ALL_PINS_PWMOUT 6 #define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN_PPM 11 #define PIOS_SERVOPORT_ALL_PINS_PWMOUT_IN 12 @@ -1648,102 +1138,12 @@ const struct pios_servo_cfg pios_servo_cfg_out_in = { #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM12, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource14, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM12, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource15, - }, - .remap = GPIO_AF_TIM12, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource6, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource7, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource8, - }, - .remap = GPIO_AF_TIM8, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - .pin_source = GPIO_PinSource9, - }, - .remap = GPIO_AF_TIM8, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), + TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), }; const struct pios_pwm_cfg pios_pwm_cfg = { @@ -1840,36 +1240,6 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; - -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ #ifdef PIOS_INCLUDE_WS2811 #include #define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD)) @@ -1936,3 +1306,196 @@ void PIOS_WS2811_irq_handler(void) PIOS_WS2811_DMA_irq_handler(); } #endif // PIOS_INCLUDE_WS2811 + +/** + * Sensor configurations + */ + +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, +}; +void PIOS_ADC_DMC_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); +} + +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" + +#ifdef PIOS_HMC5X83_HAS_GPIOS +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(pios_hmc5x83_internal_id); +} + +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = pios_board_internal_mag_handler, + .line = EXTI_Line7, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line7, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_HMC5X83_HAS_GPIOS */ + +static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = &pios_exti_hmc5x83_cfg, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag +}; + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_internal_cfg; +} + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_external_cfg; +} +#endif /* PIOS_INCLUDE_HMC5X83 */ + +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS5611) +#include "pios_ms5611.h" +static const struct pios_ms5611_cfg pios_ms5611_cfg = { + .oversampling = MS5611_OSR_512, +}; +const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms5611_cfg; +} +#endif /* PIOS_INCLUDE_MS5611 */ + + +/** + * Configuration for the MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +#include "pios_mpu6000_config.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 12 for 666Hz + .Smpl_rate_div_no_dlp = 11, + // with dlp on output rate is 500Hz + .Smpl_rate_div_dlp = 1, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG +}; +#endif /* PIOS_INCLUDE_MPU6000 */ + + diff --git a/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c b/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c index 5352c7f73..b867b81d7 100644 --- a/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c @@ -25,7 +25,7 @@ #include #include - +#include /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -39,6 +39,12 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + +static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) +{ + return -1; +} + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +74,7 @@ void PIOS_Board_Init() #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 782cdb1ca..bac459834 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -30,19 +30,16 @@ #include #include #include -#include -#include -#include -#include -#include #include -#include -#include +#include + #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -53,312 +50,72 @@ */ #include "../board_hw_defs.c" -/** - * Sensor configurations - */ - -#if defined(PIOS_INCLUDE_ADC) -#include "pios_adc_priv.h" -void PIOS_ADC_DMC_irq_handler(void); -void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); -struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, -}; -void PIOS_ADC_DMC_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); -} - -#endif /* if defined(PIOS_INCLUDE_ADC) */ - -#if defined(PIOS_INCLUDE_HMC5X83) -#include "pios_hmc5x83.h" -pios_hmc5x83_dev_t onboard_mag = 0; -pios_hmc5x83_dev_t external_mag = 0; - -bool pios_board_internal_mag_handler() -{ - return PIOS_HMC5x83_IRQHandler(onboard_mag); -} - -static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = pios_board_internal_mag_handler, - .line = EXTI_Line7, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line7, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = &pios_exti_hmc5x83_cfg, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, -}; - -static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = NULL, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag -}; -#endif /* PIOS_INCLUDE_HMC5X83 */ - -/** - * Configuration for the MS5611 chip - */ -#if defined(PIOS_INCLUDE_MS5611) -#include "pios_ms5611.h" -static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_512, -}; -#endif /* PIOS_INCLUDE_MS5611 */ - - -/** - * Configuration for the MPU6000 chip - */ -#if defined(PIOS_INCLUDE_MPU6000) -#include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" -static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 12 for 666Hz - .Smpl_rate_div_no_dlp = 11, - // with dlp on output rate is 500Hz - .Smpl_rate_div_dlp = 1, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG -}; -#endif /* PIOS_INCLUDE_MPU6000 */ - -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 - -#define PIOS_COM_GPS_RX_BUF_LEN 128 -#define PIOS_COM_GPS_TX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 - -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 - -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 -uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_gps_id = 0; -uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; -uint32_t pios_com_msp_id = 0; -uint32_t pios_com_mavlink_id = 0; -uint32_t pios_com_vcp_id = 0; - -#if defined(PIOS_INCLUDE_RFM22B) -uint32_t pios_rfm22b_id = 0; -#endif uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size = 0 make the port rx only - * rx size = 0 make the port tx only - * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) +static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = { + [HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_RCVRPORT_PPMMSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_RCVRPORT_PPMMAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_RM_RCVRPORT_PPMGPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_RCVRPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_RCVRPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_RCVRPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_RCVRPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_RM_RCVRPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, +}; + +static const PIOS_BOARD_IO_UART_Function main_function_map[] = { + [HWSETTINGS_RM_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_MAINPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_RM_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_RM_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + +static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { + [HWSETTINGS_RM_FLEXIPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RM_FLEXIPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_RM_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + [HWSETTINGS_RM_FLEXIPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_RM_FLEXIPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_RM_FLEXIPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSETTINGS_RM_FLEXIPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_RM_FLEXIPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_RM_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RM_FLEXIPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_RM_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, +}; + + +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; } - - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - -static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg) -{ - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - - PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; -} - -static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) -{ - uint32_t pios_ppm_id; - - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + + return -1; } /** @@ -366,14 +123,8 @@ static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ - -#include - void PIOS_Board_Init(void) { - /* Delay system */ - PIOS_DELAY_Init(); - const struct pios_board_info *bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) @@ -446,12 +197,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ /* Initialize the alarms library */ AlarmsInitialize(); @@ -477,397 +223,67 @@ void PIOS_Board_Init(void) AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } - - // PIOS_IAP_Init(); - -#if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; -#endif - - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); - -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - - /* Configure main USART port */ - uint8_t hwsettings_mainport; - HwSettingsRM_MainPortGet(&hwsettings_mainport); - switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DISABLED: - break; - case HWSETTINGS_RM_MAINPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_MAINPORT_GPS: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif - break; - case HWSETTINGS_RM_MAINPORT_DSM: - // Force binding to zero on the main port - hwsettings_DSMxBind = 0; - - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_MAINPORT_MSP: - PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_MAINPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_mainport */ - - if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { - GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init); - GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); - } - + /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RM_FLEXIPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_FLEXIPORT_I2C: + + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); + } + #if defined(PIOS_INCLUDE_I2C) + /* Set up internal I2C bus */ + if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + PIOS_DELAY_WaitmS(50); + + if (hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } - PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ -#if defined(PIOS_INCLUDE_HMC5X83) - // get auxmag type - AuxMagSettingsTypeOptions option; - AuxMagSettingsInitialize(); - AuxMagSettingsTypeGet(&option); - // if the aux mag type is FlexiPort then set it up - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - // attach the 5x83 mag to the previously inited I2C2 - external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor - // and before registering some other fast and important sensor - // as that would cause delay and time jitter for the second fast sensor - PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } -#endif /* PIOS_INCLUDE_HMC5X83 */ -#endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_RM_FLEXIPORT_GPS: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - case HWSETTINGS_RM_FLEXIPORT_DSM: - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rm_flexiport */ - - - /* Initialize the RFM22B radio COM device. */ -#if defined(PIOS_INCLUDE_RFM22B) - - /* Fetch the OPLinkSettings object. */ - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - - // Initialize out status object. - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); - oplinkStatus.BoardType = bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision = bdinfo->board_rev; - - /* Is the radio turned on? */ - bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); - bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); - bool ppm_mode = (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE); - if (oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) { - /* Configure the RFM22B device. */ - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { - PIOS_Assert(0); - } - - /* Configure the radio com interface */ - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; - - // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. - enum rfm22b_datarate datarate = RFM22_datarate_64000; - switch (oplinkSettings.ComSpeed) { - case OPLINKSETTINGS_COMSPEED_4800: - datarate = RFM22_datarate_9600; - break; - case OPLINKSETTINGS_COMSPEED_9600: - datarate = RFM22_datarate_19200; - break; - case OPLINKSETTINGS_COMSPEED_19200: - datarate = RFM22_datarate_32000; - break; - case OPLINKSETTINGS_COMSPEED_38400: - datarate = RFM22_datarate_64000; - break; - case OPLINKSETTINGS_COMSPEED_57600: - datarate = RFM22_datarate_100000; - break; - case OPLINKSETTINGS_COMSPEED_115200: - datarate = RFM22_datarate_192000; - break; - } - - /* Set the radio configuration parameters. */ - PIOS_RFM22B_SetDeviceID(pios_rfm22b_id, oplinkSettings.CustomDeviceID); - PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); - PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap); - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode); - - /* Set the modem Tx power level */ - switch (oplinkSettings.MaxRFPower) { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - default: - // do nothing - break; - } - - /* Reinitialize the modem. */ - PIOS_RFM22B_Reinit(pios_rfm22b_id); - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + PIOS_DELAY_WaitmS(50); } + + PIOS_BOARD_IO_Configure_I2C(pios_i2c_mag_pressure_adapter_id, pios_i2c_flexiport_adapter_id); +#endif - OPLinkStatusSet(&oplinkStatus); +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + /* Configure main USART port */ + + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; + + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + MAIN_USART_INVERTER_DISABLE); + } + + uint8_t hwsettings_mainport; + HwSettingsRM_MainPortGet(&hwsettings_mainport); + + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); + } + +#if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); #endif /* PIOS_INCLUDE_RFM22B */ + #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) const struct pios_servo_cfg *pios_servo_cfg; @@ -878,99 +294,51 @@ void PIOS_Board_Init(void) /* Configure the receiver port*/ uint8_t hwsettings_rcvrport; HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); - // + + if (hwsettings_rcvrport < NELEMENTS(flexiio_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexiio_cfg, flexiio_function_map[hwsettings_rcvrport]); + } + + // Configure rcvrport PPM/PWM/OUTPUTS switch (hwsettings_rcvrport) { - case HWSETTINGS_RM_RCVRPORT_DISABLED: - break; - case HWSETTINGS_RM_RCVRPORT_PWM: + case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - /* Set up the receiver port. Later this should be optional */ - PIOS_Board_configure_pwm(&pios_pwm_cfg); + /* Set up the receiver port. Later this should be optional */ + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_RM_RCVRPORT_PPM: - case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_RM_RCVRPORT_PPMPWM: - case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY: - case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE: - case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE: - case HWSETTINGS_RM_RCVRPORT_PPMMSP: - case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: - case HWSETTINGS_RM_RCVRPORT_PPMGPS: + break; + case HWSETTINGS_RM_RCVRPORT_PPM: + case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RM_RCVRPORT_PPMPWM: + case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY: + case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE: + case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE: + case HWSETTINGS_RM_RCVRPORT_PPMMSP: + case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: + case HWSETTINGS_RM_RCVRPORT_PPMGPS: #if defined(PIOS_INCLUDE_PPM) - if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { - // configure servo outputs and the remaining 5 inputs as outputs - pios_servo_cfg = &pios_servo_cfg_out_in_ppm; - } - - PIOS_Board_configure_ppm(&pios_ppm_cfg); - - // enable pwm on the remaining channels - if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { - PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); - } - - break; + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { + // configure servo outputs and the remaining 5 inputs as outputs + pios_servo_cfg = &pios_servo_cfg_out_in_ppm; + } + + // enable pwm on the remaining channels + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_ppm_cfg); + } + + break; #endif /* PIOS_INCLUDE_PPM */ - case HWSETTINGS_RM_RCVRPORT_OUTPUTS: - // configure only the servo outputs - pios_servo_cfg = &pios_servo_cfg_out_in; - break; + case HWSETTINGS_RM_RCVRPORT_OUTPUTS: + // configure only the servo outputs + pios_servo_cfg = &pios_servo_cfg_out_in; + break; } - - // Configure rcvrport usart - switch (hwsettings_rcvrport) { - case HWSETTINGS_RM_RCVRPORT_TELEMETRY: - case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE: - case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE: -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - break; - case HWSETTINGS_RM_RCVRPORT_COMBRIDGE: - case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RM_RCVRPORT_MSP: - case HWSETTINGS_RM_RCVRPORT_PPMMSP: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RM_RCVRPORT_MAVLINK: - case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RM_RCVRPORT_GPS: - case HWSETTINGS_RM_RCVRPORT_PPMGPS: - PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); - break; - } - -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#if defined(PIOS_INCLUDE_OPLINKRCVR) - { - OPLinkReceiverInitialize(); - uint32_t pios_oplinkrcvr_id; - PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id); - uint32_t pios_oplinkrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; - } -#endif /* PIOS_INCLUDE_OPLINKRCVR */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif #ifndef PIOS_ENABLE_DEBUG_PINS // pios_servo_cfg points to the correct configuration based on input port settings @@ -979,6 +347,17 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif +#if defined(PIOS_INCLUDE_MPU6000) + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); + PIOS_MPU6000_Register(); +#endif + +#ifdef PIOS_INCLUDE_WS2811 + PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg); +#endif // PIOS_INCLUDE_WS2811 + +#ifdef PIOS_INCLUDE_ADC // Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout GPIO_InitTypeDef gpioA8 = { .GPIO_Speed = GPIO_Speed_2MHz, @@ -988,61 +367,8 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_OD, }; GPIO_Init(GPIOA, &gpioA8); - - // init I2C1 for use with the internal mag and baro - if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { - PIOS_DEBUG_Assert(0); - } - - PIOS_DELAY_WaitmS(50); - -#if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); -#endif - -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - -#if defined(PIOS_INCLUDE_HMC5X83) - // attach the 5x83 mag to the previously inited I2C1 - onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_pressure_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); -#endif - -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); -#endif - -#if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); -#endif - -#ifdef PIOS_INCLUDE_WS2811 -#include - PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg); -#endif // PIOS_INCLUDE_WS2811 -#ifdef PIOS_INCLUDE_ADC - { - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); - } - } - } + + PIOS_BOARD_IO_Configure_ADC(); #endif // PIOS_INCLUDE_ADC } diff --git a/flight/targets/boards/revolution/bootloader/pios_board.c b/flight/targets/boards/revolution/bootloader/pios_board.c index e0065e2a9..595eac507 100644 --- a/flight/targets/boards/revolution/bootloader/pios_board.c +++ b/flight/targets/boards/revolution/bootloader/pios_board.c @@ -46,7 +46,6 @@ static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, return -1; } - void PIOS_Board_Init() { if (board_init_complete) { From ddc4f3d8556ed55723ca6c4f40295b54922610f3 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 19 Apr 2017 15:57:34 +0200 Subject: [PATCH 03/20] LP-480 coptercontrol build ok --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- flight/modules/ComUsbBridge/ComUsbBridge.c | 2 + flight/modules/GPS/GPS.c | 1 + flight/modules/Osd/osdoutput/osdoutput.c | 2 + flight/modules/Telemetry/telemetry.c | 2 + flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 2 + .../UAVOMavlinkBridge/UAVOMavlinkBridge.c | 3 + flight/pios/common/pios_board_io.c | 9 +- flight/pios/common/pios_dsm.c | 4 + flight/pios/common/pios_usb_desc_hid_cdc.c | 4 + flight/pios/inc/pios_usb_desc_hid_cdc_priv.h | 2 + flight/pios/stm32f10x/inc/pios_servo_config.h | 49 + flight/pios/stm32f10x/pios_sys.c | 60 +- flight/pios/stm32f10x/pios_usart.c | 79 +- flight/pios/stm32f4xx/pios_usart.c | 2 +- .../boards/coptercontrol/board_hw_defs.c | 858 +++--------------- .../boards/coptercontrol/bootloader/main.c | 1 + .../coptercontrol/bootloader/pios_board.c | 10 +- .../coptercontrol/firmware/pios_board.c | 671 ++------------ .../targets/boards/coptercontrol/pios_board.h | 35 +- .../boards/discoveryf4bare/pios_board.h | 28 - flight/targets/boards/revolution/pios_board.h | 32 - flight/targets/boards/revonano/pios_board.h | 31 - flight/targets/boards/sparky2/pios_board.h | 30 - 24 files changed, 417 insertions(+), 1502 deletions(-) create mode 100644 flight/pios/stm32f10x/inc/pios_servo_config.h diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index bf7efbcf1..1bdc89da6 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_discoveryf4bare; + buildArgumentsString = ef_coptercontrol; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 15f9f5db9..fdc9a0144 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -36,6 +36,8 @@ #include +#include + // **************** // Private functions diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index dfc2ac2c6..b9ce967c4 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -45,6 +45,7 @@ #include "WorldMagModel.h" #include "CoordinateConversions.h" #include +#include #include "GPS.h" #include "NMEA.h" diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 7f18b4720..f600aa148 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -43,6 +43,8 @@ #include "hwsettings.h" #include "flightstatus.h" +#include + static bool osdoutputEnabled; enum osd_hk_sync { diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 9796e8f59..a3bbf0a77 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -69,6 +69,8 @@ #include "hwsettings.h" #include "taskinfo.h" +#include + // Private constants #define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE // Three different stack size parameter are accepted for Telemetry(RX PIOS_TELEM_RX_STACK_SIZE) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 43bbbe433..6114a963c 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -68,6 +68,8 @@ #include "pios_sensors.h" +#include + #define PIOS_INCLUDE_MSP_BRIDGE diff --git a/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c b/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c index bca1ab6fa..03f97252d 100644 --- a/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c +++ b/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c @@ -58,6 +58,9 @@ #include "custom_types.h" +#include + + #define OPLINK_LOW_RSSI -110 #define OPLINK_HIGH_RSSI -10 diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index f3b2ac92e..7b742b758 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -110,6 +110,7 @@ uint32_t pios_com_debug_id; /* DebugConsole */ uint32_t pios_com_telem_usb_id; /* USB telemetry */ #ifdef PIOS_INCLUDE_USB_RCTX +# include "pios_usb_rctx_priv.h" uint32_t pios_usb_rctx_id; #endif @@ -255,10 +256,8 @@ void PIOS_BOARD_IO_Configure_USB() break; case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { /* this will reinstall endpoint callbacks */ - PIOS_Assert(0); - } + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { /* this will reinstall endpoint callbacks */ + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_USB_RCTX */ break; @@ -266,9 +265,11 @@ void PIOS_BOARD_IO_Configure_USB() #endif /* PIOS_INCLUDE_USB_HID */ +#ifndef STM32F10X if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } +#endif } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_dsm.c b/flight/pios/common/pios_dsm.c index 23d3601e3..af52ce985 100644 --- a/flight/pios/common/pios_dsm.c +++ b/flight/pios/common/pios_dsm.c @@ -126,9 +126,13 @@ static void PIOS_DSM_Bind(struct stm32_gpio *rxpin, uint8_t bind) GPIO_InitTypeDef bindInit = { .GPIO_Pin = rxpin->init.GPIO_Pin, .GPIO_Speed = GPIO_Speed_2MHz, +#ifdef STM32F10X + .GPIO_Mode = GPIO_Mode_Out_PP, +#else .GPIO_Mode = GPIO_Mode_OUT, .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_NOPULL +#endif }; GPIO_Init(rxpin->gpio, &bindInit); diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index d14add637..14bb69f61 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -332,6 +332,10 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { .data_tx_ep = 1, }; +const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { + .data_if = 2, + .data_tx_ep = 1, +}; int32_t PIOS_USB_DESC_HID_CDC_Init(void) { diff --git a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h index 6a3145a8b..77d3b2347 100644 --- a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h @@ -33,11 +33,13 @@ #include #include #include +#include extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); extern const struct pios_usb_cdc_cfg pios_usb_cdc_cfg; extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; +extern const struct pios_usb_rctx_cfg pios_usb_rctx_cfg; #endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ diff --git a/flight/pios/stm32f10x/inc/pios_servo_config.h b/flight/pios/stm32f10x/inc/pios_servo_config.h new file mode 100644 index 000000000..b5464ebae --- /dev/null +++ b/flight/pios/stm32f10x/inc/pios_servo_config.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file pios_servp_config.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief Architecture specific macros and definitions + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIOS_SERVO_CONFIG_H_ +#define PIOS_SERVO_CONFIG_H_ + +/** + * Generic servo pin configuration structure for an STM32F10x + */ +#define TIM_SERVO_CHANNEL_CONFIG(_timer, _channel, _gpio, _pin, _remap) \ +{ \ + .timer = _timer, \ + .timer_chan = TIM_Channel_##_channel, \ + .pin = { \ + .gpio = GPIO##_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_pin, \ + .GPIO_Mode = GPIO_Mode_IPD, \ + .GPIO_Speed = GPIO_Speed_2MHz,\ + }, \ + .pin_source = GPIO_PinSource##_pin, \ + }, \ + .remap = _remap, \ +} + + +#endif /* PIOS_SERVO_CONFIG_H_ */ diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 63791fa22..19a6061b6 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -53,9 +53,63 @@ void PIOS_SYS_Init(void) /* Init the delay system */ PIOS_DELAY_Init(); - /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | - RCC_APB2Periph_AFIO, ENABLE); + /* + * Turn on all the peripheral clocks. + * Micromanaging clocks makes no sense given the power situation in the system, so + * light up everything we might reasonably use here and just leave it on. + */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | + RCC_AHBPeriph_DMA2, + ENABLE); + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_TIM4 | + RCC_APB1Periph_TIM5 | + RCC_APB1Periph_TIM6 | + RCC_APB1Periph_TIM7 | + RCC_APB1Periph_TIM12 | + RCC_APB1Periph_TIM13 | + RCC_APB1Periph_TIM14 | + RCC_APB1Periph_WWDG | + RCC_APB1Periph_SPI2 | + RCC_APB1Periph_SPI3 | + RCC_APB1Periph_USART2 | + RCC_APB1Periph_USART3 | + RCC_APB1Periph_UART4 | + RCC_APB1Periph_UART5 | + RCC_APB1Periph_I2C1 | + RCC_APB1Periph_I2C2 | + RCC_APB1Periph_USB | + RCC_APB1Periph_CAN1 | + RCC_APB1Periph_CAN2 | + RCC_APB1Periph_BKP | + RCC_APB1Periph_PWR | + RCC_APB1Periph_DAC, + ENABLE); + + + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | + RCC_APB2Periph_GPIOB | + RCC_APB2Periph_GPIOC | + RCC_APB2Periph_GPIOD | + RCC_APB2Periph_GPIOE | + RCC_APB2Periph_ADC1 | + RCC_APB2Periph_ADC2 | + RCC_APB2Periph_ADC3 | + RCC_APB2Periph_TIM1 | + RCC_APB2Periph_TIM8 | + RCC_APB2Periph_TIM9 | + RCC_APB2Periph_TIM10 | + RCC_APB2Periph_TIM11 | + RCC_APB2Periph_TIM15 | + RCC_APB2Periph_TIM16 | + RCC_APB2Periph_TIM17 | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_AFIO, + ENABLE); #if (PIOS_USB_ENABLED) /* Ensure that pull-up is active on detect pin */ diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index 82cd577cd..bc165f38f 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -43,6 +43,7 @@ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback r static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); +static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); const struct pios_com_driver pios_usart_com_driver = { .set_baud = PIOS_USART_ChangeBaud, @@ -51,6 +52,7 @@ const struct pios_com_driver pios_usart_com_driver = { .rx_start = PIOS_USART_RxStart, .bind_tx_cb = PIOS_USART_RegisterTxCallback, .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .ioctl = PIOS_USART_Ioctl, }; enum pios_usart_dev_magic { @@ -67,6 +69,7 @@ struct pios_usart_dev { uint32_t tx_out_context; uint32_t rx_dropped; + uint8_t irq_channel; }; static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) @@ -74,6 +77,30 @@ static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) return usart_dev->magic == PIOS_USART_DEV_MAGIC; } +const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + return usart_dev->cfg; +} + +static int32_t PIOS_USART_SetIrqPrio(struct pios_usart_dev *usart_dev, uint8_t irq_prio) +{ + NVIC_InitTypeDef init = { + .NVIC_IRQChannel = usart_dev->irq_channel, + .NVIC_IRQChannelPreemptionPriority = irq_prio, + .NVIC_IRQChannelCmd = ENABLE, + }; + + NVIC_Init(&init); + + return 0; +} + #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_usart_dev *PIOS_USART_alloc(void) { @@ -155,8 +182,8 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; - /* Copy the comm parameter structure */ - usart_dev->init = cfg->init; + /* Initialize the comm parameter structure */ + USART_StructInit(&usart_dev->init); // 9600 8n1 /* Enable the USART Pins Software Remapping */ if (usart_dev->cfg->remap) { @@ -167,19 +194,6 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); - /* Enable USART clock */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); - break; - case (uint32_t)USART2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); - break; - case (uint32_t)USART3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); - break; - } - /* Configure the USART */ USART_Init(usart_dev->cfg->regs, &usart_dev->init); @@ -189,15 +203,20 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) switch ((uint32_t)usart_dev->cfg->regs) { case (uint32_t)USART1: PIOS_USART_1_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART1_IRQn; break; case (uint32_t)USART2: PIOS_USART_2_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART2_IRQn; break; case (uint32_t)USART3: PIOS_USART_3_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART3_IRQn; break; } - NVIC_Init(&usart_dev->cfg->irq.init); + + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); @@ -430,6 +449,34 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) #endif /* PIOS_INCLUDE_FREERTOS */ } +static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_IRQ_PRIO: + return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); + + case PIOS_IOCTL_USART_GET_RXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->rx; + break; + case PIOS_IOCTL_USART_GET_TXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->tx; + break; + default: + if (usart_dev->cfg->ioctl) { + return usart_dev->cfg->ioctl(usart_id, ctl, param); + } + return -1; + } + + return 0; +} + #endif /* PIOS_INCLUDE_USART */ /** diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 950cbac77..c6f72674d 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -208,7 +208,7 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; - /* Copy the comm parameter structure */ + /* Initialize the comm parameter structure */ USART_StructInit(&usart_dev->init); // 9600 8n1 /* Map pins to USART function */ diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 4222540ef..f44e552f5 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -549,322 +549,59 @@ static const struct pios_tim_clock_cfg tim_4_cfg = { }, }; +#include "pios_servo_config.h" + static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM4, 1, B, 6, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, B, 5, GPIO_PartialRemap_TIM3), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1, 0), }; static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM4, 4, B, 9, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 2, B, 7, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM1, 1, A, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, B, 4, GPIO_PartialRemap_TIM3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2, 0), }; static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM4, 4, B, 9, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 2, B, 7, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM1, 1, A, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, B, 4, GPIO_PartialRemap_TIM3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2, 0), // Receiver port pins // S3-S6 inputs are used as outputs in this case - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1, 0), }; -static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, -}; +static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11, GPIO_PartialRemap2_TIM2); #if defined(PIOS_INCLUDE_USART) #include "pios_usart_priv.h" -static const struct pios_usart_cfg pios_usart_generic_main_cfg = { +// Inverter for SBUS handling +#define MAIN_USART_INVERTER_GPIO GPIOB +#define MAIN_USART_INVERTER_PIN GPIO_Pin_2 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET + +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + +static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOA, .init = { @@ -881,26 +618,11 @@ static const struct pios_usart_cfg pios_usart_generic_main_cfg = { .GPIO_Mode = GPIO_Mode_AF_PP, }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; -static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { +static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOB, .init = { @@ -919,417 +641,6 @@ static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { }, }; -#if defined(PIOS_INCLUDE_DSM) -/* - * Spektrum/JR DSM USART - */ -#include - -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#if defined(PIOS_INCLUDE_HOTT) -/* - * HOTT USART - */ -#include - -static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; -#endif /* PIOS_INCLUDE_HOTT */ - -#if defined(PIOS_INCLUDE_EXBUS) -/* - * EXBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; -#endif /* PIOS_INCLUDE_EXBUS */ - -#if defined(PIOS_INCLUDE_SRXL) -/* - * SRXL USART - */ -#include - -static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -#endif /* PIOS_INCLUDE_SRXL */ - -#if defined(PIOS_INCLUDE_IBUS) -/* - * IBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_ibus_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -#endif /* PIOS_INCLUDE_IBUS */ - -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_clk_func = RCC_APB2PeriphClockCmd, - .gpio_clk_periph = RCC_APB2Periph_GPIOB, - .gpio_inv_enable = Bit_SET, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - - #endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -1653,6 +964,23 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { .vsense_active_low = false }; +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) +{ + switch (board_revision) { + case BOARD_REVISION_CC: + return &pios_usb_main_cfg_cc; + + break; + case BOARD_REVISION_CC3D: + return &pios_usb_main_cfg_cc3d; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; +} + #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" @@ -1665,36 +993,60 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, +/** + * Configuration for MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +#include "pios_mpu6000_config.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line3, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_RCTX) -#include - -const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { - .data_if = 2, - .data_tx_ep = 1, +static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 8 for 1000 Hz + .Smpl_rate_div_no_dlp = 7, + // Clock at 1 khz, downsampled by 1 for 1000 Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 2 }; +#endif /* PIOS_INCLUDE_MPU6000 */ -#endif /* PIOS_INCLUDE_USB_RCTX */ -#if defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c index 20549d1fa..3495976df 100644 --- a/flight/targets/boards/coptercontrol/bootloader/main.c +++ b/flight/targets/boards/coptercontrol/bootloader/main.c @@ -34,6 +34,7 @@ #include #include #include +#include extern void FLASH_Download(); #define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1) diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c index 4fd27d5ee..dda9fa5f5 100644 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -44,6 +44,14 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; + +#if defined(PIOS_INCLUDE_USART) +static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) +{ + return -1; +} +#endif + void PIOS_Board_Init(void) { if (board_init_complete) { @@ -84,7 +92,7 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 7556dff22..fdc6f517f 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -30,12 +30,11 @@ #include #include #include -#include -#include #include #include #include + #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif @@ -43,6 +42,8 @@ #include #endif +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -53,185 +54,59 @@ */ #include "../board_hw_defs.c" -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook(); static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 - -#define PIOS_COM_GPS_RX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 - -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 -uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_telem_rf_id; -uint32_t pios_com_telem_usb_id; -uint32_t pios_com_vcp_id; -uint32_t pios_com_gps_id; -uint32_t pios_com_bridge_id; -uint32_t pios_com_hkosd_id; -uint32_t pios_com_msp_id; -uint32_t pios_com_mavlink_id; -uint32_t pios_usb_rctx_id; - uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id = 0; - -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size = 0 make the port rx only - * rx size = 0 make the port tx only - * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) -{ - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - -static void PIOS_Board_configure_ibus(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_ibus_id; - - if (PIOS_USART_Init(&pios_usart_ibus_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_id; - if (PIOS_IBUS_Init(&pios_ibus_id, &pios_usart_com_driver, pios_usart_ibus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_rcvr_id; - if (PIOS_RCVR_Init(&pios_ibus_rcvr_id, &pios_ibus_rcvr_driver, pios_ibus_id)) { - PIOS_Assert(0); - } - - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS] = pios_ibus_rcvr_id; -} - -/** - * Configuration for MPU6000 chip - */ -#if defined(PIOS_INCLUDE_MPU6000) -#include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" -static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line3, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, +static const PIOS_BOARD_IO_UART_Function main_function_map[] = { + [HWSETTINGS_CC_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_CC_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_CC_MAINPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_CC_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_CC_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_CC_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_CC_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_CC_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 8 for 1000 Hz - .Smpl_rate_div_no_dlp = 7, - // Clock at 1 khz, downsampled by 1 for 1000 Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 2 +static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { + [HWSETTINGS_CC_FLEXIPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_CC_FLEXIPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_CC_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + [HWSETTINGS_CC_FLEXIPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_CC_FLEXIPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_CC_FLEXIPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSETTINGS_CC_FLEXIPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_CC_FLEXIPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_CC_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_CC_FLEXIPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_CC_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_CC_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -#endif /* PIOS_INCLUDE_MPU6000 */ + +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; + } + + return -1; +} + /** * PIOS_Board_Init() @@ -352,368 +227,53 @@ void PIOS_Board_Init(void) PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_flexiport); + + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: -#if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - -#endif /* PIOS_INCLUDE_USB */ - - /* Configure the main IO port */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - uint8_t hwsettings_cc_mainport; - HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); - - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DISABLED: - break; - case HWSETTINGS_CC_MAINPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case HWSETTINGS_CC_MAINPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_MAINPORT_DSM: -#if defined(PIOS_INCLUDE_DSM) - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_CC_MAINPORT_MSP: - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_CC_MAINPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_CC_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, 0, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } - - /* Configure the flexi port */ - uint8_t hwsettings_cc_flexiport; - HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); - - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_CC_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_CC_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_CC_FLEXIPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_FLEXIPORT_PPM: -#if defined(PIOS_INCLUDE_PPM_FLEXI) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM_FLEXI */ - break; - case HWSETTINGS_CC_FLEXIPORT_DSM: -#if defined(PIOS_INCLUDE_DSM) - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); -#endif /* PIOS_INCLUDE_DSM */ - break; - - case HWSETTINGS_CC_FLEXIPORT_HOTTSUMD: - case HWSETTINGS_CC_FLEXIPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - - case HWSETTINGS_CC_FLEXIPORT_EXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; - - case HWSETTINGS_CC_FLEXIPORT_SRXL: -#if defined(PIOS_INCLUDE_SRXL) - { - uint32_t pios_usart_srxl_id; - if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_id; - if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_rcvr_id; - if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; - } -#endif /* PIOS_INCLUDE_SRXL */ - break; - - case HWSETTINGS_CC_FLEXIPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_flexi_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - - case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_FLEXIPORT_I2C: + switch(hwsettings_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } - } #endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_CC_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, 0, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; + break; + case HWSETTINGS_CC_FLEXIPORT_PPM: +#if defined(PIOS_INCLUDE_PPM_FLEXI) + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_flexi_cfg); +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + break; + } + + /* Configure main USART port */ + + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }; + + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + MAIN_USART_INVERTER_DISABLE); + } + + uint8_t hwsettings_mainport; + HwSettingsCC_MainPortGet(&hwsettings_mainport); + + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } /* Configure the rcvr port */ @@ -731,79 +291,32 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) { - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg); - } else { - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - } - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PPM((hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ break; case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_with_ppm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: break; } -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 54e7d2fc8..f01fe9cc8 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -130,36 +130,25 @@ extern uint32_t pios_i2c_flexi_adapter_id; // ------------------------- #define PIOS_COM_MAX_DEVS 3 -extern uint32_t pios_com_telem_rf_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 -#if defined(PIOS_INCLUDE_GPS) -extern uint32_t pios_com_gps_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#endif /* PIOS_INCLUDE_GPS */ +#define PIOS_COM_GPS_RX_BUF_LEN 32 -extern uint32_t pios_com_bridge_id; -#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -extern uint32_t pios_com_vcp_id; -#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 -extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 -extern uint32_t pios_com_msp_id; -#define PIOS_COM_MSP (pios_com_msp_id) - -extern uint32_t pios_com_mavlink_id; -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) +#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 // ------------------------- // ADC diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index a6373ccd3..4b129899d 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -122,34 +122,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ // ------------------------- // Packet Handler diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index fb0f469e8..122cdb71f 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -141,38 +141,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_hott_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_RF (pios_com_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_HOTT (pios_com_hott_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ // ------------------------- // Packet Handler diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 43c55959c..5e1909dfd 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -141,37 +141,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_RF (pios_com_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ - // ------------------------- // Packet Handler // ------------------------- diff --git a/flight/targets/boards/sparky2/pios_board.h b/flight/targets/boards/sparky2/pios_board.h index 63986b0fb..37fe24a12 100644 --- a/flight/targets/boards/sparky2/pios_board.h +++ b/flight/targets/boards/sparky2/pios_board.h @@ -143,36 +143,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_RF (pios_com_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ // ------------------------- // Packet Handler From 5cebebe33b0eabbe3eba8902725bbce5df14760e Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Tue, 21 Feb 2017 18:48:00 +0100 Subject: [PATCH 04/20] LP-480 More work in progress. --- flight/modules/System/systemmod.c | 2 +- flight/pios/common/pios_board_io.c | 82 ++++++++++--------- flight/pios/common/pios_usb_desc_hid_cdc.c | 3 +- flight/pios/inc/pios_board_io.h | 31 ++++++- flight/pios/inc/pios_usb_desc_hid_cdc_priv.h | 6 +- .../discoveryf4bare/firmware/pios_board.c | 12 ++- .../targets/boards/oplinkmini/board_hw_defs.c | 55 ------------- .../boards/revolution/firmware/pios_board.c | 13 ++- .../boards/sparky2/firmware/pios_board.c | 13 ++- shared/uavobjectdefinition/hwsettings.xml | 2 +- 10 files changed, 114 insertions(+), 105 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index a4d87975b..fb59876be 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -62,7 +62,7 @@ #include #include #include - +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 7b742b758..7fa5c327e 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -354,6 +354,17 @@ static const struct uart_function uart_function_map[] = { .com_rx_buf_len = PIOS_COM_HKOSD_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_HKOSD_TX_BUF_LEN, }, +#ifdef PIOS_INCLUDE_DEBUG_CONSOLE + [PIOS_BOARD_IO_UART_DEBUGCONSOLE] = { + .com_id = &pios_com_debug_id, + .com_tx_buf_len = PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, + }, +#endif + [PIOS_BOARD_IO_UART_COMBRIDGE] = { + .com_id = &pios_com_bridge_id, + .com_rx_buf_len = PIOS_COM_BRIDGE_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_BRIDGE_TX_BUF_LEN, + }, #ifdef PIOS_INCLUDE_RCVR # ifdef PIOS_INCLUDE_IBUS [PIOS_BOARD_IO_UART_IBUS] = { @@ -505,19 +516,7 @@ void PIOS_BOARD_IO_Configure_GCSRCVR() #ifdef PIOS_INCLUDE_RFM22B -#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) -static void PIOS_Board_PPM_callback(const int16_t *channels) -{ - OPLinkReceiverData opl_rcvr; - - for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { - opl_rcvr.Channel[i] = (i < RFM22B_PPM_NUM_CHANNELS) ? channels[i] : PIOS_RCVR_TIMEOUT; - } - OPLinkReceiverSet(&opl_rcvr); -} -#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */ - -void PIOS_BOARD_IO_Configure_RFM22B() +void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Function radioaux_function) { #if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); @@ -547,7 +546,6 @@ void PIOS_BOARD_IO_Configure_RFM22B() (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); bool ppm_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) || (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); - bool ppm_only = (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL); bool is_enabled = ((oplinkSettings.Protocol != OPLINKSETTINGS_PROTOCOL_DISABLED) && ((oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) || openlrs)); if (is_enabled) { @@ -556,22 +554,22 @@ void PIOS_BOARD_IO_Configure_RFM22B() uint32_t openlrs_id; const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(pios_board_info_blob.board_rev); - PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg); - { - uint32_t openlrsrcvr_id; - PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); - uint32_t openlrsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS] = openlrsrcvr_rcvr_id; + uint32_t openlrs_id; + PIOS_OpenLRS_Init(&openlrs_id, spi_id, 0, openlrs_cfg); + + uint32_t openlrsrcvr_id; + PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); + + uint32_t openlrsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_OPENLRS_RCVR && PIOS_INCLUDE_RCVR */ } else { /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(pios_board_info_blob.board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { + if (PIOS_RFM22B_Init(&pios_rfm22b_id, spi_id, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { PIOS_Assert(0); } @@ -622,12 +620,6 @@ void PIOS_BOARD_IO_Configure_RFM22B() PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap); PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode); -#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) - /* Set the PPM callback if we should be receiving PPM. */ - if (ppm_mode || (ppm_only && !is_coordinator)) { - PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback); - } -#endif /* Set the modem Tx power level */ switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: @@ -665,22 +657,32 @@ void PIOS_BOARD_IO_Configure_RFM22B() // TODO: this is in preparation for full mavlink support and is used by LP-368 uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN; - uint8_t hwsettings_radioaux; - HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - - switch (hwsettings_radioaux) { - case HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE: - case HWSETTINGS_RADIOAUXSTREAM_DISABLED: - break; - case HWSETTINGS_RADIOAUXSTREAM_MAVLINK: - { + switch (radioaux_function) { + default: ; + case PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE: +#ifdef PIOS_INCLUDE_DEBUG_CONSOLE + if (PIOS_COM_Init(&pios_com_debug_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, + 0, 0, + 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } +#endif + break; + case PIOS_BOARD_IO_RADIOAUX_COMBRIDGE: + if(PIOS_COM_Init(&pios_com_bridge_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, + 0, PIOS_COM_BRIDGE_RX_BUF_LEN, + 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) + { + PIOS_Assert(0); + } + break; + case PIOS_BOARD_IO_RADIOAUX_MAVLINK: if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, 0, mavlink_rx_size, 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } - } } } else { oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index 14bb69f61..255bf6dce 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -331,11 +331,12 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { .data_rx_ep = 1, .data_tx_ep = 1, }; - +#ifdef PIOS_INCLUDE_USB_RCTX const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { .data_if = 2, .data_tx_ep = 1, }; +#endif int32_t PIOS_USB_DESC_HID_CDC_Init(void) { diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index bbcd5c2cc..6b374deba 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -98,7 +98,8 @@ extern uint32_t pios_com_telem_rf_id; /* RFM22B telemetry */ #ifdef PIOS_INCLUDE_RFM22B -extern uint32_t pios_com_rf_id; +extern uint32_t pios_rfm22b_id; /* RFM22B handle */ +extern uint32_t pios_com_rf_id; /* oplink telemetry */ # define PIOS_COM_RF (pios_com_rf_id) # ifndef PIOS_COM_RFM22B_RF_RX_BUF_LEN # define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 @@ -179,10 +180,34 @@ typedef enum { PIOS_BOARD_IO_UART_SRXL, /* rcvr */ PIOS_BOARD_IO_UART_IBUS, /* rcvr */ PIOS_BOARD_IO_UART_EXBUS, /* rcvr */ +// PIOS_BOARD_IO_UART_FRSKY_SPORT_TELEMETRY, /* com */ } PIOS_BOARD_IO_UART_Function; +typedef enum { + PIOS_BOARD_IO_RADIOAUX_NONE, + PIOS_BOARD_IO_RADIOAUX_MAVLINK, + PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, + PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, +// PIOS_BOARD_IO_RADIOAUX_FRSKY_SPORT_TELEMETRY, +} PIOS_BOARD_IO_RADIOAUX_Function; + +typedef enum { + PIOS_BOARD_IO_USB_HID_NONE, + PIOS_BOARD_IO_USB_HID_TELEMETRY, + PIOS_BOARD_IO_USB_HID_RCTX, +} PIOS_BOARD_IO_USB_HID_Function; + +typedef enum { + PIOS_BOARD_IO_USB_CDC_NONE, + PIOS_BOARD_IO_USB_CDC_TELEMETRY, + PIOS_BOARD_IO_USB_CDC_COMBRIDGE, + PIOS_BOARD_IO_USB_CDC_MAVLINK, + PIOS_BOARD_IO_USB_CDC_DEBUGCONSOLE, +// PIOS_BOARD_IO_USB_CDC_MSP, +} PIOS_BOARD_IO_USB_CDC_Function; + #ifdef PIOS_INCLUDE_USB -void PIOS_BOARD_IO_Configure_USB(); +void PIOS_BOARD_IO_Configure_USB(PIOS_BOARD_IO_USB_HID_Function hid_function, PIOS_BOARD_IO_USB_CDC_Function cdc_function); # if defined(PIOS_INCLUDE_USB_HID) # include extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; @@ -198,7 +223,7 @@ void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg); void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *usart_cfg, PIOS_BOARD_IO_UART_Function function); #ifdef PIOS_INCLUDE_RFM22B -void PIOS_BOARD_IO_Configure_RFM22B(); +void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Function function); #endif #ifdef PIOS_INCLUDE_I2C diff --git a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h index 77d3b2347..1ff5c1298 100644 --- a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h @@ -33,13 +33,17 @@ #include #include #include -#include +#ifdef PIOS_INCLUDE_USB_RCTX +# include +#endif extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); extern const struct pios_usb_cdc_cfg pios_usb_cdc_cfg; extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; +#ifdef PIOS_INCLUDE_USB_RCTX extern const struct pios_usb_rctx_cfg pios_usb_rctx_cfg; +#endif #endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index bac459834..7e8c49c52 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -98,6 +98,11 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; +static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { + [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, + [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, + [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, +}; int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { @@ -280,7 +285,12 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_RFM22B) - PIOS_BOARD_IO_Configure_RFM22B(); + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); + + if(hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { + PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); + } #endif /* PIOS_INCLUDE_RFM22B */ diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index 3dde19b94..d05d3dbfb 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -693,22 +693,6 @@ const struct pios_ppm_out_cfg pios_flexi_ppm_out_cfg = { */ static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOA, .init = { @@ -729,22 +713,6 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOB, .init = { @@ -839,29 +807,6 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ - #if defined(PIOS_INCLUDE_FLASH_EEPROM) #include diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index cc1f93ddf..60bdfce81 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -97,6 +97,12 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; +static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { + [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, + [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, + [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, +}; + int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { @@ -282,7 +288,12 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_RFM22B) - PIOS_BOARD_IO_Configure_RFM22B(); + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); + + if(hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { + PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); + } #endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) diff --git a/flight/targets/boards/sparky2/firmware/pios_board.c b/flight/targets/boards/sparky2/firmware/pios_board.c index 1c0561dcb..28e232e36 100644 --- a/flight/targets/boards/sparky2/firmware/pios_board.c +++ b/flight/targets/boards/sparky2/firmware/pios_board.c @@ -93,6 +93,12 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_SPK2_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; +static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { + [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, + [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, + [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, +}; + int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); @@ -258,7 +264,12 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_RFM22B) - PIOS_BOARD_IO_Configure_RFM22B(); + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); + + if(hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { + PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); + } #endif /* PIOS_INCLUDE_RFM22B */ /* Initialize inverter gpio and set it to off */ diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 6f74f8425..0f4aa1b75 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -29,7 +29,7 @@ - + From c9f6ba55a0e68919378ee9ceb1fbe0697d8f576c Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Thu, 13 Apr 2017 15:12:40 +0200 Subject: [PATCH 05/20] LP-480 USB config cleanup. Coptercontrol & Revo tested. Booted. Usb functional. --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- .../modules/UAVOHottBridge/uavohottbridge.c | 2 + flight/pios/common/pios_board_io.c | 160 ++++++++---------- flight/pios/inc/pios_board_io.h | 38 ++--- flight/pios/stm32f10x/pios_sys.c | 4 +- .../boards/coptercontrol/board_hw_defs.c | 10 -- .../coptercontrol/bootloader/pios_board.c | 4 + .../boards/revolution/bootloader/main.c | 1 + .../targets/boards/revonano/bootloader/main.c | 1 + 9 files changed, 98 insertions(+), 124 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 1bdc89da6..7e01ae919 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_coptercontrol; + buildArgumentsString = ef_revolution; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c index 9342e0eab..663045d08 100644 --- a/flight/modules/UAVOHottBridge/uavohottbridge.c +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -55,6 +55,8 @@ #include "pios_sensors.h" #include "uavohottbridge.h" +#include "pios_board_io.h" + #if defined(PIOS_INCLUDE_HOTT_BRIDGE) #if defined(PIOS_HoTT_STACK_SIZE) diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 7fa5c327e..0848531fb 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -105,6 +105,10 @@ uint32_t pios_com_vcp_id; /* USB VCP */ uint32_t pios_com_debug_id; /* DebugConsole */ #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#ifdef PIOS_INCLUDE_HOTT_BRIDGE +uint32_t pios_com_hott_id; +#endif + #ifdef PIOS_INCLUDE_USB uint32_t pios_com_telem_usb_id; /* USB telemetry */ @@ -134,105 +138,83 @@ pios_hmc5x83_dev_t pios_hmc5x83_internal_id; #endif /* PIOS_INCLUDE_COM_MSG */ +#if defined(PIOS_INCLUDE_USB_CDC) +static void PIOS_BOARD_IO_VCP_Init(uint32_t *com_id, uint16_t rx_buf_len, uint16_t tx_buf_len, uint32_t pios_usb_id) +{ +#if defined(PIOS_INCLUDE_COM) + uint32_t pios_usb_cdc_id = 0; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_Init(com_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + 0, rx_buf_len, /* Let Init() allocate this buffer */ + 0, tx_buf_len)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_COM */ +} +#endif + +static void PIOS_BOARD_IO_HID_Init(uint32_t *com_id, uint16_t rx_buf_len, uint16_t tx_buf_len, uint32_t pios_usb_id) +{ +#ifdef PIOS_INCLUDE_USB_CDC + const struct pios_usb_hid_cfg *hid_cfg = &pios_usb_hid_cfg; +#else + const struct pios_usb_hid_cfg *hid_cfg = &pios_usb_hid_only_cfg; +#endif + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + + if (PIOS_COM_Init(com_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + 0, rx_buf_len, /* Let Init() allocate this buffer */ + 0, tx_buf_len)) { /* Let Init() allocate this buffer */ + PIOS_Assert(0); + } +} + void PIOS_BOARD_IO_Configure_USB() { /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); #if defined(PIOS_INCLUDE_USB_CDC) + uint8_t hwsettings_usb_vcpport; + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } - usb_hid_present = true; - usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } - usb_hid_present = true; #endif uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(pios_board_info_blob.board_rev)); - uint8_t hwsettings_usb_vcpport; - uint8_t hwsettings_usb_hidport; - - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - #if defined(PIOS_INCLUDE_USB_CDC) - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - /* Why do we need to init these if function is *NONE* ?? */ -#if defined(PIOS_INCLUDE_USB_CDC) - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } -#else - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { - PIOS_Assert(0); - } -#endif - switch (hwsettings_usb_vcpport) { case HWSETTINGS_USB_VCPPORT_DISABLED: break; case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - 0, PIOS_COM_TELEM_USB_RX_BUF_LEN, /* Let Init() allocate this buffer */ - 0, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_COM */ + PIOS_BOARD_IO_VCP_Init(&pios_com_telem_usb_id, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, pios_usb_id); break; case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - 0, PIOS_COM_BRIDGE_RX_BUF_LEN, /* Let Init() allocate this buffer */ - 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_COM */ + PIOS_BOARD_IO_VCP_Init(&pios_com_vcp_id, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, pios_usb_id); break; case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - 0, 0, /* No RX buffer */ - 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ - PIOS_Assert(0); - } + PIOS_BOARD_IO_VCP_Init(&pios_com_debug_id, 0, PIOS_COM_BRIDGE_TX_BUF_LEN, pios_usb_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_VCPPORT_MAVLINK: - if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - 0, PIOS_COM_MAVLINK_RX_BUF_LEN, /* Let Init() allocate this buffer */ - 0, PIOS_COM_MAVLINK_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ - PIOS_Assert(0); - } + PIOS_BOARD_IO_VCP_Init(&pios_com_mavlink_id, PIOS_COM_MAVLINK_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, pios_usb_id); break; } #endif /* PIOS_INCLUDE_USB_CDC */ @@ -244,19 +226,11 @@ void PIOS_BOARD_IO_Configure_USB() case HWSETTINGS_USB_HIDPORT_DISABLED: break; case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - 0, PIOS_COM_TELEM_USB_RX_BUF_LEN, /* Let Init() allocate this buffer */ - 0, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { /* Let Init() allocate this buffer */ - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ + PIOS_BOARD_IO_HID_Init(&pios_com_telem_usb_id, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, pios_usb_id); break; case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { /* this will reinstall endpoint callbacks */ + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_USB_RCTX */ @@ -266,9 +240,7 @@ void PIOS_BOARD_IO_Configure_USB() #endif /* PIOS_INCLUDE_USB_HID */ #ifndef STM32F10X - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } + PIOS_USBHOOK_Activate(); #endif } @@ -354,6 +326,13 @@ static const struct uart_function uart_function_map[] = { .com_rx_buf_len = PIOS_COM_HKOSD_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_HKOSD_TX_BUF_LEN, }, +#ifdef PIOS_INCLUDE_HOTT_BRIDGE + [PIOS_BOARD_IO_UART_HOTT_BRIDGE] = { + .com_id = &pios_com_hott_id, + .com_rx_buf_len = PIOS_COM_HOTT_BRIDGE_RX_BUF_LEN, + .com_tx_buf_len = PIOS_COM_HOTT_BRIDGE_TX_BUF_LEN, + }, +#endif #ifdef PIOS_INCLUDE_DEBUG_CONSOLE [PIOS_BOARD_IO_UART_DEBUGCONSOLE] = { .com_id = &pios_com_debug_id, @@ -554,7 +533,6 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func uint32_t openlrs_id; const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(pios_board_info_blob.board_rev); - uint32_t openlrs_id; PIOS_OpenLRS_Init(&openlrs_id, spi_id, 0, openlrs_cfg); uint32_t openlrsrcvr_id; @@ -564,6 +542,7 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { PIOS_Assert(0); } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS] = openlrsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_OPENLRS_RCVR && PIOS_INCLUDE_RCVR */ } else { /* Configure the RFM22B device. */ @@ -572,6 +551,15 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func if (PIOS_RFM22B_Init(&pios_rfm22b_id, spi_id, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { PIOS_Assert(0); } +#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) + uint32_t pios_oplinkrcvr_id; + PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id); + uint32_t pios_oplinkrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */ /* Configure the radio com interface */ if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, @@ -689,16 +677,6 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func } OPLinkStatusSet(&oplinkStatus); - -#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) - uint32_t pios_oplinkrcvr_id; - PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id); - uint32_t pios_oplinkrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */ } #endif /* ifdef PIOS_INCLUDE_RFM22B */ diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 6b374deba..57dd3736e 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -140,6 +140,18 @@ extern uint32_t pios_com_mavlink_id; # define PIOS_COM_MAVLINK_RX_BUF_LEN 128 #endif +/* HoTT Telemetry */ +#ifdef PIOS_INCLUDE_HOTT_BRIDGE +# ifndef PIOS_COM_HOTT_BRIDGE_RX_BUF_LEN +# define PIOS_COM_HOTT_BRIDGE_RX_BUF_LEN 512 +# endif +# ifndef PIOS_COM_HOTT_BRIDGE_TX_BUF_LEN +# define PIOS_COM_HOTT_BRIDGE_TX_BUF_LEN 512 +# endif +extern uint32_t pios_com_hott_id; +# define PIOS_COM_HOTT (pios_com_hott_id) +#endif + /* USB VCP */ extern uint32_t pios_com_vcp_id; #define PIOS_COM_VCP (pios_com_vcp_id) @@ -181,6 +193,7 @@ typedef enum { PIOS_BOARD_IO_UART_IBUS, /* rcvr */ PIOS_BOARD_IO_UART_EXBUS, /* rcvr */ // PIOS_BOARD_IO_UART_FRSKY_SPORT_TELEMETRY, /* com */ + PIOS_BOARD_IO_UART_HOTT_BRIDGE, /* com */ } PIOS_BOARD_IO_UART_Function; typedef enum { @@ -191,27 +204,12 @@ typedef enum { // PIOS_BOARD_IO_RADIOAUX_FRSKY_SPORT_TELEMETRY, } PIOS_BOARD_IO_RADIOAUX_Function; -typedef enum { - PIOS_BOARD_IO_USB_HID_NONE, - PIOS_BOARD_IO_USB_HID_TELEMETRY, - PIOS_BOARD_IO_USB_HID_RCTX, -} PIOS_BOARD_IO_USB_HID_Function; - -typedef enum { - PIOS_BOARD_IO_USB_CDC_NONE, - PIOS_BOARD_IO_USB_CDC_TELEMETRY, - PIOS_BOARD_IO_USB_CDC_COMBRIDGE, - PIOS_BOARD_IO_USB_CDC_MAVLINK, - PIOS_BOARD_IO_USB_CDC_DEBUGCONSOLE, -// PIOS_BOARD_IO_USB_CDC_MSP, -} PIOS_BOARD_IO_USB_CDC_Function; - #ifdef PIOS_INCLUDE_USB -void PIOS_BOARD_IO_Configure_USB(PIOS_BOARD_IO_USB_HID_Function hid_function, PIOS_BOARD_IO_USB_CDC_Function cdc_function); -# if defined(PIOS_INCLUDE_USB_HID) -# include -extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; -# endif /* PIOS_INCLUDE_USB_HID */ +void PIOS_BOARD_IO_Configure_USB(); +//# if defined(PIOS_INCLUDE_USB_HID) +//# include +//extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; +//# endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ #ifdef PIOS_INCLUDE_PWM void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 19a6061b6..4644293b2 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -82,8 +82,8 @@ void PIOS_SYS_Init(void) RCC_APB1Periph_I2C1 | RCC_APB1Periph_I2C2 | RCC_APB1Periph_USB | - RCC_APB1Periph_CAN1 | - RCC_APB1Periph_CAN2 | +// RCC_APB1Periph_CAN1 | /* bxCAN unfortunately interferes with USB */ +// RCC_APB1Periph_CAN2 | RCC_APB1Periph_BKP | RCC_APB1Periph_PWR | RCC_APB1Periph_DAC, diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index f44e552f5..d4d97f589 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -981,18 +981,8 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) return NULL; } -#include "pios_usb_board_data_priv.h" -#include "pios_usb_desc_hid_cdc_priv.h" -#include "pios_usb_desc_hid_only_priv.h" - #endif /* PIOS_INCLUDE_USB */ -#if defined(PIOS_INCLUDE_COM_MSG) - -#include - -#endif /* PIOS_INCLUDE_COM_MSG */ - /** * Configuration for MPU6000 chip */ diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c index dda9fa5f5..466566d55 100644 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -36,6 +36,10 @@ */ #include "../board_hw_defs.c" +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#include + uint32_t pios_com_telem_usb_id; /** diff --git a/flight/targets/boards/revolution/bootloader/main.c b/flight/targets/boards/revolution/bootloader/main.c index 09a54a094..aa5c7999a 100644 --- a/flight/targets/boards/revolution/bootloader/main.c +++ b/flight/targets/boards/revolution/bootloader/main.c @@ -35,6 +35,7 @@ #include /* PIOS_USBHOOK_* */ #include #include +#include extern void FLASH_Download(); void check_bor(); diff --git a/flight/targets/boards/revonano/bootloader/main.c b/flight/targets/boards/revonano/bootloader/main.c index 09a54a094..aa5c7999a 100644 --- a/flight/targets/boards/revonano/bootloader/main.c +++ b/flight/targets/boards/revonano/bootloader/main.c @@ -35,6 +35,7 @@ #include /* PIOS_USBHOOK_* */ #include #include +#include extern void FLASH_Download(); void check_bor(); From 9eb1f3b5e5293ac4fde2b9ccd8a60bb15b03371c Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 19 Apr 2017 15:58:31 +0200 Subject: [PATCH 06/20] LP-480 Moved sensors initialization to pios_board_sensors.c --- flight/make/apps-defs.mk | 1 + flight/pios/common/pios_board_io.c | 177 +++-------------- flight/pios/common/pios_board_sensors.c | 187 ++++++++++++++++++ flight/pios/common/pios_usb_desc_hid_cdc.c | 2 +- flight/pios/inc/pios_board_hw.h | 6 + flight/pios/inc/pios_board_io.h | 38 ++-- flight/pios/inc/pios_board_sensors.h | 33 ++++ flight/pios/stm32f10x/inc/pios_servo_config.h | 28 +-- flight/pios/stm32f10x/pios_sys.c | 10 +- flight/pios/stm32f10x/pios_usart.c | 56 +++--- flight/pios/stm32f4xx/pios_usart.c | 2 +- .../boards/coptercontrol/board_hw_defs.c | 59 +++--- .../coptercontrol/firmware/pios_board.c | 82 ++++---- .../targets/boards/coptercontrol/pios_board.h | 10 +- .../boards/discoveryf4bare/board_hw_defs.c | 75 +++---- .../boards/discoveryf4bare/bootloader/main.c | 1 + .../discoveryf4bare/firmware/pios_board.c | 138 +++++++------ .../boards/discoveryf4bare/pios_board.h | 31 +-- .../targets/boards/oplinkmini/board_hw_defs.c | 8 +- .../targets/boards/revolution/board_hw_defs.c | 51 ++--- .../boards/revolution/firmware/pios_board.c | 29 ++- flight/targets/boards/revolution/pios_board.h | 34 ++-- .../targets/boards/revonano/board_hw_defs.c | 89 +++++---- .../boards/revonano/firmware/pios_board.c | 66 +++---- flight/targets/boards/revonano/pios_board.h | 34 ++-- flight/targets/boards/sparky2/board_hw_defs.c | 59 +++--- .../boards/sparky2/firmware/pios_board.c | 91 ++++----- flight/targets/boards/sparky2/pios_board.h | 35 ++-- 28 files changed, 766 insertions(+), 666 deletions(-) create mode 100644 flight/pios/common/pios_board_sensors.c create mode 100644 flight/pios/inc/pios_board_sensors.h diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index 32623ef69..7b661bb8e 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -101,6 +101,7 @@ SRC += $(PIOSCOMMON)/pios_servo.c SRC += $(PIOSCOMMON)/pios_openlrs.c SRC += $(PIOSCOMMON)/pios_openlrs_rcvr.c SRC += $(PIOSCOMMON)/pios_board_io.c +SRC += $(PIOSCOMMON)/pios_board_sensors.c ## Misc library functions SRC += $(FLIGHTLIB)/sanitycheck.c diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 0848531fb..6ee788051 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -67,21 +67,12 @@ # endif /* PIOS_INCLUDE_RCVR */ #endif /* PIOS_INCLUDE_RFM22B */ -#ifdef PIOS_INCLUDE_ADC -# include -#endif - -#ifdef PIOS_INCLUDE_MS5611 -# include -#endif - #ifdef PIOS_INCLUDE_RCVR # include "manualcontrolsettings.h" uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; /* Receivers */ #endif /* PIOS_INCLUDE_RCVR */ #include "hwsettings.h" -#include "auxmagsettings.h" #include "gcsreceiver.h" #ifdef PIOS_INCLUDE_GPS @@ -118,14 +109,6 @@ uint32_t pios_com_telem_usb_id; /* USB telemetry */ uint32_t pios_usb_rctx_id; #endif -#if defined(PIOS_INCLUDE_HMC5X83) -# include "pios_hmc5x83.h" -# if defined(PIOS_HMC5X83_HAS_GPIOS) -pios_hmc5x83_dev_t pios_hmc5x83_internal_id; -# endif -#endif - - #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" @@ -185,7 +168,7 @@ void PIOS_BOARD_IO_Configure_USB() #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hwsettings_usb_vcpport; HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - + if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } @@ -297,38 +280,38 @@ struct uart_function { }; static const struct uart_function uart_function_map[] = { - [PIOS_BOARD_IO_UART_TELEMETRY] = { + [PIOS_BOARD_IO_UART_TELEMETRY] = { .com_id = &pios_com_telem_rf_id, .com_rx_buf_len = PIOS_COM_TELEM_RF_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_TELEM_RF_TX_BUF_LEN, }, - [PIOS_BOARD_IO_UART_MAVLINK] = { + [PIOS_BOARD_IO_UART_MAVLINK] = { .com_id = &pios_com_mavlink_id, .com_rx_buf_len = PIOS_COM_MAVLINK_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_MAVLINK_TX_BUF_LEN, }, - [PIOS_BOARD_IO_UART_MSP] = { + [PIOS_BOARD_IO_UART_MSP] = { .com_id = &pios_com_msp_id, .com_rx_buf_len = PIOS_COM_MSP_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_MSP_TX_BUF_LEN, }, - [PIOS_BOARD_IO_UART_GPS] = { + [PIOS_BOARD_IO_UART_GPS] = { .com_id = &pios_com_gps_id, .com_rx_buf_len = PIOS_COM_GPS_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_GPS_TX_BUF_LEN, }, - [PIOS_BOARD_IO_UART_OSDHK] = { + [PIOS_BOARD_IO_UART_OSDHK] = { .com_id = &pios_com_hkosd_id, .com_rx_buf_len = PIOS_COM_HKOSD_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_HKOSD_TX_BUF_LEN, }, #ifdef PIOS_INCLUDE_HOTT_BRIDGE - [PIOS_BOARD_IO_UART_HOTT_BRIDGE] = { - .com_id = &pios_com_hott_id, + [PIOS_BOARD_IO_UART_HOTT_BRIDGE] = { + .com_id = &pios_com_hott_id, .com_rx_buf_len = PIOS_COM_HOTT_BRIDGE_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_HOTT_BRIDGE_TX_BUF_LEN, }, @@ -339,67 +322,67 @@ static const struct uart_function uart_function_map[] = { .com_tx_buf_len = PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, }, #endif - [PIOS_BOARD_IO_UART_COMBRIDGE] = { + [PIOS_BOARD_IO_UART_COMBRIDGE] = { .com_id = &pios_com_bridge_id, .com_rx_buf_len = PIOS_COM_BRIDGE_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_BRIDGE_TX_BUF_LEN, }, #ifdef PIOS_INCLUDE_RCVR # ifdef PIOS_INCLUDE_IBUS - [PIOS_BOARD_IO_UART_IBUS] = { + [PIOS_BOARD_IO_UART_IBUS] = { .rcvr_init = &PIOS_IBUS_Init, .rcvr_driver = &pios_ibus_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS, }, # endif /* PIOS_INCLUDE_IBUS */ # ifdef PIOS_INCLUDE_EXBUS - [PIOS_BOARD_IO_UART_EXBUS] = { + [PIOS_BOARD_IO_UART_EXBUS] = { .rcvr_init = &PIOS_EXBUS_Init, .rcvr_driver = &pios_exbus_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS, }, # endif /* PIOS_INCLUDE_EXBUS */ # ifdef PIOS_INCLUDE_SRXL - [PIOS_BOARD_IO_UART_SRXL] = { + [PIOS_BOARD_IO_UART_SRXL] = { .rcvr_init = &PIOS_SRXL_Init, .rcvr_driver = &pios_srxl_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL, }, # endif /* PIOS_INCLUDE_SRXL */ # ifdef PIOS_INCLUDE_HOTT - [PIOS_BOARD_IO_UART_HOTT_SUMD] = { + [PIOS_BOARD_IO_UART_HOTT_SUMD] = { .rcvr_init = &PIOS_HOTT_Init_SUMD, .rcvr_driver = &pios_hott_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT, }, - [PIOS_BOARD_IO_UART_HOTT_SUMH] = { + [PIOS_BOARD_IO_UART_HOTT_SUMH] = { .rcvr_init = &PIOS_HOTT_Init_SUMH, .rcvr_driver = &pios_hott_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT, }, # endif /* PIOS_INCLUDE_HOTT */ # ifdef PIOS_INCLUDE_DSM - [PIOS_BOARD_IO_UART_DSM_MAIN] = { + [PIOS_BOARD_IO_UART_DSM_MAIN] = { .rcvr_init = &PIOS_DSM_Init_Helper, .rcvr_driver = &pios_dsm_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, }, - [PIOS_BOARD_IO_UART_DSM_FLEXI] = { + [PIOS_BOARD_IO_UART_DSM_FLEXI] = { .rcvr_init = &PIOS_DSM_Init_Helper, .rcvr_driver = &pios_dsm_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, }, - [PIOS_BOARD_IO_UART_DSM_RCVR] = { + [PIOS_BOARD_IO_UART_DSM_RCVR] = { .rcvr_init = &PIOS_DSM_Init_Helper, .rcvr_driver = &pios_dsm_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT, }, # endif /* PIOS_INCLUDE_DSM */ # ifdef PIOS_INCLUDE_SBUS - [PIOS_BOARD_IO_UART_SBUS] = { + [PIOS_BOARD_IO_UART_SBUS] = { .rcvr_init = &PIOS_SBus_Init_Helper, .rcvr_driver = &pios_sbus_rcvr_driver, .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS, @@ -495,7 +478,7 @@ void PIOS_BOARD_IO_Configure_GCSRCVR() #ifdef PIOS_INCLUDE_RFM22B -void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Function radioaux_function) +void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_function) { #if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); @@ -533,11 +516,11 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func uint32_t openlrs_id; const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(pios_board_info_blob.board_rev); - PIOS_OpenLRS_Init(&openlrs_id, spi_id, 0, openlrs_cfg); + PIOS_OpenLRS_Init(&openlrs_id, PIOS_SPI_RFM22B_ADAPTER, 0, openlrs_cfg); uint32_t openlrsrcvr_id; PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); - + uint32_t openlrsrcvr_rcvr_id; if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { PIOS_Assert(0); @@ -548,7 +531,7 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(pios_board_info_blob.board_rev); - if (PIOS_RFM22B_Init(&pios_rfm22b_id, spi_id, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_SPI_RFM22B_ADAPTER, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { PIOS_Assert(0); } #if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR) @@ -646,7 +629,7 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN; switch (radioaux_function) { - default: ; + default:; case PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE: #ifdef PIOS_INCLUDE_DEBUG_CONSOLE if (PIOS_COM_Init(&pios_com_debug_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, @@ -655,15 +638,14 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func PIOS_Assert(0); } #endif - break; + break; case PIOS_BOARD_IO_RADIOAUX_COMBRIDGE: - if(PIOS_COM_Init(&pios_com_bridge_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - 0, PIOS_COM_BRIDGE_RX_BUF_LEN, - 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) - { + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, + 0, PIOS_COM_BRIDGE_RX_BUF_LEN, + 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } - break; + break; case PIOS_BOARD_IO_RADIOAUX_MAVLINK: if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, 0, mavlink_rx_size, @@ -679,106 +661,3 @@ void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Func OPLinkStatusSet(&oplinkStatus); } #endif /* ifdef PIOS_INCLUDE_RFM22B */ - -#ifdef PIOS_INCLUDE_I2C -void PIOS_BOARD_IO_Configure_I2C(uint32_t i2c_internal_id, uint32_t i2c_external_id) -{ - // internal HMC5x83 mag -# ifdef PIOS_INCLUDE_HMC5X83_INTERNAL - const struct pios_hmc5x83_cfg *hmc5x83_internal_cfg = PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(pios_board_info_blob.board_rev); - - if (hmc5x83_internal_cfg) { - // attach the 5x83 mag to internal i2c bus - - pios_hmc5x83_dev_t internal_mag = PIOS_HMC5x83_Init(hmc5x83_internal_cfg, i2c_internal_id, 0); -# ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -# endif /* PIOS_INCLUDE_WDG */ - -#ifdef PIOS_HMC5X83_HAS_GPIOS - pios_hmc5x83_internal_id = internal_mag; -#endif - // add this sensor to the sensor task's list - PIOS_HMC5x83_Register(internal_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); - } - -# endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */ - - // internal ms5611 baro -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(PIOS_BOARD_HW_DEFS_GetMS5611Cfg(pios_board_info_blob.board_rev), i2c_internal_id); - PIOS_MS5611_Register(); -#endif - - // internal bmp280 baro - // internal MPU6000 imu (i2c) - // internal eeprom (revo nano) - NOT HERE, should be initialized earlier - - - /* Initialize external sensors */ - - // aux mag -# ifdef PIOS_INCLUDE_HMC5X83 - AuxMagSettingsInitialize(); - - AuxMagSettingsTypeOptions option; - AuxMagSettingsTypeGet(&option); - - const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev); - - if (hmc5x83_external_cfg) { - uint32_t i2c_id = 0; - - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - // i2c_external - i2c_id = i2c_external_id; - } else if (option == AUXMAGSETTINGS_TYPE_I2C) { - // i2c_internal (or Sparky2/F3 dedicated I2C port) - i2c_id = i2c_internal_id; - } - - if (i2c_id) { - uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, i2c_id, 0); -# ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -# endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor - // and before registering some other fast and important sensor - // as that would cause delay and time jitter for the second fast sensor - PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } - } -# endif /* PIOS_INCLUDE_HMC5X83 */ - - // external ETASV3 Eagletree Airspeed v3 - // external MS4525D PixHawk Airpeed based on MS4525DO - - // BMA180 accelerometer? - // bmp085/bmp180 baro - - // hmc5843 mag - // i2c esc (?) - // UBX DCC -} -#endif /* ifdef PIOS_INCLUDE_I2C */ - -#ifdef PIOS_INCLUDE_ADC -void PIOS_BOARD_IO_Configure_ADC() -{ - PIOS_ADC_Init(PIOS_BOARD_HW_DEFS_GetAdcCfg(pios_board_info_blob.board_rev)); - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); - } - } -} -#endif /* PIOS_INCLUDE_ADC */ diff --git a/flight/pios/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c new file mode 100644 index 000000000..fecfe17dd --- /dev/null +++ b/flight/pios/common/pios_board_sensors.c @@ -0,0 +1,187 @@ +/** + ****************************************************************************** + * + * @file pios_board_sensors.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief board sensors setup + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "pios_board_info.h" +#include "pios_board_sensors.h" +#include "pios_board_hw.h" + +#include "uavobjectmanager.h" +#include "auxmagsettings.h" +#include "hwsettings.h" +#include + +#ifdef PIOS_INCLUDE_MPU6000 +# include +# include +#endif + +#ifdef PIOS_INCLUDE_MS5611 +# include +#endif + +#if defined(PIOS_INCLUDE_ADXL345) +# include +#endif + +#ifdef PIOS_INCLUDE_MPU9250 +# include +# include +#endif + +#if defined(PIOS_INCLUDE_HMC5X83) +# include "pios_hmc5x83.h" +# if defined(PIOS_HMC5X83_HAS_GPIOS) +pios_hmc5x83_dev_t pios_hmc5x83_internal_id; +# endif +#endif + +#ifdef PIOS_INCLUDE_ADC +# include "pios_adc_priv.h" +#endif + +void PIOS_BOARD_Sensors_Configure() +{ +#ifdef PIOS_INCLUDE_MPU6000 + const struct pios_mpu6000_cfg *mpu6000_cfg = PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(pios_board_info_blob.board_rev); + if (mpu6000_cfg) { + PIOS_MPU6000_Init(PIOS_SPI_MPU6000_ADAPTER, 0, mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + PIOS_MPU6000_Register(); +#endif + } +#endif /* PIOS_INCLUDE_MPU6000 */ + +#ifdef PIOS_INCLUDE_ADXL345 + PIOS_ADXL345_Init(PIOS_SPI_ADXL345_ADAPTER, 0); +#endif + +#ifdef PIOS_INCLUDE_MPU9250 + const struct pios_mpu9250_cfg *mpu9250_cfg = PIOS_BOARD_HW_DEFS_GetMPU9250Cfg(pios_board_info_blob.board_rev); + if (mpu9250_cfg) { + PIOS_MPU9250_Init(PIOS_SPI_MPU9250_ADAPTER, 0, mpu9250_cfg); + PIOS_MPU9250_CONFIG_Configure(); + PIOS_MPU9250_MainRegister(); + PIOS_MPU9250_MagRegister(); + } +#endif /* PIOS_INCLUDE_MPU9250 */ + + + // internal HMC5x83 mag +# ifdef PIOS_INCLUDE_HMC5X83_INTERNAL + const struct pios_hmc5x83_cfg *hmc5x83_internal_cfg = PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(pios_board_info_blob.board_rev); + + if (hmc5x83_internal_cfg) { + // attach the 5x83 mag to internal i2c bus + + pios_hmc5x83_dev_t internal_mag = PIOS_HMC5x83_Init(hmc5x83_internal_cfg, PIOS_I2C_HMC5X83_INTERNAL_ADAPTER, 0); +# ifdef PIOS_INCLUDE_WDG + // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed + // this is not in a loop, so it is safe + PIOS_WDG_Clear(); +# endif /* PIOS_INCLUDE_WDG */ + +#ifdef PIOS_HMC5X83_HAS_GPIOS + pios_hmc5x83_internal_id = internal_mag; +#endif + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(internal_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); + } + +# endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */ + +# ifdef PIOS_INCLUDE_HMC5X83 + AuxMagSettingsInitialize(); + + AuxMagSettingsTypeOptions option; + AuxMagSettingsTypeGet(&option); + + const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev); + + if (hmc5x83_external_cfg) { + uint32_t i2c_id = 0; + + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // i2c_external + i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; + } else if (option == AUXMAGSETTINGS_TYPE_I2C) { + // i2c_internal (or Sparky2/F3 dedicated I2C port) + i2c_id = PIOS_I2C_FLEXI_ADAPTER; + } + + if (i2c_id) { + uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, i2c_id, 0); +# ifdef PIOS_INCLUDE_WDG + // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed + // this is not in a loop, so it is safe + PIOS_WDG_Clear(); +# endif /* PIOS_INCLUDE_WDG */ + // add this sensor to the sensor task's list + // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor + // and before registering some other fast and important sensor + // as that would cause delay and time jitter for the second fast sensor + PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); + // mag alarm is cleared later, so use I2C + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); + } + } +# endif /* PIOS_INCLUDE_HMC5X83 */ + + // internal ms5611 baro +#if defined(PIOS_INCLUDE_MS5611) + const struct pios_ms5611_cfg *ms5611_cfg = PIOS_BOARD_HW_DEFS_GetMS5611Cfg(pios_board_info_blob.board_rev); + if (ms5611_cfg) { + PIOS_MS5611_Init(ms5611_cfg, PIOS_I2C_MS5611_INTERNAL_ADAPTER); + PIOS_MS5611_Register(); + } +#endif + +#ifdef PIOS_INCLUDE_ADC + PIOS_ADC_Init(PIOS_BOARD_HW_DEFS_GetAdcCfg(pios_board_info_blob.board_rev)); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingArrayGet(adc_config); + for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { + PIOS_ADC_PinSetup(i); + } + } +#endif +#endif /* PIOS_INCLUDE_ADC */ + + // internal bmp280 baro + // internal MPU6000 imu (i2c) + + // external ETASV3 Eagletree Airspeed v3 + // external MS4525D PixHawk Airpeed based on MS4525DO + + // BMA180 accelerometer? + // bmp085/bmp180 baro + + // hmc5843 mag + // i2c esc (?) + // UBX DCC +} diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index 255bf6dce..fe2a60698 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -320,7 +320,7 @@ static const struct usb_config_hid_cdc config_hid_cdc = { const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { .ctrl_if = 0, .ctrl_tx_ep = 2, - + .data_if = 1, .data_rx_ep = 3, .data_tx_ep = 3, diff --git a/flight/pios/inc/pios_board_hw.h b/flight/pios/inc/pios_board_hw.h index ce344b9ce..4c786d99a 100644 --- a/flight/pios/inc/pios_board_hw.h +++ b/flight/pios/inc/pios_board_hw.h @@ -53,4 +53,10 @@ const struct pios_bmp280_cfg *PIOS_BOARD_HW_DEFS_GetBMP280Cfg(uint32_t board_rev #ifdef PIOS_INCLUDE_ADC const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(uint32_t board_revision); #endif +#ifdef PIOS_INCLUDE_MPU6000 +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_MPU9250 +const struct pios_mpu9250_cfg *PIOS_BOARD_HW_DEFS_GetMPU9250Cfg(uint32_t board_revision); +#endif #endif /* PIOS_BOARD_HW_H */ diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 57dd3736e..9ba4dc0d0 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -112,32 +112,32 @@ extern uint32_t pios_com_rf_id; /* oplink telemetry */ /* HK OSD ?? */ extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_OSDHK (pios_com_hkosd_id) #ifndef PIOS_COM_HKOSD_RX_BUF_LEN -# define PIOS_COM_HKOSD_RX_BUF_LEN 22 +# define PIOS_COM_HKOSD_RX_BUF_LEN 22 #endif #ifndef PIOS_COM_HKOSD_TX_BUF_LEN -# define PIOS_COM_HKOSD_TX_BUF_LEN 22 +# define PIOS_COM_HKOSD_TX_BUF_LEN 22 #endif /* MSP */ extern uint32_t pios_com_msp_id; -#define PIOS_COM_MSP (pios_com_msp_id) +#define PIOS_COM_MSP (pios_com_msp_id) #ifndef PIOS_COM_MSP_TX_BUF_LEN -# define PIOS_COM_MSP_TX_BUF_LEN 128 +# define PIOS_COM_MSP_TX_BUF_LEN 128 #endif #ifndef PIOS_COM_MSP_RX_BUF_LEN -# define PIOS_COM_MSP_RX_BUF_LEN 64 +# define PIOS_COM_MSP_RX_BUF_LEN 64 #endif /* MAVLink */ extern uint32_t pios_com_mavlink_id; -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) +#define PIOS_COM_MAVLINK (pios_com_mavlink_id) #ifndef PIOS_COM_MAVLINK_TX_BUF_LEN -# define PIOS_COM_MAVLINK_TX_BUF_LEN 128 +# define PIOS_COM_MAVLINK_TX_BUF_LEN 128 #endif #ifndef PIOS_COM_MAVLINK_RX_BUF_LEN -# define PIOS_COM_MAVLINK_RX_BUF_LEN 128 +# define PIOS_COM_MAVLINK_RX_BUF_LEN 128 #endif /* HoTT Telemetry */ @@ -149,7 +149,7 @@ extern uint32_t pios_com_mavlink_id; # define PIOS_COM_HOTT_BRIDGE_TX_BUF_LEN 512 # endif extern uint32_t pios_com_hott_id; -# define PIOS_COM_HOTT (pios_com_hott_id) +# define PIOS_COM_HOTT (pios_com_hott_id) #endif /* USB VCP */ @@ -192,7 +192,7 @@ typedef enum { PIOS_BOARD_IO_UART_SRXL, /* rcvr */ PIOS_BOARD_IO_UART_IBUS, /* rcvr */ PIOS_BOARD_IO_UART_EXBUS, /* rcvr */ -// PIOS_BOARD_IO_UART_FRSKY_SPORT_TELEMETRY, /* com */ +// PIOS_BOARD_IO_UART_FRSKY_SPORT_TELEMETRY, /* com */ PIOS_BOARD_IO_UART_HOTT_BRIDGE, /* com */ } PIOS_BOARD_IO_UART_Function; @@ -201,15 +201,15 @@ typedef enum { PIOS_BOARD_IO_RADIOAUX_MAVLINK, PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, -// PIOS_BOARD_IO_RADIOAUX_FRSKY_SPORT_TELEMETRY, +// PIOS_BOARD_IO_RADIOAUX_FRSKY_SPORT_TELEMETRY, } PIOS_BOARD_IO_RADIOAUX_Function; #ifdef PIOS_INCLUDE_USB void PIOS_BOARD_IO_Configure_USB(); -//# if defined(PIOS_INCLUDE_USB_HID) -//# include -//extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; -//# endif /* PIOS_INCLUDE_USB_HID */ +// # if defined(PIOS_INCLUDE_USB_HID) +// # include +// extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; +// # endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ #ifdef PIOS_INCLUDE_PWM void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); @@ -221,11 +221,7 @@ void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg); void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *usart_cfg, PIOS_BOARD_IO_UART_Function function); #ifdef PIOS_INCLUDE_RFM22B -void PIOS_BOARD_IO_Configure_RFM22B(uint32_t spi_id, PIOS_BOARD_IO_RADIOAUX_Function function); -#endif - -#ifdef PIOS_INCLUDE_I2C -void PIOS_BOARD_IO_Configure_I2C(uint32_t i2c_internal_id, uint32_t i2c_external_id); +void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function function); #endif #ifdef PIOS_INCLUDE_GCSRCVR diff --git a/flight/pios/inc/pios_board_sensors.h b/flight/pios/inc/pios_board_sensors.h new file mode 100644 index 000000000..71a292062 --- /dev/null +++ b/flight/pios/inc/pios_board_sensors.h @@ -0,0 +1,33 @@ +/** + ****************************************************************************** + * + * @file pios_board_sensors.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief board sensors setup + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIOS_BOARD_SENSORS_H +#define PIOS_BOARD_SENSORS_H + +#include "pios.h" + +void PIOS_BOARD_Sensors_Configure(); + +#endif /* PIOS_BOARD_SENSORS_H */ diff --git a/flight/pios/stm32f10x/inc/pios_servo_config.h b/flight/pios/stm32f10x/inc/pios_servo_config.h index b5464ebae..f8a03fcb7 100644 --- a/flight/pios/stm32f10x/inc/pios_servo_config.h +++ b/flight/pios/stm32f10x/inc/pios_servo_config.h @@ -30,20 +30,20 @@ * Generic servo pin configuration structure for an STM32F10x */ #define TIM_SERVO_CHANNEL_CONFIG(_timer, _channel, _gpio, _pin, _remap) \ -{ \ - .timer = _timer, \ - .timer_chan = TIM_Channel_##_channel, \ - .pin = { \ - .gpio = GPIO##_gpio, \ - .init = { \ - .GPIO_Pin = GPIO_Pin_##_pin, \ - .GPIO_Mode = GPIO_Mode_IPD, \ - .GPIO_Speed = GPIO_Speed_2MHz,\ - }, \ - .pin_source = GPIO_PinSource##_pin, \ - }, \ - .remap = _remap, \ -} + { \ + .timer = _timer, \ + .timer_chan = TIM_Channel_##_channel, \ + .pin = { \ + .gpio = GPIO##_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_pin, \ + .GPIO_Mode = GPIO_Mode_IPD, \ + .GPIO_Speed = GPIO_Speed_2MHz, \ + }, \ + .pin_source = GPIO_PinSource##_pin, \ + }, \ + .remap = _remap, \ + } #endif /* PIOS_SERVO_CONFIG_H_ */ diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 4644293b2..012e6b730 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -58,11 +58,11 @@ void PIOS_SYS_Init(void) * Micromanaging clocks makes no sense given the power situation in the system, so * light up everything we might reasonably use here and just leave it on. */ - + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | RCC_AHBPeriph_DMA2, ENABLE); - + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | @@ -82,13 +82,13 @@ void PIOS_SYS_Init(void) RCC_APB1Periph_I2C1 | RCC_APB1Periph_I2C2 | RCC_APB1Periph_USB | -// RCC_APB1Periph_CAN1 | /* bxCAN unfortunately interferes with USB */ -// RCC_APB1Periph_CAN2 | +// RCC_APB1Periph_CAN1 | /* bxCAN unfortunately interferes with USB */ +// RCC_APB1Periph_CAN2 | RCC_APB1Periph_BKP | RCC_APB1Periph_PWR | RCC_APB1Periph_DAC, ENABLE); - + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index bc165f38f..cc2d13fc0 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -52,7 +52,7 @@ const struct pios_com_driver pios_usart_com_driver = { .rx_start = PIOS_USART_RxStart, .bind_tx_cb = PIOS_USART_RegisterTxCallback, .bind_rx_cb = PIOS_USART_RegisterRxCallback, - .ioctl = PIOS_USART_Ioctl, + .ioctl = PIOS_USART_Ioctl, }; enum pios_usart_dev_magic { @@ -69,7 +69,7 @@ struct pios_usart_dev { uint32_t tx_out_context; uint32_t rx_dropped; - uint8_t irq_channel; + uint8_t irq_channel; }; static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) @@ -80,11 +80,11 @@ static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - + bool valid = PIOS_USART_validate(usart_dev); - + PIOS_Assert(valid); - + return usart_dev->cfg; } @@ -95,9 +95,9 @@ static int32_t PIOS_USART_SetIrqPrio(struct pios_usart_dev *usart_dev, uint8_t i .NVIC_IRQChannelPreemptionPriority = irq_prio, .NVIC_IRQChannelCmd = ENABLE, }; - + NVIC_Init(&init); - + return 0; } @@ -180,7 +180,7 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) } /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + usart_dev->cfg = cfg; /* Initialize the comm parameter structure */ USART_StructInit(&usart_dev->init); // 9600 8n1 @@ -214,9 +214,9 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) usart_dev->irq_channel = USART3_IRQn; break; } - + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); - + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); @@ -452,28 +452,28 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - + bool valid = PIOS_USART_validate(usart_dev); - + PIOS_Assert(valid); - + switch (ctl) { - case PIOS_IOCTL_USART_SET_IRQ_PRIO: - return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); - - case PIOS_IOCTL_USART_GET_RXGPIO: - *(struct stm32_gpio *)param = usart_dev->cfg->rx; - break; - case PIOS_IOCTL_USART_GET_TXGPIO: - *(struct stm32_gpio *)param = usart_dev->cfg->tx; - break; - default: - if (usart_dev->cfg->ioctl) { - return usart_dev->cfg->ioctl(usart_id, ctl, param); - } - return -1; + case PIOS_IOCTL_USART_SET_IRQ_PRIO: + return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); + + case PIOS_IOCTL_USART_GET_RXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->rx; + break; + case PIOS_IOCTL_USART_GET_TXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->tx; + break; + default: + if (usart_dev->cfg->ioctl) { + return usart_dev->cfg->ioctl(usart_id, ctl, param); + } + return -1; } - + return 0; } diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index c6f72674d..7b9f93649 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -59,7 +59,7 @@ const struct pios_com_driver pios_usart_com_driver = { .rx_start = PIOS_USART_RxStart, .bind_tx_cb = PIOS_USART_RegisterTxCallback, .bind_rx_cb = PIOS_USART_RegisterRxCallback, - .ioctl = PIOS_USART_Ioctl, + .ioctl = PIOS_USART_Ioctl, }; enum pios_usart_dev_magic { diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index d4d97f589..9fde0bb45 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -186,11 +186,11 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { }, }; -static uint32_t pios_spi_gyro_id; +uint32_t pios_spi_gyro_adapter_id; void PIOS_SPI_gyro_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + PIOS_SPI_IRQ_Handler(pios_spi_gyro_adapter_id); } @@ -402,11 +402,11 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = { }, }; -static uint32_t pios_spi_flash_accel_id; +uint32_t pios_spi_flash_accel_adapter_id; void PIOS_SPI_flash_accel_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); + PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_adapter_id); } #endif /* PIOS_INCLUDE_SPI */ @@ -485,6 +485,11 @@ void PIOS_ADC_handler() { PIOS_ADC_DMA_Handler(); } + +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} #endif /* if defined(PIOS_INCLUDE_ADC) */ #include "pios_tim_priv.h" @@ -593,16 +598,16 @@ static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL #include "pios_usart_priv.h" // Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOB -#define MAIN_USART_INVERTER_PIN GPIO_Pin_2 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET +#define MAIN_USART_INVERTER_GPIO GPIOB +#define MAIN_USART_INVERTER_PIN GPIO_Pin_2 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -610,7 +615,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -618,12 +623,12 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_Mode = GPIO_Mode_AF_PP, }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, + .ioctl = PIOS_BOARD_USART_Ioctl, }; static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, - .rx = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -631,7 +636,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -967,16 +972,16 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) { switch (board_revision) { - case BOARD_REVISION_CC: - return &pios_usb_main_cfg_cc; - - break; - case BOARD_REVISION_CC3D: - return &pios_usb_main_cfg_cc3d; - - break; - default: - PIOS_DEBUG_Assert(0); + case BOARD_REVISION_CC: + return &pios_usb_main_cfg_cc; + + break; + case BOARD_REVISION_CC3D: + return &pios_usb_main_cfg_cc3d; + + break; + default: + PIOS_DEBUG_Assert(0); } return NULL; } @@ -988,7 +993,6 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) */ #if defined(PIOS_INCLUDE_MPU6000) #include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { .vector = PIOS_MPU6000_IRQHandler, .line = EXTI_Line3, @@ -1037,6 +1041,9 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .std_prescaler = PIOS_SPI_PRESCALER_64, .max_downsample = 2 }; + +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu6000_cfg; +} #endif /* PIOS_INCLUDE_MPU6000 */ - - diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index fdc6f517f..baffcae28 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -38,11 +38,9 @@ #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif -#if defined(PIOS_INCLUDE_ADXL345) -#include -#endif #include +#include /* * Pull in the board-specific static HW definitions. @@ -91,19 +89,19 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - + switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; } - + return -1; } @@ -133,12 +131,12 @@ void PIOS_Board_Init(void) switch (bdinfo->board_rev) { case BOARD_REVISION_CC: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { + if (PIOS_SPI_Init(&pios_spi_flash_accel_adapter_id, &pios_spi_flash_accel_cfg_cc)) { PIOS_Assert(0); } break; case BOARD_REVISION_CC3D: - if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { + if (PIOS_SPI_Init(&pios_spi_flash_accel_adapter_id, &pios_spi_flash_accel_cfg_cc3d)) { PIOS_Assert(0); } break; @@ -151,7 +149,7 @@ void PIOS_Board_Init(void) uintptr_t flash_id; switch (bdinfo->board_rev) { case BOARD_REVISION_CC: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_adapter_id, 1)) { PIOS_DEBUG_Assert(0); } if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { @@ -159,7 +157,7 @@ void PIOS_Board_Init(void) } break; case BOARD_REVISION_CC3D: - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_adapter_id, 0)) { PIOS_DEBUG_Assert(0); } if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { @@ -233,28 +231,28 @@ void PIOS_Board_Init(void) /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsCC_FlexiPortGet(&hwsettings_flexiport); - + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } - switch(hwsettings_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_I2C: + switch (hwsettings_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { - PIOS_Assert(0); - } + if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { + PIOS_Assert(0); + } #endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_CC_FLEXIPORT_PPM: + break; + case HWSETTINGS_CC_FLEXIPORT_PPM: #if defined(PIOS_INCLUDE_PPM_FLEXI) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_flexi_cfg); + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_flexi_cfg); #endif /* PIOS_INCLUDE_PPM_FLEXI */ - break; + break; } /* Configure main USART port */ - + /* Initialize inverter gpio and set it to off */ { GPIO_InitTypeDef inverterGPIOInit = { @@ -262,16 +260,16 @@ void PIOS_Board_Init(void) .GPIO_Mode = GPIO_Mode_Out_PP, .GPIO_Speed = GPIO_Speed_2MHz, }; - + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, MAIN_USART_INVERTER_PIN, MAIN_USART_INVERTER_DISABLE); } - + uint8_t hwsettings_mainport; HwSettingsCC_MainPortGet(&hwsettings_mainport); - + if (hwsettings_mainport < NELEMENTS(main_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } @@ -298,7 +296,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM((hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM((hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ break; case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: @@ -341,33 +339,23 @@ void PIOS_Board_Init(void) switch (bdinfo->board_rev) { case BOARD_REVISION_CC: - // Revision 1 with invensense gyros, start the ADC -#if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); -#endif -#if defined(PIOS_INCLUDE_ADXL345) - PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); -#endif break; case BOARD_REVISION_CC3D: // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); -#if defined(PIOS_INCLUDE_MPU6000) - // Set up the SPI interface to the serial flash - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + // Set up the SPI interface to the mpu6000 + if (PIOS_SPI_Init(&pios_spi_gyro_adapter_id, &pios_spi_gyro_cfg)) { PIOS_Assert(0); } - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - init_test = !PIOS_MPU6000_Driver.test(0); -#endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_Assert(0); } + PIOS_BOARD_Sensors_Configure(); + /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index f01fe9cc8..b25605749 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -116,19 +116,23 @@ extern uint32_t pios_i2c_flexi_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_SPI_MAX_DEVS 2 +#define PIOS_SPI_MAX_DEVS 2 +extern uint32_t pios_spi_gyro_adapter_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_adapter_id) +extern uint32_t pios_spi_flash_accel_adapter_id; +#define PIOS_SPI_ADXL345_ADAPTER (pios_spi_flash_accel_adapter_id) // ------------------------- // PIOS_USART // ------------------------- -#define PIOS_USART_MAX_DEVS 2 +#define PIOS_USART_MAX_DEVS 2 // ------------------------- // PIOS_COM // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 3 +#define PIOS_COM_MAX_DEVS 3 #define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 diff --git a/flight/targets/boards/discoveryf4bare/board_hw_defs.c b/flight/targets/boards/discoveryf4bare/board_hw_defs.c index 678a1500a..bcae0c028 100644 --- a/flight/targets/boards/discoveryf4bare/board_hw_defs.c +++ b/flight/targets/boards/discoveryf4bare/board_hw_defs.c @@ -343,15 +343,15 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { } }; -static uint32_t pios_spi_gyro_id; +uint32_t pios_spi_gyro_adapter_id; void PIOS_SPI_gyro_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + PIOS_SPI_IRQ_Handler(pios_spi_gyro_adapter_id); } -#if false +#ifdef PIOS_INCLUDE_RFM22B /* * SPI3 Interface * Used for Flash and the RFM22B @@ -479,11 +479,11 @@ static const struct pios_spi_cfg pios_spi_telem_flash_cfg = { }, }; -uint32_t pios_spi_telem_flash_id; +uint32_t pios_spi_telem_flash_adapter_id; void PIOS_SPI_telem_flash_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); + PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_adapter_id); } @@ -606,17 +606,17 @@ static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = { * MAIN USART */ // Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET +#define MAIN_USART_INVERTER_GPIO GPIOC +#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -626,7 +626,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -636,7 +636,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, + .ioctl = PIOS_BOARD_USART_Ioctl, }; /* @@ -645,7 +645,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, - .rx = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -655,7 +655,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -671,7 +671,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, - .dtr = { + .dtr = { // FlexIO pin 9 .gpio = GPIOC, .init = { @@ -682,7 +682,7 @@ static const struct pios_usart_cfg pios_usart_flexiio_cfg = { }, }, - .tx = { + .tx = { // * 7: PC6 = TIM8 CH1, USART6 TX .gpio = GPIOC, .init = { @@ -692,10 +692,10 @@ static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, - .pin_source = GPIO_PinSource6, + .pin_source = GPIO_PinSource6, }, - .rx = { + .rx = { // * 8: PC7 = TIM8 CH2, USART6 RX .gpio = GPIOC, .init = { @@ -705,7 +705,7 @@ static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, - .pin_source = GPIO_PinSource7, + .pin_source = GPIO_PinSource7, } }; @@ -1066,18 +1066,18 @@ static const struct pios_tim_clock_cfg tim_12_cfg = { #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), - TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), - TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3), - TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), - TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1), - TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0), // PWM pins on FlexiIO(receiver) port TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), // * 6: PB15 = SPI2 MOSI, TIM12 CH2 - TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), // * 7: PC6 = TIM8 CH1, USART6 TX - TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), // * 8: PC7 = TIM8 CH2, USART6 RX - TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), // * 9: PC8 = TIM8 CH3 - TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), // * 10: PC9 = TIM8 CH4 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), // * 7: PC6 = TIM8 CH1, USART6 TX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), // * 8: PC7 = TIM8 CH2, USART6 RX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), // * 9: PC8 = TIM8 CH3 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), // * 10: PC9 = TIM8 CH4 TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS }; @@ -1140,10 +1140,10 @@ const struct pios_servo_cfg pios_servo_cfg_out_in = { static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), }; const struct pios_pwm_cfg pios_pwm_cfg = { @@ -1448,7 +1448,6 @@ const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unu */ #if defined(PIOS_INCLUDE_MPU6000) #include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { .vector = PIOS_MPU6000_IRQHandler, .line = EXTI_Line4, @@ -1496,6 +1495,10 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .filter = PIOS_MPU6000_LOWPASS_256_HZ, .orientation = PIOS_MPU6000_TOP_180DEG }; + +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu6000_cfg; +} + #endif /* PIOS_INCLUDE_MPU6000 */ - - diff --git a/flight/targets/boards/discoveryf4bare/bootloader/main.c b/flight/targets/boards/discoveryf4bare/bootloader/main.c index 09a54a094..aa5c7999a 100644 --- a/flight/targets/boards/discoveryf4bare/bootloader/main.c +++ b/flight/targets/boards/discoveryf4bare/bootloader/main.c @@ -35,6 +35,7 @@ #include /* PIOS_USBHOOK_* */ #include #include +#include extern void FLASH_Download(); void check_bor(); diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 7e8c49c52..5ffc19ec6 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -39,6 +39,7 @@ #endif #include +#include /* * Pull in the board-specific static HW definitions. @@ -101,25 +102,25 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, - [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, + [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, }; int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - + switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; } - + return -1; } @@ -143,17 +144,20 @@ void PIOS_Board_Init(void) #endif -#if false +#ifdef PIOS_INCLUDE_MPU6000 /* Set up the SPI interface to the gyro/acelerometer */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + if (PIOS_SPI_Init(&pios_spi_gyro_adapter_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } +#endif /* PIOS_INCLUDE_MPU6000 */ - /* Set up the SPI interface to the flash and rfm22b */ - if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { +#ifdef PIOS_INCLUDE_RFM22B + /* Set up the SPI interface to the rfm22b */ + if (PIOS_SPI_Init(&pios_spi_telem_flash_adapter_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } -#endif +#endif /* PIOS_INCLUDE_RFM22B */ + #if defined(PIOS_INCLUDE_FLASH) /* Connect flash to the appropriate interface and configure it */ uintptr_t flash_id; @@ -229,30 +233,28 @@ void PIOS_Board_Init(void) } /* Configure IO ports */ - + /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } - + #if defined(PIOS_INCLUDE_I2C) /* Set up internal I2C bus */ if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_DELAY_WaitmS(50); - + if (hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } PIOS_DELAY_WaitmS(50); } - - PIOS_BOARD_IO_Configure_I2C(pios_i2c_mag_pressure_adapter_id, pios_i2c_flexiport_adapter_id); #endif #if defined(PIOS_INCLUDE_USB) @@ -260,7 +262,7 @@ void PIOS_Board_Init(void) #endif /* Configure main USART port */ - + /* Initialize inverter gpio and set it to off */ { GPIO_InitTypeDef inverterGPIOInit = { @@ -270,30 +272,30 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }; - + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, MAIN_USART_INVERTER_PIN, MAIN_USART_INVERTER_DISABLE); } - + uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); - + if (hwsettings_mainport < NELEMENTS(main_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - + #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioaux; HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - if(hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { + if (hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); } #endif /* PIOS_INCLUDE_RFM22B */ - + #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) const struct pios_servo_cfg *pios_servo_cfg; @@ -304,47 +306,47 @@ void PIOS_Board_Init(void) /* Configure the receiver port*/ uint8_t hwsettings_rcvrport; HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); - + if (hwsettings_rcvrport < NELEMENTS(flexiio_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_flexiio_cfg, flexiio_function_map[hwsettings_rcvrport]); } - + // Configure rcvrport PPM/PWM/OUTPUTS switch (hwsettings_rcvrport) { - case HWSETTINGS_RM_RCVRPORT_PWM: + case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - /* Set up the receiver port. Later this should be optional */ - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); + /* Set up the receiver port. Later this should be optional */ + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ - break; - case HWSETTINGS_RM_RCVRPORT_PPM: - case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: - case HWSETTINGS_RM_RCVRPORT_PPMPWM: - case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY: - case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE: - case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE: - case HWSETTINGS_RM_RCVRPORT_PPMMSP: - case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: - case HWSETTINGS_RM_RCVRPORT_PPMGPS: + break; + case HWSETTINGS_RM_RCVRPORT_PPM: + case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RM_RCVRPORT_PPMPWM: + case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY: + case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE: + case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE: + case HWSETTINGS_RM_RCVRPORT_PPMMSP: + case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: + case HWSETTINGS_RM_RCVRPORT_PPMGPS: #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); - - if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { - // configure servo outputs and the remaining 5 inputs as outputs - pios_servo_cfg = &pios_servo_cfg_out_in_ppm; - } - - // enable pwm on the remaining channels - if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_ppm_cfg); - } - - break; + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { + // configure servo outputs and the remaining 5 inputs as outputs + pios_servo_cfg = &pios_servo_cfg_out_in_ppm; + } + + // enable pwm on the remaining channels + if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_ppm_cfg); + } + + break; #endif /* PIOS_INCLUDE_PPM */ - case HWSETTINGS_RM_RCVRPORT_OUTPUTS: - // configure only the servo outputs - pios_servo_cfg = &pios_servo_cfg_out_in; - break; + case HWSETTINGS_RM_RCVRPORT_OUTPUTS: + // configure only the servo outputs + pios_servo_cfg = &pios_servo_cfg_out_in; + break; } #ifdef PIOS_INCLUDE_GCSRCVR PIOS_BOARD_IO_Configure_GCSRCVR(); @@ -357,16 +359,12 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif -#if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - PIOS_MPU6000_Register(); -#endif - + PIOS_BOARD_Sensors_Configure(); + #ifdef PIOS_INCLUDE_WS2811 PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg); #endif // PIOS_INCLUDE_WS2811 - + #ifdef PIOS_INCLUDE_ADC // Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout GPIO_InitTypeDef gpioA8 = { @@ -377,8 +375,6 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_OD, }; GPIO_Init(GPIOA, &gpioA8); - - PIOS_BOARD_IO_Configure_ADC(); #endif // PIOS_INCLUDE_ADC } diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index 4b129899d..9c3d1ba73 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -85,29 +85,34 @@ // PIOS_SPI // See also pios_board.c // ------------------------ -#define PIOS_SPI_MAX_DEVS 3 +#define PIOS_SPI_MAX_DEVS 3 +extern uint32_t pios_spi_telem_flash_adapter_id; +#define PIOS_SPI_RFM22B_ADAPTER (pios_spi_telem_flash_adapter_id) +extern uint32_t pios_spi_gyro_adapter_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_adapter_id) // ------------------------ // PIOS_WDG // ------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 // ------------------------ // PIOS_I2C // See also pios_board.c // ------------------------ -#define PIOS_I2C_MAX_DEVS 3 +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_HMC5X83_INTERNAL_ADAPTER (pios_i2c_mag_pressure_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- // PIOS_USART @@ -121,7 +126,7 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 4 +#define PIOS_COM_MAX_DEVS 4 // ------------------------- // Packet Handler diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index d05d3dbfb..0810721cb 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -693,7 +693,7 @@ const struct pios_ppm_out_cfg pios_flexi_ppm_out_cfg = { */ static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -701,7 +701,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -713,7 +713,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, - .rx = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -721,7 +721,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 7e040bdb9..6edf839f9 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -464,11 +464,11 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { } }; -static uint32_t pios_spi_gyro_id; +uint32_t pios_spi_gyro_adapter_id; void PIOS_SPI_gyro_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + PIOS_SPI_IRQ_Handler(pios_spi_gyro_adapter_id); } @@ -599,11 +599,11 @@ static const struct pios_spi_cfg pios_spi_telem_flash_cfg = { }, }; -uint32_t pios_spi_telem_flash_id; +uint32_t pios_spi_telem_flash_adapter_id; void PIOS_SPI_telem_flash_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); + PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_adapter_id); } @@ -794,10 +794,10 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { */ // Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET +#define MAIN_USART_INVERTER_GPIO GPIOC +#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); @@ -1254,18 +1254,18 @@ static const struct pios_tim_clock_cfg tim_12_cfg = { #include static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), - TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), - TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3), - TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), - TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1), - TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM9, 2, A, 3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 2, A, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM5, 1, A, 0), // PWM pins on FlexiIO(receiver) port TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), // * 6: PB15 = SPI2 MOSI, TIM12 CH2 - TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), // * 7: PC6 = TIM8 CH1, USART6 TX - TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), // * 8: PC7 = TIM8 CH2, USART6 RX - TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), // * 9: PC8 = TIM8 CH3 - TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), // * 10: PC9 = TIM8 CH4 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), // * 7: PC6 = TIM8 CH1, USART6 TX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), // * 8: PC7 = TIM8 CH2, USART6 RX + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), // * 9: PC8 = TIM8 CH3 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), // * 10: PC9 = TIM8 CH4 TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), // * 5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS }; @@ -1328,10 +1328,10 @@ const struct pios_servo_cfg pios_servo_cfg_out_in = { static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { TIM_SERVO_CHANNEL_CONFIG(TIM12, 1, B, 14), TIM_SERVO_CHANNEL_CONFIG(TIM12, 2, B, 15), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), - TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, C, 6), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 2, C, 7), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, C, 8), + TIM_SERVO_CHANNEL_CONFIG(TIM8, 4, C, 9), }; const struct pios_pwm_cfg pios_pwm_cfg = { @@ -1738,7 +1738,6 @@ const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unu */ #if defined(PIOS_INCLUDE_MPU6000) #include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { .vector = PIOS_MPU6000_IRQHandler, .line = EXTI_Line4, @@ -1789,4 +1788,10 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .std_prescaler = PIOS_SPI_PRESCALER_64, .max_downsample = 20, }; + +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu6000_cfg; +} + #endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 60bdfce81..47279154e 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -38,6 +38,7 @@ #endif #include +#include /* * Pull in the board-specific static HW definitions. @@ -100,7 +101,7 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, - [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, + [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, }; @@ -143,12 +144,12 @@ void PIOS_Board_Init(void) #endif /* Set up the SPI interface to the gyro/acelerometer */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + if (PIOS_SPI_Init(&pios_spi_gyro_adapter_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ - if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { + if (PIOS_SPI_Init(&pios_spi_telem_flash_adapter_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } @@ -157,7 +158,7 @@ void PIOS_Board_Init(void) uintptr_t flash_id; // Initialize the external USER flash - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_adapter_id, 1)) { PIOS_DEBUG_Assert(0); } @@ -246,8 +247,6 @@ void PIOS_Board_Init(void) } PIOS_DELAY_WaitmS(50); } - - PIOS_BOARD_IO_Configure_I2C(pios_i2c_mag_pressure_adapter_id, pios_i2c_flexiport_adapter_id); #endif /* Moved this here to allow DSM binding on flexiport */ @@ -273,7 +272,7 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }; - + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, MAIN_USART_INVERTER_PIN, @@ -290,9 +289,9 @@ void PIOS_Board_Init(void) #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioaux; HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - - if(hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { - PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); + + if (hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { + PIOS_BOARD_IO_Configure_RFM22B(radioaux_function_map[hwsettings_radioaux]); } #endif /* PIOS_INCLUDE_RFM22B */ @@ -359,16 +358,12 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif -#if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - PIOS_MPU6000_Register(); -#endif + PIOS_BOARD_Sensors_Configure(); #ifdef PIOS_INCLUDE_WS2811 HwSettingsWS2811LED_OutOptions ws2811_pin_settings; HwSettingsWS2811LED_OutGet(&ws2811_pin_settings); - + if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED && ws2811_pin_settings < NELEMENTS(pios_ws2811_pin_cfg)) { PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]); } @@ -384,8 +379,6 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_OD, }; GPIO_Init(GPIOA, &gpioA8); - - PIOS_BOARD_IO_Configure_ADC(); #endif // PIOS_INCLUDE_ADC DEBUG_PRINTF(2, "Board complete\r\n"); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 122cdb71f..6c4f67aaa 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -103,30 +103,36 @@ // PIOS_SPI // See also pios_board.c // ------------------------ -#define PIOS_SPI_MAX_DEVS 3 +#define PIOS_SPI_MAX_DEVS 3 +extern uint32_t pios_spi_gyro_adapter_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_adapter_id) +extern uint32_t pios_spi_telem_flash_adapter_id; +#define PIOS_SPI_RFM22B_ADAPTER (pios_spi_telem_flash_adapter_id) // ------------------------ // PIOS_WDG // ------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 // ------------------------ // PIOS_I2C // See also pios_board.c // ------------------------ -#define PIOS_I2C_MAX_DEVS 3 +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_HMC5X83_INTERNAL_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_MS5611_INTERNAL_ADAPTER (pios_i2c_mag_pressure_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- // PIOS_USART @@ -140,7 +146,7 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 4 +#define PIOS_COM_MAX_DEVS 4 // ------------------------- // Packet Handler diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index e84a8cc35..fb4d9f129 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -42,29 +42,29 @@ * o5 | PA0 | TIM5_CH1 | ADC1_0 * o6 | PA1 | TIM5_CH2 | ADC1_1 */ -#define MAIN_USART_REGS USART2 -#define MAIN_USART_REMAP GPIO_AF_USART2 -#define MAIN_USART_IRQ USART2_IRQn -#define MAIN_USART_RX_GPIO GPIOA -#define MAIN_USART_RX_PIN GPIO_Pin_3 -#define MAIN_USART_TX_GPIO GPIOA -#define MAIN_USART_TX_PIN GPIO_Pin_2 +#define MAIN_USART_REGS USART2 +#define MAIN_USART_REMAP GPIO_AF_USART2 +#define MAIN_USART_IRQ USART2_IRQn +#define MAIN_USART_RX_GPIO GPIOA +#define MAIN_USART_RX_PIN GPIO_Pin_3 +#define MAIN_USART_TX_GPIO GPIOA +#define MAIN_USART_TX_PIN GPIO_Pin_2 // Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_15 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET +#define MAIN_USART_INVERTER_GPIO GPIOC +#define MAIN_USART_INVERTER_PIN GPIO_Pin_15 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET -#define FLEXI_USART_REGS USART1 -#define FLEXI_USART_REMAP GPIO_AF_USART1 -#define FLEXI_USART_IRQ USART1_IRQn -#define FLEXI_USART_RX_GPIO GPIOB -#define FLEXI_USART_RX_PIN GPIO_Pin_7 -#define FLEXI_USART_TX_GPIO GPIOB -#define FLEXI_USART_TX_PIN GPIO_Pin_6 +#define FLEXI_USART_REGS USART1 +#define FLEXI_USART_REMAP GPIO_AF_USART1 +#define FLEXI_USART_IRQ USART1_IRQn +#define FLEXI_USART_RX_GPIO GPIOB +#define FLEXI_USART_RX_PIN GPIO_Pin_7 +#define FLEXI_USART_TX_GPIO GPIOB +#define FLEXI_USART_TX_PIN GPIO_Pin_6 // ReceiverPort pin 3 -#define FLEXI_USART_DTR_GPIO GPIOB -#define FLEXI_USART_DTR_PIN GPIO_Pin_10 +#define FLEXI_USART_DTR_GPIO GPIOB +#define FLEXI_USART_DTR_PIN GPIO_Pin_10 #if defined(PIOS_INCLUDE_LED) @@ -115,8 +115,8 @@ const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) #include /* - * SPI1 Interface - * Used for MPU6000 gyro and accelerometer + * SPI2 Interface + * Used for MPU9250 gyro and accelerometer */ void PIOS_SPI_gyro_irq_handler(void); void DMA1_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); @@ -229,11 +229,11 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { } }; -static uint32_t pios_spi_gyro_id; +uint32_t pios_spi_gyro_adapter_id; void PIOS_SPI_gyro_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + PIOS_SPI_IRQ_Handler(pios_spi_gyro_adapter_id); } #endif /* PIOS_INCLUDE_SPI */ @@ -249,7 +249,7 @@ static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *par static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = MAIN_USART_REGS, .remap = MAIN_USART_REMAP, - .rx = { + .rx = { .gpio = MAIN_USART_RX_GPIO, .init = { .GPIO_Pin = MAIN_USART_RX_PIN, @@ -259,7 +259,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = MAIN_USART_TX_GPIO, .init = { .GPIO_Pin = MAIN_USART_TX_PIN, @@ -278,7 +278,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = FLEXI_USART_REGS, .remap = FLEXI_USART_REMAP, - .rx = { + .rx = { .gpio = FLEXI_USART_RX_GPIO, .init = { .GPIO_Pin = FLEXI_USART_RX_PIN, @@ -288,7 +288,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = FLEXI_USART_TX_GPIO, .init = { .GPIO_Pin = FLEXI_USART_TX_PIN, @@ -298,15 +298,15 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, -// .dtr = { -// .gpio = FLEXI_USART_DTR_GPIO, -// .init = { -// .GPIO_Pin = FLEXI_USART_DTR_PIN, -// .GPIO_Speed = GPIO_Speed_25MHz, -// .GPIO_Mode = GPIO_Mode_OUT, -// .GPIO_OType = GPIO_OType_PP, -// }, -// }, +// .dtr = { +// .gpio = FLEXI_USART_DTR_GPIO, +// .init = { +// .GPIO_Pin = FLEXI_USART_DTR_PIN, +// .GPIO_Speed = GPIO_Speed_25MHz, +// .GPIO_Mode = GPIO_Mode_OUT, +// .GPIO_OType = GPIO_OType_PP, +// }, +// }, }; #if defined(PIOS_INCLUDE_COM) @@ -329,7 +329,7 @@ __attribute__((alias("PIOS_I2C_pressure_adapter_ev_irq_handler"))); void I2C3_ER_IRQHandler() __attribute__((alias("PIOS_I2C_pressure_adapter_er_irq_handler"))); -static const struct pios_i2c_adapter_cfg pios_i2c_pressure_adapter_cfg = { +static const struct pios_i2c_adapter_cfg pios_i2c_eeprom_pressure_adapter_cfg = { .regs = I2C3, .remapSCL = GPIO_AF_I2C3, .remapSDA = GPIO_AF9_I2C3, @@ -382,17 +382,17 @@ static const struct pios_i2c_adapter_cfg pios_i2c_pressure_adapter_cfg = { }, }; -uint32_t pios_i2c_pressure_adapter_id; +uint32_t pios_i2c_eeprom_pressure_adapter_id; void PIOS_I2C_pressure_adapter_ev_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_pressure_adapter_id); + PIOS_I2C_EV_IRQ_Handler(pios_i2c_eeprom_pressure_adapter_id); } void PIOS_I2C_pressure_adapter_er_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_pressure_adapter_id); + PIOS_I2C_ER_IRQ_Handler(pios_i2c_eeprom_pressure_adapter_id); } @@ -937,7 +937,6 @@ struct pios_flash_eeprom_cfg flash_main_chip_cfg = { */ - #if defined(PIOS_INCLUDE_ADC) #include "pios_adc_priv.h" @@ -1019,7 +1018,6 @@ const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unu */ #if defined(PIOS_INCLUDE_MPU9250) #include "pios_mpu9250.h" -#include "pios_mpu9250_config.h" static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { .vector = PIOS_MPU9250_IRQHandler, .line = EXTI_Line15, @@ -1070,4 +1068,9 @@ static const struct pios_mpu9250_cfg pios_mpu9250_cfg = { .std_prescaler = PIOS_SPI_PRESCALER_64, .max_downsample = 26, }; + +const struct pios_mpu9250_cfg *PIOS_BOARD_HW_DEFS_GetMPU9250Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu9250_cfg; +} #endif /* PIOS_INCLUDE_MPU9250 */ diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 6d95791f4..d4883768d 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -41,6 +41,7 @@ #endif #include +#include /* * Pull in the board-specific static HW definitions. @@ -97,19 +98,19 @@ static const PIOS_BOARD_IO_UART_Function main_function_map[] = { int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - + switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; } - + return -1; } @@ -129,18 +130,12 @@ void PIOS_Board_Init(void) #endif /* Set up the SPI interface to the gyro/acelerometer */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + if (PIOS_SPI_Init(&pios_spi_gyro_adapter_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } -#if false - /* Set up the SPI interface to the flash and rfm22b */ - if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { - PIOS_DEBUG_Assert(0); - } -#endif #ifdef PIOS_INCLUDE_I2C - if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { + if (PIOS_I2C_Init(&pios_i2c_eeprom_pressure_adapter_id, &pios_i2c_eeprom_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } #endif @@ -150,7 +145,7 @@ void PIOS_Board_Init(void) uintptr_t flash_id = 0; // Initialize the external USER flash - if (PIOS_Flash_EEPROM_Init(&flash_id, &flash_main_chip_cfg, pios_i2c_pressure_adapter_id, 0x50)) { + if (PIOS_Flash_EEPROM_Init(&flash_id, &flash_main_chip_cfg, pios_i2c_eeprom_pressure_adapter_id, 0x50)) { PIOS_DEBUG_Assert(0); } @@ -213,7 +208,7 @@ void PIOS_Board_Init(void) HwSettingsSetDefaults(HwSettingsHandle(), 0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } - + #if defined(PIOS_INCLUDE_USB) PIOS_BOARD_IO_Configure_USB(); #endif @@ -221,21 +216,21 @@ void PIOS_Board_Init(void) /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } /* Configure main USART port */ - + /* Initialize inverter gpio and set it to off */ { GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP }; GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); @@ -243,10 +238,10 @@ void PIOS_Board_Init(void) MAIN_USART_INVERTER_PIN, MAIN_USART_INVERTER_DISABLE); } - + uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); - + if (hwsettings_mainport < NELEMENTS(main_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } @@ -317,16 +312,7 @@ void PIOS_Board_Init(void) PIOS_DELAY_WaitmS(50); -#if defined(PIOS_INCLUDE_ADC) - PIOS_BOARD_IO_Configure_ADC(); -#endif - -#if defined(PIOS_INCLUDE_MPU9250) - PIOS_MPU9250_Init(pios_spi_gyro_id, 0, &pios_mpu9250_cfg); - PIOS_MPU9250_CONFIG_Configure(); - PIOS_MPU9250_MainRegister(); - PIOS_MPU9250_MagRegister(); -#endif + PIOS_BOARD_Sensors_Configure(); // Attach the board config check hook SANITYCHECK_AttachHook(&RevoNanoConfigHook); diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 5e1909dfd..79a884a02 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -103,30 +103,32 @@ // PIOS_SPI // See also pios_board.c // ------------------------ -#define PIOS_SPI_MAX_DEVS 3 - +#define PIOS_SPI_MAX_DEVS 3 +extern uint32_t pios_spi_gyro_adapter_id; +#define PIOS_SPI_MPU9250_ADAPTER (pios_spi_gyro_adapter_id) // ------------------------ // PIOS_WDG // ------------------------ -#define PIOS_WATCHDOG_TIMEOUT 500 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WATCHDOG_TIMEOUT 500 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 // ------------------------ // PIOS_I2C // See also pios_board.c // ------------------------ -#define PIOS_I2C_MAX_DEVS 3 -extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_MAX_DEVS 3 +extern uint32_t pios_i2c_eeprom_pressure_adapter_id; +#define PIOS_I2C_MS5611_INTERNAL_ADAPTER (pios_i2c_eeprom_pressure_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- // PIOS_USART @@ -140,7 +142,7 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 4 +#define PIOS_COM_MAX_DEVS 4 // ------------------------- // Packet Handler // ------------------------- diff --git a/flight/targets/boards/sparky2/board_hw_defs.c b/flight/targets/boards/sparky2/board_hw_defs.c index 3a531ce1e..8316bc3df 100644 --- a/flight/targets/boards/sparky2/board_hw_defs.c +++ b/flight/targets/boards/sparky2/board_hw_defs.c @@ -254,11 +254,11 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { } }; -static uint32_t pios_spi_gyro_id; +uint32_t pios_spi_gyro_adapter_id; void PIOS_SPI_gyro_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); + PIOS_SPI_IRQ_Handler(pios_spi_gyro_adapter_id); } @@ -389,11 +389,11 @@ static const struct pios_spi_cfg pios_spi_telem_flash_cfg = { }, }; -uint32_t pios_spi_telem_flash_id; +uint32_t pios_spi_telem_flash_adapter_id; void PIOS_SPI_telem_flash_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_id); + PIOS_SPI_IRQ_Handler(pios_spi_telem_flash_adapter_id); } @@ -549,7 +549,7 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -559,7 +559,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -577,7 +577,7 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, - .rx = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -587,7 +587,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -605,10 +605,10 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { */ // Inverter for SBUS handling -#define RCVR_USART_INVERTER_GPIO GPIOC -#define RCVR_USART_INVERTER_PIN GPIO_Pin_4 -#define RCVR_USART_INVERTER_ENABLE Bit_SET -#define RCVR_USART_INVERTER_DISABLE Bit_RESET +#define RCVR_USART_INVERTER_GPIO GPIOC +#define RCVR_USART_INVERTER_PIN GPIO_Pin_4 +#define RCVR_USART_INVERTER_ENABLE Bit_SET +#define RCVR_USART_INVERTER_DISABLE Bit_RESET static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); @@ -616,7 +616,7 @@ static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *par static const struct pios_usart_cfg pios_usart_rcvr_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, - .rx = { + .rx = { .gpio = GPIOC, .init = { .GPIO_Pin = GPIO_Pin_7, @@ -626,7 +626,7 @@ static const struct pios_usart_cfg pios_usart_rcvr_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, + .ioctl = PIOS_BOARD_USART_Ioctl, }; #if defined(PIOS_INCLUDE_COM) @@ -642,14 +642,14 @@ static const struct pios_usart_cfg pios_usart_rcvr_cfg = { /* * I2C Adapters */ -void PIOS_i2c_mag_pressure_adapter_ev_irq_handler(void); -void PIOS_i2c_mag_pressure_adapter_er_irq_handler(void); +void PIOS_i2c_pressure_adapter_ev_irq_handler(void); +void PIOS_i2c_pressure_adapter_er_irq_handler(void); void I2C1_EV_IRQHandler() -__attribute__((alias("PIOS_i2c_mag_pressure_adapter_ev_irq_handler"))); +__attribute__((alias("PIOS_i2c_pressure_adapter_ev_irq_handler"))); void I2C1_ER_IRQHandler() -__attribute__((alias("PIOS_i2c_mag_pressure_adapter_er_irq_handler"))); +__attribute__((alias("PIOS_i2c_pressure_adapter_er_irq_handler"))); -static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = { +static const struct pios_i2c_adapter_cfg pios_i2c_pressure_adapter_cfg = { .regs = I2C1, .remapSCL = GPIO_AF_I2C1, .remapSDA = GPIO_AF_I2C1, @@ -702,17 +702,17 @@ static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = { }, }; -uint32_t pios_i2c_mag_pressure_adapter_id; -void PIOS_i2c_mag_pressure_adapter_ev_irq_handler(void) +uint32_t pios_i2c_pressure_adapter_id; +void PIOS_i2c_pressure_adapter_ev_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); + PIOS_I2C_EV_IRQ_Handler(pios_i2c_pressure_adapter_id); } -void PIOS_i2c_mag_pressure_adapter_er_irq_handler(void) +void PIOS_i2c_pressure_adapter_er_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_mag_pressure_adapter_id); + PIOS_I2C_ER_IRQ_Handler(pios_i2c_pressure_adapter_id); } @@ -788,8 +788,8 @@ void PIOS_I2C_flexiport_adapter_er_irq_handler(void) } -void PIOS_i2c_mag_pressure_adapter_ev_irq_handler(void); -void PIOS_i2c_mag_pressure_adapter_er_irq_handler(void); +void PIOS_i2c_pressure_adapter_ev_irq_handler(void); +void PIOS_i2c_pressure_adapter_er_irq_handler(void); #endif /* PIOS_INCLUDE_I2C */ @@ -1307,7 +1307,6 @@ const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unu */ #if defined(PIOS_INCLUDE_MPU9250) #include "pios_mpu9250.h" -#include "pios_mpu9250_config.h" static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { .vector = PIOS_MPU9250_IRQHandler, .line = EXTI_Line5, @@ -1358,5 +1357,9 @@ static const struct pios_mpu9250_cfg pios_mpu9250_cfg = { .std_prescaler = PIOS_SPI_PRESCALER_64, .max_downsample = 26, }; -#endif /* PIOS_INCLUDE_MPU9250 */ +const struct pios_mpu9250_cfg *PIOS_BOARD_HW_DEFS_GetMPU9250Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu9250_cfg; +} +#endif /* PIOS_INCLUDE_MPU9250 */ diff --git a/flight/targets/boards/sparky2/firmware/pios_board.c b/flight/targets/boards/sparky2/firmware/pios_board.c index 28e232e36..a9b72f485 100644 --- a/flight/targets/boards/sparky2/firmware/pios_board.c +++ b/flight/targets/boards/sparky2/firmware/pios_board.c @@ -41,7 +41,7 @@ #endif #include - +#include /* * Pull in the board-specific static HW definitions. @@ -57,24 +57,24 @@ uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; static const PIOS_BOARD_IO_UART_Function rcvr_function_map[] = { - [HWSETTINGS_SPK2_RCVRPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, - [HWSETTINGS_SPK2_RCVRPORT_DSM] = PIOS_BOARD_IO_UART_DSM_RCVR, - [HWSETTINGS_SPK2_RCVRPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, - [HWSETTINGS_SPK2_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, - [HWSETTINGS_SPK2_RCVRPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_SPK2_RCVRPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_SPK2_RCVRPORT_DSM] = PIOS_BOARD_IO_UART_DSM_RCVR, + [HWSETTINGS_SPK2_RCVRPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_SPK2_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_SPK2_RCVRPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, [HWSETTINGS_SPK2_RCVRPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, [HWSETTINGS_SPK2_RCVRPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, }; static const PIOS_BOARD_IO_UART_Function main_function_map[] = { - [HWSETTINGS_SPK2_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, - [HWSETTINGS_SPK2_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, - [HWSETTINGS_SPK2_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_SPK2_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_SPK2_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_SPK2_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, [HWSETTINGS_SPK2_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, [HWSETTINGS_SPK2_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, - [HWSETTINGS_SPK2_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, - [HWSETTINGS_SPK2_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, - [HWSETTINGS_SPK2_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_SPK2_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_SPK2_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_SPK2_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { @@ -96,25 +96,25 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, - [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, + [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, }; int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - + switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_rcvr_cfg.regs) { /* rcvr port */ - GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, - RCVR_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? RCVR_USART_INVERTER_ENABLE : RCVR_USART_INVERTER_DISABLE); - - return 0; - } - break; + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_rcvr_cfg.regs) { /* rcvr port */ + GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, + RCVR_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? RCVR_USART_INVERTER_ENABLE : RCVR_USART_INVERTER_DISABLE); + + return 0; + } + break; } - + return -1; } @@ -140,12 +140,12 @@ void PIOS_Board_Init(void) #endif /* Set up the SPI interface to the gyro/acelerometer */ - if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + if (PIOS_SPI_Init(&pios_spi_gyro_adapter_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ - if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { + if (PIOS_SPI_Init(&pios_spi_telem_flash_adapter_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } @@ -154,7 +154,7 @@ void PIOS_Board_Init(void) uintptr_t flash_id = 0; // Initialize the external USER flash - if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_adapter_id, 1)) { PIOS_DEBUG_Assert(0); } @@ -224,26 +224,24 @@ void PIOS_Board_Init(void) /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsSPK2_FlexiPortGet(&hwsettings_flexiport); - + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } - + #if defined(PIOS_INCLUDE_I2C) /* Set up internal I2C bus */ - if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_DELAY_WaitmS(50); - + if (hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } PIOS_DELAY_WaitmS(50); } - - PIOS_BOARD_IO_Configure_I2C(pios_i2c_mag_pressure_adapter_id, pios_i2c_flexiport_adapter_id); #endif /* Moved this here to allow binding on flexiport */ @@ -259,16 +257,17 @@ void PIOS_Board_Init(void) uint8_t hwsettings_mainport; HwSettingsSPK2_MainPortGet(&hwsettings_mainport); + if (hwsettings_mainport < NELEMENTS(main_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - + #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioaux; HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - - if(hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { - PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); + + if (hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { + PIOS_BOARD_IO_Configure_RFM22B(radioaux_function_map[hwsettings_radioaux]); } #endif /* PIOS_INCLUDE_RFM22B */ @@ -281,7 +280,7 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }; - + GPIO_Init(RCVR_USART_INVERTER_GPIO, &inverterGPIOInit); GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, RCVR_USART_INVERTER_PIN, @@ -292,16 +291,16 @@ void PIOS_Board_Init(void) // Sparky2 receiver input on PC7 TIM8 CH2 // include PPM,S.Bus,DSM,SRXL,IBus,EX.Bus,HoTT SUMD,HoTT SUMH /* Configure the receiver port*/ - + uint8_t hwsettings_rcvrport; HwSettingsSPK2_RcvrPortGet(&hwsettings_rcvrport); - + if (hwsettings_rcvrport < NELEMENTS(rcvr_function_map)) { PIOS_BOARD_IO_Configure_UART(&pios_usart_rcvr_cfg, rcvr_function_map[hwsettings_rcvrport]); } #if defined(PIOS_INCLUDE_PPM) - if(hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_PPM) { + if (hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_PPM) { PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); } #endif @@ -316,12 +315,7 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif -#if defined(PIOS_INCLUDE_MPU9250) - PIOS_MPU9250_Init(pios_spi_gyro_id, 0, &pios_mpu9250_cfg); - PIOS_MPU9250_CONFIG_Configure(); - PIOS_MPU9250_MainRegister(); - PIOS_MPU9250_MagRegister(); -#endif + PIOS_BOARD_Sensors_Configure(); #ifdef PIOS_INCLUDE_WS2811 HwSettingsWS2811LED_OutOptions ws2811_pin_settings; @@ -342,10 +336,7 @@ void PIOS_Board_Init(void) .GPIO_OType = GPIO_OType_OD, }; GPIO_Init(GPIOA, &gpioA8); - - PIOS_BOARD_IO_Configure_ADC(); #endif /* PIOS_INCLUDE_ADC */ - } /** diff --git a/flight/targets/boards/sparky2/pios_board.h b/flight/targets/boards/sparky2/pios_board.h index 37fe24a12..d63e5cf3e 100644 --- a/flight/targets/boards/sparky2/pios_board.h +++ b/flight/targets/boards/sparky2/pios_board.h @@ -105,30 +105,35 @@ // PIOS_SPI // See also pios_board.c // ------------------------ -#define PIOS_SPI_MAX_DEVS 3 +#define PIOS_SPI_MAX_DEVS 3 +extern uint32_t pios_spi_telem_flash_adapter_id; +#define PIOS_SPI_RFM22B_ADAPTER (pios_spi_telem_flash_adapter_id) +extern uint32_t pios_spi_gyro_adapter_id; +#define PIOS_SPI_MPU9250_ADAPTER (pios_spi_gyro_adapter_id) // ------------------------ // PIOS_WDG // ------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 // ------------------------ // PIOS_I2C // See also pios_board.c // ------------------------ -#define PIOS_I2C_MAX_DEVS 3 -extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_MAX_DEVS 3 +extern uint32_t pios_i2c_pressure_adapter_id; +#define PIOS_I2C_MS5611_INTERNAL_ADAPTER (pios_i2c_pressure_adapter_id) +#define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_pressure_adapter_id) extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) -#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- // PIOS_USART @@ -142,7 +147,7 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 4 +#define PIOS_COM_MAX_DEVS 4 // ------------------------- // Packet Handler From 16020c18390fcdf100547ab8632a08536c3a524b Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sat, 15 Apr 2017 01:48:13 +0200 Subject: [PATCH 07/20] LP-480 GPSplatinum pios_usart fixes. Bootloader now fits again. --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- flight/pios/common/pios_com.c | 4 ++ flight/pios/stm32f0x/pios_usart.c | 52 ++++++++++++++----- .../boards/gpsplatinum/board_hw_defs.c | 34 ++++++------ .../gpsplatinum/bootloader/pios_board.c | 3 ++ .../boards/gpsplatinum/firmware/pios_board.c | 2 + 6 files changed, 65 insertions(+), 32 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 7e01ae919..c224603ce 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_revolution; + buildArgumentsString = ef_gpsplatinum; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index 26794095f..1a813ba3c 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -119,12 +119,16 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui PIOS_Assert(driver); if ((rx_buffer_len > 0) && !rx_buffer) { +#if defined(PIOS_INCLUDE_FREERTOS) rx_buffer = (uint8_t *)pios_malloc(rx_buffer_len); +#endif PIOS_Assert(rx_buffer); } if ((tx_buffer_len > 0) && !tx_buffer) { +#if defined(PIOS_INCLUDE_FREERTOS) tx_buffer = (uint8_t *)pios_malloc(tx_buffer_len); +#endif PIOS_Assert(tx_buffer); } diff --git a/flight/pios/stm32f0x/pios_usart.c b/flight/pios/stm32f0x/pios_usart.c index 0ce5de3cc..21a1dceed 100644 --- a/flight/pios/stm32f0x/pios_usart.c +++ b/flight/pios/stm32f0x/pios_usart.c @@ -38,7 +38,9 @@ #define PIOS_UART_TX_BUFFER 10 /* Provide a COM driver */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); +#ifndef BOOTLOADER static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); +#endif static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); @@ -46,7 +48,9 @@ static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { .set_baud = PIOS_USART_ChangeBaud, +#ifndef BOOTLOADER .set_config = PIOS_USART_ChangeConfig, +#endif .tx_start = PIOS_USART_TxStart, .rx_start = PIOS_USART_RxStart, .bind_tx_cb = PIOS_USART_RegisterTxCallback, @@ -71,6 +75,7 @@ struct pios_usart_dev { uint8_t tx_buffer[PIOS_UART_TX_BUFFER]; uint8_t tx_len; uint8_t tx_pos; + uint8_t irq_channel; }; static struct pios_usart_dev *PIOS_USART_validate(uint32_t usart_id) @@ -138,13 +143,27 @@ static void PIOS_USART_2_irq_handler(void) { PIOS_USART_generic_irq_handler(PIOS_USART_2_id); } - +#if defined(STM32F072) static uint32_t PIOS_USART_3_id; void USART3_IRQHandler(void) __attribute__((alias("PIOS_USART_3_irq_handler"))); static void PIOS_USART_3_irq_handler(void) { PIOS_USART_generic_irq_handler(PIOS_USART_3_id); } +#endif + +static int32_t PIOS_USART_SetIrqPrio(struct pios_usart_dev *usart_dev, uint8_t irq_prio) +{ + NVIC_InitTypeDef init = { + .NVIC_IRQChannel = usart_dev->irq_channel, + .NVIC_IRQChannelPriority = irq_prio, + .NVIC_IRQChannelCmd = ENABLE, + }; + + NVIC_Init(&init); + + return 0; +} /** * Initialise a single USART device @@ -162,10 +181,10 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) } /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; + usart_dev->cfg = cfg; - /* Copy the comm parameter structure */ - usart_dev->init = cfg->init; + /* Initialize the comm parameter structure */ + USART_StructInit(&usart_dev->init); // 9600 8n1 /* Enable the USART Pins Software Remapping */ if (usart_dev->cfg->remap) { @@ -182,22 +201,27 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) case (uint32_t)USART1: RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); PIOS_USART_1_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART1_IRQn; break; case (uint32_t)USART2: RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); PIOS_USART_2_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART2_IRQn; break; +#if defined(STM32F072) case (uint32_t)USART3: RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); PIOS_USART_3_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART3_4_IRQn; break; +#endif } /* Configure the USART */ USART_Init(cfg->regs, (USART_InitTypeDef *)&usart_dev->init); *usart_id = (uint32_t)usart_dev; - NVIC_Init((NVIC_InitTypeDef *)&cfg->irq.init); + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); USART_ITConfig(cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(cfg->regs, USART_IT_TXE, ENABLE); USART_ITConfig(cfg->regs, USART_IT_ORE, DISABLE); @@ -238,7 +262,7 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) USART_Cmd(usart_dev->cfg->regs, ENABLE); } - +#ifndef BOOTLOADER /** * Changes configuration of the USART peripheral without re-initialising. * \param[in] usart_id USART name (GPS, TELEM, AUX) @@ -323,6 +347,7 @@ static void PIOS_USART_ChangeConfig(uint32_t usart_id, */ USART_Cmd(usart_dev->cfg->regs, ENABLE); } +#endif /* BOOTLOADER */ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { @@ -358,8 +383,8 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) /* Check if RXNE flag is set */ bool rx_need_yield = false; - if (USART_GetITStatus(usart_dev->cfg->regs, USART_IT_RXNE) != RESET) { - uint8_t byte = USART_ReceiveData(usart_dev->cfg->regs) & 0x00FF; + if (usart_dev->cfg->regs->ISR & USART_ISR_RXNE) { + uint8_t byte = usart_dev->cfg->regs->RDR & 0xFF; if (usart_dev->rx_in_cb) { uint16_t rc; rc = (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); @@ -372,7 +397,7 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) /* Check if TXE flag is set */ bool tx_need_yield = false; - if (USART_GetITStatus(usart_dev->cfg->regs, USART_IT_TXE) != RESET) { + if (usart_dev->cfg->regs->ISR & USART_ISR_TXE) { if (usart_dev->tx_out_cb) { if (!usart_dev->tx_len) { usart_dev->tx_len = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, usart_dev->tx_buffer, PIOS_UART_TX_BUFFER, NULL, &tx_need_yield); @@ -380,19 +405,18 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) } if (usart_dev->tx_len > 0) { /* Send the byte we've been given */ - USART_SendData(usart_dev->cfg->regs, usart_dev->tx_buffer[usart_dev->tx_pos++]); + usart_dev->cfg->regs->TDR = usart_dev->tx_buffer[usart_dev->tx_pos++]; usart_dev->tx_len--; } else { /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + usart_dev->cfg->regs->CR1 &= ~(USART_CR1_TXEIE); } } else { /* No bytes to send, disable TXE interrupt */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + usart_dev->cfg->regs->CR1 &= ~(USART_CR1_TXEIE); } } - USART_ClearITPendingBit(usart_dev->cfg->regs, USART_IT_ORE); - USART_ClearITPendingBit(usart_dev->cfg->regs, USART_IT_TC); + usart_dev->cfg->regs->ICR = USART_ICR_ORECF | USART_ICR_TCCF; #if defined(PIOS_INCLUDE_FREERTOS) if (rx_need_yield || tx_need_yield) { vPortYield(); diff --git a/flight/targets/boards/gpsplatinum/board_hw_defs.c b/flight/targets/boards/gpsplatinum/board_hw_defs.c index 83b7f4528..1243db383 100644 --- a/flight/targets/boards/gpsplatinum/board_hw_defs.c +++ b/flight/targets/boards/gpsplatinum/board_hw_defs.c @@ -268,22 +268,22 @@ static const struct pios_hmc5x83_cfg pios_mag_cfg = { static const struct pios_usart_cfg pios_usart_generic_main_cfg = { .regs = USART1, .remap = GPIO_AF_1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { +// .init = { +// .USART_BaudRate = 57600, +// .USART_WordLength = USART_WordLength_8b, +// .USART_Parity = USART_Parity_No, +// .USART_StopBits = USART_StopBits_1, +// .USART_HardwareFlowControl = USART_HardwareFlowControl_None, +// .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, +// }, +// .irq = { +// .init = { +// .NVIC_IRQChannel = USART1_IRQn, +// .NVIC_IRQChannelPriority = PIOS_IRQ_PRIO_MID, +// .NVIC_IRQChannelCmd = ENABLE, +// }, +// }, + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -292,7 +292,7 @@ static const struct pios_usart_cfg pios_usart_generic_main_cfg = { .GPIO_Mode = GPIO_Mode_AF, }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, diff --git a/flight/targets/boards/gpsplatinum/bootloader/pios_board.c b/flight/targets/boards/gpsplatinum/bootloader/pios_board.c index 14bddba5e..8c3d730df 100644 --- a/flight/targets/boards/gpsplatinum/bootloader/pios_board.c +++ b/flight/targets/boards/gpsplatinum/bootloader/pios_board.c @@ -70,9 +70,12 @@ void setupCom() if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } + if (PIOS_COM_Init(&PIOS_COM_TELEM_USB, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_MAIN_RX_BUF_LEN, tx_buffer, PIOS_COM_MAIN_TX_BUF_LEN)) { PIOS_Assert(0); } + + PIOS_COM_ChangeBaud(PIOS_COM_TELEM_USB, 57600); } diff --git a/flight/targets/boards/gpsplatinum/firmware/pios_board.c b/flight/targets/boards/gpsplatinum/firmware/pios_board.c index d8b29bd22..4bc62fda3 100644 --- a/flight/targets/boards/gpsplatinum/firmware/pios_board.c +++ b/flight/targets/boards/gpsplatinum/firmware/pios_board.c @@ -114,6 +114,8 @@ void PIOS_Board_Init(void) tx_buffer, PIOS_COM_MAIN_TX_BUF_LEN)) { PIOS_Assert(0); } + + PIOS_COM_ChangeBaud(pios_com_main_id, 57600); } #endif From a6f7cc6cdf312c577b22da95c1e3787ce2463a55 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sat, 15 Apr 2017 01:48:53 +0200 Subject: [PATCH 08/20] LP-480 gpsplatinum board_hw_defs cleanup --- flight/targets/boards/gpsplatinum/board_hw_defs.c | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/flight/targets/boards/gpsplatinum/board_hw_defs.c b/flight/targets/boards/gpsplatinum/board_hw_defs.c index 1243db383..489d49bf5 100644 --- a/flight/targets/boards/gpsplatinum/board_hw_defs.c +++ b/flight/targets/boards/gpsplatinum/board_hw_defs.c @@ -268,21 +268,6 @@ static const struct pios_hmc5x83_cfg pios_mag_cfg = { static const struct pios_usart_cfg pios_usart_generic_main_cfg = { .regs = USART1, .remap = GPIO_AF_1, -// .init = { -// .USART_BaudRate = 57600, -// .USART_WordLength = USART_WordLength_8b, -// .USART_Parity = USART_Parity_No, -// .USART_StopBits = USART_StopBits_1, -// .USART_HardwareFlowControl = USART_HardwareFlowControl_None, -// .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, -// }, -// .irq = { -// .init = { -// .NVIC_IRQChannel = USART1_IRQn, -// .NVIC_IRQChannelPriority = PIOS_IRQ_PRIO_MID, -// .NVIC_IRQChannelCmd = ENABLE, -// }, -// }, .rx = { .gpio = GPIOA, .init = { From 83326eaca3c59529270e69d2013d85f4337d8606 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 16 Apr 2017 02:38:42 +0200 Subject: [PATCH 09/20] LP-480 all targets build nicely. --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- flight/pios/common/pios_board_io.c | 83 +- flight/pios/common/pios_board_sensors.c | 39 +- flight/pios/inc/pios_board_hw.h | 11 +- flight/pios/inc/pios_board_io.h | 31 +- .../boards/coptercontrol/board_hw_defs.c | 13 +- .../coptercontrol/bootloader/pios_board.c | 7 - .../boards/discoveryf4bare/board_hw_defs.c | 4 +- .../discoveryf4bare/bootloader/pios_board.c | 5 - .../boards/gpsplatinum/board_hw_defs.c | 2 +- .../targets/boards/oplinkmini/board_hw_defs.c | 7 +- .../boards/oplinkmini/bootloader/main.c | 1 + .../boards/oplinkmini/bootloader/pios_board.c | 2 +- flight/targets/boards/oplinkmini/pios_board.h | 5 +- flight/targets/boards/osd/board_hw_defs.c | 100 +- .../boards/osd/bootloader/pios_board.c | 11 +- .../targets/boards/osd/firmware/pios_board.c | 139 +-- .../targets/boards/revolution/board_hw_defs.c | 16 +- .../boards/revolution/bootloader/pios_board.c | 13 +- .../targets/boards/revonano/board_hw_defs.c | 2 + .../boards/revonano/bootloader/pios_board.c | 5 - .../targets/boards/revoproto/board_hw_defs.c | 753 +++++++--------- .../boards/revoproto/bootloader/main.c | 1 + .../boards/revoproto/bootloader/pios_board.c | 9 +- .../boards/revoproto/firmware/pios_board.c | 853 +++--------------- flight/targets/boards/revoproto/pios_board.h | 77 +- flight/targets/boards/sparky2/board_hw_defs.c | 4 +- .../targets/boards/sparky2/bootloader/main.c | 1 + .../boards/sparky2/bootloader/pios_board.c | 6 - 29 files changed, 655 insertions(+), 1547 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index c224603ce..d2a27e574 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_gpsplatinum; + buildArgumentsString = ef_osd; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 6ee788051..6b639c150 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -87,6 +87,10 @@ uint32_t pios_rfm22b_id; /* RFM22B handle */ uint32_t pios_com_rf_id; /* RFM22B telemetry */ #endif +#ifdef PIOS_INCLUDE_OPENLRS +uint32_t pios_openlrs_id; /* OpenLRS handle */ +#endif + uint32_t pios_com_hkosd_id; /* HK OSD ?? */ uint32_t pios_com_msp_id; /* MSP */ uint32_t pios_com_mavlink_id; /* MAVLink */ @@ -157,18 +161,47 @@ static void PIOS_BOARD_IO_HID_Init(uint32_t *com_id, uint16_t rx_buf_len, uint16 } } +PIOS_BOARD_IO_USB_HID_Function PIOS_BOARD_IO_HWSettings_USB_HID_Function() +{ + static const PIOS_BOARD_IO_USB_HID_Function map[] = { + [HWSETTINGS_USB_HIDPORT_USBTELEMETRY] = PIOS_BOARD_IO_USB_HID_TELEMETRY, + [HWSETTINGS_USB_HIDPORT_RCTRANSMITTER] = PIOS_BOARD_IO_USB_HID_RCTX, + }; + uint8_t hwsettings_usb_hidport; + + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + + return (hwsettings_usb_hidport < NELEMENTS(map)) ? map[hwsettings_usb_hidport] : PIOS_BOARD_IO_USB_HID_NONE; +} + +PIOS_BOARD_IO_USB_VCP_Function PIOS_BOARD_IO_HWSettings_USB_VCP_Function() +{ + static const PIOS_BOARD_IO_USB_VCP_Function map[] = { + [HWSETTINGS_USB_VCPPORT_USBTELEMETRY] = PIOS_BOARD_IO_USB_VCP_TELEMETRY, + [HWSETTINGS_USB_VCPPORT_COMBRIDGE] = PIOS_BOARD_IO_USB_VCP_COMBRIDGE, + [HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE, + [HWSETTINGS_USB_VCPPORT_MAVLINK] = PIOS_BOARD_IO_USB_VCP_MAVLINK, + }; + + uint8_t hwsettings_usb_vcpport; + + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + + return (hwsettings_usb_vcpport < NELEMENTS(map)) ? map[hwsettings_usb_vcpport] : PIOS_BOARD_IO_USB_VCP_NONE; +} + void PIOS_BOARD_IO_Configure_USB() +{ + PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_HWSettings_USB_HID_Function(), + PIOS_BOARD_IO_HWSettings_USB_VCP_Function()); +} + +void PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_USB_HID_Function hid_function, __attribute__((unused)) PIOS_BOARD_IO_USB_VCP_Function vcp_function) { /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - #if defined(PIOS_INCLUDE_USB_CDC) - uint8_t hwsettings_usb_vcpport; - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } @@ -182,21 +215,21 @@ void PIOS_BOARD_IO_Configure_USB() PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(pios_board_info_blob.board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: + switch (vcp_function) { + case PIOS_BOARD_IO_USB_VCP_NONE: break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: + case PIOS_BOARD_IO_USB_VCP_TELEMETRY: PIOS_BOARD_IO_VCP_Init(&pios_com_telem_usb_id, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, pios_usb_id); break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: + case PIOS_BOARD_IO_USB_VCP_COMBRIDGE: PIOS_BOARD_IO_VCP_Init(&pios_com_vcp_id, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, pios_usb_id); break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: + case PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_BOARD_IO_VCP_Init(&pios_com_debug_id, 0, PIOS_COM_BRIDGE_TX_BUF_LEN, pios_usb_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; - case HWSETTINGS_USB_VCPPORT_MAVLINK: + case PIOS_BOARD_IO_USB_VCP_MAVLINK: PIOS_BOARD_IO_VCP_Init(&pios_com_mavlink_id, PIOS_COM_MAVLINK_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, pios_usb_id); break; } @@ -205,13 +238,13 @@ void PIOS_BOARD_IO_Configure_USB() #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: + switch (hid_function) { + case PIOS_BOARD_IO_USB_HID_NONE: break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: + case PIOS_BOARD_IO_USB_HID_TELEMETRY: PIOS_BOARD_IO_HID_Init(&pios_com_telem_usb_id, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, pios_usb_id); break; - case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: + case PIOS_BOARD_IO_USB_HID_RCTX: #if defined(PIOS_INCLUDE_USB_RCTX) if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { PIOS_Assert(0); @@ -274,9 +307,11 @@ struct uart_function { uint32_t *com_id; uint16_t com_rx_buf_len; uint16_t com_tx_buf_len; +#ifdef PIOS_INCLUDE_RCVR int32_t (*rcvr_init)(uint32_t *target, const struct pios_com_driver *driver, uint32_t lower_id); const struct pios_rcvr_driver *rcvr_driver; ManualControlSettingsChannelGroupsOptions rcvr_group; +#endif }; static const struct uart_function uart_function_map[] = { @@ -297,13 +332,13 @@ static const struct uart_function uart_function_map[] = { .com_rx_buf_len = PIOS_COM_MSP_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_MSP_TX_BUF_LEN, }, - +#ifdef PIOS_INCLUDE_GPS [PIOS_BOARD_IO_UART_GPS] = { .com_id = &pios_com_gps_id, .com_rx_buf_len = PIOS_COM_GPS_RX_BUF_LEN, .com_tx_buf_len = PIOS_COM_GPS_TX_BUF_LEN, }, - +#endif [PIOS_BOARD_IO_UART_OSDHK] = { .com_id = &pios_com_hkosd_id, .com_rx_buf_len = PIOS_COM_HKOSD_RX_BUF_LEN, @@ -409,7 +444,9 @@ void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *hw_config, PIOS_B 0, uart_function_map[function].com_tx_buf_len)) { PIOS_Assert(0); } - } else if (uart_function_map[function].rcvr_init) { + } +#ifdef PIOS_INCLUDE_RCVR + else if (uart_function_map[function].rcvr_init) { uint32_t usart_id; if (PIOS_USART_Init(&usart_id, hw_config)) { @@ -429,6 +466,7 @@ void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *hw_config, PIOS_B pios_rcvr_group_map[uart_function_map[function].rcvr_group] = rcvr_id; } +#endif /* PIOS_INCLUDE_RCVR */ } #ifdef PIOS_INCLUDE_PWM @@ -513,13 +551,12 @@ void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_fun if (is_enabled) { if (openlrs) { #if defined(PIOS_INCLUDE_OPENLRS_RCVR) && defined(PIOS_INCLUDE_RCVR) - uint32_t openlrs_id; const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(pios_board_info_blob.board_rev); - PIOS_OpenLRS_Init(&openlrs_id, PIOS_SPI_RFM22B_ADAPTER, 0, openlrs_cfg); + PIOS_OpenLRS_Init(&pios_openlrs_id, PIOS_SPI_RFM22B_ADAPTER, 0, openlrs_cfg); uint32_t openlrsrcvr_id; - PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id); + PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, pios_openlrs_id); uint32_t openlrsrcvr_rcvr_id; if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) { @@ -529,7 +566,7 @@ void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_fun #endif /* PIOS_INCLUDE_OPENLRS_RCVR && PIOS_INCLUDE_RCVR */ } else { /* Configure the RFM22B device. */ - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(pios_board_info_blob.board_rev); + const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22bCfg(pios_board_info_blob.board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_SPI_RFM22B_ADAPTER, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { PIOS_Assert(0); diff --git a/flight/pios/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c index fecfe17dd..3b0e58b6c 100644 --- a/flight/pios/common/pios_board_sensors.c +++ b/flight/pios/common/pios_board_sensors.c @@ -42,7 +42,7 @@ # include #endif -#if defined(PIOS_INCLUDE_ADXL345) +#ifdef PIOS_INCLUDE_ADXL345 # include #endif @@ -51,13 +51,21 @@ # include #endif -#if defined(PIOS_INCLUDE_HMC5X83) +#ifdef PIOS_INCLUDE_HMC5X83 # include "pios_hmc5x83.h" -# if defined(PIOS_HMC5X83_HAS_GPIOS) +# ifdef PIOS_HMC5X83_HAS_GPIOS pios_hmc5x83_dev_t pios_hmc5x83_internal_id; # endif #endif +#ifdef PIOS_INCLUDE_L3GD20 +# include "pios_l3gd20.h" +#endif + +#ifdef PIOS_INCLUDE_BMA180 +# include "pios_bma180.h" +#endif + #ifdef PIOS_INCLUDE_ADC # include "pios_adc_priv.h" #endif @@ -75,10 +83,6 @@ void PIOS_BOARD_Sensors_Configure() } #endif /* PIOS_INCLUDE_MPU6000 */ -#ifdef PIOS_INCLUDE_ADXL345 - PIOS_ADXL345_Init(PIOS_SPI_ADXL345_ADAPTER, 0); -#endif - #ifdef PIOS_INCLUDE_MPU9250 const struct pios_mpu9250_cfg *mpu9250_cfg = PIOS_BOARD_HW_DEFS_GetMPU9250Cfg(pios_board_info_blob.board_rev); if (mpu9250_cfg) { @@ -89,6 +93,27 @@ void PIOS_BOARD_Sensors_Configure() } #endif /* PIOS_INCLUDE_MPU9250 */ +#ifdef PIOS_INCLUDE_ADXL345 + if (PIOS_BOARD_HW_DEFS_GetADXL345Cfg(pios_board_info_blob.board_rev)) { + PIOS_ADXL345_Init(PIOS_SPI_ADXL345_ADAPTER, 0); + } +#endif +#if defined(PIOS_INCLUDE_L3GD20) + const struct pios_l3gd20_cfg *l3gd20_cfg = PIOS_BOARD_HW_DEFS_GetL3GD20Cfg(pios_board_info_blob.board_rev); + if (l3gd20_cfg) { + PIOS_Assert(0); // L3gd20 has not been ported to Sensor framework!!! + PIOS_L3GD20_Init(PIOS_SPI_L3GD20_ADAPTER, 0, l3gd20_cfg); + PIOS_Assert(PIOS_L3GD20_Test() == 0); + } +#endif +#if defined(PIOS_INCLUDE_BMA180) + const struct pios_bma180_cfg *bma180_cfg = PIOS_BOARD_HW_DEFS_GetBMA180Cfg(pios_board_info_blob.board_rev); + if (bma180_cfg) { + PIOS_Assert(0); // BMA180 has not been ported to Sensor framework!!! + PIOS_BMA180_Init(PIOS_SPI_BMA180_ADAPTER, 0, bma180_cfg); + PIOS_Assert(PIOS_BMA180_Test() == 0); + } +#endif // internal HMC5x83 mag # ifdef PIOS_INCLUDE_HMC5X83_INTERNAL diff --git a/flight/pios/inc/pios_board_hw.h b/flight/pios/inc/pios_board_hw.h index 4c786d99a..53d3054f8 100644 --- a/flight/pios/inc/pios_board_hw.h +++ b/flight/pios/inc/pios_board_hw.h @@ -33,7 +33,7 @@ const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision); #endif #ifdef PIOS_INCLUDE_RFM22B -const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision); +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22bCfg(uint32_t board_revision); #endif #ifdef PIOS_INCLUDE_OPENLRS const struct pios_openlrs_cfg *PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(uint32_t board_revision); @@ -59,4 +59,13 @@ const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(uint32_t board_r #ifdef PIOS_INCLUDE_MPU9250 const struct pios_mpu9250_cfg *PIOS_BOARD_HW_DEFS_GetMPU9250Cfg(uint32_t board_revision); #endif +#ifdef PIOS_INCLUDE_ADXL345 +bool PIOS_BOARD_HW_DEFS_GetADXL345Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_L3GD20 +const struct pios_l3gd20_cfg *PIOS_BOARD_HW_DEFS_GetL3GD20Cfg(uint32_t board_revision); +#endif +#ifdef PIOS_INCLUDE_BMA180 +const struct pios_bma180_cfg *PIOS_BOARD_HW_DEFS_GetBMA180Cfg(uint32_t board_revision); +#endif #endif /* PIOS_BOARD_HW_H */ diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 9ba4dc0d0..d186f5ea5 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -95,7 +95,6 @@ extern uint32_t pios_com_telem_rf_id; # define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 #endif - /* RFM22B telemetry */ #ifdef PIOS_INCLUDE_RFM22B extern uint32_t pios_rfm22b_id; /* RFM22B handle */ @@ -109,6 +108,10 @@ extern uint32_t pios_com_rf_id; /* oplink telemetry */ # endif #endif +#ifdef PIOS_INCLUDE_OPENLRS +extern uint32_t pios_openlrs_id; /* openlrs handle */ +#endif + /* HK OSD ?? */ extern uint32_t pios_com_hkosd_id; @@ -175,7 +178,7 @@ extern pios_hmc5x83_dev_t pios_hmc5x83_internal_id; #endif typedef enum { - PIOS_BOARD_IO_UART_NONE, + PIOS_BOARD_IO_UART_NONE = 0, PIOS_BOARD_IO_UART_TELEMETRY, /* com */ PIOS_BOARD_IO_UART_MAVLINK, /* com */ PIOS_BOARD_IO_UART_MSP, /* com */ @@ -197,7 +200,7 @@ typedef enum { } PIOS_BOARD_IO_UART_Function; typedef enum { - PIOS_BOARD_IO_RADIOAUX_NONE, + PIOS_BOARD_IO_RADIOAUX_NONE = 0, PIOS_BOARD_IO_RADIOAUX_MAVLINK, PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, @@ -206,11 +209,23 @@ typedef enum { #ifdef PIOS_INCLUDE_USB void PIOS_BOARD_IO_Configure_USB(); -// # if defined(PIOS_INCLUDE_USB_HID) -// # include -// extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; -// # endif /* PIOS_INCLUDE_USB_HID */ -#endif /* PIOS_INCLUDE_USB */ + +typedef enum { + PIOS_BOARD_IO_USB_HID_NONE = 0, + PIOS_BOARD_IO_USB_HID_TELEMETRY, + PIOS_BOARD_IO_USB_HID_RCTX, +} PIOS_BOARD_IO_USB_HID_Function; + +typedef enum { + PIOS_BOARD_IO_USB_VCP_NONE = 0, + PIOS_BOARD_IO_USB_VCP_TELEMETRY, + PIOS_BOARD_IO_USB_VCP_COMBRIDGE, + PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE, + PIOS_BOARD_IO_USB_VCP_MAVLINK, +} PIOS_BOARD_IO_USB_VCP_Function; + +void PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_USB_HID_Function hid_function, PIOS_BOARD_IO_USB_VCP_Function vcp_function); +#endif #ifdef PIOS_INCLUDE_PWM void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); #endif diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 9fde0bb45..d6b93ff4c 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -486,9 +486,9 @@ void PIOS_ADC_handler() PIOS_ADC_DMA_Handler(); } -const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(uint32_t board_revision) { - return &pios_adc_cfg; + return (board_revision == BOARD_REVISION_CC) ? &pios_adc_cfg : 0; } #endif /* if defined(PIOS_INCLUDE_ADC) */ @@ -1044,6 +1044,13 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) { - return &pios_mpu6000_cfg; + return (board_revision == BOARD_REVISION_CC3D) ? &pios_mpu6000_cfg : 0; } #endif /* PIOS_INCLUDE_MPU6000 */ + +#ifdef PIOS_INCLUDE_ADXL345 +bool PIOS_BOARD_HW_DEFS_GetADXL345Cfg(uint32_t board_revision) +{ + return board_revision == BOARD_REVISION_CC; +} +#endif diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c index 466566d55..39c4e9e8a 100644 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -49,13 +49,6 @@ uint32_t pios_com_telem_usb_id; */ static bool board_init_complete = false; -#if defined(PIOS_INCLUDE_USART) -static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) -{ - return -1; -} -#endif - void PIOS_Board_Init(void) { if (board_init_complete) { diff --git a/flight/targets/boards/discoveryf4bare/board_hw_defs.c b/flight/targets/boards/discoveryf4bare/board_hw_defs.c index bcae0c028..15b2012a8 100644 --- a/flight/targets/boards/discoveryf4bare/board_hw_defs.c +++ b/flight/targets/boards/discoveryf4bare/board_hw_defs.c @@ -537,7 +537,7 @@ const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = { .gpio_direction = GPIO0_TX_GPIO1_RX, }; -const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision) +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22bCfg(uint32_t board_revision) { switch (board_revision) { case 2: @@ -600,6 +600,7 @@ static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = { }; #endif /* PIOS_INCLUDE_FLASH */ +#ifdef PIOS_INCLUDE_USART #include /* @@ -708,6 +709,7 @@ static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .pin_source = GPIO_PinSource7, } }; +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) diff --git a/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c b/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c index b867b81d7..fd64e58fa 100644 --- a/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c @@ -40,11 +40,6 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) -{ - return -1; -} - void PIOS_Board_Init() { if (board_init_complete) { diff --git a/flight/targets/boards/gpsplatinum/board_hw_defs.c b/flight/targets/boards/gpsplatinum/board_hw_defs.c index 489d49bf5..7b19737f4 100644 --- a/flight/targets/boards/gpsplatinum/board_hw_defs.c +++ b/flight/targets/boards/gpsplatinum/board_hw_defs.c @@ -268,7 +268,7 @@ static const struct pios_hmc5x83_cfg pios_mag_cfg = { static const struct pios_usart_cfg pios_usart_generic_main_cfg = { .regs = USART1, .remap = GPIO_AF_1, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index 0810721cb..85130655d 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -307,7 +307,7 @@ struct pios_rfm22b_cfg pios_rfm22b_cfg = { }; // ! Compatibility layer for various hardware revisions -const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(__attribute__((unused)) uint32_t board_revision) +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22bCfg(__attribute__((unused)) uint32_t board_revision) { return &pios_rfm22b_cfg; } @@ -795,6 +795,11 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { .vsense_active_low = false }; +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_usb_main_cfg; +} + #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" diff --git a/flight/targets/boards/oplinkmini/bootloader/main.c b/flight/targets/boards/oplinkmini/bootloader/main.c index 2dc3842ed..566caea73 100644 --- a/flight/targets/boards/oplinkmini/bootloader/main.c +++ b/flight/targets/boards/oplinkmini/bootloader/main.c @@ -34,6 +34,7 @@ #include #include #include +#include extern void FLASH_Download(); #define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1) diff --git a/flight/targets/boards/oplinkmini/bootloader/pios_board.c b/flight/targets/boards/oplinkmini/bootloader/pios_board.c index 4dad876dc..5eea6e97a 100644 --- a/flight/targets/boards/oplinkmini/bootloader/pios_board.c +++ b/flight/targets/boards/oplinkmini/bootloader/pios_board.c @@ -76,7 +76,7 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index ab7e3630a..e96c0b12d 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -159,12 +159,13 @@ extern uint32_t pios_i2c_flexi_adapter_id; // PIOS_SPI // See also pios_board.c // ------------------------ -#define PIOS_SPI_MAX_DEVS 1 +#define PIOS_SPI_MAX_DEVS 1 +#define PIOS_SPI_RFM22B_ADAPTER (pios_spi_rfm22b_id) // ------------------------- // PIOS_USART // ------------------------- -#define PIOS_USART_MAX_DEVS 3 +#define PIOS_USART_MAX_DEVS 3 // ------------------------- // PIOS_COM diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c index ed86e0915..d38d509ce 100644 --- a/flight/targets/boards/osd/board_hw_defs.c +++ b/flight/targets/boards/osd/board_hw_defs.c @@ -233,24 +233,7 @@ void PIOS_SPI_sdcard_irq_handler(void) static const struct pios_usart_cfg pios_usart_gps_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -260,7 +243,7 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -280,24 +263,7 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { static const struct pios_usart_cfg pios_usart_aux_cfg = { .regs = USART2, .remap = GPIO_AF_USART2, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_3, @@ -307,7 +273,7 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_2, @@ -328,24 +294,7 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { static const struct pios_usart_cfg pios_usart_telem_main_cfg = { .regs = UART4, .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_1, @@ -355,7 +304,7 @@ static const struct pios_usart_cfg pios_usart_telem_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_0, @@ -509,41 +458,12 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { .vsense_active_low = false }; -#include "pios_usb_board_data_priv.h" -#include "pios_usb_desc_hid_cdc_priv.h" -#include "pios_usb_desc_hid_only_priv.h" -#include "pios_usbhook.h" - +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_usb_main_cfg; +} #endif /* PIOS_INCLUDE_USB */ -#if defined(PIOS_INCLUDE_COM_MSG) - -#include - -#endif /* PIOS_INCLUDE_COM_MSG */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_VIDEO) diff --git a/flight/targets/boards/osd/bootloader/pios_board.c b/flight/targets/boards/osd/bootloader/pios_board.c index 7e9858f18..bbd7232e9 100644 --- a/flight/targets/boards/osd/bootloader/pios_board.c +++ b/flight/targets/boards/osd/bootloader/pios_board.c @@ -26,6 +26,15 @@ #include #include + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#include "pios_usbhook.h" + +#include + + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -64,7 +73,7 @@ void PIOS_Board_Init() #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index 680c93c2e..5a7f5649a 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -31,6 +31,8 @@ #include #include +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -84,25 +86,10 @@ void PIOS_ADC_DMC_irq_handler(void) static void Clock(uint32_t spektrum_id); - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128 - -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 - -#define PIOS_COM_GPS_RX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_AUX_RX_BUF_LEN 512 +#define PIOS_COM_AUX_TX_BUF_LEN 512 uint32_t pios_com_aux_id; -uint32_t pios_com_gps_id; -uint32_t pios_com_telem_usb_id; -uint32_t pios_com_telem_rf_id; uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id = 0; @@ -216,125 +203,9 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); - -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ - #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_GPS) diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 6edf839f9..e4ff0a76e 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -657,7 +657,7 @@ const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = { .gpio_direction = GPIO0_TX_GPIO1_RX, }; -const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(uint32_t board_revision) +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22bCfg(uint32_t board_revision) { switch (board_revision) { case 2: @@ -787,6 +787,7 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { #endif /* PIOS_INCLUDE_FLASH */ +#ifdef PIOS_INCLUDE_USART #include /* @@ -896,6 +897,7 @@ static const struct pios_usart_cfg pios_usart_flexiio_cfg = { .pin_source = GPIO_PinSource7, } }; +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -1447,20 +1449,8 @@ const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) } return NULL; } - -#include "pios_usb_board_data_priv.h" -#include "pios_usb_desc_hid_cdc_priv.h" -#include "pios_usb_desc_hid_only_priv.h" -#include "pios_usbhook.h" - #endif /* PIOS_INCLUDE_USB */ -#if defined(PIOS_INCLUDE_COM_MSG) - -#include - -#endif /* PIOS_INCLUDE_COM_MSG */ - #ifdef PIOS_INCLUDE_WS2811 #include #include diff --git a/flight/targets/boards/revolution/bootloader/pios_board.c b/flight/targets/boards/revolution/bootloader/pios_board.c index 595eac507..8c0394c04 100644 --- a/flight/targets/boards/revolution/bootloader/pios_board.c +++ b/flight/targets/boards/revolution/bootloader/pios_board.c @@ -27,6 +27,14 @@ #include #include + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#include "pios_usbhook.h" + +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -41,11 +49,6 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) -{ - return -1; -} - void PIOS_Board_Init() { if (board_init_complete) { diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index fb4d9f129..00540256a 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -238,6 +238,7 @@ void PIOS_SPI_gyro_irq_handler(void) #endif /* PIOS_INCLUDE_SPI */ +#ifdef PIOS_INCLUDE_USART #include /* @@ -308,6 +309,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { // }, // }, }; +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) diff --git a/flight/targets/boards/revonano/bootloader/pios_board.c b/flight/targets/boards/revonano/bootloader/pios_board.c index 595eac507..3639353b9 100644 --- a/flight/targets/boards/revonano/bootloader/pios_board.c +++ b/flight/targets/boards/revonano/bootloader/pios_board.c @@ -41,11 +41,6 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) -{ - return -1; -} - void PIOS_Board_Init() { if (board_init_complete) { diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c index bda92eb18..fb47680fe 100644 --- a/flight/targets/boards/revoproto/board_hw_defs.c +++ b/flight/targets/boards/revoproto/board_hw_defs.c @@ -195,7 +195,7 @@ static const struct pios_spi_cfg pios_spi_accel_cfg = { } }; -static uint32_t pios_spi_accel_id; +uint32_t pios_spi_accel_id; void PIOS_SPI_accel_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ @@ -612,7 +612,6 @@ void PIOS_OVERO_irq_handler(void) #endif /* PIOS_OVERO_SPI */ - #include #ifdef PIOS_INCLUDE_COM_TELEM @@ -622,24 +621,7 @@ void PIOS_OVERO_irq_handler(void) static const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .remap = GPIO_AF_USART2, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOD, .init = { .GPIO_Pin = GPIO_Pin_6, @@ -649,7 +631,7 @@ static const struct pios_usart_cfg pios_usart_telem_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOD, .init = { .GPIO_Pin = GPIO_Pin_5, @@ -670,24 +652,7 @@ static const struct pios_usart_cfg pios_usart_telem_cfg = { static const struct pios_usart_cfg pios_usart_gps_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -697,7 +662,7 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -718,24 +683,7 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { static const struct pios_usart_cfg pios_usart_aux_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOC, .init = { .GPIO_Pin = GPIO_Pin_7, @@ -745,7 +693,7 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOC, .init = { .GPIO_Pin = GPIO_Pin_6, @@ -763,27 +711,19 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { /* * AUX USART SBUS ( UART/ S Bus label on rev2) */ + +// Inverter for SBUS handling +#define MAIN_USART_INVERTER_GPIO GPIOC +#define MAIN_USART_INVERTER_PIN GPIO_Pin_3 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET + +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { .regs = UART4, .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_1, @@ -793,7 +733,7 @@ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_0, @@ -803,6 +743,7 @@ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; #endif /* PIOS_INCLUDE_COM_AUXSBUS */ @@ -814,24 +755,7 @@ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = - USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -841,7 +765,7 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -855,327 +779,6 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { #endif /* PIOS_INCLUDE_COM_FLEXI */ -#if defined(PIOS_INCLUDE_DSM) -/* - * Spektrum/JR DSM USART - */ -#include - -static const struct pios_usart_cfg pios_usart_dsm_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_aux_cfg = { - .bind = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_dsm_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_auxsbus_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .remap = GPIO_AF_USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, -}; - -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_inv_enable = Bit_SET, - .gpio_inv_disable = Bit_RESET, - .gpio_clk_func = RCC_AHB1PeriphClockCmd, - .gpio_clk_periph = RCC_AHB1Periph_GPIOC, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_auxsbus_cfg = { - .regs = UART4, - .remap = GPIO_AF_UART4, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = UART4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = { - .regs = USART6, - .remap = GPIO_AF_USART6, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART6_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - #if defined(PIOS_INCLUDE_COM) #include @@ -2031,47 +1634,297 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { }, .vsense_active_low = false }; - -#include "pios_usb_board_data_priv.h" -#include "pios_usb_desc_hid_cdc_priv.h" -#include "pios_usb_desc_hid_only_priv.h" -#include "pios_usbhook.h" - +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_usb_main_cfg; +} #endif /* PIOS_INCLUDE_USB */ -#if defined(PIOS_INCLUDE_COM_MSG) -#include +/** + * Sensor configurations + */ -#endif /* PIOS_INCLUDE_COM_MSG */ - -#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC1, + .dma = { + .irq = { + .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), + .init = { + .NVIC_IRQChannel = DMA2_Stream4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Stream4, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR + }, + } + }, + .half_flag = DMA_IT_HTIF4, + .full_flag = DMA_IT_TCIF4, }; -#endif /* PIOS_INCLUDE_USB_HID && !PIOS_INCLUDE_USB_CDC */ +void PIOS_ADC_DMC_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); +} +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC) -#include +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, +#ifdef PIOS_HMC5X83_HAS_GPIOS +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(pios_hmc5x83_internal_id); +} +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = pios_board_internal_mag_handler, + .line = EXTI_Line5, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line5, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_HMC5X83_HAS_GPIOS */ - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, +static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = &pios_exti_hmc5x83_cfg, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, }; -#include +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_internal_cfg; +} -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, }; -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ + +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_external_cfg; +} +#endif /* PIOS_INCLUDE_HMC5X83 */ + +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS5611) +#include "pios_ms5611.h" +static const struct pios_ms5611_cfg pios_ms5611_cfg = { + .oversampling = MS5611_OSR_4096, +}; +const struct pios_ms5611_cfg *PIOS_BOARD_HW_DEFS_GetMS5611Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms5611_cfg; +} +#endif /* PIOS_INCLUDE_MS5611 */ + +/** + * Configuration for the BMA180 chip + */ +#if defined(PIOS_INCLUDE_BMA180) +#include "pios_bma180.h" +static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { + .vector = PIOS_BMA180_IRQHandler, + .line = EXTI_Line4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line4, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; +static const struct pios_bma180_cfg pios_bma180_cfg = { + .exti_cfg = &pios_exti_bma180_cfg, + .bandwidth = BMA_BW_600HZ, + .range = BMA_RANGE_8G, +}; +const struct pios_bma180_cfg *PIOS_BOARD_HW_DEFS_GetBMA180Cfg(uint32_t board_revision) +{ + return (board_revision == 0x01) ? &pios_bma180_cfg : 0; +} +#endif /* PIOS_INCLUDE_BMA180 */ + +/** + * Configuration for the MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line8, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line8, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz + .Smpl_rate_div_no_dlp = 0, + // with dlp on output rate is 1000Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_0DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 16, +}; + +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(uint32_t board_revision) +{ + return (board_revision == 0x02) ? &pios_mpu6000_cfg : 0; +} +#endif /* PIOS_INCLUDE_MPU6000 */ + +/** + * Configuration for L3GD20 chip + */ +#if defined(PIOS_INCLUDE_L3GD20) +#include "pios_l3gd20.h" +static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { + .vector = PIOS_L3GD20_IRQHandler, + .line = EXTI_Line8, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI9_5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line8, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { + .exti_cfg = &pios_exti_l3gd20_cfg, + .range = PIOS_L3GD20_SCALE_500_DEG, +}; + +const struct pios_l3gd20_cfg *PIOS_BOARD_HW_DEFS_GetL3GD20Cfg(uint32_t board_revision) +{ + return (board_revision == 0x01) ? &pios_l3gd20_cfg : 0; +} +#endif /* PIOS_INCLUDE_L3GD20 */ diff --git a/flight/targets/boards/revoproto/bootloader/main.c b/flight/targets/boards/revoproto/bootloader/main.c index 09a54a094..aa5c7999a 100644 --- a/flight/targets/boards/revoproto/bootloader/main.c +++ b/flight/targets/boards/revoproto/bootloader/main.c @@ -35,6 +35,7 @@ #include /* PIOS_USBHOOK_* */ #include #include +#include extern void FLASH_Download(); void check_bor(); diff --git a/flight/targets/boards/revoproto/bootloader/pios_board.c b/flight/targets/boards/revoproto/bootloader/pios_board.c index 7e9858f18..72fda9e5d 100644 --- a/flight/targets/boards/revoproto/bootloader/pios_board.c +++ b/flight/targets/boards/revoproto/bootloader/pios_board.c @@ -26,6 +26,13 @@ #include #include +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#include "pios_usbhook.h" + +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -64,7 +71,7 @@ void PIOS_Board_Init() #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 025acce6e..167f19bd0 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -31,7 +31,9 @@ #include #include #include -#include + +#include +#include /* * Pull in the board-specific static HW definitions. @@ -43,365 +45,9 @@ */ #include "../board_hw_defs.c" -/** - * Sensor configurations - */ - -#if defined(PIOS_INCLUDE_ADC) -#include "pios_adc_priv.h" -void PIOS_ADC_DMC_irq_handler(void); -void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); -struct pios_adc_cfg pios_adc_cfg = { - .adc_dev = ADC1, - .dma = { - .irq = { - .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4), - .init = { - .NVIC_IRQChannel = DMA2_Stream4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA2_Stream4, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR - }, - } - }, - .half_flag = DMA_IT_HTIF4, - .full_flag = DMA_IT_TCIF4, -}; -void PIOS_ADC_DMC_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_ADC_DMA_Handler(); -} - -#endif /* if defined(PIOS_INCLUDE_ADC) */ - -#if defined(PIOS_INCLUDE_HMC5X83) -#include "pios_hmc5x83.h" - -pios_hmc5x83_dev_t onboard_mag = 0; -pios_hmc5x83_dev_t external_mag = 0; - -#ifdef PIOS_HMC5X83_HAS_GPIOS -bool pios_board_internal_mag_handler() -{ - return PIOS_HMC5x83_IRQHandler(onboard_mag); -} -static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = pios_board_internal_mag_handler, - .line = EXTI_Line5, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line5, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_HMC5X83_HAS_GPIOS */ - -static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = &pios_exti_hmc5x83_cfg, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, -}; - -static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = NULL, -#endif - .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .TempCompensation = false, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, -}; -#endif /* PIOS_INCLUDE_HMC5X83 */ - -/** - * Configuration for the MS5611 chip - */ -#if defined(PIOS_INCLUDE_MS5611) -#include "pios_ms5611.h" -static const struct pios_ms5611_cfg pios_ms5611_cfg = { - .oversampling = MS5611_OSR_4096, -}; -#endif /* PIOS_INCLUDE_MS5611 */ - -/** - * Configuration for the BMA180 chip - */ -#if defined(PIOS_INCLUDE_BMA180) -#include "pios_bma180.h" -static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { - .vector = PIOS_BMA180_IRQHandler, - .line = EXTI_Line4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line4, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; -static const struct pios_bma180_cfg pios_bma180_cfg = { - .exti_cfg = &pios_exti_bma180_cfg, - .bandwidth = BMA_BW_600HZ, - .range = BMA_RANGE_8G, -}; -#endif /* PIOS_INCLUDE_BMA180 */ - -/** - * Configuration for the MPU6000 chip - */ -#if defined(PIOS_INCLUDE_MPU6000) -#include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" -static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line8, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line8, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz - .Smpl_rate_div_no_dlp = 0, - // with dlp on output rate is 1000Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_0DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 16, -}; -#endif /* PIOS_INCLUDE_MPU6000 */ - -/** - * Configuration for L3GD20 chip - */ -#if defined(PIOS_INCLUDE_L3GD20) -#include "pios_l3gd20.h" -static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { - .vector = PIOS_L3GD20_IRQHandler, - .line = EXTI_Line8, - .pin = { - .gpio = GPIOD, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_IN, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI9_5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line8, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - -static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { - .exti_cfg = &pios_exti_l3gd20_cfg, - .range = PIOS_L3GD20_SCALE_500_DEG, -}; -#endif /* PIOS_INCLUDE_L3GD20 */ - -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 - -#define PIOS_COM_GPS_RX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_AUX_RX_BUF_LEN 512 -#define PIOS_COM_AUX_TX_BUF_LEN 512 - -#define PIOS_COM_HKOSD_RX_BUF_LEN 22 -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 - -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 - -uint32_t pios_com_aux_id = 0; -uint32_t pios_com_gps_id = 0; -uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_rf_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_overo_id = 0; -uint32_t pios_com_hkosd_id = 0; -uint32_t pios_com_msp_id = 0; -uint32_t pios_com_mavlink_id = 0; - uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; -uint32_t pios_com_vcp_id = 0; - -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. tx size = 0 make the port rx only - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) -{ - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - if (tx_buf_len > 0) { // this is the case for rx/tx ports - uint8_t *tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } - } else { // rx only port - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - NULL, 0)) { - PIOS_Assert(0); - } - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -410,10 +56,27 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm #include +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_auxsbus_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; + } + + return -1; +} + void PIOS_Board_Init(void) { - const struct pios_board_info *bdinfo = &pios_board_info_blob; - /* Delay system */ PIOS_DELAY_Init(); @@ -516,293 +179,117 @@ void PIOS_Board_Init(void) // PIOS_IAP_Init(); + /* Configure USB */ #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + MAIN_USART_INVERTER_DISABLE); } - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); - } -#endif /* PIOS_INCLUDE_USB */ - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); /* Configure Telemetry port */ + static const PIOS_BOARD_IO_UART_Function usart_telem_function_map[] = { + [HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, +// [HWSETTINGS_RV_TELEMETRYPORT_COMAUX] = ?? &pios_com_aux_id // UNUSED + [HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RV_TELEMETRYPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RV_TELEMETRYPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK + }; + uint8_t hwsettings_rv_telemetryport; HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); - switch (hwsettings_rv_telemetryport) { - case HWSETTINGS_RV_TELEMETRYPORT_DISABLED: - break; - case HWSETTINGS_RV_TELEMETRYPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_MSP: - PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RV_TELEMETRYPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_telem_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - } /* hwsettings_rv_telemetryport */ + if (hwsettings_rv_telemetryport < NELEMENTS(usart_telem_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_telem_cfg, usart_telem_function_map[hwsettings_rv_telemetryport]); + } + /* Configure GPS port */ + static const PIOS_BOARD_IO_UART_Function usart_gps_function_map[] = { + [HWSETTINGS_RV_GPSPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RV_GPSPORT_GPS] = PIOS_BOARD_IO_UART_GPS, +// [HWSETTINGS_RV_GPSPORT_COMAUX] = + [HWSETTINGS_RV_GPSPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RV_GPSPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RV_GPSPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + }; + uint8_t hwsettings_rv_gpsport; HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); - switch (hwsettings_rv_gpsport) { - case HWSETTINGS_RV_GPSPORT_DISABLED: - break; - case HWSETTINGS_RV_GPSPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; - - case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - - case HWSETTINGS_RV_GPSPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_GPSPORT_MSP: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RV_GPSPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_gps_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - } /* hwsettings_rv_gpsport */ + if (hwsettings_rv_gpsport < NELEMENTS(usart_gps_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_gps_cfg, usart_gps_function_map[hwsettings_rv_gpsport]); + } /* Configure AUXPort */ + static const PIOS_BOARD_IO_UART_Function usart_aux_function_map[] = { + [HWSETTINGS_RV_AUXPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_RV_AUXPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + // [HWSETTINGS_RV_AUXPORT_COMAUX] = + [HWSETTINGS_RV_AUXPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RV_AUXPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RV_AUXPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_RV_AUXPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + }; + uint8_t hwsettings_rv_auxport; HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); - switch (hwsettings_rv_auxport) { - case HWSETTINGS_RV_AUXPORT_DISABLED: - break; + if (hwsettings_rv_auxport < NELEMENTS(usart_aux_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_aux_cfg, usart_aux_function_map[hwsettings_rv_auxport]); + } - case HWSETTINGS_RV_AUXPORT_TELEMETRY: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); - break; + /* Configure AUXSbusPort */ + static const PIOS_BOARD_IO_UART_Function usart_auxsbus_function_map[] = { + [HWSETTINGS_RV_AUXSBUSPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_RV_AUXSBUSPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, /* MAIN group, same as usart_aux! */ + // [HWSETTINGS_RV_AUXSBUSPORT_COMAUX] = + [HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RV_AUXSBUSPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RV_AUXSBUSPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSETTINGS_RV_AUXSBUSPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + }; - case HWSETTINGS_RV_AUXPORT_DSM: - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_aux_cfg, &pios_dsm_aux_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RV_AUXPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_AUXPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_AUXPORT_MSP: - PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RV_AUXPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_aux_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RV_AUXPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rv_auxport */ - /* Configure AUXSbusPort */ - // TODO: ensure that the serial invertion pin is setted correctly uint8_t hwsettings_rv_auxsbusport; HwSettingsRV_AuxSBusPortGet(&hwsettings_rv_auxsbusport); - switch (hwsettings_rv_auxsbusport) { - case HWSETTINGS_RV_AUXSBUSPORT_DISABLED: - break; - case HWSETTINGS_RV_AUXSBUSPORT_SBUS: -#ifdef PIOS_INCLUDE_SBUS - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - - case HWSETTINGS_RV_AUXSBUSPORT_DSM: - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_auxsbus_cfg, &pios_dsm_auxsbus_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RV_AUXSBUSPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_MSP: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_RV_AUXSBUSPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } /* hwsettings_rv_auxport */ + if (hwsettings_rv_auxsbusport < NELEMENTS(usart_auxsbus_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_auxsbus_cfg, usart_auxsbus_function_map[hwsettings_rv_auxsbusport]); + } /* Configure FlexiPort */ + static const PIOS_BOARD_IO_UART_Function usart_flexi_function_map[] = { + [HWSETTINGS_RV_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + // [HWSETTINGS_RV_FLEXIPORT_COMAUX] = + [HWSETTINGS_RV_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_RV_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_RV_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + }; uint8_t hwsettings_rv_flexiport; HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); - switch (hwsettings_rv_flexiport) { - case HWSETTINGS_RV_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_RV_FLEXIPORT_I2C: + if (hwsettings_rv_flexiport < NELEMENTS(usart_flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, usart_flexi_function_map[hwsettings_rv_flexiport]); + } + #if defined(PIOS_INCLUDE_I2C) + if (hwsettings_rv_flexiport == HWSETTINGS_RV_FLEXIPORT_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } @@ -814,47 +301,8 @@ void PIOS_Board_Init(void) // to avoid making something else fail when HMC5X83 is removed PIOS_WDG_Clear(); #endif /* PIOS_INCLUDE_WDG */ -#if defined(PIOS_INCLUDE_HMC5X83) - // get auxmag type - AuxMagSettingsTypeOptions option; - AuxMagSettingsInitialize(); - AuxMagSettingsTypeGet(&option); - // if the aux mag type is FlexiPort then set it up - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - // attach the 5x83 mag to the previously inited I2C2 - external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); - // mag alarm is cleared later, so use I2C - AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); - } -#endif /* PIOS_INCLUDE_HMC5X83 */ + } #endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_RV_FLEXIPORT_DSM: - // TODO: Define the various Channelgroup for Revo dsm inputs and handle here - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); - break; - case HWSETTINGS_RV_FLEXIPORT_COMAUX: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); - break; - case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_RV_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_RV_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - } /* hwsettings_rv_flexiport */ - /* Configure the receiver port*/ uint8_t hwsettings_rcvrport; @@ -865,32 +313,13 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_RV_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - { - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RV_RCVRPORT_PPM: case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ case HWSETTINGS_RV_RCVRPORT_OUTPUTS: @@ -929,17 +358,9 @@ void PIOS_Board_Init(void) } #endif - -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif #ifndef PIOS_ENABLE_DEBUG_PINS switch (hwsettings_rcvrport) { @@ -972,69 +393,7 @@ void PIOS_Board_Init(void) PIOS_DELAY_WaitmS(50); -#if defined(PIOS_INCLUDE_ADC) - PIOS_ADC_Init(&pios_adc_cfg); -#endif - -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined - // to avoid making something else fail when HMC5X83 is removed - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - -#if defined(PIOS_INCLUDE_HMC5X83) - // attach the 5x83 mag to the previously inited I2C1 - onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_adapter_id, 0); -#ifdef PIOS_INCLUDE_WDG - // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed - // this is not in a loop, so it is safe - PIOS_WDG_Clear(); -#endif /* PIOS_INCLUDE_WDG */ - // add this sensor to the sensor task's list - PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); -#endif - -#if defined(PIOS_INCLUDE_MS5611) - PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); - PIOS_MS5611_Register(); -#endif - - switch (bdinfo->board_rev) { - case 0x01: -#if defined(PIOS_INCLUDE_L3GD20) - PIOS_Assert(0); // L3gd20 has not been ported to Sensor framework!!! - PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); - PIOS_Assert(PIOS_L3GD20_Test() == 0); -#endif -#if defined(PIOS_INCLUDE_BMA180) - PIOS_Assert(0); // BMA180 has not been ported to Sensor framework!!! - PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); - PIOS_Assert(PIOS_BMA180_Test() == 0); -#endif - break; - case 0x02: -#if defined(PIOS_INCLUDE_MPU6000) - PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); - PIOS_MPU6000_CONFIG_Configure(); - PIOS_MPU6000_Register(); -#endif - break; - default: - PIOS_DEBUG_Assert(0); - } -#ifdef PIOS_INCLUDE_ADC - { - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); - } - } - } -#endif // PIOS_INCLUDE_ADC + PIOS_BOARD_Sensors_Configure(); } /** diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h index 60572cdc7..66d4c5cfe 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -81,30 +81,41 @@ // PIOS_SPI // See also pios_board.c // ------------------------ -#define PIOS_SPI_MAX_DEVS 3 +#define PIOS_SPI_MAX_DEVS 3 +extern uint32_t pios_spi_gyro_id; +extern uint32_t pios_spi_accel_id; +#define PIOS_SPI_L3GD20_ADAPTER (pios_spi_gyro_id) +#define PIOS_SPI_BMA180_ADAPTER (pios_spi_accel_id) +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_id) // ------------------------ // PIOS_WDG // ------------------------ -#define PIOS_WATCHDOG_TIMEOUT 250 -#define PIOS_WDG_REGISTER RTC_BKP_DR4 -#define PIOS_WDG_ACTUATOR 0x0001 -#define PIOS_WDG_STABILIZATION 0x0002 -#define PIOS_WDG_ATTITUDE 0x0004 -#define PIOS_WDG_MANUAL 0x0008 -#define PIOS_WDG_SENSORS 0x0010 -#define PIOS_WDG_AUTOTUNE 0x0020 +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_SENSORS 0x0010 +#define PIOS_WDG_AUTOTUNE 0x0020 // ------------------------ // PIOS_I2C // See also pios_board.c // ------------------------ -#define PIOS_I2C_MAX_DEVS 3 +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_adapter_id) +extern uint32_t pios_i2c_pressure_adapter_id; + +#define PIOS_I2C_MS5611_INTERNAL_ADAPTER (pios_i2c_pressure_adapter_id) +#define PIOS_I2C_HMC5X83_INTERNAL_ADAPTER (pios_i2c_mag_adapter_id) + extern uint32_t pios_i2c_flexiport_adapter_id; -#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) -#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_flexiport_adapter_id) + // ------------------------- // PIOS_USART @@ -119,26 +130,26 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_aux_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_DEBUG PIOS_COM_AUX -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) +// extern uint32_t pios_com_telem_rf_id; +// extern uint32_t pios_com_gps_id; +// extern uint32_t pios_com_aux_id; +// extern uint32_t pios_com_telem_usb_id; +// extern uint32_t pios_com_bridge_id; +// extern uint32_t pios_com_vcp_id; +// extern uint32_t pios_com_hkosd_id; +// extern uint32_t pios_com_msp_id; +// extern uint32_t pios_com_mavlink_id; +// +// #define PIOS_COM_AUX (pios_com_aux_id) +// #define PIOS_COM_GPS (pios_com_gps_id) +// #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +// #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +// #define PIOS_COM_BRIDGE (pios_com_bridge_id) +// #define PIOS_COM_VCP (pios_com_vcp_id) +// #define PIOS_COM_DEBUG PIOS_COM_AUX +// #define PIOS_COM_OSDHK (pios_com_hkosd_id) +// #define PIOS_COM_MSP (pios_com_msp_id) +// #define PIOS_COM_MAVLINK (pios_com_mavlink_id) // ------------------------ // TELEMETRY diff --git a/flight/targets/boards/sparky2/board_hw_defs.c b/flight/targets/boards/sparky2/board_hw_defs.c index 8316bc3df..18ed6962f 100644 --- a/flight/targets/boards/sparky2/board_hw_defs.c +++ b/flight/targets/boards/sparky2/board_hw_defs.c @@ -439,7 +439,7 @@ const struct pios_rfm22b_cfg pios_rfm22b_cfg = { .gpio_direction = GPIO0_TX_GPIO1_RX, }; -const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22Cfg(__attribute__((unused)) uint32_t board_revision) +const struct pios_rfm22b_cfg *PIOS_BOARD_HW_DEFS_GetRfm22bCfg(__attribute__((unused)) uint32_t board_revision) { return &pios_rfm22b_cfg; } @@ -540,6 +540,7 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { #endif /* PIOS_INCLUDE_FLASH */ +#ifdef PIOS_INCLUDE_USART #include /* @@ -628,6 +629,7 @@ static const struct pios_usart_cfg pios_usart_rcvr_cfg = { }, .ioctl = PIOS_BOARD_USART_Ioctl, }; +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) diff --git a/flight/targets/boards/sparky2/bootloader/main.c b/flight/targets/boards/sparky2/bootloader/main.c index 70b18409e..02d53d948 100644 --- a/flight/targets/boards/sparky2/bootloader/main.c +++ b/flight/targets/boards/sparky2/bootloader/main.c @@ -36,6 +36,7 @@ #include /* PIOS_USBHOOK_* */ #include #include +#include extern void FLASH_Download(); void check_bor(); diff --git a/flight/targets/boards/sparky2/bootloader/pios_board.c b/flight/targets/boards/sparky2/bootloader/pios_board.c index f93555938..dfdbcbfce 100644 --- a/flight/targets/boards/sparky2/bootloader/pios_board.c +++ b/flight/targets/boards/sparky2/bootloader/pios_board.c @@ -25,7 +25,6 @@ #include #include - /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -40,11 +39,6 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; -static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) -{ - return -1; -} - void PIOS_Board_Init() { if (board_init_complete) { From 1548312823aec256b086e49aa5b4a4ee46d65ce7 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 16 Apr 2017 02:44:07 +0200 Subject: [PATCH 10/20] LP-480 CopterControl will expect ADC_Init() only if board revision has analog gyro, so be prepared to get NULL from PIOS_BOARD_HW_DEFS_GetAdcCfg() --- flight/pios/common/pios_board_sensors.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/flight/pios/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c index 3b0e58b6c..7d204757f 100644 --- a/flight/pios/common/pios_board_sensors.c +++ b/flight/pios/common/pios_board_sensors.c @@ -185,16 +185,19 @@ void PIOS_BOARD_Sensors_Configure() #endif #ifdef PIOS_INCLUDE_ADC - PIOS_ADC_Init(PIOS_BOARD_HW_DEFS_GetAdcCfg(pios_board_info_blob.board_rev)); -#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES - uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingArrayGet(adc_config); - for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { - if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { - PIOS_ADC_PinSetup(i); + const struct pios_adc_cfg *adc_cfg = PIOS_BOARD_HW_DEFS_GetAdcCfg(pios_board_info_blob.board_rev); + if(adc_cfg) { + PIOS_ADC_Init(adc_cfg); +# ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; + HwSettingsADCRoutingArrayGet(adc_config); + for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { + if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) { + PIOS_ADC_PinSetup(i); + } } +# endif } -#endif #endif /* PIOS_INCLUDE_ADC */ // internal bmp280 baro From 280e771ded933b12f86ff140b07b08a3ed259eca Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 16 Apr 2017 18:11:20 +0200 Subject: [PATCH 11/20] LP-480 Make PIOS_BOARD_IO_USB_[VCP|HID]_Function enums based on HWSETTINGS_USB_[HID|VCP]PORT values to allow simple type casting. --- flight/pios/common/pios_board_io.c | 38 ++++++------------------------ flight/pios/inc/pios_board_io.h | 20 +++++++++------- 2 files changed, 19 insertions(+), 39 deletions(-) diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 6b639c150..78c59e682 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -161,39 +161,15 @@ static void PIOS_BOARD_IO_HID_Init(uint32_t *com_id, uint16_t rx_buf_len, uint16 } } -PIOS_BOARD_IO_USB_HID_Function PIOS_BOARD_IO_HWSettings_USB_HID_Function() -{ - static const PIOS_BOARD_IO_USB_HID_Function map[] = { - [HWSETTINGS_USB_HIDPORT_USBTELEMETRY] = PIOS_BOARD_IO_USB_HID_TELEMETRY, - [HWSETTINGS_USB_HIDPORT_RCTRANSMITTER] = PIOS_BOARD_IO_USB_HID_RCTX, - }; - uint8_t hwsettings_usb_hidport; - - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - return (hwsettings_usb_hidport < NELEMENTS(map)) ? map[hwsettings_usb_hidport] : PIOS_BOARD_IO_USB_HID_NONE; -} - -PIOS_BOARD_IO_USB_VCP_Function PIOS_BOARD_IO_HWSettings_USB_VCP_Function() -{ - static const PIOS_BOARD_IO_USB_VCP_Function map[] = { - [HWSETTINGS_USB_VCPPORT_USBTELEMETRY] = PIOS_BOARD_IO_USB_VCP_TELEMETRY, - [HWSETTINGS_USB_VCPPORT_COMBRIDGE] = PIOS_BOARD_IO_USB_VCP_COMBRIDGE, - [HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE, - [HWSETTINGS_USB_VCPPORT_MAVLINK] = PIOS_BOARD_IO_USB_VCP_MAVLINK, - }; - - uint8_t hwsettings_usb_vcpport; - - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - return (hwsettings_usb_vcpport < NELEMENTS(map)) ? map[hwsettings_usb_vcpport] : PIOS_BOARD_IO_USB_VCP_NONE; -} - void PIOS_BOARD_IO_Configure_USB() { - PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_HWSettings_USB_HID_Function(), - PIOS_BOARD_IO_HWSettings_USB_VCP_Function()); + uint8_t hwsettings_usb_hidport; + uint8_t hwsettings_usb_vcpport; + + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + + PIOS_BOARD_IO_Configure_USB_Function((PIOS_BOARD_IO_USB_HID_Function)hwsettings_usb_hidport, (PIOS_BOARD_IO_USB_VCP_Function)hwsettings_usb_vcpport); } void PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_USB_HID_Function hid_function, __attribute__((unused)) PIOS_BOARD_IO_USB_VCP_Function vcp_function) diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index d186f5ea5..1d929228a 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -208,23 +208,27 @@ typedef enum { } PIOS_BOARD_IO_RADIOAUX_Function; #ifdef PIOS_INCLUDE_USB +# ifndef BOOTLOADER +# include "hwsettings.h" + void PIOS_BOARD_IO_Configure_USB(); typedef enum { - PIOS_BOARD_IO_USB_HID_NONE = 0, - PIOS_BOARD_IO_USB_HID_TELEMETRY, - PIOS_BOARD_IO_USB_HID_RCTX, + PIOS_BOARD_IO_USB_HID_NONE = HWSETTINGS_USB_HIDPORT_DISABLED, + PIOS_BOARD_IO_USB_HID_TELEMETRY = HWSETTINGS_USB_HIDPORT_USBTELEMETRY, + PIOS_BOARD_IO_USB_HID_RCTX = HWSETTINGS_USB_HIDPORT_RCTRANSMITTER, } PIOS_BOARD_IO_USB_HID_Function; typedef enum { - PIOS_BOARD_IO_USB_VCP_NONE = 0, - PIOS_BOARD_IO_USB_VCP_TELEMETRY, - PIOS_BOARD_IO_USB_VCP_COMBRIDGE, - PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE, - PIOS_BOARD_IO_USB_VCP_MAVLINK, + PIOS_BOARD_IO_USB_VCP_NONE = HWSETTINGS_USB_VCPPORT_DISABLED, + PIOS_BOARD_IO_USB_VCP_TELEMETRY = HWSETTINGS_USB_VCPPORT_USBTELEMETRY, + PIOS_BOARD_IO_USB_VCP_COMBRIDGE = HWSETTINGS_USB_VCPPORT_COMBRIDGE, + PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE = HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE, + PIOS_BOARD_IO_USB_VCP_MAVLINK = HWSETTINGS_USB_VCPPORT_MAVLINK, } PIOS_BOARD_IO_USB_VCP_Function; void PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_USB_HID_Function hid_function, PIOS_BOARD_IO_USB_VCP_Function vcp_function); +# endif #endif #ifdef PIOS_INCLUDE_PWM void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); From 97210850c39db0be927ca7ea3b4a5d3d428a1aab Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 16 Apr 2017 18:13:55 +0200 Subject: [PATCH 12/20] LP-480 add missing #include --- flight/pios/inc/pios_board_io.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 1d929228a..2c9489528 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -209,7 +209,8 @@ typedef enum { #ifdef PIOS_INCLUDE_USB # ifndef BOOTLOADER -# include "hwsettings.h" +# include "uavobjectmanager.h" +# include "hwsettings.h" void PIOS_BOARD_IO_Configure_USB(); From 9d01cbe4d43a9c33f3962c5f2327e6eb30da28d4 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 16 Apr 2017 14:06:28 -0700 Subject: [PATCH 13/20] LP-480 Comments out PIOS_INCLUDE_USART and PIOS_INCLUDE_PWM from simposix pios_config.h to allow simposix fw build. --- flight/pios/inc/pios_board_io.h | 4 ++++ flight/targets/boards/simposix/firmware/inc/pios_config.h | 6 +++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 2c9489528..de46a8bae 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -33,7 +33,9 @@ #include #endif +#ifdef PIOS_INCLUDE_USART #include +#endif #ifdef PIOS_INCLUDE_PWM #include @@ -238,7 +240,9 @@ void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg); #endif +#ifdef PIOS_INCLUDE_USART void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *usart_cfg, PIOS_BOARD_IO_UART_Function function); +#endif #ifdef PIOS_INCLUDE_RFM22B void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function function); diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index d422ddeff..36f84be85 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -52,7 +52,7 @@ #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR -#define PIOS_INCLUDE_USART +//#define PIOS_INCLUDE_USART // #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID // #define PIOS_INCLUDE_GPIO @@ -87,8 +87,8 @@ #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_DSM // #define PIOS_INCLUDE_SBUS -#define PIOS_INCLUDE_PPM -#define PIOS_INCLUDE_PWM +//#define PIOS_INCLUDE_PPM +//#define PIOS_INCLUDE_PWM /* #define PIOS_INCLUDE_GCSRCVR */ /* #define PIOS_INCLUDE_OPLINKRCVR */ #define PIOS_INCLUDE_IAP From 86022e2defced66e523f9416eceb0c1d8f832f92 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 16 Apr 2017 22:35:57 +0200 Subject: [PATCH 14/20] LP-480 removed unused PIOS_BOARD_IO_Configure_WS2811() and PIOS_BOARD_IO_Configure_ADC() from pios_board_io.h --- flight/pios/inc/pios_board_io.h | 9 --------- 1 file changed, 9 deletions(-) diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index de46a8bae..75a4f18cd 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -252,13 +252,4 @@ void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function function); void PIOS_BOARD_IO_Configure_GCSRCVR(); #endif -#ifdef PIOS_INCLUDE_WS2811 -void PIOS_BOARD_IO_Configure_WS2811(); -#endif - -#ifdef PIOS_INCLUDE_ADC -void PIOS_BOARD_IO_Configure_ADC(); -#endif - - #endif /* PIOS_BOARD_IO_H */ From 05ee4b8dace8715753e4b63938c5e49c28120c10 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 17 Apr 2017 00:50:08 +0200 Subject: [PATCH 15/20] LP-480 Move PIOS_COM_SetHalfDuplex() functionality to IOCTL. Remove RX/TX mode and change parameter order for PIOS_COM_ChangeConfig() and com driver set_config to match the databits/parity/stopbits scheme (like for example 8N1). --- .../modules/UAVOHottBridge/uavohottbridge.c | 3 +- flight/pios/common/pios_board_io.c | 4 +- flight/pios/common/pios_board_sensors.c | 2 +- flight/pios/common/pios_com.c | 28 +--- flight/pios/common/pios_dsm.c | 2 +- flight/pios/common/pios_exbus.c | 2 +- flight/pios/common/pios_hott.c | 2 +- flight/pios/common/pios_ibus.c | 2 +- flight/pios/common/pios_sbus.c | 2 +- flight/pios/common/pios_srxl.c | 2 +- flight/pios/inc/pios_board_io.h | 8 +- flight/pios/inc/pios_com.h | 6 +- flight/pios/inc/pios_usart.h | 12 +- flight/pios/stm32f0x/pios_usart.c | 103 +++++++------ flight/pios/stm32f10x/pios_usart.c | 98 ++++++------ flight/pios/stm32f4xx/pios_usart.c | 142 ++++++++---------- .../simposix/firmware/inc/pios_config.h | 6 +- 17 files changed, 200 insertions(+), 224 deletions(-) diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c index 663045d08..2fbab124e 100644 --- a/flight/modules/UAVOHottBridge/uavohottbridge.c +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -123,7 +123,8 @@ static int32_t uavoHoTTBridgeInitialize(void) // HoTT telemetry baudrate is fixed to 19200 PIOS_COM_ChangeBaud(PIOS_COM_HOTT, 19200); - PIOS_COM_SetHalfDuplex(PIOS_COM_HOTT, true); + bool param = true; + PIOS_COM_Ioctl(PIOS_COM_HOTT, PIOS_IOCTL_USART_SET_HALFDUPLEX, ¶m); HoTTBridgeSettingsInitialize(); HoTTBridgeStatusInitialize(); diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 78c59e682..e43b3053c 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -165,10 +165,10 @@ void PIOS_BOARD_IO_Configure_USB() { uint8_t hwsettings_usb_hidport; uint8_t hwsettings_usb_vcpport; - + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - + PIOS_BOARD_IO_Configure_USB_Function((PIOS_BOARD_IO_USB_HID_Function)hwsettings_usb_hidport, (PIOS_BOARD_IO_USB_VCP_Function)hwsettings_usb_vcpport); } diff --git a/flight/pios/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c index 7d204757f..4abfe1e02 100644 --- a/flight/pios/common/pios_board_sensors.c +++ b/flight/pios/common/pios_board_sensors.c @@ -186,7 +186,7 @@ void PIOS_BOARD_Sensors_Configure() #ifdef PIOS_INCLUDE_ADC const struct pios_adc_cfg *adc_cfg = PIOS_BOARD_HW_DEFS_GetAdcCfg(pios_board_info_blob.board_rev); - if(adc_cfg) { + if (adc_cfg) { PIOS_ADC_Init(adc_cfg); # ifndef PIOS_EXCLUDE_ADVANCED_FEATURES uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM]; diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index 1a813ba3c..fb8fe3044 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -296,31 +296,7 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) return 0; } -/** - * Change the port type to halfduplex (shared rx/tx medium) - * \param[in] port COM port - * \param[in] halfduplex enabled - * \return -1 if port not available - * \return 0 on success - */ -int32_t PIOS_COM_SetHalfDuplex(uint32_t com_id, bool halfduplex) -{ - struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; - - if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } - - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_halfduplex) { - com_dev->driver->set_halfduplex(com_dev->lower_id, halfduplex); - } - - return 0; -} - -int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode) +int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate) { struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; @@ -331,7 +307,7 @@ int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_le /* Invoke the driver function if it exists */ if (com_dev->driver->set_config) { - com_dev->driver->set_config(com_dev->lower_id, word_len, stop_bits, parity, baud_rate, mode); + com_dev->driver->set_config(com_dev->lower_id, word_len, parity, stop_bits, baud_rate); } return 0; diff --git a/flight/pios/common/pios_dsm.c b/flight/pios/common/pios_dsm.c index af52ce985..747e2b91f 100644 --- a/flight/pios/common/pios_dsm.c +++ b/flight/pios/common/pios_dsm.c @@ -332,7 +332,7 @@ int32_t PIOS_DSM_Init(uint32_t *dsm_id, /* Set comm driver parameters */ PIOS_DEBUG_Assert(driver->set_config); - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_No, PIOS_COM_StopBits_1, 115200); /* Set irq priority */ if (driver->ioctl) { diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c index c29564075..edb416233 100644 --- a/flight/pios/common/pios_exbus.c +++ b/flight/pios/common/pios_exbus.c @@ -309,7 +309,7 @@ int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, /* Set comm driver parameters */ PIOS_DEBUG_Assert(driver->set_config); - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 125000, PIOS_COM_Mode_Rx); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_No, PIOS_COM_StopBits_1, 125000); /* Set irq priority */ if (driver->ioctl) { diff --git a/flight/pios/common/pios_hott.c b/flight/pios/common/pios_hott.c index 04f2fc940..c823cc8fe 100644 --- a/flight/pios/common/pios_hott.c +++ b/flight/pios/common/pios_hott.c @@ -327,7 +327,7 @@ int32_t PIOS_HOTT_Init(uint32_t *hott_id, /* Set comm driver parameters */ PIOS_DEBUG_Assert(driver->set_config); - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_No, PIOS_COM_StopBits_1, 115200); /* Set irq priority */ if (driver->ioctl) { diff --git a/flight/pios/common/pios_ibus.c b/flight/pios/common/pios_ibus.c index 953ef8865..8dd4595e6 100644 --- a/flight/pios/common/pios_ibus.c +++ b/flight/pios/common/pios_ibus.c @@ -176,7 +176,7 @@ int32_t PIOS_IBUS_Init(uint32_t *ibus_id, const struct pios_com_driver *driver, /* Set comm driver parameters */ PIOS_DEBUG_Assert(driver->set_config); - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_No, PIOS_COM_StopBits_1, 115200); /* Set irq priority */ if (driver->ioctl) { diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 979c6a856..f7cd3f475 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -168,7 +168,7 @@ int32_t PIOS_SBus_Init(uint32_t *sbus_id, /* Set rest of the parameters */ if (driver->set_config) { - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_2, PIOS_COM_Parity_Even, 100000, PIOS_COM_Mode_Rx); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_Even, PIOS_COM_StopBits_2, 100000); } /* Set inverted UART and IRQ priority */ diff --git a/flight/pios/common/pios_srxl.c b/flight/pios/common/pios_srxl.c index b0595ed8e..4dcd69d85 100644 --- a/flight/pios/common/pios_srxl.c +++ b/flight/pios/common/pios_srxl.c @@ -158,7 +158,7 @@ int32_t PIOS_SRXL_Init(uint32_t *srxl_id, /* Set comm driver parameters */ PIOS_DEBUG_Assert(driver->set_config); - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_StopBits_1, PIOS_COM_Parity_No, 115200, PIOS_COM_Mode_Rx); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_No, PIOS_COM_StopBits_1, 115200); /* Set irq priority */ if (driver->ioctl) { diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 75a4f18cd..cf0533b02 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -224,15 +224,15 @@ typedef enum { typedef enum { PIOS_BOARD_IO_USB_VCP_NONE = HWSETTINGS_USB_VCPPORT_DISABLED, - PIOS_BOARD_IO_USB_VCP_TELEMETRY = HWSETTINGS_USB_VCPPORT_USBTELEMETRY, - PIOS_BOARD_IO_USB_VCP_COMBRIDGE = HWSETTINGS_USB_VCPPORT_COMBRIDGE, + PIOS_BOARD_IO_USB_VCP_TELEMETRY = HWSETTINGS_USB_VCPPORT_USBTELEMETRY, + PIOS_BOARD_IO_USB_VCP_COMBRIDGE = HWSETTINGS_USB_VCPPORT_COMBRIDGE, PIOS_BOARD_IO_USB_VCP_DEBUGCONSOLE = HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE, PIOS_BOARD_IO_USB_VCP_MAVLINK = HWSETTINGS_USB_VCPPORT_MAVLINK, } PIOS_BOARD_IO_USB_VCP_Function; void PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_USB_HID_Function hid_function, PIOS_BOARD_IO_USB_VCP_Function vcp_function); -# endif -#endif +# endif // ifndef BOOTLOADER +#endif // ifdef PIOS_INCLUDE_USB #ifdef PIOS_INCLUDE_PWM void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); #endif diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index a150691a3..113f38d6f 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -71,8 +71,7 @@ enum PIOS_COM_Mode { struct pios_com_driver { void (*set_baud)(uint32_t id, uint32_t baud); - void (*set_halfduplex)(uint32_t id, bool halfduplex); - void (*set_config)(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); + void (*set_config)(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); void (*set_ctrl_line)(uint32_t id, uint32_t mask, uint32_t state); void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); @@ -92,8 +91,7 @@ struct pios_com_driver { /* Public Functions */ extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); -extern int32_t PIOS_COM_SetHalfDuplex(uint32_t com_id, bool halfduplex); -extern int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); +extern int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state); extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); extern int32_t PIOS_COM_RegisterBaudRateCallback(uint32_t usart_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context); diff --git a/flight/pios/inc/pios_usart.h b/flight/pios/inc/pios_usart.h index 49ac3c92a..35e35c89b 100644 --- a/flight/pios/inc/pios_usart.h +++ b/flight/pios/inc/pios_usart.h @@ -43,15 +43,15 @@ enum PIOS_USART_Inverted { PIOS_USART_Inverted_RxTx = (PIOS_USART_Inverted_Rx | PIOS_USART_Inverted_Tx) }; -#define PIOS_IOCTL_USART_SET_INVERTED COM_IOCTL(COM_IOCTL_TYPE_USART, 1, enum PIOS_USART_Inverted) -#define PIOS_IOCTL_USART_SET_SWAPPIN COM_IOCTL(COM_IOCTL_TYPE_USART, 2, bool) -#define PIOS_IOCTL_USART_SET_ONEWIRE COM_IOCTL(COM_IOCTL_TYPE_USART, 3, bool) +#define PIOS_IOCTL_USART_SET_INVERTED COM_IOCTL(COM_IOCTL_TYPE_USART, 1, enum PIOS_USART_Inverted) +#define PIOS_IOCTL_USART_SET_SWAPPIN COM_IOCTL(COM_IOCTL_TYPE_USART, 2, bool) +#define PIOS_IOCTL_USART_SET_HALFDUPLEX COM_IOCTL(COM_IOCTL_TYPE_USART, 3, bool) -#define PIOS_IOCTL_USART_GET_RXGPIO COM_IOCTL(COM_IOCTL_TYPE_USART, 4, struct stm32_gpio) -#define PIOS_IOCTL_USART_GET_TXGPIO COM_IOCTL(COM_IOCTL_TYPE_USART, 5, struct stm32_gpio) +#define PIOS_IOCTL_USART_GET_RXGPIO COM_IOCTL(COM_IOCTL_TYPE_USART, 4, struct stm32_gpio) +#define PIOS_IOCTL_USART_GET_TXGPIO COM_IOCTL(COM_IOCTL_TYPE_USART, 5, struct stm32_gpio) /* PIOS_IRQ_PRIO_ values */ -#define PIOS_IOCTL_USART_SET_IRQ_PRIO COM_IOCTL(COM_IOCTL_TYPE_USART, 6, uint8_t) +#define PIOS_IOCTL_USART_SET_IRQ_PRIO COM_IOCTL(COM_IOCTL_TYPE_USART, 6, uint8_t) #endif /* PIOS_USART_H */ diff --git a/flight/pios/stm32f0x/pios_usart.c b/flight/pios/stm32f0x/pios_usart.c index 21a1dceed..7666bc8df 100644 --- a/flight/pios/stm32f0x/pios_usart.c +++ b/flight/pios/stm32f0x/pios_usart.c @@ -39,7 +39,7 @@ /* Provide a COM driver */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); #ifndef BOOTLOADER -static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); +static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); #endif static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); @@ -186,15 +186,8 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Initialize the comm parameter structure */ USART_StructInit(&usart_dev->init); // 9600 8n1 - /* Enable the USART Pins Software Remapping */ - if (usart_dev->cfg->remap) { - GPIO_PinAFConfig(cfg->rx.gpio, __builtin_ctz(cfg->rx.init.GPIO_Pin), cfg->remap); - GPIO_PinAFConfig(cfg->tx.gpio, __builtin_ctz(cfg->tx.init.GPIO_Pin), cfg->remap); - } - - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(cfg->rx.gpio, (GPIO_InitTypeDef *)&cfg->rx.init); - GPIO_Init(cfg->tx.gpio, (GPIO_InitTypeDef *)&cfg->tx.init); + /* We will set modes later, depending on installed callbacks */ + usart_dev->init.USART_Mode = 0; /* Enable USART clock */ switch ((uint32_t)cfg->regs) { @@ -217,21 +210,51 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) #endif } - /* Configure the USART */ - USART_Init(cfg->regs, (USART_InitTypeDef *)&usart_dev->init); *usart_id = (uint32_t)usart_dev; PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); - USART_ITConfig(cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(cfg->regs, USART_IT_TXE, ENABLE); - USART_ITConfig(cfg->regs, USART_IT_ORE, DISABLE); - USART_ITConfig(cfg->regs, USART_IT_TC, DISABLE); - /* Enable USART */ - USART_Cmd(cfg->regs, ENABLE); + + /* Disable overrun detection */ + USART_OverrunDetectionConfig(usart_dev->cfg->regs, USART_OVRDetection_Disable); return 0; } +static void PIOS_USART_Setup(struct pios_usart_dev *usart_dev) +{ + /* Configure RX GPIO */ + if ((usart_dev->init.USART_Mode & USART_Mode_Rx) && (usart_dev->cfg->rx.gpio)) { + if (usart_dev->cfg->remap) { + GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, + __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), + usart_dev->cfg->remap); + } + + GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + } + + /* Configure TX GPIO */ + if ((usart_dev->init.USART_Mode & USART_Mode_Tx) && usart_dev->cfg->tx.gpio) { + if (usart_dev->cfg->remap) { + GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, + __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), + usart_dev->cfg->remap); + } + + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + } + + /* Write new configuration */ + USART_Init(usart_dev->cfg->regs, &usart_dev->init); + + /* + * Re enable USART. + */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); +} + static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { const struct pios_usart_dev *usart_dev = PIOS_USART_validate(usart_id); @@ -257,10 +280,7 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) /* Use our working copy of the usart init structure */ usart_dev->init.USART_BaudRate = baud; - /* Write back the modified configuration */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); - - USART_Cmd(usart_dev->cfg->regs, ENABLE); + PIOS_USART_Setup(usart_dev); } #ifndef BOOTLOADER /** @@ -275,10 +295,9 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) */ static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, - enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, - uint32_t baud_rate, - enum PIOS_COM_Mode mode) + enum PIOS_COM_StopBits stop_bits, + uint32_t baud_rate) { struct pios_usart_dev *usart_dev = PIOS_USART_validate(usart_id); @@ -325,27 +344,7 @@ static void PIOS_USART_ChangeConfig(uint32_t usart_id, usart_dev->init.USART_BaudRate = baud_rate; } - switch (mode) { - case PIOS_COM_Mode_Rx: - usart_dev->init.USART_Mode = USART_Mode_Rx; - break; - case PIOS_COM_Mode_Tx: - usart_dev->init.USART_Mode = USART_Mode_Tx; - break; - case PIOS_COM_Mode_RxTx: - usart_dev->init.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; - break; - default: - break; - } - - /* Write back the modified configuration */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); - - /* - * Re enable USART. - */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + PIOS_USART_Setup(usart_dev); } #endif /* BOOTLOADER */ @@ -357,8 +356,12 @@ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback r * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ - usart_dev->rx_in_context = context; + usart_dev->rx_in_context = context; usart_dev->rx_in_cb = rx_in_cb; + + usart_dev->init.USART_Mode |= USART_Mode_Rx; + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) @@ -369,8 +372,12 @@ static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback t * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ - usart_dev->tx_out_context = context; + usart_dev->tx_out_context = context; usart_dev->tx_out_cb = tx_out_cb; + + usart_dev->init.USART_Mode |= USART_Mode_Tx; + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index cc2d13fc0..cc1e23232 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -37,7 +37,7 @@ #include /* Provide a COM driver */ -static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); +static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); @@ -185,17 +185,10 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Initialize the comm parameter structure */ USART_StructInit(&usart_dev->init); // 9600 8n1 - /* Enable the USART Pins Software Remapping */ - if (usart_dev->cfg->remap) { - GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); - } + /* We will set modes later, depending on installed callbacks */ + usart_dev->init.USART_Mode = 0; - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); - GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); - - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); + /* DTR handling? */ *usart_id = (uint32_t)usart_dev; @@ -217,18 +210,44 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); - return 0; out_fail: return -1; } +static void PIOS_USART_Setup(struct pios_usart_dev *usart_dev) +{ + /* Configure RX GPIO */ + if ((usart_dev->init.USART_Mode & USART_Mode_Rx) && (usart_dev->cfg->rx.gpio)) { + if (usart_dev->cfg->remap) { + GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); + } + + GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + } + + /* Configure TX GPIO */ + if ((usart_dev->init.USART_Mode & USART_Mode_Tx) && usart_dev->cfg->tx.gpio) { + if (usart_dev->cfg->remap) { + GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); + } + + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + } + + /* Write new configuration */ + USART_Init(usart_dev->cfg->regs, &usart_dev->init); + + /* + * Re enable USART. + */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); +} + + static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; @@ -266,8 +285,7 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) /* Use our working copy of the usart init structure */ usart_dev->init.USART_BaudRate = baud; - /* Write back the modified configuration */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); + PIOS_USART_Setup(usart_dev); } /** @@ -282,10 +300,9 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) */ static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, - enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, - uint32_t baud_rate, - enum PIOS_COM_Mode mode) + enum PIOS_COM_StopBits stop_bits, + uint32_t baud_rate) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; @@ -339,27 +356,7 @@ static void PIOS_USART_ChangeConfig(uint32_t usart_id, usart_dev->init.USART_BaudRate = baud_rate; } - switch (mode) { - case PIOS_COM_Mode_Rx: - usart_dev->init.USART_Mode = USART_Mode_Rx; - break; - case PIOS_COM_Mode_Tx: - usart_dev->init.USART_Mode = USART_Mode_Tx; - break; - case PIOS_COM_Mode_RxTx: - usart_dev->init.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; - break; - default: - break; - } - - /* Write back the modified configuration */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); - - /* - * Re enable USART. - */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) @@ -374,8 +371,12 @@ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback r * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ - usart_dev->rx_in_context = context; + usart_dev->rx_in_context = context; usart_dev->rx_in_cb = rx_in_cb; + + usart_dev->init.USART_Mode |= USART_Mode_Rx; + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) @@ -390,8 +391,12 @@ static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback t * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ - usart_dev->tx_out_context = context; + usart_dev->tx_out_context = context; usart_dev->tx_out_cb = tx_out_cb; + + usart_dev->init.USART_Mode |= USART_Mode_Tx; + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) @@ -467,6 +472,9 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) case PIOS_IOCTL_USART_GET_TXGPIO: *(struct stm32_gpio *)param = usart_dev->cfg->tx; break; + case PIOS_IOCTL_USART_SET_HALFDUPLEX: + USART_HalfDuplexCmd(usart_dev->cfg->regs, *(bool *)param ? ENABLE : DISABLE); + break; default: if (usart_dev->cfg->ioctl) { return usart_dev->cfg->ioctl(usart_id, ctl, param); diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 7b9f93649..3731bb658 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -41,8 +41,7 @@ /* Provide a COM driver */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); -static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex); -static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); +static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state); static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); @@ -51,15 +50,14 @@ static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .set_halfduplex = PIOS_USART_SetHalfDuplex, - .set_config = PIOS_USART_ChangeConfig, - .set_ctrl_line = PIOS_USART_SetCtrlLine, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, - .ioctl = PIOS_USART_Ioctl, + .set_baud = PIOS_USART_ChangeBaud, + .set_config = PIOS_USART_ChangeConfig, + .set_ctrl_line = PIOS_USART_SetCtrlLine, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .ioctl = PIOS_USART_Ioctl, }; enum pios_usart_dev_magic { @@ -211,20 +209,9 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Initialize the comm parameter structure */ USART_StructInit(&usart_dev->init); // 9600 8n1 - /* Map pins to USART function */ - /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ - if (usart_dev->cfg->remap) { - GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, - __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), - usart_dev->cfg->remap); - GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, - __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), - usart_dev->cfg->remap); - } + /* We will set modes later, depending on installed callbacks */ + usart_dev->init.USART_Mode = 0; - /* Initialize the USART Rx and Tx pins */ - GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); - GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); /* If a DTR line is specified, initialize it */ if (usart_dev->cfg->dtr.gpio) { @@ -232,9 +219,6 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) PIOS_USART_SetCtrlLine((uint32_t)usart_dev, COM_CTRL_LINE_DTR_MASK, 0); } - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->init); - *usart_id = (uint32_t)usart_dev; /* Configure USART Interrupts */ @@ -267,13 +251,6 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) break; } PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - - // FIXME XXX Clear / reset uart here - sends NUL char else - - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); return 0; @@ -281,6 +258,42 @@ out_fail: return -1; } +static void PIOS_USART_Setup(struct pios_usart_dev *usart_dev) +{ + /* Configure RX GPIO */ + if ((usart_dev->init.USART_Mode & USART_Mode_Rx) && (usart_dev->cfg->rx.gpio)) { + if (usart_dev->cfg->remap) { + GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, + __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), + usart_dev->cfg->remap); + } + + GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); + + /* just enable RX right away, cause rcvr modules do not call rx_start method */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); + } + + /* Configure TX GPIO */ + if ((usart_dev->init.USART_Mode & USART_Mode_Tx) && usart_dev->cfg->tx.gpio) { + if (usart_dev->cfg->remap) { + GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, + __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), + usart_dev->cfg->remap); + } + + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + } + + /* Write new configuration */ + USART_Init(usart_dev->cfg->regs, &usart_dev->init); + + /* + * Re enable USART. + */ + USART_Cmd(usart_dev->cfg->regs, ENABLE); +} + static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; @@ -319,24 +332,7 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) /* Use our working copy of the usart init structure */ usart_dev->init.USART_BaudRate = baud; - /* Write back the modified configuration */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); -} - -/** - * Sets the USART peripheral into half duplex mode - * \param[in] usart_id USART name (GPS, TELEM, AUX) - * \param[in] bool wether to set half duplex or not - */ -static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex) -{ - struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - - PIOS_Assert(valid); - - USART_HalfDuplexCmd(usart_dev->cfg->regs, halfduplex ? ENABLE : DISABLE); + PIOS_USART_Setup(usart_dev); } /** @@ -351,10 +347,9 @@ static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex) */ static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, - enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, - uint32_t baud_rate, - enum PIOS_COM_Mode mode) + enum PIOS_COM_StopBits stop_bits, + uint32_t baud_rate) { struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; @@ -408,27 +403,7 @@ static void PIOS_USART_ChangeConfig(uint32_t usart_id, usart_dev->init.USART_BaudRate = baud_rate; } - switch (mode) { - case PIOS_COM_Mode_Rx: - usart_dev->init.USART_Mode = USART_Mode_Rx; - break; - case PIOS_COM_Mode_Tx: - usart_dev->init.USART_Mode = USART_Mode_Tx; - break; - case PIOS_COM_Mode_RxTx: - usart_dev->init.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; - break; - default: - break; - } - - /* Write back the modified configuration */ - USART_Init(usart_dev->cfg->regs, &usart_dev->init); - - /* - * Re enable USART. - */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state) @@ -459,8 +434,12 @@ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback r * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ - usart_dev->rx_in_context = context; + usart_dev->rx_in_context = context; usart_dev->rx_in_cb = rx_in_cb; + + usart_dev->init.USART_Mode |= USART_Mode_Rx; + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) @@ -475,8 +454,12 @@ static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback t * Order is important in these assignments since ISR uses _cb * field to determine if it's ok to dereference _cb and _context */ - usart_dev->tx_out_context = context; + usart_dev->tx_out_context = context; usart_dev->tx_out_cb = tx_out_cb; + + usart_dev->init.USART_Mode |= USART_Mode_Tx; + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) @@ -547,6 +530,9 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) case PIOS_IOCTL_USART_GET_TXGPIO: *(struct stm32_gpio *)param = usart_dev->cfg->tx; break; + case PIOS_IOCTL_USART_SET_HALFDUPLEX: + USART_HalfDuplexCmd(usart_dev->cfg->regs, *(bool *)param ? ENABLE : DISABLE); + break; default: if (usart_dev->cfg->ioctl) { return usart_dev->cfg->ioctl(usart_id, ctl, param); diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 36f84be85..8e29dc590 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -52,7 +52,7 @@ #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR -//#define PIOS_INCLUDE_USART +// #define PIOS_INCLUDE_USART // #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID // #define PIOS_INCLUDE_GPIO @@ -87,8 +87,8 @@ #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_DSM // #define PIOS_INCLUDE_SBUS -//#define PIOS_INCLUDE_PPM -//#define PIOS_INCLUDE_PWM +// #define PIOS_INCLUDE_PPM +// #define PIOS_INCLUDE_PWM /* #define PIOS_INCLUDE_GCSRCVR */ /* #define PIOS_INCLUDE_OPLINKRCVR */ #define PIOS_INCLUDE_IAP From bb68ed2c5a88e65a5fb9ad60a4eb009ae55f90fb Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 21 Apr 2017 12:01:42 +0200 Subject: [PATCH 16/20] LP-480 RevoNano USART2 sbus baud rate workaround --- flight/pios/common/pios_sbus.c | 6 +++++- flight/targets/boards/revonano/pios_board.h | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index f7cd3f475..e424aa8f6 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -35,6 +35,10 @@ // Define to report number of frames since last dropped instead of weighted ave #undef SBUS_GOOD_FRAME_COUNT +#ifndef PIOS_SBUS_BAUD_RATE +#define PIOS_SBUS_BAUD_RATE 100000 +#endif + #include #include "pios_sbus_priv.h" @@ -168,7 +172,7 @@ int32_t PIOS_SBus_Init(uint32_t *sbus_id, /* Set rest of the parameters */ if (driver->set_config) { - driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_Even, PIOS_COM_StopBits_2, 100000); + driver->set_config(lower_id, PIOS_COM_Word_length_8b, PIOS_COM_Parity_Even, PIOS_COM_StopBits_2, PIOS_SBUS_BAUD_RATE); } /* Set inverted UART and IRQ priority */ diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 79a884a02..f6bcc20f2 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -233,7 +233,7 @@ extern uint32_t pios_packet_handler; // ------------------------- #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) - +#define PIOS_SBUS_BAUD_RATE 99999 /* f411 / 96mhz sysclk / usart2 baud rate weirdness */ // ------------------------- // Receiver HOTT input // ------------------------- From 3d876d703572b5a0c6d20418f15cee0eb2c00f84 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 24 Apr 2017 16:10:18 +0200 Subject: [PATCH 17/20] LP-480 Removed RadioAuxStream function from PIOS_BOARD_IO_Configure_RFM22B(), added another function to configure radioaux from hwsettings (revo & sparky2 only). --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 +- .../modules/RadioComBridge/RadioComBridge.c | 4 +- flight/pios/common/pios_board_io.c | 106 ++++---- flight/pios/common/pios_rfm22b.c | 4 +- flight/pios/inc/pios_board_io.h | 46 ++-- .../coptercontrol/firmware/pios_board.c | 12 +- .../discoveryf4bare/firmware/pios_board.c | 20 +- .../boards/oplinkmini/firmware/pios_board.c | 229 ++---------------- flight/targets/boards/oplinkmini/pios_board.h | 10 +- .../boards/revolution/firmware/pios_board.c | 21 +- .../boards/revonano/firmware/pios_board.c | 6 +- .../boards/revoproto/firmware/pios_board.c | 6 +- .../boards/sparky2/firmware/pios_board.c | 16 +- 13 files changed, 142 insertions(+), 340 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index d2a27e574..f4baa0291 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_osd; + buildArgumentsString = ef_oplinkmini; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 835390655..0ffa41bc8 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -41,8 +41,9 @@ #include #include #include - +#include #include +#include // **************** // Private constants @@ -57,6 +58,7 @@ #define SERIAL_RX_BUF_LEN 100 #define PPM_INPUT_TIMEOUT 100 +#define PIOS_PPM_RECEIVER pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] // **************** // Private types diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index e43b3053c..025b4cec0 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -84,7 +84,8 @@ uint32_t pios_com_telem_rf_id; /* Serial port telemetry */ #ifdef PIOS_INCLUDE_RFM22B uint32_t pios_rfm22b_id; /* RFM22B handle */ -uint32_t pios_com_rf_id; /* RFM22B telemetry */ +uint32_t pios_com_pri_radio_id; /* oplink primary com stream */ +uint32_t pios_com_aux_radio_id; /* oplink aux com stream */ #endif #ifdef PIOS_INCLUDE_OPENLRS @@ -402,6 +403,24 @@ static const struct uart_function uart_function_map[] = { #endif /* PIOS_INCLUDE_RCVR */ }; +void PIOS_BOARD_IO_Configure_UART_COM(const struct pios_usart_cfg *hw_config, + uint16_t rx_buf_len, + uint16_t tx_buf_len, + uint32_t *com_id) +{ + uint32_t usart_id; + + if (PIOS_USART_Init(&usart_id, hw_config)) { + PIOS_Assert(0); + } + + if (PIOS_COM_Init(com_id, &pios_usart_com_driver, usart_id, + 0, rx_buf_len, + 0, tx_buf_len)) { + PIOS_Assert(0); + } +} + void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *hw_config, PIOS_BOARD_IO_UART_Function function) { if (function >= NELEMENTS(uart_function_map)) { @@ -409,17 +428,10 @@ void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *hw_config, PIOS_B } if (uart_function_map[function].com_id) { - uint32_t usart_id; - - if (PIOS_USART_Init(&usart_id, hw_config)) { - PIOS_Assert(0); - } - - if (PIOS_COM_Init(uart_function_map[function].com_id, &pios_usart_com_driver, usart_id, - 0, uart_function_map[function].com_rx_buf_len, - 0, uart_function_map[function].com_tx_buf_len)) { - PIOS_Assert(0); - } + PIOS_BOARD_IO_Configure_UART_COM(hw_config, + uart_function_map[function].com_rx_buf_len, + uart_function_map[function].com_tx_buf_len, + uart_function_map[function].com_id); } #ifdef PIOS_INCLUDE_RCVR else if (uart_function_map[function].rcvr_init) { @@ -446,7 +458,7 @@ void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *hw_config, PIOS_B } #ifdef PIOS_INCLUDE_PWM -void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg) +void PIOS_BOARD_IO_Configure_PWM_RCVR(const struct pios_pwm_cfg *pwm_cfg) { /* Set up the receiver port. Later this should be optional */ uint32_t pios_pwm_id; @@ -462,7 +474,7 @@ void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg) #endif /* PIOS_INCLUDE_PWM */ #ifdef PIOS_INCLUDE_PPM -void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg) +void PIOS_BOARD_IO_Configure_PPM_RCVR(const struct pios_ppm_cfg *ppm_cfg) { uint32_t pios_ppm_id; @@ -477,7 +489,7 @@ void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg) #endif /* PIOS_INCLUDE_PPM */ #ifdef PIOS_INCLUDE_GCSRCVR -void PIOS_BOARD_IO_Configure_GCSRCVR() +void PIOS_BOARD_IO_Configure_GCS_RCVR() { GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; @@ -492,7 +504,7 @@ void PIOS_BOARD_IO_Configure_GCSRCVR() #ifdef PIOS_INCLUDE_RFM22B -void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_function) +void PIOS_BOARD_IO_Configure_RFM22B() { #if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); @@ -558,9 +570,16 @@ void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_fun #endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */ /* Configure the radio com interface */ - if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - 0, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - 0, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + if (PIOS_COM_Init(&pios_com_pri_radio_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + 0, PIOS_COM_PRI_RADIO_RX_BUF_LEN, + 0, PIOS_COM_PRI_RADIO_TX_BUF_LEN)) { + PIOS_Assert(0); + } + + // Initialize the aux radio com interface + if (PIOS_COM_Init(&pios_com_aux_radio_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, + 0, PIOS_COM_AUX_RADIO_RX_BUF_LEN, + 0, PIOS_COM_AUX_RADIO_TX_BUF_LEN)) { PIOS_Assert(0); } @@ -637,35 +656,6 @@ void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_fun /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); - - // TODO: this is in preparation for full mavlink support and is used by LP-368 - uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN; - - switch (radioaux_function) { - default:; - case PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE: -#ifdef PIOS_INCLUDE_DEBUG_CONSOLE - if (PIOS_COM_Init(&pios_com_debug_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - 0, 0, - 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } -#endif - break; - case PIOS_BOARD_IO_RADIOAUX_COMBRIDGE: - if (PIOS_COM_Init(&pios_com_bridge_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - 0, PIOS_COM_BRIDGE_RX_BUF_LEN, - 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - break; - case PIOS_BOARD_IO_RADIOAUX_MAVLINK: - if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - 0, mavlink_rx_size, - 0, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } } } else { oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; @@ -673,4 +663,24 @@ void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function radioaux_fun OPLinkStatusSet(&oplinkStatus); } + + +void PIOS_BOARD_IO_Configure_RadioAuxStream(HwSettingsRadioAuxStreamOptions radioaux) +{ + switch (radioaux) { + case HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE: +#ifdef PIOS_INCLUDE_DEBUG_CONSOLE + pios_com_debug_id = pios_com_aux_radio_id; +#endif + break; + case HWSETTINGS_RADIOAUXSTREAM_MAVLINK: + pios_com_mavlink_id = pios_com_aux_radio_id; + break; + case HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE: + pios_com_bridge_id = pios_com_aux_radio_id; + break; + case HWSETTINGS_RADIOAUXSTREAM_DISABLED: + break; + } +} #endif /* ifdef PIOS_INCLUDE_RFM22B */ diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 7fa05e07b..0579c99a9 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -783,8 +783,8 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len) return false; } - rfm22b_dev->tx_packet_handle = p; - rfm22b_dev->packet_start_time = pios_rfm22_time_ms(); + rfm22b_dev->tx_packet_handle = p; + rfm22b_dev->packet_start_time = pios_rfm22_time_ms(); if (rfm22b_dev->packet_start_time == 0) { rfm22b_dev->packet_start_time = 1; } diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index cf0533b02..34e01704e 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -91,22 +91,31 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_telem_rf_id; #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #ifndef PIOS_COM_TELEM_RF_RX_BUF_LEN -# define PIOS_COM_TELEM_RF_RX_BUF_LEN 512 +# define PIOS_COM_TELEM_RF_RX_BUF_LEN 128 #endif #ifndef PIOS_COM_TELEM_RF_TX_BUF_LEN -# define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 +# define PIOS_COM_TELEM_RF_TX_BUF_LEN 128 #endif /* RFM22B telemetry */ #ifdef PIOS_INCLUDE_RFM22B extern uint32_t pios_rfm22b_id; /* RFM22B handle */ -extern uint32_t pios_com_rf_id; /* oplink telemetry */ -# define PIOS_COM_RF (pios_com_rf_id) -# ifndef PIOS_COM_RFM22B_RF_RX_BUF_LEN -# define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 +extern uint32_t pios_com_pri_radio_id; /* oplink primary com stream */ +extern uint32_t pios_com_aux_radio_id; /* oplink aux com stream */ +# define PIOS_COM_RF (pios_com_pri_radio_id) +# define PIOS_COM_PRI_RADIO (pios_com_pri_radio_id) +# define PIOS_COM_AUX_RADIO (pios_com_aux_radio_id) +# ifndef PIOS_COM_PRI_RADIO_RX_BUF_LEN +# define PIOS_COM_PRI_RADIO_RX_BUF_LEN 128 # endif -# ifndef PIOS_COM_RFM22B_RF_TX_BUF_LEN -# define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 +# ifndef PIOS_COM_PRI_RADIO_TX_BUF_LEN +# define PIOS_COM_PRI_RADIO_TX_BUF_LEN 128 +# endif +# ifndef PIOS_COM_AUX_RADIO_RX_BUF_LEN +# define PIOS_COM_AUX_RADIO_RX_BUF_LEN 128 +# endif +# ifndef PIOS_COM_AUX_RADIO_TX_BUF_LEN +# define PIOS_COM_AUX_RADIO_TX_BUF_LEN 128 # endif #endif @@ -201,13 +210,6 @@ typedef enum { PIOS_BOARD_IO_UART_HOTT_BRIDGE, /* com */ } PIOS_BOARD_IO_UART_Function; -typedef enum { - PIOS_BOARD_IO_RADIOAUX_NONE = 0, - PIOS_BOARD_IO_RADIOAUX_MAVLINK, - PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, - PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, -// PIOS_BOARD_IO_RADIOAUX_FRSKY_SPORT_TELEMETRY, -} PIOS_BOARD_IO_RADIOAUX_Function; #ifdef PIOS_INCLUDE_USB # ifndef BOOTLOADER @@ -234,22 +236,26 @@ void PIOS_BOARD_IO_Configure_USB_Function(PIOS_BOARD_IO_USB_HID_Function hid_fun # endif // ifndef BOOTLOADER #endif // ifdef PIOS_INCLUDE_USB #ifdef PIOS_INCLUDE_PWM -void PIOS_BOARD_IO_Configure_PWM(const struct pios_pwm_cfg *pwm_cfg); +void PIOS_BOARD_IO_Configure_PWM_RCVR(const struct pios_pwm_cfg *pwm_cfg); #endif #ifdef PIOS_INCLUDE_PPM -void PIOS_BOARD_IO_Configure_PPM(const struct pios_ppm_cfg *ppm_cfg); +void PIOS_BOARD_IO_Configure_PPM_RCVR(const struct pios_ppm_cfg *ppm_cfg); #endif - #ifdef PIOS_INCLUDE_USART void PIOS_BOARD_IO_Configure_UART(const struct pios_usart_cfg *usart_cfg, PIOS_BOARD_IO_UART_Function function); +void PIOS_BOARD_IO_Configure_UART_COM(const struct pios_usart_cfg *hw_config, + uint16_t rx_buf_len, + uint16_t tx_buf_len, + uint32_t *com_id); #endif #ifdef PIOS_INCLUDE_RFM22B -void PIOS_BOARD_IO_Configure_RFM22B(PIOS_BOARD_IO_RADIOAUX_Function function); +void PIOS_BOARD_IO_Configure_RFM22B(); +void PIOS_BOARD_IO_Configure_RadioAuxStream(HwSettingsRadioAuxStreamOptions radioaux); /* not for OPLM */ #endif #ifdef PIOS_INCLUDE_GCSRCVR -void PIOS_BOARD_IO_Configure_GCSRCVR(); +void PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif #endif /* PIOS_BOARD_IO_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index baffcae28..47509d294 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -246,7 +246,7 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_CC_FLEXIPORT_PPM: #if defined(PIOS_INCLUDE_PPM_FLEXI) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_flexi_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_flexi_cfg); #endif /* PIOS_INCLUDE_PPM_FLEXI */ break; } @@ -289,23 +289,23 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: #if defined(PIOS_INCLUDE_PWM) - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM((hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR((hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ break; case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_with_ppm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_with_ppm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: @@ -313,7 +313,7 @@ void PIOS_Board_Init(void) } #ifdef PIOS_INCLUDE_GCSRCVR - PIOS_BOARD_IO_Configure_GCSRCVR(); + PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif /* Remap AFIO pin for PB4 (Servo 5 Out)*/ diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 5ffc19ec6..544f98c78 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -99,12 +99,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { - [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, - [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, - [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, -}; - int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); @@ -287,12 +281,12 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); + uint8_t hwsettings_radioaux; HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - if (hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { - PIOS_BOARD_IO_Configure_RFM22B(pios_spi_telem_flash_id, radioaux_function_map[hwsettings_radioaux]); - } + PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #endif /* PIOS_INCLUDE_RFM22B */ @@ -316,7 +310,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) /* Set up the receiver port. Later this should be optional */ - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -329,7 +323,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: case HWSETTINGS_RM_RCVRPORT_PPMGPS: #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { // configure servo outputs and the remaining 5 inputs as outputs @@ -338,7 +332,7 @@ void PIOS_Board_Init(void) // enable pwm on the remaining channels if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_ppm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_ppm_cfg); } break; @@ -349,7 +343,7 @@ void PIOS_Board_Init(void) break; } #ifdef PIOS_INCLUDE_GCSRCVR - PIOS_BOARD_IO_Configure_GCSRCVR(); + PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif #ifndef PIOS_ENABLE_DEBUG_PINS diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 7b0787821..61730031e 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -37,6 +37,7 @@ #ifdef PIOS_INCLUDE_SERVO #include #endif +#include /* * Pull in the board-specific static HW definitions. @@ -48,36 +49,17 @@ */ #include "../board_hw_defs.c" -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128 - -#define PIOS_COM_TELEM_VCP_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_VCP_TX_BUF_LEN 128 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 128 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 128 - -#define PIOS_COM_TELEM_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_TX_BUF_LEN 128 - uint32_t pios_com_hid_id = 0; -uint32_t pios_com_vcp_id = 0; +// uint32_t pios_com_vcp_id = 0; /* this is provided by pios_board_io.c */ uint32_t pios_com_main_id = 0; uint32_t pios_com_flexi_id = 0; -uint32_t pios_com_bridge_id = 0; uint32_t pios_com_gcs_id = 0; uint32_t pios_com_gcs_out_id = 0; -#if defined(PIOS_INCLUDE_PPM) -uint32_t pios_ppm_rcvr_id = 0; -#endif #if defined(PIOS_INCLUDE_PPM_OUT) uint32_t pios_ppm_out_id = 0; #endif #if defined(PIOS_INCLUDE_RFM22B) #include -uint32_t pios_rfm22b_id = 0; -uint32_t pios_com_pri_radio_id = 0; -uint32_t pios_com_aux_radio_id = 0; uint32_t pios_com_pri_radio_out_id = 0; uint32_t pios_com_aux_radio_out_id = 0; #endif @@ -87,41 +69,6 @@ uintptr_t pios_user_fs_id = 0; static uint8_t servo_count = 0; -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size of 0 make the port rx only - * rx size of 0 make the port tx only - * having both tx and rx size of 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) -{ - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - - // Forward definitions static void PIOS_Board_PPM_callback(uint32_t context, const int16_t *channels); @@ -184,10 +131,7 @@ void PIOS_Board_Init(void) PIOS_IAP_WriteBootCmd(2, 0); } -#if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ /* Retrieve the settings object. */ OPLinkSettingsData oplinkSettings; @@ -195,12 +139,7 @@ void PIOS_Board_Init(void) /* Determine the modem protocols */ bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR); - bool openlrs = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPENLRS); bool ppm_only = (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL); - bool data_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATA) || - (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); - bool is_enabled = ((oplinkSettings.Protocol != OPLINKSETTINGS_PROTOCOL_DISABLED) && - ((oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) || openlrs)); bool ppm_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) || (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); bool servo_main = false; @@ -300,9 +239,9 @@ void PIOS_Board_Init(void) case OPLINKSETTINGS_MAINPORT_TELEMETRY: case OPLINKSETTINGS_MAINPORT_SERIAL: #ifndef PIOS_RFM22B_DEBUG_ON_TELEM - PIOS_Board_configure_com(&pios_usart_main_cfg, - PIOS_COM_TELEM_RX_BUF_LEN, PIOS_COM_TELEM_TX_BUF_LEN, - &pios_usart_com_driver, &pios_com_main_id); + PIOS_BOARD_IO_Configure_UART_COM(&pios_usart_main_cfg, + PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, + &pios_com_main_id); PIOS_COM_ChangeBaud(pios_com_main_id, mainComSpeed); #endif /* !PIOS_RFM22B_DEBUG_ON_TELEM */ break; @@ -310,12 +249,7 @@ void PIOS_Board_Init(void) #if defined(PIOS_INCLUDE_PPM) /* PPM input is configured on the coordinator modem and output on the remote modem. */ if (is_coordinator) { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); - - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_main_cfg); } // For some reason, PPM output on the main port doesn't work. #if defined(PIOS_INCLUDE_PPM_OUT) @@ -359,9 +293,9 @@ void PIOS_Board_Init(void) case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: case OPLINKSETTINGS_FLEXIPORT_SERIAL: #ifndef PIOS_RFM22B_DEBUG_ON_TELEM - PIOS_Board_configure_com(&pios_usart_flexi_cfg, - PIOS_COM_TELEM_RX_BUF_LEN, PIOS_COM_TELEM_TX_BUF_LEN, - &pios_usart_com_driver, &pios_com_flexi_id); + PIOS_BOARD_IO_Configure_UART_COM(&pios_usart_flexi_cfg, + PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, + &pios_com_flexi_id); PIOS_COM_ChangeBaud(pios_com_flexi_id, flexiComSpeed); #endif /* !PIOS_RFM22B_DEBUG_ON_TELEM */ break; @@ -369,12 +303,7 @@ void PIOS_Board_Init(void) #if defined(PIOS_INCLUDE_PPM) /* PPM input is configured on the coordinator modem and output on the remote modem. */ if (is_coordinator) { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_flexi_cfg); } // For some reason, PPM output on the flexi port doesn't work. #if defined(PIOS_INCLUDE_PPM_OUT) @@ -410,138 +339,11 @@ void PIOS_Board_Init(void) PIOS_Servo_SetBankMode(1, PIOS_SERVO_BANK_MODE_PWM); #endif - // Initialize out status object. - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); + PIOS_BOARD_IO_Configure_RFM22B(); - // Get our hardware information. - const struct pios_board_info *bdinfo = &pios_board_info_blob; - - oplinkStatus.BoardType = bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision = bdinfo->board_rev; - - /* Initialize the RFM22B radio COM device. */ - if (is_enabled) { - if (openlrs) { -#if defined(PIOS_INCLUDE_OPENLRS) - const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(bdinfo->board_rev); - uint32_t openlrs_id; - - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; - - PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg); - PIOS_OpenLRS_RegisterPPMCallback(openlrs_id, PIOS_Board_PPM_callback, 0); -#endif /* PIOS_INCLUDE_OPENLRS */ - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; - - // Configure the RFM22B device - const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - - if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) { - PIOS_Assert(0); - } - - // Configure the radio com interface - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_pri_radio_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } - - // Initialize the aux radio com interface - uint8_t *auxrx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RX_BUF_LEN); - uint8_t *auxtx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_TX_BUF_LEN); - PIOS_Assert(auxrx_buffer); - PIOS_Assert(auxtx_buffer); - if (PIOS_COM_Init(&pios_com_aux_radio_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id, - auxrx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - auxtx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } - - // Set the modem (over the air) datarate. - enum rfm22b_datarate datarate = RFM22_datarate_64000; - switch (oplinkSettings.AirDataRate) { - case OPLINKSETTINGS_AIRDATARATE_9600: - datarate = RFM22_datarate_9600; - break; - case OPLINKSETTINGS_AIRDATARATE_19200: - datarate = RFM22_datarate_19200; - break; - case OPLINKSETTINGS_AIRDATARATE_32000: - datarate = RFM22_datarate_32000; - break; - case OPLINKSETTINGS_AIRDATARATE_57600: - datarate = RFM22_datarate_57600; - break; - case OPLINKSETTINGS_AIRDATARATE_64000: - datarate = RFM22_datarate_64000; - break; - case OPLINKSETTINGS_AIRDATARATE_100000: - datarate = RFM22_datarate_100000; - break; - case OPLINKSETTINGS_AIRDATARATE_128000: - datarate = RFM22_datarate_128000; - break; - case OPLINKSETTINGS_AIRDATARATE_192000: - datarate = RFM22_datarate_192000; - break; - case OPLINKSETTINGS_AIRDATARATE_256000: - datarate = RFM22_datarate_256000; - break; - } - - /* Set the modem Tx power level */ - switch (oplinkSettings.MaxRFPower) { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - default: - // do nothing - break; - } - - // Set the radio configuration parameters. - PIOS_RFM22B_SetDeviceID(pios_rfm22b_id, oplinkSettings.CustomDeviceID); - PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); - PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap); - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode); - - /* Set the PPM callback if we should be receiving PPM. */ - if (ppm_mode || (ppm_only && !is_coordinator)) { - PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback, 0); - } - } // openlrs - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + /* Set the PPM callback if we should be receiving PPM. */ + if (ppm_mode || (ppm_only && !is_coordinator)) { + PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback, 0); } // Configure the primary radio stream destination. @@ -622,9 +424,6 @@ void PIOS_Board_Init(void) break; } - // Update the object - OPLinkStatusSet(&oplinkStatus); - /* Remap AFIO pin */ GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index e96c0b12d..15f07034b 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -172,7 +172,13 @@ extern uint32_t pios_i2c_flexi_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 5 +#define PIOS_COM_MAX_DEVS 5 + +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128 + +#define PIOS_COM_TELEM_VCP_RX_BUF_LEN 128 +#define PIOS_COM_TELEM_VCP_TX_BUF_LEN 128 // The direct com ports extern uint32_t pios_com_hid_id; @@ -192,7 +198,6 @@ extern uint32_t pios_com_pri_radio_out_id; // The destination port for the auxiliary radio port. extern uint32_t pios_com_aux_radio_out_id; // The PPM IDs -extern uint32_t pios_ppm_rcvr_id; extern uint32_t pios_ppm_out_id; #define PIOS_COM_HID (pios_com_hid_id) #define PIOS_COM_VCP (pios_com_vcp_id) @@ -205,7 +210,6 @@ extern uint32_t pios_ppm_out_id; #define PIOS_COM_GCS (pios_com_gcs_id) #define PIOS_COM_GCS_OUT (pios_com_gcs_out_id) #define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) #define PIOS_PPM_OUTPUT (pios_ppm_out_id) #define RFM22_DEBUG 1 diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 47279154e..e958bb7c8 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -98,13 +98,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { - [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, - [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, - [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, -}; - - int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); @@ -287,12 +280,12 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); + uint8_t hwsettings_radioaux; HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - if (hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { - PIOS_BOARD_IO_Configure_RFM22B(radioaux_function_map[hwsettings_radioaux]); - } + PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) @@ -314,7 +307,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) /* Set up the receiver port. Later this should be optional */ - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -328,7 +321,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMGPS: case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY: #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { // configure servo outputs and the remaining 5 inputs as outputs @@ -337,7 +330,7 @@ void PIOS_Board_Init(void) // enable pwm on the remaining channels if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_ppm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_ppm_cfg); } break; @@ -348,7 +341,7 @@ void PIOS_Board_Init(void) break; } #ifdef PIOS_INCLUDE_GCSRCVR - PIOS_BOARD_IO_Configure_GCSRCVR(); + PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif #ifndef PIOS_ENABLE_DEBUG_PINS diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index d4883768d..5f8fb7799 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -263,7 +263,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) /* Set up the receiver port. Later this should be optional */ - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -275,7 +275,7 @@ void PIOS_Board_Init(void) pios_servo_cfg = &pios_servo_cfg_out_in_ppm; } - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); break; #endif /* PIOS_INCLUDE_PPM */ @@ -287,7 +287,7 @@ void PIOS_Board_Init(void) #ifdef PIOS_INCLUDE_GCSRCVR - PIOS_BOARD_IO_Configure_GCSRCVR(); + PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif #ifdef PIOS_INCLUDE_WS2811 diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 167f19bd0..66242f413 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -313,13 +313,13 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_RV_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RV_RCVRPORT_PPM: case HWSETTINGS_RV_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ case HWSETTINGS_RV_RCVRPORT_OUTPUTS: @@ -359,7 +359,7 @@ void PIOS_Board_Init(void) #endif #ifdef PIOS_INCLUDE_GCSRCVR - PIOS_BOARD_IO_Configure_GCSRCVR(); + PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif #ifndef PIOS_ENABLE_DEBUG_PINS diff --git a/flight/targets/boards/sparky2/firmware/pios_board.c b/flight/targets/boards/sparky2/firmware/pios_board.c index a9b72f485..d9d7fd9ee 100644 --- a/flight/targets/boards/sparky2/firmware/pios_board.c +++ b/flight/targets/boards/sparky2/firmware/pios_board.c @@ -93,12 +93,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_SPK2_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -static const PIOS_BOARD_IO_RADIOAUX_Function radioaux_function_map[] = { - [HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE] = PIOS_BOARD_IO_RADIOAUX_DEBUGCONSOLE, - [HWSETTINGS_RADIOAUXSTREAM_MAVLINK] = PIOS_BOARD_IO_RADIOAUX_MAVLINK, - [HWSETTINGS_RADIOAUXSTREAM_COMBRIDGE] = PIOS_BOARD_IO_RADIOAUX_COMBRIDGE, -}; - int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) { const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); @@ -263,12 +257,12 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); + uint8_t hwsettings_radioaux; HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - if (hwsettings_radioaux < NELEMENTS(radioaux_function_map)) { - PIOS_BOARD_IO_Configure_RFM22B(radioaux_function_map[hwsettings_radioaux]); - } + PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #endif /* PIOS_INCLUDE_RFM22B */ /* Initialize inverter gpio and set it to off */ @@ -301,12 +295,12 @@ void PIOS_Board_Init(void) #if defined(PIOS_INCLUDE_PPM) if (hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_PPM) { - PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); } #endif #ifdef PIOS_INCLUDE_GCSRCVR - PIOS_BOARD_IO_Configure_GCSRCVR(); + PIOS_BOARD_IO_Configure_GCS_RCVR(); #endif #ifndef PIOS_ENABLE_DEBUG_PINS From 01f5baaed297d608ad274794c5b6fdc36d29a55f Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Tue, 25 Apr 2017 13:11:37 +0200 Subject: [PATCH 18/20] LP-480 Removed PIOS_BOARD_USART_Ioctl() and moved inverter handling to pios_usart directly. Handling is now conditionally built in based on PIOS_USART_INVERTER_PORT define. --- flight/pios/common/pios_dsm.c | 2 +- flight/pios/inc/pios_com.h | 2 + flight/pios/inc/pios_usart.h | 3 ++ flight/pios/stm32f10x/pios_usart.c | 48 ++++++++++++++++-- flight/pios/stm32f4xx/pios_usart.c | 49 +++++++++++++++++-- .../boards/coptercontrol/board_hw_defs.c | 9 ---- .../coptercontrol/firmware/pios_board.c | 35 ------------- .../targets/boards/coptercontrol/pios_board.h | 15 ++++-- .../boards/discoveryf4bare/board_hw_defs.c | 8 --- .../discoveryf4bare/firmware/pios_board.c | 36 -------------- .../boards/discoveryf4bare/pios_board.h | 8 ++- .../targets/boards/revolution/board_hw_defs.c | 9 ---- .../boards/revolution/firmware/pios_board.c | 36 -------------- flight/targets/boards/revolution/pios_board.h | 8 ++- .../targets/boards/revonano/board_hw_defs.c | 40 ++++++--------- .../boards/revonano/firmware/pios_board.c | 36 -------------- flight/targets/boards/revonano/pios_board.h | 8 ++- .../targets/boards/revoproto/board_hw_defs.c | 9 ---- .../boards/revoproto/firmware/pios_board.c | 36 -------------- flight/targets/boards/revoproto/pios_board.h | 8 ++- flight/targets/boards/sparky2/board_hw_defs.c | 10 ---- .../boards/sparky2/firmware/pios_board.c | 35 ------------- flight/targets/boards/sparky2/pios_board.h | 9 +++- 23 files changed, 156 insertions(+), 303 deletions(-) diff --git a/flight/pios/common/pios_dsm.c b/flight/pios/common/pios_dsm.c index 747e2b91f..9f66a1aff 100644 --- a/flight/pios/common/pios_dsm.c +++ b/flight/pios/common/pios_dsm.c @@ -318,7 +318,7 @@ int32_t PIOS_DSM_Init(uint32_t *dsm_id, PIOS_DEBUG_Assert(driver->ioctl); - if ((driver->ioctl)(lower_id, PIOS_IOCTL_USART_GET_RXGPIO, &rxpin) < 0) { + if ((driver->ioctl)(lower_id, PIOS_IOCTL_USART_GET_DSMBIND, &rxpin) < 0) { return -1; } diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index 113f38d6f..4b827ad8d 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -146,6 +146,8 @@ enum pios_com_ioctl_type { COM_IOCTL_TYPE_SOFT_UART, }; +#define COM_IOCTL_ENOSYS (-1) /* Function not implemented */ + #endif /* PIOS_COM_H */ /** diff --git a/flight/pios/inc/pios_usart.h b/flight/pios/inc/pios_usart.h index 35e35c89b..18208f4a4 100644 --- a/flight/pios/inc/pios_usart.h +++ b/flight/pios/inc/pios_usart.h @@ -53,6 +53,9 @@ enum PIOS_USART_Inverted { /* PIOS_IRQ_PRIO_ values */ #define PIOS_IOCTL_USART_SET_IRQ_PRIO COM_IOCTL(COM_IOCTL_TYPE_USART, 6, uint8_t) +#define PIOS_IOCTL_USART_GET_DSMBIND COM_IOCTL(COM_IOCTL_TYPE_USART, 7, struct stm32_gpio) + + #endif /* PIOS_USART_H */ /** diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index cc1e23232..4f3881ac3 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -190,7 +190,21 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* DTR handling? */ - *usart_id = (uint32_t)usart_dev; +#ifdef PIOS_USART_INVERTER_PORT + /* Initialize inverter gpio and set it to off */ + if (usart_dev->cfg->regs == PIOS_USART_INVERTER_PORT) { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = PIOS_USART_INVERTER_PIN, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }; + GPIO_Init(PIOS_USART_INVERTER_GPIO, &inverterGPIOInit); + + GPIO_WriteBit(PIOS_USART_INVERTER_GPIO, + PIOS_USART_INVERTER_PIN, + PIOS_USART_INVERTER_DISABLE); + } +#endif /* Configure USART Interrupts */ switch ((uint32_t)usart_dev->cfg->regs) { @@ -210,6 +224,8 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); + *usart_id = (uint32_t)usart_dev; + return 0; out_fail: @@ -462,10 +478,35 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) PIOS_Assert(valid); + /* First try board specific IOCTL to allow overriding default functions */ + if (usart_dev->cfg->ioctl) { + int32_t ret = usart_dev->cfg->ioctl(usart_id, ctl, param); + if (ret != COM_IOCTL_ENOSYS) { + return ret; + } + } + switch (ctl) { case PIOS_IOCTL_USART_SET_IRQ_PRIO: return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); +#ifdef PIOS_USART_INVERTER_PORT + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_dev->cfg->regs != PIOS_USART_INVERTER_PORT) { + return COM_IOCTL_ENOSYS; /* don't know how */ + } + GPIO_WriteBit(PIOS_USART_INVERTER_GPIO, + PIOS_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? PIOS_USART_INVERTER_ENABLE : PIOS_USART_INVERTER_DISABLE); + + break; +#endif /* PIOS_USART_INVERTER_PORT */ + case PIOS_IOCTL_USART_GET_DSMBIND: +#ifdef PIOS_USART_INVERTER_PORT + if (usart_dev->cfg->regs == PIOS_USART_INVERTER_PORT) { + return -2; /* do not allow dsm bind on port with inverter */ + } +#endif /* otherwise, return RXGPIO */ case PIOS_IOCTL_USART_GET_RXGPIO: *(struct stm32_gpio *)param = usart_dev->cfg->rx; break; @@ -476,10 +517,7 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) USART_HalfDuplexCmd(usart_dev->cfg->regs, *(bool *)param ? ENABLE : DISABLE); break; default: - if (usart_dev->cfg->ioctl) { - return usart_dev->cfg->ioctl(usart_id, ctl, param); - } - return -1; + return COM_IOCTL_ENOSYS; /* unknown ioctl */ } return 0; diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 3731bb658..c14a07c5d 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -218,8 +218,23 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) GPIO_Init(usart_dev->cfg->dtr.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->dtr.init); PIOS_USART_SetCtrlLine((uint32_t)usart_dev, COM_CTRL_LINE_DTR_MASK, 0); } +#ifdef PIOS_USART_INVERTER_PORT + /* Initialize inverter gpio and set it to off */ + if (usart_dev->cfg->regs == PIOS_USART_INVERTER_PORT) { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = PIOS_USART_INVERTER_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; + GPIO_Init(PIOS_USART_INVERTER_GPIO, &inverterGPIOInit); - *usart_id = (uint32_t)usart_dev; + GPIO_WriteBit(PIOS_USART_INVERTER_GPIO, + PIOS_USART_INVERTER_PIN, + PIOS_USART_INVERTER_DISABLE); + } +#endif /* Configure USART Interrupts */ switch ((uint32_t)usart_dev->cfg->regs) { @@ -252,6 +267,8 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) } PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); + *usart_id = (uint32_t)usart_dev; + return 0; out_fail: @@ -520,10 +537,35 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) PIOS_Assert(valid); + /* First try board specific IOCTL to allow overriding default functions */ + if (usart_dev->cfg->ioctl) { + int32_t ret = usart_dev->cfg->ioctl(usart_id, ctl, param); + if (ret != COM_IOCTL_ENOSYS) { + return ret; + } + } + switch (ctl) { case PIOS_IOCTL_USART_SET_IRQ_PRIO: return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); +#ifdef PIOS_USART_INVERTER_PORT + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_dev->cfg->regs != PIOS_USART_INVERTER_PORT) { + return COM_IOCTL_ENOSYS; /* don't know how */ + } + GPIO_WriteBit(PIOS_USART_INVERTER_GPIO, + PIOS_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? PIOS_USART_INVERTER_ENABLE : PIOS_USART_INVERTER_DISABLE); + + break; +#endif /* PIOS_USART_INVERTER_PORT */ + case PIOS_IOCTL_USART_GET_DSMBIND: +#ifdef PIOS_USART_INVERTER_PORT + if (usart_dev->cfg->regs == PIOS_USART_INVERTER_PORT) { + return -2; /* do not allow dsm bind on port with inverter */ + } +#endif /* otherwise, return RXGPIO */ case PIOS_IOCTL_USART_GET_RXGPIO: *(struct stm32_gpio *)param = usart_dev->cfg->rx; break; @@ -534,10 +576,7 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) USART_HalfDuplexCmd(usart_dev->cfg->regs, *(bool *)param ? ENABLE : DISABLE); break; default: - if (usart_dev->cfg->ioctl) { - return usart_dev->cfg->ioctl(usart_id, ctl, param); - } - return -1; + return COM_IOCTL_ENOSYS; /* unknown ioctl */ } return 0; diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index d6b93ff4c..949234a6d 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -597,14 +597,6 @@ static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL #include "pios_usart_priv.h" -// Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOB -#define MAIN_USART_INVERTER_PIN GPIO_Pin_2 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET - -static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); - static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .rx = { @@ -623,7 +615,6 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_Mode = GPIO_Mode_AF_PP, }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, }; static const struct pios_usart_cfg pios_usart_flexi_cfg = { diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 47509d294..06f9d54ac 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -86,26 +86,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_CC_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) -{ - const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - - switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; - } - - return -1; -} - - /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -252,21 +232,6 @@ void PIOS_Board_Init(void) } /* Configure main USART port */ - - /* Initialize inverter gpio and set it to off */ - { - GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }; - - GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - MAIN_USART_INVERTER_DISABLE); - } - uint8_t hwsettings_mainport; HwSettingsCC_MainPortGet(&hwsettings_mainport); diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index b25605749..370955e2a 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -116,16 +116,23 @@ extern uint32_t pios_i2c_flexi_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_SPI_MAX_DEVS 2 +#define PIOS_SPI_MAX_DEVS 2 extern uint32_t pios_spi_gyro_adapter_id; -#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_adapter_id) +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_adapter_id) extern uint32_t pios_spi_flash_accel_adapter_id; -#define PIOS_SPI_ADXL345_ADAPTER (pios_spi_flash_accel_adapter_id) +#define PIOS_SPI_ADXL345_ADAPTER (pios_spi_flash_accel_adapter_id) // ------------------------- // PIOS_USART // ------------------------- -#define PIOS_USART_MAX_DEVS 2 +#define PIOS_USART_MAX_DEVS 2 + +// Inverter for SBUS handling +#define PIOS_USART_INVERTER_PORT USART1 +#define PIOS_USART_INVERTER_GPIO GPIOB +#define PIOS_USART_INVERTER_PIN GPIO_Pin_2 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM diff --git a/flight/targets/boards/discoveryf4bare/board_hw_defs.c b/flight/targets/boards/discoveryf4bare/board_hw_defs.c index 15b2012a8..629038f58 100644 --- a/flight/targets/boards/discoveryf4bare/board_hw_defs.c +++ b/flight/targets/boards/discoveryf4bare/board_hw_defs.c @@ -606,13 +606,6 @@ static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = { /* * MAIN USART */ -// Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET - -static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, @@ -637,7 +630,6 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, }; /* diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 544f98c78..e7d8e9c27 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -99,25 +99,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) -{ - const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - - switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; - } - - return -1; -} - /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -256,23 +237,6 @@ void PIOS_Board_Init(void) #endif /* Configure main USART port */ - - /* Initialize inverter gpio and set it to off */ - { - GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }; - - GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - MAIN_USART_INVERTER_DISABLE); - } - uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index 9c3d1ba73..926887822 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -119,7 +119,13 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_USART_MAX_DEVS 5 +#define PIOS_USART_MAX_DEVS 5 +// Inverter for SBUS handling +#define PIOS_USART_INVERTER_PORT USART1 +#define PIOS_USART_INVERTER_GPIO GPIOC +#define PIOS_USART_INVERTER_PIN GPIO_Pin_0 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index e4ff0a76e..e99ec1543 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -794,14 +794,6 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { * MAIN USART */ -// Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_0 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET - -static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); - static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, @@ -825,7 +817,6 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, }; /* diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index e958bb7c8..63494acd0 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -98,25 +98,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) -{ - const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - - switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; - } - - return -1; -} - /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -255,23 +236,6 @@ void PIOS_Board_Init(void) #endif /* Configure main USART port */ - - /* Initialize inverter gpio and set it to off */ - { - GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }; - - GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - MAIN_USART_INVERTER_DISABLE); - } - uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 6c4f67aaa..c58bf75e0 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -139,7 +139,13 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_USART_MAX_DEVS 5 +#define PIOS_USART_MAX_DEVS 5 + +#define PIOS_USART_INVERTER_PORT USART1 +#define PIOS_USART_INVERTER_GPIO GPIOC +#define PIOS_USART_INVERTER_PIN GPIO_Pin_0 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 00540256a..6a4aee3d2 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -42,29 +42,24 @@ * o5 | PA0 | TIM5_CH1 | ADC1_0 * o6 | PA1 | TIM5_CH2 | ADC1_1 */ -#define MAIN_USART_REGS USART2 -#define MAIN_USART_REMAP GPIO_AF_USART2 -#define MAIN_USART_IRQ USART2_IRQn -#define MAIN_USART_RX_GPIO GPIOA -#define MAIN_USART_RX_PIN GPIO_Pin_3 -#define MAIN_USART_TX_GPIO GPIOA -#define MAIN_USART_TX_PIN GPIO_Pin_2 -// Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_15 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET +#define MAIN_USART_REGS USART2 +#define MAIN_USART_REMAP GPIO_AF_USART2 +#define MAIN_USART_IRQ USART2_IRQn +#define MAIN_USART_RX_GPIO GPIOA +#define MAIN_USART_RX_PIN GPIO_Pin_3 +#define MAIN_USART_TX_GPIO GPIOA +#define MAIN_USART_TX_PIN GPIO_Pin_2 -#define FLEXI_USART_REGS USART1 -#define FLEXI_USART_REMAP GPIO_AF_USART1 -#define FLEXI_USART_IRQ USART1_IRQn -#define FLEXI_USART_RX_GPIO GPIOB -#define FLEXI_USART_RX_PIN GPIO_Pin_7 -#define FLEXI_USART_TX_GPIO GPIOB -#define FLEXI_USART_TX_PIN GPIO_Pin_6 +#define FLEXI_USART_REGS USART1 +#define FLEXI_USART_REMAP GPIO_AF_USART1 +#define FLEXI_USART_IRQ USART1_IRQn +#define FLEXI_USART_RX_GPIO GPIOB +#define FLEXI_USART_RX_PIN GPIO_Pin_7 +#define FLEXI_USART_TX_GPIO GPIOB +#define FLEXI_USART_TX_PIN GPIO_Pin_6 // ReceiverPort pin 3 -#define FLEXI_USART_DTR_GPIO GPIOB -#define FLEXI_USART_DTR_PIN GPIO_Pin_10 +#define FLEXI_USART_DTR_GPIO GPIOB +#define FLEXI_USART_DTR_PIN GPIO_Pin_10 #if defined(PIOS_INCLUDE_LED) @@ -245,8 +240,6 @@ void PIOS_SPI_gyro_irq_handler(void) * MAIN USART */ -static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); - static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = MAIN_USART_REGS, .remap = MAIN_USART_REMAP, @@ -270,7 +263,6 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, }; /* diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 5f8fb7799..42cb939d8 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -95,25 +95,6 @@ static const PIOS_BOARD_IO_UART_Function main_function_map[] = { [HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) -{ - const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - - switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; - } - - return -1; -} - void PIOS_Board_Init(void) { const struct pios_board_info *bdinfo = &pios_board_info_blob; @@ -222,23 +203,6 @@ void PIOS_Board_Init(void) } /* Configure main USART port */ - - /* Initialize inverter gpio and set it to off */ - { - GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }; - - GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - MAIN_USART_INVERTER_DISABLE); - } - uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index f6bcc20f2..47940f961 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -135,7 +135,13 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_USART_MAX_DEVS 5 +#define PIOS_USART_MAX_DEVS 5 +// Inverter for SBUS handling +#define PIOS_USART_INVERTER_PORT USART2 +#define PIOS_USART_INVERTER_GPIO GPIOC +#define PIOS_USART_INVERTER_PIN GPIO_Pin_15 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c index fb47680fe..b35a52245 100644 --- a/flight/targets/boards/revoproto/board_hw_defs.c +++ b/flight/targets/boards/revoproto/board_hw_defs.c @@ -712,14 +712,6 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { * AUX USART SBUS ( UART/ S Bus label on rev2) */ -// Inverter for SBUS handling -#define MAIN_USART_INVERTER_GPIO GPIOC -#define MAIN_USART_INVERTER_PIN GPIO_Pin_3 -#define MAIN_USART_INVERTER_ENABLE Bit_SET -#define MAIN_USART_INVERTER_DISABLE Bit_RESET - -static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); - static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { .regs = UART4, .remap = GPIO_AF_UART4, @@ -743,7 +735,6 @@ static const struct pios_usart_cfg pios_usart_auxsbus_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, }; #endif /* PIOS_INCLUDE_COM_AUXSBUS */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 66242f413..bf957aeb1 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -56,25 +56,6 @@ uintptr_t pios_user_fs_id; #include -int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) -{ - const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - - switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_auxsbus_cfg.regs) { /* main port */ - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); - - return 0; - } - break; - } - - return -1; -} - void PIOS_Board_Init(void) { /* Delay system */ @@ -183,23 +164,6 @@ void PIOS_Board_Init(void) #if defined(PIOS_INCLUDE_USB) PIOS_BOARD_IO_Configure_USB(); #endif - - /* Initialize inverter gpio and set it to off */ - { - GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = MAIN_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }; - - GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); - GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, - MAIN_USART_INVERTER_PIN, - MAIN_USART_INVERTER_DISABLE); - } - /* Configure IO ports */ /* Configure Telemetry port */ diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h index 66d4c5cfe..71a562dbf 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -122,7 +122,13 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_USART_MAX_DEVS 5 +#define PIOS_USART_MAX_DEVS 5 +// Inverter for SBUS handling +#define PIOS_USART_INVERTER_PORT UART4 +#define PIOS_USART_INVERTER_GPIO GPIOC +#define PIOS_USART_INVERTER_PIN GPIO_Pin_3 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM diff --git a/flight/targets/boards/sparky2/board_hw_defs.c b/flight/targets/boards/sparky2/board_hw_defs.c index 18ed6962f..f7134734d 100644 --- a/flight/targets/boards/sparky2/board_hw_defs.c +++ b/flight/targets/boards/sparky2/board_hw_defs.c @@ -605,15 +605,6 @@ static const struct pios_usart_cfg pios_usart_flexi_cfg = { * RCVR PORT */ -// Inverter for SBUS handling -#define RCVR_USART_INVERTER_GPIO GPIOC -#define RCVR_USART_INVERTER_PIN GPIO_Pin_4 -#define RCVR_USART_INVERTER_ENABLE Bit_SET -#define RCVR_USART_INVERTER_DISABLE Bit_RESET - -static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); - - static const struct pios_usart_cfg pios_usart_rcvr_cfg = { .regs = USART6, .remap = GPIO_AF_USART6, @@ -627,7 +618,6 @@ static const struct pios_usart_cfg pios_usart_rcvr_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .ioctl = PIOS_BOARD_USART_Ioctl, }; #endif /* PIOS_INCLUDE_USART */ diff --git a/flight/targets/boards/sparky2/firmware/pios_board.c b/flight/targets/boards/sparky2/firmware/pios_board.c index d9d7fd9ee..4fc33f4ab 100644 --- a/flight/targets/boards/sparky2/firmware/pios_board.c +++ b/flight/targets/boards/sparky2/firmware/pios_board.c @@ -93,25 +93,6 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_SPK2_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) -{ - const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); - - switch (ctl) { - case PIOS_IOCTL_USART_SET_INVERTED: - if (usart_cfg->regs == pios_usart_rcvr_cfg.regs) { /* rcvr port */ - GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, - RCVR_USART_INVERTER_PIN, - (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? RCVR_USART_INVERTER_ENABLE : RCVR_USART_INVERTER_DISABLE); - - return 0; - } - break; - } - - return -1; -} - /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -265,22 +246,6 @@ void PIOS_Board_Init(void) PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #endif /* PIOS_INCLUDE_RFM22B */ - /* Initialize inverter gpio and set it to off */ - { - GPIO_InitTypeDef inverterGPIOInit = { - .GPIO_Pin = RCVR_USART_INVERTER_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }; - - GPIO_Init(RCVR_USART_INVERTER_GPIO, &inverterGPIOInit); - GPIO_WriteBit(RCVR_USART_INVERTER_GPIO, - RCVR_USART_INVERTER_PIN, - RCVR_USART_INVERTER_DISABLE); - } - // Configure the receiver port // Sparky2 receiver input on PC7 TIM8 CH2 // include PPM,S.Bus,DSM,SRXL,IBus,EX.Bus,HoTT SUMD,HoTT SUMH diff --git a/flight/targets/boards/sparky2/pios_board.h b/flight/targets/boards/sparky2/pios_board.h index d63e5cf3e..9213d1d6e 100644 --- a/flight/targets/boards/sparky2/pios_board.h +++ b/flight/targets/boards/sparky2/pios_board.h @@ -140,7 +140,14 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // // See also pios_board.c // ------------------------- -#define PIOS_USART_MAX_DEVS 5 +#define PIOS_USART_MAX_DEVS 5 + +// Inverter for SBUS handling +#define PIOS_USART_INVERTER_PORT USART6 +#define PIOS_USART_INVERTER_GPIO GPIOC +#define PIOS_USART_INVERTER_PIN GPIO_Pin_4 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM From 58e70c8849ec4ffb8916c387aabfd09a97aa1e94 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Tue, 25 Apr 2017 18:40:54 +0200 Subject: [PATCH 19/20] LP-480 reverted irelevant .xcodeproj file --- .../OpenPilotOSX.xcodeproj/project.pbxproj | 628 +++++++++--------- 1 file changed, 298 insertions(+), 330 deletions(-) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index f4baa0291..14b7c9ae2 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -1,330 +1,298 @@ -// !$*UTF8*$! -{ - archiveVersion = 1; - classes = { - }; - objectVersion = 46; - objects = { - -/* Begin PBXFileReference section */ - 4354B66314FED9FE004BA3B4 /* flight */ = {isa = PBXFileReference; lastKnownFileType = folder; name = flight; path = ../..; sourceTree = SOURCE_ROOT; }; - 65173C9F12EBFD1700D6A7CB /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../../Makefile; sourceTree = SOURCE_ROOT; }; - 65904F1814632C1700FD9482 /* firmware-defs.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "firmware-defs.mk"; sourceTree = ""; }; - 65904F2214632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2314632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2414632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2514632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2614632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2714632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65904F2814632C1700FD9482 /* version-info.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = "version-info.py"; sourceTree = ""; }; - 65904F2914632C1700FD9482 /* firmware_info.c.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmware_info.c.template; sourceTree = ""; }; - 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcs_version_info.h.template; sourceTree = ""; }; - 65904F2D14632C1700FD9482 /* README.txt */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = README.txt; sourceTree = ""; }; - 65904F2E14632C1700FD9482 /* shell_script.reg */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = shell_script.reg; sourceTree = ""; }; - 65904F2F14632C1700FD9482 /* install */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = install; sourceTree = ""; }; - 65904F3014632C1700FD9482 /* make */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make; sourceTree = ""; }; - 65904F3114632C1700FD9482 /* make.sh */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make.sh; sourceTree = ""; }; - 65904F3214632C1700FD9482 /* sh.cmd */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = sh.cmd; sourceTree = ""; }; - 65904F34146362F300FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; - 65E466BC14E244020075459C /* uavobjectdefinition */ = {isa = PBXFileReference; lastKnownFileType = folder; path = uavobjectdefinition; sourceTree = ""; }; -/* End PBXFileReference section */ - -/* Begin PBXGroup section */ - 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */ = { - isa = PBXGroup; - children = ( - 4354B66314FED9FE004BA3B4 /* flight */, - 65904F1614632C1700FD9482 /* make */, - 65C35E4E12EFB2F3004811C2 /* shared */, - 65173C9F12EBFD1700D6A7CB /* Makefile */, - ); - name = OpenPilotOSX; - sourceTree = ""; - }; - 65904F1614632C1700FD9482 /* make */ = { - isa = PBXGroup; - children = ( - 65904F1714632C1700FD9482 /* boards */, - 65904F1814632C1700FD9482 /* firmware-defs.mk */, - 65904F1914632C1700FD9482 /* scripts */, - 65904F1A14632C1700FD9482 /* templates */, - 65904F1B14632C1700FD9482 /* winx86 */, - ); - name = make; - path = OpenPilotOSX.xcodeproj/../../../../make; - sourceTree = ""; - }; - 65904F1714632C1700FD9482 /* boards */ = { - isa = PBXGroup; - children = ( - 65904F33146362F300FD9482 /* revolution */, - 65904F1C14632C1700FD9482 /* ahrs */, - 65904F1D14632C1700FD9482 /* coptercontrol */, - 65904F1E14632C1700FD9482 /* esc */, - 65904F1F14632C1700FD9482 /* ins */, - 65904F2014632C1700FD9482 /* openpilot */, - 65904F2114632C1700FD9482 /* oplink */, - ); - path = boards; - sourceTree = ""; - }; - 65904F1914632C1700FD9482 /* scripts */ = { - isa = PBXGroup; - children = ( - 65904F2814632C1700FD9482 /* version-info.py */, - ); - path = scripts; - sourceTree = ""; - }; - 65904F1A14632C1700FD9482 /* templates */ = { - isa = PBXGroup; - children = ( - 65904F2914632C1700FD9482 /* firmware_info.c.template */, - 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */, - ); - path = templates; - sourceTree = ""; - }; - 65904F1B14632C1700FD9482 /* winx86 */ = { - isa = PBXGroup; - children = ( - 65904F2B14632C1700FD9482 /* bin */, - 65904F2C14632C1700FD9482 /* cmd */, - 65904F2D14632C1700FD9482 /* README.txt */, - 65904F2E14632C1700FD9482 /* shell_script.reg */, - ); - path = winx86; - sourceTree = ""; - }; - 65904F1C14632C1700FD9482 /* ahrs */ = { - isa = PBXGroup; - children = ( - 65904F2214632C1700FD9482 /* board-info.mk */, - ); - path = ahrs; - sourceTree = ""; - }; - 65904F1D14632C1700FD9482 /* coptercontrol */ = { - isa = PBXGroup; - children = ( - 65904F2314632C1700FD9482 /* board-info.mk */, - ); - path = coptercontrol; - sourceTree = ""; - }; - 65904F1E14632C1700FD9482 /* esc */ = { - isa = PBXGroup; - children = ( - 65904F2414632C1700FD9482 /* board-info.mk */, - ); - path = esc; - sourceTree = ""; - }; - 65904F1F14632C1700FD9482 /* ins */ = { - isa = PBXGroup; - children = ( - 65904F2514632C1700FD9482 /* board-info.mk */, - ); - path = ins; - sourceTree = ""; - }; - 65904F2014632C1700FD9482 /* openpilot */ = { - isa = PBXGroup; - children = ( - 65904F2614632C1700FD9482 /* board-info.mk */, - ); - path = openpilot; - sourceTree = ""; - }; - 65904F2114632C1700FD9482 /* oplink */ = { - isa = PBXGroup; - children = ( - 65904F2714632C1700FD9482 /* board-info.mk */, - ); - path = oplink; - sourceTree = ""; - }; - 65904F2B14632C1700FD9482 /* bin */ = { - isa = PBXGroup; - children = ( - 65904F2F14632C1700FD9482 /* install */, - 65904F3014632C1700FD9482 /* make */, - ); - path = bin; - sourceTree = ""; - }; - 65904F2C14632C1700FD9482 /* cmd */ = { - isa = PBXGroup; - children = ( - 65904F3114632C1700FD9482 /* make.sh */, - 65904F3214632C1700FD9482 /* sh.cmd */, - ); - path = cmd; - sourceTree = ""; - }; - 65904F33146362F300FD9482 /* revolution */ = { - isa = PBXGroup; - children = ( - 65904F34146362F300FD9482 /* board-info.mk */, - ); - path = revolution; - sourceTree = ""; - }; - 65C35E4E12EFB2F3004811C2 /* shared */ = { - isa = PBXGroup; - children = ( - 65E466BC14E244020075459C /* uavobjectdefinition */, - ); - name = shared; - path = ../../../shared; - sourceTree = SOURCE_ROOT; - }; -/* End PBXGroup section */ - -/* Begin PBXLegacyTarget section */ - 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { - isa = PBXLegacyTarget; - buildArgumentsString = ef_oplinkmini; - buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; - buildPhases = ( - ); - buildToolPath = /usr/bin/make; - buildWorkingDirectory = ../../..; - dependencies = ( - ); - name = OpenPilotOSX; - passBuildSettingsInEnvironment = 1; - productName = OpenPilotOSX; - }; -/* End PBXLegacyTarget section */ - -/* Begin PBXProject section */ - 08FB7793FE84155DC02AAC07 /* Project object */ = { - isa = PBXProject; - attributes = { - LastUpgradeCheck = 0820; - ORGANIZATIONNAME = OpenPilot; - }; - buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */; - compatibilityVersion = "Xcode 3.2"; - developmentRegion = English; - hasScannedForEncodings = 1; - knownRegions = ( - English, - Japanese, - French, - German, - ); - mainGroup = 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */; - projectDirPath = ""; - projectRoot = ../../..; - targets = ( - 6581071511DE809D0049FB12 /* OpenPilotOSX */, - ); - }; -/* End PBXProject section */ - -/* Begin XCBuildConfiguration section */ - 1DEB928A08733DD80010E9CD /* Debug */ = { - isa = XCBuildConfiguration; - buildSettings = { - CLANG_ANALYZER_LOCALIZABILITY_NONLOCALIZED = YES; - CLANG_WARN_BOOL_CONVERSION = YES; - CLANG_WARN_CONSTANT_CONVERSION = YES; - CLANG_WARN_EMPTY_BODY = YES; - CLANG_WARN_ENUM_CONVERSION = YES; - CLANG_WARN_INFINITE_RECURSION = YES; - CLANG_WARN_INT_CONVERSION = YES; - CLANG_WARN_SUSPICIOUS_MOVE = YES; - CLANG_WARN_UNREACHABLE_CODE = YES; - CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; - ENABLE_STRICT_OBJC_MSGSEND = YES; - ENABLE_TESTABILITY = YES; - GCC_C_LANGUAGE_STANDARD = gnu99; - GCC_NO_COMMON_BLOCKS = YES; - GCC_OPTIMIZATION_LEVEL = 0; - GCC_WARN_64_TO_32_BIT_CONVERSION = YES; - GCC_WARN_ABOUT_RETURN_TYPE = YES; - GCC_WARN_UNDECLARED_SELECTOR = YES; - GCC_WARN_UNINITIALIZED_AUTOS = YES; - GCC_WARN_UNUSED_FUNCTION = YES; - GCC_WARN_UNUSED_VARIABLE = YES; - ONLY_ACTIVE_ARCH = YES; - PREBINDING = NO; - SDKROOT = macosx; - }; - name = Debug; - }; - 1DEB928B08733DD80010E9CD /* Release */ = { - isa = XCBuildConfiguration; - buildSettings = { - CLANG_ANALYZER_LOCALIZABILITY_NONLOCALIZED = YES; - CLANG_WARN_BOOL_CONVERSION = YES; - CLANG_WARN_CONSTANT_CONVERSION = YES; - CLANG_WARN_EMPTY_BODY = YES; - CLANG_WARN_ENUM_CONVERSION = YES; - CLANG_WARN_INFINITE_RECURSION = YES; - CLANG_WARN_INT_CONVERSION = YES; - CLANG_WARN_SUSPICIOUS_MOVE = YES; - CLANG_WARN_UNREACHABLE_CODE = YES; - CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; - ENABLE_STRICT_OBJC_MSGSEND = YES; - GCC_C_LANGUAGE_STANDARD = gnu99; - GCC_NO_COMMON_BLOCKS = YES; - GCC_WARN_64_TO_32_BIT_CONVERSION = YES; - GCC_WARN_ABOUT_RETURN_TYPE = YES; - GCC_WARN_UNDECLARED_SELECTOR = YES; - GCC_WARN_UNINITIALIZED_AUTOS = YES; - GCC_WARN_UNUSED_FUNCTION = YES; - GCC_WARN_UNUSED_VARIABLE = YES; - PREBINDING = NO; - SDKROOT = macosx; - }; - name = Release; - }; - 6581071611DE809D0049FB12 /* Debug */ = { - isa = XCBuildConfiguration; - buildSettings = { - COPY_PHASE_STRIP = NO; - GCC_DYNAMIC_NO_PIC = NO; - GCC_OPTIMIZATION_LEVEL = 0; - PRODUCT_NAME = OpenPilotOSX; - }; - name = Debug; - }; - 6581071711DE809D0049FB12 /* Release */ = { - isa = XCBuildConfiguration; - buildSettings = { - COPY_PHASE_STRIP = YES; - DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; - GCC_ENABLE_FIX_AND_CONTINUE = NO; - PRODUCT_NAME = OpenPilotOSX; - ZERO_LINK = NO; - }; - name = Release; - }; -/* End XCBuildConfiguration section */ - -/* Begin XCConfigurationList section */ - 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */ = { - isa = XCConfigurationList; - buildConfigurations = ( - 1DEB928A08733DD80010E9CD /* Debug */, - 1DEB928B08733DD80010E9CD /* Release */, - ); - defaultConfigurationIsVisible = 0; - defaultConfigurationName = Release; - }; - 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */ = { - isa = XCConfigurationList; - buildConfigurations = ( - 6581071611DE809D0049FB12 /* Debug */, - 6581071711DE809D0049FB12 /* Release */, - ); - defaultConfigurationIsVisible = 0; - defaultConfigurationName = Release; - }; -/* End XCConfigurationList section */ - }; - rootObject = 08FB7793FE84155DC02AAC07 /* Project object */; -} +// !$*UTF8*$! +{ + archiveVersion = 1; + classes = { + }; + objectVersion = 45; + objects = { + +/* Begin PBXFileReference section */ + 4354B66314FED9FE004BA3B4 /* flight */ = {isa = PBXFileReference; lastKnownFileType = folder; name = flight; path = ../..; sourceTree = SOURCE_ROOT; }; + 65173C9F12EBFD1700D6A7CB /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../../Makefile; sourceTree = SOURCE_ROOT; }; + 65904F1814632C1700FD9482 /* firmware-defs.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "firmware-defs.mk"; sourceTree = ""; }; + 65904F2214632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2314632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2414632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2514632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2614632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2714632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65904F2814632C1700FD9482 /* version-info.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = "version-info.py"; sourceTree = ""; }; + 65904F2914632C1700FD9482 /* firmware_info.c.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmware_info.c.template; sourceTree = ""; }; + 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcs_version_info.h.template; sourceTree = ""; }; + 65904F2D14632C1700FD9482 /* README.txt */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = README.txt; sourceTree = ""; }; + 65904F2E14632C1700FD9482 /* shell_script.reg */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = shell_script.reg; sourceTree = ""; }; + 65904F2F14632C1700FD9482 /* install */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = install; sourceTree = ""; }; + 65904F3014632C1700FD9482 /* make */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make; sourceTree = ""; }; + 65904F3114632C1700FD9482 /* make.sh */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = make.sh; sourceTree = ""; }; + 65904F3214632C1700FD9482 /* sh.cmd */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = sh.cmd; sourceTree = ""; }; + 65904F34146362F300FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = ""; }; + 65E466BC14E244020075459C /* uavobjectdefinition */ = {isa = PBXFileReference; lastKnownFileType = folder; path = uavobjectdefinition; sourceTree = ""; }; +/* End PBXFileReference section */ + +/* Begin PBXGroup section */ + 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */ = { + isa = PBXGroup; + children = ( + 4354B66314FED9FE004BA3B4 /* flight */, + 65904F1614632C1700FD9482 /* make */, + 65C35E4E12EFB2F3004811C2 /* shared */, + 65173C9F12EBFD1700D6A7CB /* Makefile */, + ); + name = OpenPilotOSX; + sourceTree = ""; + }; + 65904F1614632C1700FD9482 /* make */ = { + isa = PBXGroup; + children = ( + 65904F1714632C1700FD9482 /* boards */, + 65904F1814632C1700FD9482 /* firmware-defs.mk */, + 65904F1914632C1700FD9482 /* scripts */, + 65904F1A14632C1700FD9482 /* templates */, + 65904F1B14632C1700FD9482 /* winx86 */, + ); + name = make; + path = OpenPilotOSX.xcodeproj/../../../../make; + sourceTree = ""; + }; + 65904F1714632C1700FD9482 /* boards */ = { + isa = PBXGroup; + children = ( + 65904F33146362F300FD9482 /* revolution */, + 65904F1C14632C1700FD9482 /* ahrs */, + 65904F1D14632C1700FD9482 /* coptercontrol */, + 65904F1E14632C1700FD9482 /* esc */, + 65904F1F14632C1700FD9482 /* ins */, + 65904F2014632C1700FD9482 /* openpilot */, + 65904F2114632C1700FD9482 /* oplink */, + ); + path = boards; + sourceTree = ""; + }; + 65904F1914632C1700FD9482 /* scripts */ = { + isa = PBXGroup; + children = ( + 65904F2814632C1700FD9482 /* version-info.py */, + ); + path = scripts; + sourceTree = ""; + }; + 65904F1A14632C1700FD9482 /* templates */ = { + isa = PBXGroup; + children = ( + 65904F2914632C1700FD9482 /* firmware_info.c.template */, + 65904F2A14632C1700FD9482 /* gcs_version_info.h.template */, + ); + path = templates; + sourceTree = ""; + }; + 65904F1B14632C1700FD9482 /* winx86 */ = { + isa = PBXGroup; + children = ( + 65904F2B14632C1700FD9482 /* bin */, + 65904F2C14632C1700FD9482 /* cmd */, + 65904F2D14632C1700FD9482 /* README.txt */, + 65904F2E14632C1700FD9482 /* shell_script.reg */, + ); + path = winx86; + sourceTree = ""; + }; + 65904F1C14632C1700FD9482 /* ahrs */ = { + isa = PBXGroup; + children = ( + 65904F2214632C1700FD9482 /* board-info.mk */, + ); + path = ahrs; + sourceTree = ""; + }; + 65904F1D14632C1700FD9482 /* coptercontrol */ = { + isa = PBXGroup; + children = ( + 65904F2314632C1700FD9482 /* board-info.mk */, + ); + path = coptercontrol; + sourceTree = ""; + }; + 65904F1E14632C1700FD9482 /* esc */ = { + isa = PBXGroup; + children = ( + 65904F2414632C1700FD9482 /* board-info.mk */, + ); + path = esc; + sourceTree = ""; + }; + 65904F1F14632C1700FD9482 /* ins */ = { + isa = PBXGroup; + children = ( + 65904F2514632C1700FD9482 /* board-info.mk */, + ); + path = ins; + sourceTree = ""; + }; + 65904F2014632C1700FD9482 /* openpilot */ = { + isa = PBXGroup; + children = ( + 65904F2614632C1700FD9482 /* board-info.mk */, + ); + path = openpilot; + sourceTree = ""; + }; + 65904F2114632C1700FD9482 /* oplink */ = { + isa = PBXGroup; + children = ( + 65904F2714632C1700FD9482 /* board-info.mk */, + ); + path = oplink; + sourceTree = ""; + }; + 65904F2B14632C1700FD9482 /* bin */ = { + isa = PBXGroup; + children = ( + 65904F2F14632C1700FD9482 /* install */, + 65904F3014632C1700FD9482 /* make */, + ); + path = bin; + sourceTree = ""; + }; + 65904F2C14632C1700FD9482 /* cmd */ = { + isa = PBXGroup; + children = ( + 65904F3114632C1700FD9482 /* make.sh */, + 65904F3214632C1700FD9482 /* sh.cmd */, + ); + path = cmd; + sourceTree = ""; + }; + 65904F33146362F300FD9482 /* revolution */ = { + isa = PBXGroup; + children = ( + 65904F34146362F300FD9482 /* board-info.mk */, + ); + path = revolution; + sourceTree = ""; + }; + 65C35E4E12EFB2F3004811C2 /* shared */ = { + isa = PBXGroup; + children = ( + 65E466BC14E244020075459C /* uavobjectdefinition */, + ); + name = shared; + path = ../../../shared; + sourceTree = SOURCE_ROOT; + }; +/* End PBXGroup section */ + +/* Begin PBXLegacyTarget section */ + 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { + isa = PBXLegacyTarget; + buildArgumentsString = "$(ACTION) -f Makefile.posix"; + buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; + buildPhases = ( + ); + buildToolPath = /usr/bin/make; + buildWorkingDirectory = ../../OpenPilot; + dependencies = ( + ); + name = OpenPilotOSX; + passBuildSettingsInEnvironment = 1; + productName = OpenPilotOSX; + }; +/* End PBXLegacyTarget section */ + +/* Begin PBXProject section */ + 08FB7793FE84155DC02AAC07 /* Project object */ = { + isa = PBXProject; + attributes = { + ORGANIZATIONNAME = OpenPilot; + }; + buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */; + compatibilityVersion = "Xcode 3.1"; + developmentRegion = English; + hasScannedForEncodings = 1; + knownRegions = ( + English, + Japanese, + French, + German, + ); + mainGroup = 08FB7794FE84155DC02AAC07 /* OpenPilotOSX */; + projectDirPath = ""; + projectRoot = ../../..; + targets = ( + 6581071511DE809D0049FB12 /* OpenPilotOSX */, + ); + }; +/* End PBXProject section */ + +/* Begin XCBuildConfiguration section */ + 1DEB928A08733DD80010E9CD /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; + GCC_C_LANGUAGE_STANDARD = gnu99; + GCC_OPTIMIZATION_LEVEL = 0; + GCC_WARN_ABOUT_RETURN_TYPE = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + ONLY_ACTIVE_ARCH = YES; + PREBINDING = NO; + SDKROOT = macosx10.6; + }; + name = Debug; + }; + 1DEB928B08733DD80010E9CD /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; + GCC_C_LANGUAGE_STANDARD = gnu99; + GCC_WARN_ABOUT_RETURN_TYPE = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + PREBINDING = NO; + SDKROOT = macosx10.6; + }; + name = Release; + }; + 6581071611DE809D0049FB12 /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + COPY_PHASE_STRIP = NO; + GCC_DYNAMIC_NO_PIC = NO; + GCC_OPTIMIZATION_LEVEL = 0; + PRODUCT_NAME = OpenPilotOSX; + }; + name = Debug; + }; + 6581071711DE809D0049FB12 /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + COPY_PHASE_STRIP = YES; + DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; + GCC_ENABLE_FIX_AND_CONTINUE = NO; + PRODUCT_NAME = OpenPilotOSX; + ZERO_LINK = NO; + }; + name = Release; + }; +/* End XCBuildConfiguration section */ + +/* Begin XCConfigurationList section */ + 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "OpenPilotOSX" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 1DEB928A08733DD80010E9CD /* Debug */, + 1DEB928B08733DD80010E9CD /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 6581071611DE809D0049FB12 /* Debug */, + 6581071711DE809D0049FB12 /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; +/* End XCConfigurationList section */ + }; + rootObject = 08FB7793FE84155DC02AAC07 /* Project object */; +} From 792746f7758597142fb5e1f08de54206d68b50f4 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Thu, 27 Apr 2017 15:44:39 +0200 Subject: [PATCH 20/20] LP-480 DSM: do not fail to initialize driver for ports without bind capability. --- flight/pios/common/pios_dsm.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/flight/pios/common/pios_dsm.c b/flight/pios/common/pios_dsm.c index 9f66a1aff..a935e22c3 100644 --- a/flight/pios/common/pios_dsm.c +++ b/flight/pios/common/pios_dsm.c @@ -318,11 +318,9 @@ int32_t PIOS_DSM_Init(uint32_t *dsm_id, PIOS_DEBUG_Assert(driver->ioctl); - if ((driver->ioctl)(lower_id, PIOS_IOCTL_USART_GET_DSMBIND, &rxpin) < 0) { - return -1; + if ((driver->ioctl)(lower_id, PIOS_IOCTL_USART_GET_DSMBIND, &rxpin) == 0) { + PIOS_DSM_Bind(&rxpin, bind); } - - PIOS_DSM_Bind(&rxpin, bind); } PIOS_DSM_ResetState(dsm_dev);