diff --git a/.drone.sec b/.drone.sec index 9556323417..ef4fc06912 100644 --- a/.drone.sec +++ b/.drone.sec @@ -1 +1 @@ -eyJhbGciOiJSU0EtT0FFUCIsImVuYyI6IkExMjhHQ00ifQ.UPh8KqupamxDSZ-xztWqWKqG7ur8WGg4dW22PRVGchjWsMa_zyuTEEDkl67z_jdHKkxEerWE8J4sROWVixi_xWvEl7OgXlt7H9c4iWtGTyp7n9ibeS1wgDoYes3LbRu9lRTxiTb-8Y-MgY0zeAHTBIQHmECETKF_CE4QkPHjSQCfbb_chn4vIqryM11-QctxDwysR4VC5AO1uA4hB0RReixOuSQFI-LbuLvBozEgsrYe5dFo7-oQJt7-e2-UFoEhomTPZcWc02NznX-GjlaZxpxQQXra5NyN5--aiL2C8kcPYfnEfU4dYudNsm2tDlnxhBPA1Ji3FnDSBp_ZunsvRQ.GGds6H6tZW_Qzh2T.KjsjOsBbAmfhUIWYXJBMcrChr1ckykuPZ8XERPGWuSoMbXkklHGj0MCehIWNDuLX_wPM1wTvZE8unFJIYwYGybYQ8omAHDGOkEM3YDs6VnJ9Y8zBl6wnvTvtsQsrQWqU8vdodzSP62F81wCDCCauOFewz_gjzl6n_1Ajxj9CqTfCR17Gcuui6nrqkn0A5qikWuZz8qsUaQ94t0qVRWOFcZ4NQMH8NJwZCYW0JAAsvoQOidc2dB5zkwx3pzc2hItmzwZq3uVgWr5RFgp2LzBd.45hdSTLtI4gU9-GjkOlZ1A \ No newline at end of file +eyJhbGciOiJSU0EtT0FFUCIsImVuYyI6IkExMjhHQ00ifQ.j2lEUip1Qg6w8L4HloU9yu0tY21X_oyRADoD_-YPmMJG_DIyRTptA48aI0nrg3ijAv2cyp-Ahbyo7dYCvaWFkZoHXTBuw9UNj16YbzbuykDB-InZTxlHD31pqcVhPEUbEUHLNxzJA3BrzkeVQpP1OBQcblMCM7fU10AVwPZZ7irOGfk48jX-56RfQdOkvO20sDm_o-1zOJtH9eIaIdlaBsoQ_P6EWnXhUbQnqzsHCdYhXMEtDamNSr1yT0qR8B6HzbRe4lvs-R1ZRyzVkjbBYUoaNiJIWVZe3vOVuvPslG0h5UsFrq20-jEhz8SIlhIWGqEhnG7urc-E6VhpFI-ZLg.VbIvTPJOh-nd0NNI.yGPiDToLsaoWe7gIpWZ2n0Io2UNxmNfvdnuHiizP8tEFNBEIujjpWdyuR-JaxQmuUwIOSZ7DxOr9Erh9VPRKOdvxlfcQjXj02WFCchrMFpB2oaBWvOJ4G8YtJruhHKC8v63rubI987YlPBbKRC0JFvF7v5uO0VGyDhHoA22EJEz3mQUx7eqkEUSl5llJ9PWxNLCK-YzmaiHhjU-w9WcWp5e9zUG_sGjIT91EWRO7iMwpd-zCE5Z3_PQb2sEc5G_JrbIXdSxX3TOfHnPEn9V0.zZ0-_CUitMCNxyTU4TwqyA \ No newline at end of file diff --git a/.drone.yml b/.drone.yml index 3cc9d4365f..608aaeef7d 100644 --- a/.drone.yml +++ b/.drone.yml @@ -1,3 +1,6 @@ +# If you change this file, you have to regenerate the .drone.sec file! +# Please refer to LP-394 for details, or ask on slack. + build: image: teaci/msys$$arch pull: true @@ -6,11 +9,13 @@ build: - if [ $$arch = 32 ]; then target=i686; fi - if [ $$arch = 64 ]; then target=x86_64; fi - echo -e "[librepilot-mingw]\nSigLevel = Optional TrustAll\nServer = http://download.librepilot.org/repo/mingw" >> /etc/pacman.conf - - pacman -Syu --noconfirm --noprogressbar --needed git unzip tar mingw-w64-${target}-toolchain mingw-w64-${target}-ccache mingw-w64-${target}-qt5 mingw-w64-${target}-SDL mingw-w64-${target}-mesa mingw-w64-${target}-openssl mingw-w64-${target}-gdal-minimal mingw-w64-${target}-OpenSceneGraph mingw-w64-${target}-osgearth + - pacman -Syu --noconfirm --noprogressbar --needed git unzip tar mingw-w64-${target}-toolchain mingw-w64-${target}-ccache mingw-w64-${target}-ntldd mingw-w64-${target}-qt5 mingw-w64-${target}-SDL mingw-w64-${target}-mesa mingw-w64-${target}-openssl + - pacman -Syu --noconfirm --noprogressbar --needed mingw-w64-${target}-gdal-minimal mingw-w64-${target}-OpenSceneGraph mingw-w64-${target}-osgearth + - pacman -Syu --noconfirm --noprogressbar --needed mingw-w64-${target}-gstreamer mingw-w64-${target}-gst-plugins-base mingw-w64-${target}-gst-plugins-good mingw-w64-${target}-gst-plugins-bad mingw-w64-${target}-gst-plugins-ugly mingw-w64-${target}-gst-libav - mingw32-make all_sdk_install - git config core.filemode false - mingw32-make build-info && cat build/build-info.txt - - mingw32-make opfw_resource + - mingw32-make fw_resource - mingw32-make gcs - mingw32-make package - mv `ls build/LibrePilot-*.exe` build/$$BRANCH-$$COMMIT-$$arch-package.exe @@ -34,7 +39,7 @@ publish: type: executable repository: LibrePilot package: next - version: Windows + version: Windows20172 publish: true override: true target: $$BRANCH-$${COMMIT:0:6}-$$arch-tea-ci-package.exe @@ -45,4 +50,4 @@ notify: - https://hooks.slack.com/services/$$SLACK_HOOK content_type: application/json template: > - {"text": "Tea-CI Build #{{ build.number }} finished with a {{ build.status }} status. If successful, the packages for {{ build.branch }} $${COMMIT:0:8} can be downloaded "} + {"text": "Tea-CI Build #{{ build.number }} finished with a {{ build.status }} status. If successful, the packages for {{ build.branch }} $${COMMIT:0:8} can be downloaded "} diff --git a/.gitignore b/.gitignore index 514d542388..ad5ae6346f 100644 --- a/.gitignore +++ b/.gitignore @@ -15,6 +15,7 @@ GRTAGS GSYMS GTAGS core +!core/ *~ # flight @@ -40,6 +41,7 @@ ground/gcs/.settings *.exe *.log *.opl +*.autosave /ground/gcs/share/translations/extract-mimetypes.xq /ground/gcs/src/experimental/tools/DocumentationHelper/ui_mainwindow.h /ground/gcs/src/libs/qextserialport/.hg @@ -64,15 +66,22 @@ ground/gcs/.settings /.metadata /.settings /.pydevproject + +# python artifacts +/**/*.py* + /workspace - -# Ignore Eclipse temp folder, git plugin based? -RemoteSystemsTempFiles - -# Ignore patch-related files -*.rej -*.orig -*.diff~ - -# ignore ccache storage + +#pycharm project files +/python/.idea + +# Ignore Eclipse temp folder, git plugin based? +RemoteSystemsTempFiles + +# Ignore patch-related files +*.rej +*.orig +*.diff~ + +# ignore ccache storage .ccache diff --git a/.gitmodules b/.gitmodules index 2ffdf87f5f..e69de29bb2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "overo"] - path = overo - url = gitolite@git.openpilot.org:op_overo.git diff --git a/.travis.yml b/.travis.yml index 06c8fe0e86..9e34fc8fed 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,21 +2,50 @@ language: cpp sudo: required -dist: trusty +matrix: + include: + # Linux builds + - os: linux + compiler: gcc + dist: trusty + before_install: + - sudo add-apt-repository ppa:librepilot/tools -y + - sudo apt-get update -q + - sudo apt-get install -y libc6-i386 libudev-dev libusb-1.0-0-dev libsdl1.2-dev python libopenscenegraph-dev libosgearth-dev qt56-meta-minimal qt56svg qt56script qt56serialport qt56multimedia qt56translations qt56tools + - make build_sdk_install -before_install: - - sudo add-apt-repository ppa:librepilot/tools -y - - sudo apt-get update -q - - sudo apt-get install -y libc6-i386 libudev-dev libusb-1.0-0-dev libsdl1.2-dev python libopenscenegraph-dev libosgearth-dev qt56-meta-minimal qt56svg qt56script qt56serialport qt56multimedia qt56translations qt56tools - - make build_sdk_install + # OSX build + - os: osx + compiler: clang + before_install: + - brew update + - brew install curl + - brew install p7zip + - brew install gnu-tar + # - brew install cmake + - brew outdated gdal || brew upgrade gdal + - brew outdated geos || brew upgrade geos + - brew install ccache + - export PATH="/usr/local/opt/ccache/libexec:$PATH" + - ccache -V + - brew install qt5 + - brew link --force qt5 + - make arm_sdk_install + - make osg_install + - make osgearth_install + - wget https://www.libsdl.org/release/SDL-1.2.15.dmg + - hdiutil attach SDL-1.2.15.dmg + - sudo cp -r /Volumes/SDL/SDL.framework /Library/Frameworks/ + - hdiutil detach /Volumes/SDL script: - - make config_new CCACHE=ccache - - make all_flight - - make opfw_resource - - make gcs - + - make config_new CCACHE=ccache + - make -j2 all_flight + - make -j2 fw_resource + - make -j2 gcs + git: depth: 500 cache: ccache + diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 6f3f11e01d..b4fbef76fb 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -41,7 +41,7 @@ Add the following lines at the end of your /etc/pacman.conf file: Start a MinGW-w64 Win64 Shell or a MinGW-w64 Win32 Shell. pacman -Sy - pacman -S --needed git unzip tar mingw-w64-i686-toolchain mingw-w64-i686-ccache mingw-w64-i686-qt5 mingw-w64-i686-SDL mingw-w64-i686-mesa mingw-w64-i686-openssl mingw-w64-i686-gdal-minimal mingw-w64-i686-OpenSceneGraph mingw-w64-i686-osgearth + pacman -S --needed git unzip tar mingw-w64-i686-toolchain mingw-w64-i686-ccache mingw-w64-i686-ntldd mingw-w64-i686-qt5 mingw-w64-i686-SDL mingw-w64-i686-mesa mingw-w64-i686-openssl mingw-w64-i686-gdal-minimal mingw-w64-i686-OpenSceneGraph mingw-w64-i686-osgearth Optionally install debug packages: @@ -52,7 +52,7 @@ Optionally install debug packages: Start a MinGW-w64 Win64 Shell. pacman -Sy - pacman -S --needed git unzip tar mingw-w64-x86_64-toolchain mingw-w64-x86_64-ccache mingw-w64-x86_64-qt5 mingw-w64-x86_64-SDL mingw-w64-x86_64-mesa mingw-w64-x86_64-openssl mingw-w64-x86_64-gdal-minimal mingw-w64-x86_64-OpenSceneGraph mingw-w64-x86_64-osgearth + pacman -S --needed git unzip tar mingw-w64-x86_64-toolchain mingw-w64-x86_64-ccache mingw-w64-x86_64-ntldd mingw-w64-x86_64-qt5 mingw-w64-x86_64-SDL mingw-w64-x86_64-mesa mingw-w64-x86_64-openssl mingw-w64-x86_64-gdal-minimal mingw-w64-x86_64-OpenSceneGraph mingw-w64-x86_64-osgearth Optionally install debug packages: diff --git a/CREDITS.txt b/CREDITS.txt index 45d4a9c37f..08ffa071b6 100644 --- a/CREDITS.txt +++ b/CREDITS.txt @@ -1,4 +1,5 @@ Connor Abbott +Karl Alberts Sergiy Anikeyev David Ankers Fredrik Arvidsson @@ -34,7 +35,7 @@ Andrew Finegan Kevin Finisterre Richard Flay Darren Furniss -Oliver Gaste +Olivier Gasté Cliff Geerdes Frederic Goddeeris Daniel Godin @@ -69,9 +70,11 @@ Fredrik Larsson Xavier Lecluse Richard Von Lehe Pablo Lema +Julian Lilov Matt Lipski David Llama Jasper Van Loenen +Fernando Lopez Jr. Ákos Máté Ben Matthews Greg Matthews @@ -80,9 +83,12 @@ Alessio Morale Gary Mortimer Cathy Moss Les Newell +Karsten Telling Nielsen +Jan Nijs Ken Northup Craig Nuttall Bertrand Oresve +Pashalis Padeleris Angus Peart Pablo Francisco Pérez Hidalgo John Pike @@ -130,4 +136,3 @@ Kendal Wells David Willis Dmitriy Zaitsev Vladimir Zidar - diff --git a/Makefile b/Makefile index 4fc9f03698..642931aa4d 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ # # Top level Makefile for the LibrePilot Project build system. -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2017, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2010-2013, The OpenPilot Team, http://www.openpilot.org # Use 'make help' for instructions. # @@ -50,9 +50,9 @@ export TOOLS_DIR export BUILD_DIR := $(CURDIR)/build export PACKAGE_DIR := $(BUILD_DIR)/package export DIST_DIR := $(BUILD_DIR)/dist -export OPGCSSYNTHDIR := $(BUILD_DIR)/gcs-synthetics +export GCS_SYNTH_DIR := $(BUILD_DIR)/gcs-synthetics -DIRS := $(DL_DIR) $(TOOLS_DIR) $(BUILD_DIR) $(PACKAGE_DIR) $(DIST_DIR) $(OPGCSSYNTHDIR) +DIRS := $(DL_DIR) $(TOOLS_DIR) $(BUILD_DIR) $(PACKAGE_DIR) $(DIST_DIR) $(GCS_SYNTH_DIR) # Naming for binaries and packaging etc,. export ORG_BIG_NAME := LibrePilot @@ -131,16 +131,22 @@ ifeq ($(UNAME), Linux) GCS_WITH_OSG := 1 GCS_WITH_OSGEARTH := 1 GCS_COPY_OSG := 0 + GCS_WITH_GSTREAMER := 0 + GCS_COPY_GSTREAMER := 0 else ifeq ($(UNAME), Darwin) UAVOBJGENERATOR := $(BUILD_DIR)/uavobjgenerator/uavobjgenerator GCS_WITH_OSG := 1 GCS_WITH_OSGEARTH := 0 GCS_COPY_OSG := 1 + GCS_WITH_GSTREAMER := 0 + GCS_COPY_GSTREAMER := 0 else ifeq ($(UNAME), Windows) UAVOBJGENERATOR := $(BUILD_DIR)/uavobjgenerator/uavobjgenerator.exe GCS_WITH_OSG := 1 GCS_WITH_OSGEARTH := 1 GCS_COPY_OSG := 1 + GCS_WITH_GSTREAMER := 1 + GCS_COPY_GSTREAMER := 1 endif export UAVOBJGENERATOR @@ -159,6 +165,13 @@ ifeq ($(GCS_WITH_OSG), 1) endif endif +ifeq ($(GCS_WITH_GSTREAMER), 1) + GCS_EXTRA_CONF += gstreamer + ifeq ($(GCS_COPY_GSTREAMER), 1) + GCS_EXTRA_CONF += copy_gstreamer + endif +endif + ############################## # # All targets @@ -195,7 +208,7 @@ $(UAVOBJGENERATOR): | $(UAVOBJGENERATOR_DIR) CONFIG+='$(GCS_BUILD_CONF) $(GCS_EXTRA_CONF)' ) && \ $(MAKE) --no-print-directory -w -UAVOBJ_TARGETS := gcs flight python matlab java wireshark +UAVOBJ_TARGETS := gcs flight arduino python matlab java wireshark .PHONY: uavobjects uavobjects: $(addprefix uavobjects_, $(UAVOBJ_TARGETS)) @@ -209,6 +222,23 @@ uavobjects_%: $(UAVOBJGENERATOR) $(UAVOBJGENERATOR) -$* $(UAVOBJ_XML_DIR) $(ROOT_DIR) ; \ ) +OBJECTCOUNT := $(shell find $(ROOT_DIR)/python/librepilot/uavobjects/ -name '*.py' | wc -l) +uavobjects_python_install: + $(V1) if [ $(OBJECTCOUNT) -gt 2 ]; then echo "UAVObjects already exist"; else make uavobjects_python; fi + $(V1) mkdir -p $(ROOT_DIR)/python/librepilot/uavobjects/ + $(V1) ( touch $(ROOT_DIR)/python/librepilot/uavobjects/__init__.py ) + $(V1) ( cp $(UAVOBJ_OUT_DIR)/python/* $(ROOT_DIR)/python/librepilot/uavobjects/ ) + $(V1) ( cd $(ROOT_DIR)/python/ && sudo python setup.py build && sudo python setup.py install) + +uavobjects_python_clean: + @$(ECHO) " CLEAN $(call toprel, $(ROOT_DIR)/python/librepilot/uavobjects/)" + $(V1) [ ! -d "$(ROOT_DIR)/python/librepilot/uavobjects/" ] || $(RM) -r "$(ROOT_DIR)/python/librepilot/uavobjects/" + @$(ECHO) " CLEAN $(call toprel, $(ROOT_DIR)/python/librepilot/build/)" + $(V1) [ ! -d "$(ROOT_DIR)/python/librepilot/build/" ] || sudo $(RM) -r "$(ROOT_DIR)/python/librepilot/build/" + @$(ECHO) " CLEAN $(call toprel, $(UAVOBJ_OUT_DIR)/python/)" + $(V1) [ ! -d "$(UAVOBJ_OUT_DIR)/python/" ] || $(RM) -r "$(UAVOBJ_OUT_DIR)/python/" + + uavobjects_test: $(UAVOBJGENERATOR) $(V1) $(UAVOBJGENERATOR) -v $(UAVOBJ_XML_DIR) $(ROOT_DIR) @@ -225,9 +255,9 @@ uavobjects_clean: # When building any of the "all_*" targets, tell all sub makefiles to display # additional details on each line of output to describe which build and target -# that each line applies to. The same applies also to all, opfw_resource, +# that each line applies to. The same applies also to all, fw_resource, # package targets -ifneq ($(strip $(filter all_% all opfw_resource package,$(MAKECMDGOALS))),) +ifneq ($(strip $(filter all_% all fw_resource package,$(MAKECMDGOALS))),) export ENABLE_MSG_EXTRA := yes endif @@ -318,43 +348,49 @@ uploader_clean: # ############################## # Firmware files to package -PACKAGE_FW_TARGETS := fw_coptercontrol fw_oplinkmini fw_revolution fw_osd fw_revoproto fw_gpsplatinum fw_revonano fw_sparky2 +PACKAGE_FW_TARGETS := fw_coptercontrol fw_revolution fw_revonano fw_sparky2 +PACKAGE_FW_TARGETS += fw_oplinkmini +PACKAGE_FW_TARGETS += fw_gpsplatinum +PACKAGE_FW_TARGETS += fw_osd +PACKAGE_FW_TARGETS += fw_revoproto +PACKAGE_FW_TARGETS += fw_spracingf3evo fw_spracingf3 fw_nucleof303re fw_pikoblx fw_tinyfish # Rules to generate GCS resources used to embed firmware binaries into the GCS. # They are used later by the vehicle setup wizard to update board firmware. # To open a firmware image use ":/firmware/fw_coptercontrol.opfw" -OPFW_RESOURCE := $(OPGCSSYNTHDIR)/opfw_resource.qrc +FW_RESOURCE := $(GCS_SYNTH_DIR)/fw_resource.qrc ifeq ($(WITH_PREBUILT_FW),) FIRMWARE_DIR := $(FLIGHT_OUT_DIR) # We need to build the FW targets -$(OPFW_RESOURCE): $(PACKAGE_FW_TARGETS) +$(FW_RESOURCE): $(PACKAGE_FW_TARGETS) else FIRMWARE_DIR := $(WITH_PREBUILT_FW) endif -OPFW_FILES := $(foreach fw_targ, $(PACKAGE_FW_TARGETS), $(FIRMWARE_DIR)/$(fw_targ)/$(fw_targ).opfw) -OPFW_CONTENTS := \ +FW_FILES := $(foreach fw_targ, $(PACKAGE_FW_TARGETS), $(FIRMWARE_DIR)/$(fw_targ)/$(fw_targ).opfw) +FW_CONTENTS := \ \ \ - $(foreach fw_file, $(OPFW_FILES), $(call system_path,$(fw_file))) \ + $(foreach fw_file, $(FW_FILES), $(call system_path,$(fw_file))) \ \ -.PHONY: opfw_resource -opfw_resource: $(OPFW_RESOURCE) +.PHONY: fw_resource +fw_resource: $(FW_RESOURCE) +fw_resource: $(FW_RESOURCE) -$(OPFW_RESOURCE): | $(OPGCSSYNTHDIR) - @$(ECHO) Generating OPFW resource file $(call toprel, $@) - $(V1) $(ECHO) $(QUOTE)$(OPFW_CONTENTS)$(QUOTE) > $@ +$(FW_RESOURCE): | $(GCS_SYNTH_DIR) + @$(ECHO) Generating FW resource file $(call toprel, $@) + $(V1) $(ECHO) $(QUOTE)$(FW_CONTENTS)$(QUOTE) > $@ -# If opfw_resource or all firmware are requested, GCS should depend on the resource -ifneq ($(strip $(filter opfw_resource all all_fw all_flight package,$(MAKECMDGOALS))),) -$(GCS_MAKEFILE): $(OPFW_RESOURCE) +# If fw_resource or all firmware are requested, GCS should depend on the resource +ifneq ($(strip $(filter fw_resource all all_fw all_flight package,$(MAKECMDGOALS))),) +$(GCS_MAKEFILE): $(FW_RESOURCE) endif # Packaging targets: package -# - builds all firmware, opfw_resource, gcs +# - builds all firmware, fw_resource, gcs # - copies firmware into a package directory # - calls paltform-specific packaging script @@ -567,6 +603,10 @@ config_help: @$(ECHO) " (Needed unless using system versions)" @$(ECHO) " Options: 0 or 1" @$(ECHO) + @$(ECHO) " GCS_WITH_GSTREAMER=$(GCS_WITH_GSTREAMER)" + @$(ECHO) " Build the GCS with GStreamer support, this enables the video gadget and extra PFD video views" + @$(ECHO) " Options: 0 or 1" + @$(ECHO) @$(ECHO) " CCACHE=$(CCACHE)" @$(ECHO) " A prefix to compiler invocations, usually 'ccache' or 'path/to/ccache'" @$(ECHO) @@ -602,7 +642,7 @@ help: @$(ECHO) @$(ECHO) " This Makefile is known to work on Linux and Mac in a standard shell environment." @$(ECHO) " It also works on Windows by following the instructions given on this wiki page:" - @$(ECHO) " $(WIKI_ROOT_URL)Windows+Building+and+Packaging" + @$(ECHO) " $(WIKI_URL_ROOT)Windows+Building+and+Packaging" @$(ECHO) @$(ECHO) " Here is a summary of the available targets:" @$(ECHO) @@ -710,10 +750,14 @@ help: @$(ECHO) " uavobjects_test - Parse xml-files - check for valid, duplicate ObjId's, ..." @$(ECHO) " uavobjects_ - Generate source files from a subset of the UAVObject definition XML files" @$(ECHO) " Supported groups are ($(UAVOBJ_TARGETS))" + @$(ECHO) " uavobjects_python_install" + @$(ECHO) " - Install generated python files as eggs for use with example Python scripts" + @$(ECHO) " uavobjects_python_clean" + @$(ECHO) " - Remove generated python UAVOs from build directory & pyuavtalk folder" @$(ECHO) @$(ECHO) " [Packaging]" @$(ECHO) " package - Build and package the platform-dependent package (no clean)" - @$(ECHO) " opfw_resource - Generate resources to embed firmware binaries into the GCS" + @$(ECHO) " fw_resource - Generate resources to embed firmware binaries into the GCS" @$(ECHO) " dist - Generate source archive for distribution" @$(ECHO) " fw_dist - Generate archive of firmware" @$(ECHO) " install - Install GCS to \"DESTDIR\" with prefix \"prefix\" (Linux only)" diff --git a/arduino/uavobjects/inc/uavobject.h.template b/arduino/uavobjects/inc/uavobject.h.template new file mode 100644 index 0000000000..eff87a9a23 --- /dev/null +++ b/arduino/uavobjects/inc/uavobject.h.template @@ -0,0 +1,84 @@ +/** + ****************************************************************************** + * @addtogroup UAVObjects LibrePilot UAVObjects + * @{ + * @addtogroup $(NAME) $(NAME) + * @brief $(DESCRIPTION) + * + * Autogenerated files and functions for $(NAME) Object + * + * @{ + * + * @file $(NAMELC).h + * + * @author The LibrePilot Project, https://www.librepilot.org, (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * + * @brief Arduino Header of the $(NAME) object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: $(XMLFILE). + * This is an automatically generated file. + * DO NOT modify manually. + * + * @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 $(NAMEUC)_H +#define $(NAMEUC)_H +#include +/* Object constants */ +#define $(NAMEUC)_OBJID $(OBJIDHEX) +#define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST) +#define $(NAMEUC)_ISSETTINGS $(ISSETTINGS) +#define $(NAMEUC)_ISPRIORITY $(ISPRIORITY) +#define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data) + +$(DATAFIELDINFO) + +$(DATASTRUCTURES) +/* + * Packed Object data (unaligned). + * Should only be used where 4 byte alignment can be guaranteed + * (eg a single instance on the heap) + */ +typedef struct { +$(DATAFIELDS) +} __attribute__((packed)) $(NAME)DataPacked; + +/* + * Packed Object data. + * Alignment is forced to 4 bytes + */ +typedef $(NAME)DataPacked __attribute__((aligned(4))) $(NAME)Data; + +/* + * Union to apply the data array to and to use as structured object data + */ +union { + $(NAME)DataPacked data; + byte arr[$(NAMEUC)_NUMBYTES]; + } $(NAME)DataUnion; + +#endif // $(NAMEUC)_H + +/** + * @} + * @} + */ diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index b73f5e6aa5..7f75b1d65b 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -7,5 +7,5 @@ pipelines: - apt-get install -y libc6-i386 libudev-dev libusb-1.0-0-dev libsdl1.2-dev python libopenscenegraph-dev libosgearth-dev qt56-meta-minimal qt56svg qt56script qt56serialport qt56multimedia qt56translations qt56tools - make build_sdk_install - make all_flight - - make opfw_resource + - make fw_resource - make gcs diff --git a/flight/Makefile b/flight/Makefile index 7540aa3647..e7603b6c5f 100644 --- a/flight/Makefile +++ b/flight/Makefile @@ -8,9 +8,22 @@ export OPUAVTALK := $(FLIGHT_ROOT_DIR)/uavtalk export FLIGHT_OUT_DIR ?= $(CURDIR) # Define supported board lists -ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum revonano sparky2 +ALL_BOARDS := coptercontrol revolution revonano sparky2 +ALL_BOARDS += revoproto +ALL_BOARDS += oplinkmini +ALL_BOARDS += gpsplatinum +ALL_BOARDS += osd +ALL_BOARDS += discoveryf4bare +ALL_BOARDS += ccf3d spracingf3 spracingf3evo nucleof303re pikoblx tinyfish +# SimPosix only builds on Linux +ifeq ($(UNAME), Linux) + ALL_BOARDS += simposix +endif # Short names of each board (used to display board name in parallel builds) +spracingf3_short := 'srf3' +spracingf3evo_short := 'spev' +ccf3d_short := 'cf3d' coptercontrol_short := 'cc ' oplinkmini_short := 'oplm' revolution_short := 'revo' @@ -21,12 +34,9 @@ sparky2_short := 'spk2' simposix_short := 'posx' discoveryf4bare_short := 'df4b' gpsplatinum_short := 'gps9' - -# SimPosix only builds on Linux so drop it from the list for -# all other platforms. -ifneq ($(UNAME), Linux) - ALL_BOARDS := $(filter-out simposix, $(ALL_BOARDS)) -endif +nucleof303re_short := 'nf3r' +pikoblx_short := 'piko' +tinyfish_short := 'tfsh' # Start out assuming that we'll build fw, bl and bu for all boards FW_BOARDS := $(ALL_BOARDS) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 14b7c9ae29..077df63635 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -1,298 +1,300 @@ -// !$*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 = 45; + objects = { + +/* Begin PBXFileReference section */ + 4354B66314FED9FE004BA3B4 /* flight */ = {isa = PBXFileReference; lastKnownFileType = folder; name = flight; path = ../..; sourceTree = SOURCE_ROOT; }; + 5985F6EF1ED7911600B90A89 /* ground */ = {isa = PBXFileReference; lastKnownFileType = folder; name = ground; path = ../../../ground; sourceTree = ""; }; + 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 */, + 5985F6EF1ED7911600B90A89 /* ground */, + 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 */; +} diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 61bf9191b3..0b166f9271 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -27,25 +27,29 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include #include #include -#include "CoordinateConversions.h" +#include "inc/CoordinateConversions.h" #define MIN_ALLOWABLE_MAGNITUDE 1e-30f +// Equatorial Radius +#define equatorial_radius 6378137.0d + +// Eccentricity +#define eccentricity 8.1819190842622e-2d +#define equatorial_radius_sq (equatorial_radius * equatorial_radius) +#define eccentricity_sq (eccentricity * eccentricity) + // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(int32_t LLAi[3], double ECEF[3]) +void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]) { - const double a = 6378137.0d; // Equatorial Radius - const double e = 8.1819190842622e-2d; // Eccentricity - const double e2 = e * e; // Eccentricity squared double sinLat, sinLon, cosLat, cosLon; double N; - double LLA[3] = { - (double)LLAi[0] * 1e-7d, - (double)LLAi[1] * 1e-7d, - (double)LLAi[2] * 1e-4d + double LLA[3] = { + ((double)LLAi[0]) * 1e-7d, + ((double)LLAi[1]) * 1e-7d, + ((double)LLAi[2]) * 1e-4d }; sinLat = sin(DEG2RAD_D(LLA[0])); @@ -53,60 +57,53 @@ void LLA2ECEF(int32_t LLAi[3], double ECEF[3]) cosLat = cos(DEG2RAD_D(LLA[0])); cosLon = cos(DEG2RAD_D(LLA[1])); - N = a / sqrt(1.0d - e2 * sinLat * sinLat); // prime vertical radius of curvature + N = equatorial_radius / sqrt(1.0d - eccentricity_sq * sinLat * sinLat); // prime vertical radius of curvature - ECEF[0] = (N + LLA[2]) * cosLat * cosLon; - ECEF[1] = (N + LLA[2]) * cosLat * sinLon; - ECEF[2] = ((1.0d - e2) * N + LLA[2]) * sinLat; + ECEF[0] = (float)((N + LLA[2]) * cosLat * cosLon); + ECEF[1] = (float)((N + LLA[2]) * cosLat * sinLon); + ECEF[2] = (float)(((1.0d - eccentricity_sq) * N + LLA[2]) * sinLat); } // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* -uint16_t ECEF2LLA(double ECEF[3], float LLA[3]) +void ECEF2LLA(const float ECEF[3], int32_t LLA[3]) { - /** - * LLA parameter is used to prime the iteration. - * A position within 1 meter of the specified LLA - * will be calculated within at most 3 iterations. - * If unknown: Call with any valid LLA coordinate - * will compute within at most 5 iterations. - * Suggestion: [0,0,0] - **/ - - const double a = 6378137.0f; // Equatorial Radius - const double e = 8.1819190842622e-2f; // Eccentricity - double x = ECEF[0], y = ECEF[1], z = ECEF[2]; - double Lat, N, NplusH, delta, esLat; - uint16_t iter; - -#define MAX_ITER 10 // should not take more than 5 for valid coordinates -#define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations - - LLA[1] = (float)RAD2DEG_D(atan2(y, x)); - Lat = DEG2RAD_D((double)LLA[0]); - esLat = e * sin(Lat); - N = a / sqrt(1 - esLat * esLat); - NplusH = N + (double)LLA[2]; - delta = 1; - iter = 0; - - while (((delta > ACCURACY) || (delta < -ACCURACY)) - && (iter < MAX_ITER)) { - delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH)))); - Lat = Lat - delta; - esLat = e * sin(Lat); - N = a / sqrt(1 - esLat * esLat); - NplusH = sqrt(x * x + y * y) / cos(Lat); - iter += 1; - } +/* b = math.sqrt( asq * (1-esq) ) + bsq = b*b + + ep = math.sqrt((asq - bsq)/bsq) + p = math.sqrt( math.pow(x,2) + math.pow(y,2) ) + th = math.atan2(a*z, b*p) + + lon = math.atan2(y,x) + lat = math.atan2( (z + ep*ep *b * math.pow(math.sin(th),3) ), (p - esq*a*math.pow(math.cos(th),3)) ) + N = a/( math.sqrt(1-esq*math.pow(math.sin(lat),2)) ) + alt = p / math.cos(lat) - N*/ + + const double x = ECEF[0], y = ECEF[1], z = ECEF[2]; - LLA[0] = RAD2DEG_D(Lat); - LLA[2] = NplusH - N; + const double b = sqrt(equatorial_radius_sq * (1 - eccentricity_sq)); + const double bsq = b * b; + const double ep = sqrt((equatorial_radius_sq - bsq) / bsq); - return iter < MAX_ITER; + const double p = sqrt(x * x + y * y); + const double th = atan2(equatorial_radius * z, b * p); + + double lon = atan2(y, x); + + const double lat = atan2( + (z + ep * ep * b * pow(sin(th), 3)), + (p - eccentricity_sq * equatorial_radius * pow(cos(th), 3)) + ); + const double N = equatorial_radius / (sqrt(1 - eccentricity_sq * pow(sin(lat), 2))); + const double alt = p / cos(lat) - N; + + LLA[0] = (int32_t)(RAD2DEG_D(lat) * 1e7d); + LLA[1] = (int32_t)(RAD2DEG_D(lon) * 1e7d) % ((int32_t)(180 * 1e7d)); + LLA[2] = (int32_t)(alt * 1e4d); } // ****** find ECEF to NED rotation matrix ******** -void RneFromLLA(int32_t LLAi[3], float Rne[3][3]) +void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]) { float sinLat, sinLon, cosLat, cosLon; @@ -249,16 +246,30 @@ void Quaternion2zB(const float q[4], float z[3]) // ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]) +void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]) { - double ECEF[3]; - float diff[3]; + float ECEF[3]; LLA2ECEF(LLAi, ECEF); + ECEF2Base(ECEF, BaseECEF, Rne, NED); +} + +// ****** Express LLA in a local NED Base Frame ******** +void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]) +{ + float ECEF[3]; + + Base2ECEF(NED, BaseECEF, Rne, ECEF); + ECEF2LLA(ECEF, LLAi); +} +// ****** Express ECEF in a local NED Base Frame ******** +void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]) +{ + float diff[3]; - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = (ECEF[0] - BaseECEF[0]); + diff[1] = (ECEF[1] - BaseECEF[1]); + diff[2] = (ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; @@ -266,19 +277,21 @@ void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3] } // ****** Express ECEF in a local NED Base Frame ******** -void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]) +void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]) { float diff[3]; - diff[0] = (float)(ECEF[0] - BaseECEF[0]); - diff[1] = (float)(ECEF[1] - BaseECEF[1]); - diff[2] = (float)(ECEF[2] - BaseECEF[2]); + diff[0] = Rne[0][0] * NED[0] + Rne[1][0] * NED[1] + Rne[2][0] * NED[2]; + diff[1] = Rne[0][1] * NED[0] + Rne[1][1] * NED[1] + Rne[2][1] * NED[2]; + diff[2] = Rne[0][2] * NED[0] + Rne[1][2] * NED[1] + Rne[2][2] * NED[2]; - NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; - NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; - NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; + + ECEF[0] = diff[0] + BaseECEF[0]; + ECEF[1] = diff[1] + BaseECEF[1]; + ECEF[2] = diff[2] + BaseECEF[2]; } + // ****** convert Rotation Matrix to Quaternion ******** // ****** if R converts from e to b, q is rotation from e to b **** void R2Quaternion(float R[3][3], float q[4]) diff --git a/flight/libraries/WorldMagModel.c b/flight/libraries/WorldMagModel.c index cff1403877..87fbb92910 100644 --- a/flight/libraries/WorldMagModel.c +++ b/flight/libraries/WorldMagModel.c @@ -2,7 +2,7 @@ ****************************************************************************** * * @file WorldMagModel.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2019. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Source file for the World Magnetic Model * This is a port of code available from the US NOAA. @@ -61,96 +61,96 @@ // first column not used but it will be optimized out by compiler static const float CoeffFile[91][6] = { { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }, - { 1.0f, 0.0f, -29438.5f, 0.0f, 10.7f, 0.0f }, - { 1.0f, 1.0f, -1501.1f, 4796.2f, 17.9f, -26.8f }, - { 2.0f, 0.0f, -2445.3f, 0.0f, -8.6f, 0.0f }, - { 2.0f, 1.0f, 3012.5f, -2845.6f, -3.3f, -27.1f }, - { 2.0f, 2.0f, 1676.6f, -642.0f, 2.4f, -13.3f }, - { 3.0f, 0.0f, 1351.1f, 0.0f, 3.1f, 0.0f }, - { 3.0f, 1.0f, -2352.3f, -115.3f, -6.2f, 8.4f }, - { 3.0f, 2.0f, 1225.6f, 245.0f, -0.4f, -0.4f }, - { 3.0f, 3.0f, 581.9f, -538.3f, -10.4f, 2.3f }, - { 4.0f, 0.0f, 907.2f, 0.0f, -0.4f, 0.0f }, - { 4.0f, 1.0f, 813.7f, 283.4f, 0.8f, -0.6f }, - { 4.0f, 2.0f, 120.3f, -188.6f, -9.2f, 5.3f }, - { 4.0f, 3.0f, -335.0f, 180.9f, 4.0f, 3.0f }, - { 4.0f, 4.0f, 70.3f, -329.5f, -4.2f, -5.3f }, - { 5.0f, 0.0f, -232.6f, 0.0f, -0.2f, 0.0f }, - { 5.0f, 1.0f, 360.1f, 47.4f, 0.1f, 0.4f }, - { 5.0f, 2.0f, 192.4f, 196.9f, -1.4f, 1.6f }, - { 5.0f, 3.0f, -141.0f, -119.4f, 0.0f, -1.1f }, - { 5.0f, 4.0f, -157.4f, 16.1f, 1.3f, 3.3f }, - { 5.0f, 5.0f, 4.3f, 100.1f, 3.8f, 0.1f }, - { 6.0f, 0.0f, 69.5f, 0.0f, -0.5f, 0.0f }, - { 6.0f, 1.0f, 67.4f, -20.7f, -0.2f, 0.0f }, - { 6.0f, 2.0f, 72.8f, 33.2f, -0.6f, -2.2f }, - { 6.0f, 3.0f, -129.8f, 58.8f, 2.4f, -0.7f }, - { 6.0f, 4.0f, -29.0f, -66.5f, -1.1f, 0.1f }, - { 6.0f, 5.0f, 13.2f, 7.3f, 0.3f, 1.0f }, - { 6.0f, 6.0f, -70.9f, 62.5f, 1.5f, 1.3f }, - { 7.0f, 0.0f, 81.6f, 0.0f, 0.2f, 0.0f }, - { 7.0f, 1.0f, -76.1f, -54.1f, -0.2f, 0.7f }, - { 7.0f, 2.0f, -6.8f, -19.4f, -0.4f, 0.5f }, - { 7.0f, 3.0f, 51.9f, 5.6f, 1.3f, -0.2f }, - { 7.0f, 4.0f, 15.0f, 24.4f, 0.2f, -0.1f }, - { 7.0f, 5.0f, 9.3f, 3.3f, -0.4f, -0.7f }, - { 7.0f, 6.0f, -2.8f, -27.5f, -0.9f, 0.1f }, - { 7.0f, 7.0f, 6.7f, -2.3f, 0.3f, 0.1f }, - { 8.0f, 0.0f, 24.0f, 0.0f, 0.0f, 0.0f }, - { 8.0f, 1.0f, 8.6f, 10.2f, 0.1f, -0.3f }, - { 8.0f, 2.0f, -16.9f, -18.1f, -0.5f, 0.3f }, - { 8.0f, 3.0f, -3.2f, 13.2f, 0.5f, 0.3f }, - { 8.0f, 4.0f, -20.6f, -14.6f, -0.2f, 0.6f }, - { 8.0f, 5.0f, 13.3f, 16.2f, 0.4f, -0.1f }, - { 8.0f, 6.0f, 11.7f, 5.7f, 0.2f, -0.2f }, - { 8.0f, 7.0f, -16.0f, -9.1f, -0.4f, 0.3f }, - { 8.0f, 8.0f, -2.0f, 2.2f, 0.3f, 0.0f }, - { 9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f }, - { 9.0f, 1.0f, 8.8f, -21.6f, -0.1f, -0.2f }, - { 9.0f, 2.0f, 3.1f, 10.8f, -0.1f, -0.1f }, - { 9.0f, 3.0f, -3.1f, 11.7f, 0.4f, -0.2f }, - { 9.0f, 4.0f, 0.6f, -6.8f, -0.5f, 0.1f }, - { 9.0f, 5.0f, -13.3f, -6.9f, -0.2f, 0.1f }, - { 9.0f, 6.0f, -0.1f, 7.8f, 0.1f, 0.0f }, - { 9.0f, 7.0f, 8.7f, 1.0f, 0.0f, -0.2f }, - { 9.0f, 8.0f, -9.1f, -3.9f, -0.2f, 0.4f }, - { 9.0f, 9.0f, -10.5f, 8.5f, -0.1f, 0.3f }, - { 10.0f, 0.0f, -1.9f, 0.0f, 0.0f, 0.0f }, - { 10.0f, 1.0f, -6.5f, 3.3f, 0.0f, 0.1f }, - { 10.0f, 2.0f, 0.2f, -0.3f, -0.1f, -0.1f }, - { 10.0f, 3.0f, 0.6f, 4.6f, 0.3f, 0.0f }, - { 10.0f, 4.0f, -0.6f, 4.4f, -0.1f, 0.0f }, - { 10.0f, 5.0f, 1.7f, -7.9f, -0.1f, -0.2f }, - { 10.0f, 6.0f, -0.7f, -0.6f, -0.1f, 0.1f }, - { 10.0f, 7.0f, 2.1f, -4.1f, 0.0f, -0.1f }, - { 10.0f, 8.0f, 2.3f, -2.8f, -0.2f, -0.2f }, - { 10.0f, 9.0f, -1.8f, -1.1f, -0.1f, 0.1f }, - { 10.0f, 10.0f, -3.6f, -8.7f, -0.2f, -0.1f }, - { 11.0f, 0.0f, 3.1f, 0.0f, 0.0f, 0.0f }, - { 11.0f, 1.0f, -1.5f, -0.1f, 0.0f, 0.0f }, - { 11.0f, 2.0f, -2.3f, 2.1f, -0.1f, 0.1f }, - { 11.0f, 3.0f, 2.1f, -0.7f, 0.1f, 0.0f }, - { 11.0f, 4.0f, -0.9f, -1.1f, 0.0f, 0.1f }, - { 11.0f, 5.0f, 0.6f, 0.7f, 0.0f, 0.0f }, - { 11.0f, 6.0f, -0.7f, -0.2f, 0.0f, 0.0f }, - { 11.0f, 7.0f, 0.2f, -2.1f, 0.0f, 0.1f }, - { 11.0f, 8.0f, 1.7f, -1.5f, 0.0f, 0.0f }, - { 11.0f, 9.0f, -0.2f, -2.5f, 0.0f, -0.1f }, - { 11.0f, 10.0f, 0.4f, -2.0f, -0.1f, 0.0f }, + { 1.0f, 0.0f, -29438.2f, 0.0f, 7.0f, 0.0f }, + { 1.0f, 1.0f, -1493.5f, 4796.3f, 9.0f, -30.2f }, + { 2.0f, 0.0f, -2444.5f, 0.0f, -11.0f, 0.0f }, + { 2.0f, 1.0f, 3014.7f, -2842.4f, -6.2f, -29.6f }, + { 2.0f, 2.0f, 1679.0f, -638.8f, 0.3f, -17.3f }, + { 3.0f, 0.0f, 1351.8f, 0.0f, 2.4f, 0.0f }, + { 3.0f, 1.0f, -2351.6f, -113.7f, -5.7f, 6.5f }, + { 3.0f, 2.0f, 1223.6f, 246.5f, 2.0f, -0.8f }, + { 3.0f, 3.0f, 582.3f, -537.4f, -11.0f, -2.0f }, + { 4.0f, 0.0f, 907.5f, 0.0f, -0.8f, 0.0f }, + { 4.0f, 1.0f, 814.8f, 283.3f, -0.9f, -0.4f }, + { 4.0f, 2.0f, 117.8f, -188.6f, -6.5f, 5.8f }, + { 4.0f, 3.0f, -335.6f, 180.7f, 5.2f, 3.8f }, + { 4.0f, 4.0f, 69.7f, -330.0f, -4.0f, -3.5f }, + { 5.0f, 0.0f, -232.9f, 0.0f, -0.3f, 0.0f }, + { 5.0f, 1.0f, 360.1f, 46.9f, 0.6f, 0.2f }, + { 5.0f, 2.0f, 191.7f, 196.5f, -0.8f, 2.3f }, + { 5.0f, 3.0f, -141.3f, -119.9f, 0.1f, -0.0f }, + { 5.0f, 4.0f, -157.2f, 16.0f, 1.2f, 3.3f }, + { 5.0f, 5.0f, 7.7f, 100.6f, 1.4f, -0.6f }, + { 6.0f, 0.0f, 69.4f, 0.0f, -0.8f, 0.0f }, + { 6.0f, 1.0f, 67.7f, -20.1f, -0.5f, 0.3f }, + { 6.0f, 2.0f, 72.3f, 32.8f, -0.1f, -1.5f }, + { 6.0f, 3.0f, -129.1f, 59.1f, 1.6f, -1.2f }, + { 6.0f, 4.0f, -28.4f, -67.1f, -1.6f, 0.4f }, + { 6.0f, 5.0f, 13.6f, 8.1f, 0.0f, 0.2f }, + { 6.0f, 6.0f, -70.3f, 61.9f, 1.2f, 1.3f }, + { 7.0f, 0.0f, 81.7f, 0.0f, -0.3f, 0.0f }, + { 7.0f, 1.0f, -75.9f, -54.3f, -0.2f, 0.6f }, + { 7.0f, 2.0f, -7.1f, -19.5f, -0.3f, 0.5f }, + { 7.0f, 3.0f, 52.2f, 6.0f, 0.9f, -0.8f }, + { 7.0f, 4.0f, 15.0f, 24.5f, 0.1f, -0.2f }, + { 7.0f, 5.0f, 9.1f, 3.5f, -0.6f, -1.1f }, + { 7.0f, 6.0f, -3.0f, -27.7f, -0.9f, 0.1f }, + { 7.0f, 7.0f, 5.9f, -2.9f, 0.7f, 0.2f }, + { 8.0f, 0.0f, 24.2f, 0.0f, -0.1f, 0.0f }, + { 8.0f, 1.0f, 8.9f, 10.1f, 0.2f, -0.4f }, + { 8.0f, 2.0f, -16.9f, -18.3f, -0.2f, 0.6f }, + { 8.0f, 3.0f, -3.1f, 13.3f, 0.5f, -0.1f }, + { 8.0f, 4.0f, -20.7f, -14.5f, -0.1f, 0.6f }, + { 8.0f, 5.0f, 13.3f, 16.2f, 0.4f, -0.2f }, + { 8.0f, 6.0f, 11.6f, 6.0f, 0.4f, -0.5f }, + { 8.0f, 7.0f, -16.3f, -9.2f, -0.1f, 0.5f }, + { 8.0f, 8.0f, -2.1f, 2.4f, 0.4f, 0.1f }, + { 9.0f, 0.0f, 5.5f, 0.0f, -0.1f, 0.0f }, + { 9.0f, 1.0f, 8.8f, -21.8f, -0.1f, -0.3f }, + { 9.0f, 2.0f, 3.0f, 10.7f, -0.0f, 0.1f }, + { 9.0f, 3.0f, -3.2f, 11.8f, 0.4f, -0.4f }, + { 9.0f, 4.0f, 0.6f, -6.8f, -0.4f, 0.3f }, + { 9.0f, 5.0f, -13.2f, -6.9f, 0.0f, 0.1f }, + { 9.0f, 6.0f, -0.1f, 7.9f, 0.3f, -0.0f }, + { 9.0f, 7.0f, 8.7f, 1.0f, 0.0f, -0.1f }, + { 9.0f, 8.0f, -9.1f, -3.9f, -0.0f, 0.5f }, + { 9.0f, 9.0f, -10.4f, 8.5f, -0.3f, 0.2f }, + { 10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f }, + { 10.0f, 1.0f, -6.1f, 3.3f, -0.0f, 0.0f }, + { 10.0f, 2.0f, 0.2f, -0.4f, -0.1f, 0.1f }, + { 10.0f, 3.0f, 0.6f, 4.6f, 0.2f, -0.2f }, + { 10.0f, 4.0f, -0.5f, 4.4f, -0.1f, 0.1f }, + { 10.0f, 5.0f, 1.8f, -7.9f, -0.2f, -0.1f }, + { 10.0f, 6.0f, -0.7f, -0.6f, -0.0f, 0.1f }, + { 10.0f, 7.0f, 2.2f, -4.2f, -0.1f, -0.0f }, + { 10.0f, 8.0f, 2.4f, -2.9f, -0.2f, -0.1f }, + { 10.0f, 9.0f, -1.8f, -1.1f, -0.1f, 0.2f }, + { 10.0f, 10.0f, -3.6f, -8.8f, -0.0f, -0.0f }, + { 11.0f, 0.0f, 3.0f, 0.0f, -0.0f, 0.0f }, + { 11.0f, 1.0f, -1.4f, -0.0f, 0.0f, 0.0f }, + { 11.0f, 2.0f, -2.3f, 2.1f, -0.0f, 0.1f }, + { 11.0f, 3.0f, 2.1f, -0.6f, 0.0f, 0.0f }, + { 11.0f, 4.0f, -0.8f, -1.1f, -0.0f, 0.1f }, + { 11.0f, 5.0f, 0.6f, 0.7f, -0.1f, -0.0f }, + { 11.0f, 6.0f, -0.7f, -0.2f, 0.0f, -0.0f }, + { 11.0f, 7.0f, 0.1f, -2.1f, -0.0f, 0.1f }, + { 11.0f, 8.0f, 1.7f, -1.5f, -0.0f, -0.0f }, + { 11.0f, 9.0f, -0.2f, -2.6f, -0.1f, -0.1f }, + { 11.0f, 10.0f, 0.4f, -2.0f, -0.0f, -0.0f }, { 11.0f, 11.0f, 3.5f, -2.3f, -0.1f, -0.1f }, - { 12.0f, 0.0f, -2.0f, 0.0f, 0.1f, 0.0f }, - { 12.0f, 1.0f, -0.3f, -1.0f, 0.0f, 0.0f }, - { 12.0f, 2.0f, 0.4f, 0.5f, 0.0f, 0.0f }, - { 12.0f, 3.0f, 1.3f, 1.8f, 0.1f, -0.1f }, - { 12.0f, 4.0f, -0.9f, -2.2f, -0.1f, 0.0f }, - { 12.0f, 5.0f, 0.9f, 0.3f, 0.0f, 0.0f }, - { 12.0f, 6.0f, 0.1f, 0.7f, 0.1f, 0.0f }, - { 12.0f, 7.0f, 0.5f, -0.1f, 0.0f, 0.0f }, + { 12.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f }, + { 12.0f, 1.0f, -0.1f, -1.0f, 0.0f, -0.0f }, + { 12.0f, 2.0f, 0.5f, 0.3f, -0.0f, 0.0f }, + { 12.0f, 3.0f, 1.2f, 1.8f, 0.0f, -0.1f }, + { 12.0f, 4.0f, -0.9f, -2.2f, -0.1f, 0.1f }, + { 12.0f, 5.0f, 0.9f, 0.3f, -0.0f, -0.0f }, + { 12.0f, 6.0f, 0.1f, 0.7f, 0.0f, 0.0f }, + { 12.0f, 7.0f, 0.6f, -0.1f, -0.0f, -0.0f }, { 12.0f, 8.0f, -0.4f, 0.3f, 0.0f, 0.0f }, - { 12.0f, 9.0f, -0.4f, 0.2f, 0.0f, 0.0f }, - { 12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f }, - { 12.0f, 11.0f, -0.9f, -0.2f, 0.0f, 0.0f }, - { 12.0f, 12.0f, 0.0f, 0.7f, 0.0f, 0.0f } + { 12.0f, 9.0f, -0.5f, 0.2f, -0.0f, 0.0f }, + { 12.0f, 10.0f, 0.2f, -0.9f, -0.0f, -0.0f }, + { 12.0f, 11.0f, -0.9f, -0.2f, -0.0f, 0.0f }, + { 12.0f, 12.0f, -0.0f, 0.8f, -0.1f, -0.1f } }; static WMMtype_Ellipsoid *Ellip = NULL; @@ -194,7 +194,7 @@ int WMM_Initialize() // Really, Really needs to be read from a file - out of date in 2020 at latest MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */ MagneticModel->epoch = 2015.0f; - sprintf(MagneticModel->ModelName, "WMM-2015"); + sprintf(MagneticModel->ModelName, "WMM-2015v2"); return 0; // OK } diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 7cc7f39aaa..b9a977a8b7 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -307,6 +307,7 @@ static const char *const systemalarms_alarm_names[] = { [SYSTEMALARMS_ALARM_MAGNETOMETER] = "MAG", [SYSTEMALARMS_ALARM_AIRSPEED] = "AIRSPD", [SYSTEMALARMS_ALARM_STABILIZATION] = "STAB", + [SYSTEMALARMS_ALARM_NAV] = "NAV", [SYSTEMALARMS_ALARM_GUIDANCE] = "GUIDANCE", [SYSTEMALARMS_ALARM_PATHPLAN] = "PLAN", [SYSTEMALARMS_ALARM_BATTERY] = "BATT", diff --git a/flight/libraries/cm3_fault_handlers.c b/flight/libraries/cm3_fault_handlers.c new file mode 100644 index 0000000000..512ff274b0 --- /dev/null +++ b/flight/libraries/cm3_fault_handlers.c @@ -0,0 +1,96 @@ +/* + * cm3_fault_handlers.c + * + * Created on: Apr 24, 2011 + * Author: msmith + */ + +#include +#include "inc/dcc_stdio.h" +#ifdef STM32F4 +# include +#endif +#ifdef STM32F3 +# include +#endif +#ifdef STM32F2XX +# include +#endif +#ifdef STM32F1 +# include +#endif + +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack + +struct cm3_frame { + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; +}; + +FAULT_TRAMPOLINE(HardFault); +FAULT_TRAMPOLINE(BusFault); +FAULT_TRAMPOLINE(UsageFault); + +/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) + +void HardFault_Handler2(struct cm3_frame *frame) +{ + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } +} + +void BusFault_Handler2(struct cm3_frame *frame) +{ + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } +} + +void UsageFault_Handler2(struct cm3_frame *frame) +{ + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } +} diff --git a/flight/targets/boards/discoveryf4bare/firmware/dcc_stdio.c b/flight/libraries/dcc_stdio.c similarity index 100% rename from flight/targets/boards/discoveryf4bare/firmware/dcc_stdio.c rename to flight/libraries/dcc_stdio.c diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 02369c45cf..0ef0a3bc31 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -29,14 +29,23 @@ #ifndef COORDINATECONVERSIONS_H_ #define COORDINATECONVERSIONS_H_ +#include // ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(int32_t LLAi[3], double ECEF[3]); +void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]); -// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* -uint16_t ECEF2LLA(double ECEF[3], float LLA[3]); +// ****** convert ECEF to Lat,Lon,Alt ********* +void ECEF2LLA(const float ECEF[3], int32_t LLA[3]); -void RneFromLLA(int32_t LLAi[3], float Rne[3][3]); +void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]); + +// ****** Express LLA in a local NED Base Frame and back ******** +void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]); +void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]); + +// ****** Express ECEF in a local NED Base Frame and back ******** +void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]); +void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]); // ****** find rotation matrix from rotation vector void Rv2Rot(float Rv[3], float R[3][3]); @@ -65,12 +74,6 @@ void Quaternion2yB(const float q[4], float y[3]); void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]); void Quaternion2zB(const float q[4], float z[3]); -// ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]); - -// ****** Express ECEF in a local NED Base Frame ******** -void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]); - // ****** convert Rotation Matrix to Quaternion ******** // ****** if R converts from e to b, q is rotation from e to b **** void R2Quaternion(float R[3][3], float q[4]); diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/dcc_stdio.h b/flight/libraries/inc/dcc_stdio.h similarity index 100% rename from flight/targets/boards/discoveryf4bare/firmware/inc/dcc_stdio.h rename to flight/libraries/inc/dcc_stdio.h diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index c77d44510f..86bfe5f58b 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -32,7 +32,8 @@ #ifndef INSGPS_H_ #define INSGPS_H_ -#include "stdint.h" +#include +#include /** * @addtogroup Constants @@ -54,22 +55,24 @@ // Exposed Function Prototypes void INSGPSInit(); -void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT); +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT); void INSCovariancePrediction(float dT); -void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed); +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + const float BaroAlt, uint16_t SensorsUsed); +void INSResetP(const float PDiag[13]); +void INSGetVariance(float PDiag[13]); +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]); +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]); +void INSSetGyroBias(const float gyro_bias[3]); +void INSSetAccelVar(const float accel_var[3]); +void INSSetGyroVar(const float gyro_var[3]); +void INSSetGyroBiasVar(const float gyro_bias_var[3]); +void INSSetMagNorth(const float B[3]); +void INSSetMagVar(const float scaled_mag_var[3]); +void INSSetBaroVar(const float baro_var); +void INSSetArmed(bool armed); +void INSPosVelReset(const float pos[3], const float vel[3]); -void INSResetP(float PDiag[13]); -void INSGetP(float PDiag[13]); -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]); -void INSSetPosVelVar(float PosVar[3], float VelVar[3]); -void INSSetGyroBias(float gyro_bias[3]); -void INSSetAccelVar(float accel_var[3]); -void INSSetGyroVar(float gyro_var[3]); -void INSSetGyroBiasVar(float gyro_bias_var[3]); -void INSSetMagNorth(float B[3]); -void INSSetMagVar(float scaled_mag_var[3]); -void INSSetBaroVar(float baro_var); -void INSPosVelReset(float pos[3], float vel[3]); void MagCorrection(float mag_data[3]); void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt); diff --git a/flight/libraries/inc/op_dfu.h b/flight/libraries/inc/op_dfu.h index 386609ae4d..feba9e4312 100644 --- a/flight/libraries/inc/op_dfu.h +++ b/flight/libraries/inc/op_dfu.h @@ -2,6 +2,7 @@ ****************************************************************************** * * @file op_dfu.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief * @see The GNU Public License (GPL) Version 3 @@ -26,9 +27,91 @@ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __OP_DFU_H #define __OP_DFU_H -#include "common.h" + /* Includes ------------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/ +typedef enum { + start, keepgoing, +} DownloadAction; + +/**************************************************/ +/* OP_DFU states */ +/**************************************************/ + +typedef enum { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 +} DFUStates; +/**************************************************/ +/* OP_DFU commands */ +/**************************************************/ +typedef enum { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 +} DFUCommands; + +typedef enum { + High_Density, Medium_Density +} DeviceType; +/**************************************************/ +/* OP_DFU transfer types */ +/**************************************************/ +typedef enum { + FW, // 0 + Descript +// 2 +} DFUTransfer; +/**************************************************/ +/* OP_DFU transfer port */ +/**************************************************/ +typedef enum { + Usb, // 0 + Serial +// 2 +} DFUPort; +/**************************************************/ +/* OP_DFU programable programable HW types */ +/**************************************************/ +typedef enum { + Self_flash, // 0 + Remote_flash_via_spi +// 1 +} DFUProgType; +/**************************************************/ +/* OP_DFU programable sources */ +/**************************************************/ +#define USB 0 +#define SPI 1 + +#define DownloadDelay 100000 + +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 + typedef struct { uint8_t programmingType; uint8_t readWriteFlags; diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index c311f05d33..6710dd944e 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -163,7 +163,22 @@ void INSGPSInit() // pretty much just a place holder for now ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) } -void INSResetP(float PDiag[NUMX]) +// ! Set the current flight state +void INSSetArmed(bool armed) +{ + return; + + // Speed up convergence of accel and gyro bias when not armed + if (armed) { + ekf.Q[9] = 1e-4f; + ekf.Q[8] = 2e-9f; + } else { + ekf.Q[9] = 1e-2f; + ekf.Q[8] = 2e-8f; + } +} + +void INSResetP(const float PDiag[NUMX]) { uint8_t i, j; @@ -178,7 +193,7 @@ void INSResetP(float PDiag[NUMX]) } } -void INSGetP(float PDiag[NUMX]) +void INSGetVariance(float PDiag[NUMX]) { uint8_t i; @@ -189,8 +204,7 @@ void INSGetP(float PDiag[NUMX]) } } } - -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3]) +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], __attribute__((unused)) const float accel_bias[3]) { /* Note: accel_bias not used in 13 state INS */ ekf.X[0] = pos[0]; @@ -208,7 +222,7 @@ void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __a ekf.X[12] = gyro_bias[2]; } -void INSPosVelReset(float pos[3], float vel[3]) +void INSPosVelReset(const float pos[3], const float vel[3]) { for (int i = 0; i < 6; i++) { for (int j = i; j < NUMX; j++) { @@ -228,7 +242,7 @@ void INSPosVelReset(float pos[3], float vel[3]) ekf.X[5] = vel[2]; } -void INSSetPosVelVar(float PosVar[3], float VelVar[3]) +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]) { ekf.R[0] = PosVar[0]; ekf.R[1] = PosVar[1]; @@ -238,47 +252,47 @@ void INSSetPosVelVar(float PosVar[3], float VelVar[3]) ekf.R[5] = VelVar[2]; } -void INSSetGyroBias(float gyro_bias[3]) +void INSSetGyroBias(const float gyro_bias[3]) { ekf.X[10] = gyro_bias[0]; ekf.X[11] = gyro_bias[1]; ekf.X[12] = gyro_bias[2]; } -void INSSetAccelVar(float accel_var[3]) +void INSSetAccelVar(const float accel_var[3]) { ekf.Q[3] = accel_var[0]; ekf.Q[4] = accel_var[1]; ekf.Q[5] = accel_var[2]; } -void INSSetGyroVar(float gyro_var[3]) +void INSSetGyroVar(const float gyro_var[3]) { ekf.Q[0] = gyro_var[0]; ekf.Q[1] = gyro_var[1]; ekf.Q[2] = gyro_var[2]; } -void INSSetGyroBiasVar(float gyro_bias_var[3]) +void INSSetGyroBiasVar(const float gyro_bias_var[3]) { ekf.Q[6] = gyro_bias_var[0]; ekf.Q[7] = gyro_bias_var[1]; ekf.Q[8] = gyro_bias_var[2]; } -void INSSetMagVar(float scaled_mag_var[3]) +void INSSetMagVar(const float scaled_mag_var[3]) { ekf.R[6] = scaled_mag_var[0]; ekf.R[7] = scaled_mag_var[1]; ekf.R[8] = scaled_mag_var[2]; } -void INSSetBaroVar(float baro_var) +void INSSetBaroVar(const float baro_var) { ekf.R[9] = baro_var; } -void INSSetMagNorth(float B[3]) +void INSSetMagNorth(const float B[3]) { float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); @@ -287,7 +301,7 @@ void INSSetMagNorth(float B[3]) ekf.Be[2] = B[2] * invmag; } -void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT) { float U[6]; float invqmag; @@ -371,8 +385,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt) HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR); } -void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], - float BaroAlt, uint16_t SensorsUsed) +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + const float BaroAlt, uint16_t SensorsUsed) { float Z[10] = { 0 }; float Y[10] = { 0 }; @@ -386,10 +400,9 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], Z[3] = Vel[0]; Z[4] = Vel[1]; Z[5] = Vel[2]; - // magnetometer data in any units (use unit vector) and in body frame - if (SensorsUsed & MAG_SENSORS) { + // magnetometer data in any units (use unit vector) and in body frame float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); Z[6] = mag_data[0] * invBmag; Z[7] = mag_data[1] * invBmag; diff --git a/flight/libraries/insgps14state.c b/flight/libraries/insgps14state.c new file mode 100644 index 0000000000..0a498d026d --- /dev/null +++ b/flight/libraries/insgps14state.c @@ -0,0 +1,937 @@ +/** + ****************************************************************************** + * @addtogroup Math + * @{ + * @addtogroup INSGPS + * @{ + * @brief INSGPS is a joint attitude and position estimation EKF + * + * @file insgps.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Tau Labs, http://github.com/TauLabs Copyright (C) 2012-2013. + * The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @brief An INS/GPS algorithm implemented with an EKF. + * + * @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 "insgps.h" +#include +#include +#include +#include +#include +#include + +// constants/macros/typdefs +#define NUMX 14 // number of states, X is the state vector +#define NUMW 10 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector +#pragma GCC optimize "O3" +// Private functions +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], + float Q[NUMW], float dT, float P[NUMX][NUMX]); +static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed); +static void RungeKutta(float X[NUMX], float U[NUMU], float dT); +static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); +static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], + float G[NUMX][NUMW]); +static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); +static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); + +// Private variables + +// speed optimizations, describe matrix sparsity +// derived from state equations in +// LinearizeFG() and LinearizeH(): +// +// usage F: usage G: usage H: +// -0123456789abcd 0123456789 0123456789abcd +// 0...X.......... .......... X............. +// 1....X......... .......... .X............ +// 2.....X........ .......... ..X........... +// 3......XXXX...X ...XXX.... ...X.......... +// 4......XXXX...X ...XXX.... ....X......... +// 5......XXXX...X ...XXX.... .....X........ +// 6.....ooXXXXXX. XXX....... ......XXXX.... +// 7.....oXoXXXXX. XXX....... ......XXXX.... +// 8.....oXXoXXXX. XXX....... ......XXXX.... +// 9.....oXXXoXXX. XXX....... ..X........... +// a.............. .......... +// b.............. .......... +// c.............. .......... +// d.............. .......... + +static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 14, 14, 14, 14 }; +static int8_t FrowMax[NUMX] = { 3, 4, 5, 13, 13, 13, 12, 12, 12, 12, -1, -1, -1, -1 }; + +static int8_t GrowMin[NUMX] = { 10, 10, 10, 3, 3, 3, 0, 0, 0, 0, 10, 10, 10, 10 }; +static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, -1, -1, -1, -1 }; + +static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; +static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; + +static struct EKFData { + float F[NUMX][NUMX]; + float G[NUMX][NUMW]; + float H[NUMV][NUMX]; // linearized system matrices + // global to init to zero and maintain zero elements + float Be[3]; // local magnetic unit vector in NED frame + float P[NUMX][NUMX]; + float X[NUMX]; // covariance matrix and state vector + float Q[NUMW]; + float R[NUMV]; // input noise and measurement noise variances + float K[NUMX][NUMV]; // feedback gain matrix +} ekf; + +// Global variables +struct NavStruct Nav; + +// ************* Exposed Functions **************** +// ************************************************* + +uint16_t ins_get_num_states() +{ + return NUMX; +} + +void INSGPSInit() +{ + ekf.Be[0] = 1.0f; + ekf.Be[1] = 0; + ekf.Be[2] = 0; // local magnetic unit vector + + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + ekf.P[i][j] = 0.0f; // zero all terms + ekf.F[i][j] = 0.0f; + } + + for (int j = 0; j < NUMW; j++) { + ekf.G[i][j] = 0.0f; + } + + for (int j = 0; j < NUMV; j++) { + ekf.H[j][i] = 0.0f; + ekf.K[i][j] = 0.0f; + } + + ekf.X[i] = 0.0f; + } + for (int i = 0; i < NUMW; i++) { + ekf.Q[i] = 0.0f; + } + for (int i = 0; i < NUMV; i++) { + ekf.R[i] = 0.0f; + } + + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance + ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-6f; // initial gyro bias variance (rad/s)^2 + ekf.P[13][13] = 1e-5f; // initial accel bias variance (deg/s)^2 + + ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) + ekf.X[6] = 1.0f; + ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) + ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) + ekf.X[13] = 0.0f; // initial accel bias + + ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 1e-5f; // gyro noise variance (rad/s)^2 + ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 1e-5f; // accelerometer noise variance (m/s^2)^2 + ekf.Q[6] = ekf.Q[7] = 1e-6f; // gyro x and y bias random walk variance (rad/s^2)^2 + ekf.Q[8] = 1e-6f; // gyro z bias random walk variance (rad/s^2)^2 + ekf.Q[9] = 5e-4f; // accel bias random walk variance (m/s^3)^2 + + ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + ekf.R[5] = 0.004f; // High freq GPS vertical velocity noise variance (m/s)^2 + ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance + ekf.R[9] = .05f; // High freq altimeter noise variance (m^2) +} + +// ! Set the current flight state +void INSSetArmed(bool armed) +{ + return; + + // Speed up convergence of accel and gyro bias when not armed + if (armed) { + ekf.Q[9] = 1e-4f; + ekf.Q[8] = 2e-9f; + } else { + ekf.Q[9] = 1e-2f; + ekf.Q[8] = 2e-8f; + } +} + +/** + * Get the current state estimate (null input skips that get) + * @param[out] pos The position in NED space (m) + * @param[out] vel The velocity in NED (m/s) + * @param[out] attitude Quaternion representation of attitude + * @param[out] gyros_bias Estimate of gyro bias (rad/s) + * @param[out] accel_bias Estiamte of the accel bias (m/s^2) + */ +void INSGetState(float *pos, float *vel, float *attitude, float *gyro_bias, float *accel_bias) +{ + if (pos) { + pos[0] = ekf.X[0]; + pos[1] = ekf.X[1]; + pos[2] = ekf.X[2]; + } + + if (vel) { + vel[0] = ekf.X[3]; + vel[1] = ekf.X[4]; + vel[2] = ekf.X[5]; + } + + if (attitude) { + attitude[0] = ekf.X[6]; + attitude[1] = ekf.X[7]; + attitude[2] = ekf.X[8]; + attitude[3] = ekf.X[9]; + } + + if (gyro_bias) { + gyro_bias[0] = ekf.X[10]; + gyro_bias[1] = ekf.X[11]; + gyro_bias[2] = ekf.X[12]; + } + + if (accel_bias) { + accel_bias[0] = 0.0f; + accel_bias[1] = 0.0f; + accel_bias[2] = ekf.X[13]; + } +} + +/** + * Get the variance, for visualizing the filter performance + * @param[out var_out The variances + */ +void INSGetVariance(float *var_out) +{ + for (uint32_t i = 0; i < NUMX; i++) { + var_out[i] = ekf.P[i][i]; + } +} + +void INSResetP(const float *PDiag) +{ + uint8_t i, j; + + // if PDiag[i] nonzero then clear row and column and set diagonal element + for (i = 0; i < NUMX; i++) { + if (PDiag != 0) { + for (j = 0; j < NUMX; j++) { + ekf.P[i][j] = ekf.P[j][i] = 0.0f; + } + ekf.P[i][i] = PDiag[i]; + } + } +} + +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]) +{ + ekf.X[0] = pos[0]; + ekf.X[1] = pos[1]; + ekf.X[2] = pos[2]; + ekf.X[3] = vel[0]; + ekf.X[4] = vel[1]; + ekf.X[5] = vel[2]; + ekf.X[6] = q[0]; + ekf.X[7] = q[1]; + ekf.X[8] = q[2]; + ekf.X[9] = q[3]; + ekf.X[10] = gyro_bias[0]; + ekf.X[11] = gyro_bias[1]; + ekf.X[12] = gyro_bias[2]; + ekf.X[13] = accel_bias[2]; +} + +void INSPosVelReset(const float pos[3], const float vel[3]) +{ + for (int i = 0; i < 6; i++) { + for (int j = i; j < NUMX; j++) { + ekf.P[i][j] = 0.0f; // zero the first 6 rows and columns + ekf.P[j][i] = 0.0f; + } + } + + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + + ekf.X[0] = pos[0]; + ekf.X[1] = pos[1]; + ekf.X[2] = pos[2]; + ekf.X[3] = vel[0]; + ekf.X[4] = vel[1]; + ekf.X[5] = vel[2]; +} +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]) +{ + ekf.R[0] = PosVar[0]; + ekf.R[1] = PosVar[1]; + ekf.R[2] = PosVar[2]; + ekf.R[3] = VelVar[0]; + ekf.R[4] = VelVar[1]; + ekf.R[5] = VelVar[2]; // Don't change vertical velocity, not measured +} + +void INSSetGyroBias(const float gyro_bias[3]) +{ + ekf.X[10] = gyro_bias[0]; + ekf.X[11] = gyro_bias[1]; + ekf.X[12] = gyro_bias[2]; +} + +void INSSetAccelBias(const float accel_bias[3]) +{ + ekf.X[13] = accel_bias[2]; +} + +void INSSetAccelVar(const float accel_var[3]) +{ + ekf.Q[3] = accel_var[0]; + ekf.Q[4] = accel_var[1]; + ekf.Q[5] = accel_var[2]; +} + +void INSSetGyroVar(const float gyro_var[3]) +{ + ekf.Q[0] = gyro_var[0]; + ekf.Q[1] = gyro_var[1]; + ekf.Q[2] = gyro_var[2]; +} + +void INSSetGyroBiasVar(const float gyro_bias_var[3]) +{ + ekf.Q[6] = gyro_bias_var[0]; + ekf.Q[7] = gyro_bias_var[1]; + ekf.Q[8] = gyro_bias_var[2]; +} + +void INSSetMagVar(const float scaled_mag_var[3]) +{ + ekf.R[6] = scaled_mag_var[0]; + ekf.R[7] = scaled_mag_var[1]; + ekf.R[8] = scaled_mag_var[2]; +} + +void INSSetBaroVar(const float baro_var) +{ + ekf.R[9] = baro_var; +} + +void INSSetMagNorth(const float B[3]) +{ + ekf.Be[0] = B[0]; + ekf.Be[1] = B[1]; + ekf.Be[2] = B[2]; +} + +void INSLimitBias() +{ + // The Z accel bias should never wander too much. This helps ensure the filter + // remains stable. + if (ekf.X[13] > 0.1f) { + ekf.X[13] = 0.1f; + } else if (ekf.X[13] < -0.1f) { + ekf.X[13] = -0.1f; + } + + // Make sure no gyro bias gets to more than 10 deg / s. This should be more than + // enough for well behaving sensors. + const float GYRO_BIAS_LIMIT = DEG2RAD(10); + for (int i = 10; i < 13; i++) { + if (ekf.X[i] < -GYRO_BIAS_LIMIT) { + ekf.X[i] = -GYRO_BIAS_LIMIT; + } else if (ekf.X[i] > GYRO_BIAS_LIMIT) { + ekf.X[i] = GYRO_BIAS_LIMIT; + } + } +} + +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT) +{ + float U[6]; + float invqmag; + + // rate gyro inputs in units of rad/s + U[0] = gyro_data[0]; + U[1] = gyro_data[1]; + U[2] = gyro_data[2]; + + // accelerometer inputs in units of m/s + U[3] = accel_data[0]; + U[4] = accel_data[1]; + U[5] = accel_data[2]; + + // EKF prediction step + LinearizeFG(ekf.X, U, ekf.F, ekf.G); + RungeKutta(ekf.X, U, dT); + invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + ekf.X[6] *= invqmag; + ekf.X[7] *= invqmag; + ekf.X[8] *= invqmag; + ekf.X[9] *= invqmag; + + // Update Nav solution structure + Nav.Pos[0] = ekf.X[0]; + Nav.Pos[1] = ekf.X[1]; + Nav.Pos[2] = ekf.X[2]; + Nav.Vel[0] = ekf.X[3]; + Nav.Vel[1] = ekf.X[4]; + Nav.Vel[2] = ekf.X[5]; + Nav.q[0] = ekf.X[6]; + Nav.q[1] = ekf.X[7]; + Nav.q[2] = ekf.X[8]; + Nav.q[3] = ekf.X[9]; + Nav.gyro_bias[0] = ekf.X[10]; + Nav.gyro_bias[1] = ekf.X[11]; + Nav.gyro_bias[2] = ekf.X[12]; + Nav.accel_bias[0] = 0.0f; + Nav.accel_bias[1] = 0.0f; + Nav.accel_bias[2] = ekf.X[13]; +} + +void INSCovariancePrediction(float dT) +{ + CovariancePrediction(ekf.F, ekf.G, ekf.Q, dT, ekf.P); +} + +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + const float BaroAlt, uint16_t SensorsUsed) +{ + float Z[10], Y[10]; + float invqmag; + + // GPS Position in meters and in local NED frame + Z[0] = Pos[0]; + Z[1] = Pos[1]; + Z[2] = Pos[2]; + + // GPS Velocity in meters and in local NED frame + Z[3] = Vel[0]; + Z[4] = Vel[1]; + Z[5] = Vel[2]; + + if (SensorsUsed & MAG_SENSORS) { + // magnetometer data in any units (use unit vector) and in body frame + float Rbe_a[3][3]; + float q0 = ekf.X[6]; + float q1 = ekf.X[7]; + float q2 = ekf.X[8]; + float q3 = ekf.X[9]; + float k1 = 1.0f / sqrtf(powf(q0 * q1 * 2.0f + q2 * q3 * 2.0f, 2.0f) + powf(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3, 2.0f)); + float k2 = sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f); + + Rbe_a[0][0] = k2; + Rbe_a[0][1] = 0.0f; + Rbe_a[0][2] = q0 * q2 * -2.0f + q1 * q3 * 2.0f; + Rbe_a[1][0] = k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f) * (q0 * q2 * 2.0f - q1 * q3 * 2.0f); + Rbe_a[1][1] = k1 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[1][2] = k1 * sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f) * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][0] = k1 * (q0 * q2 * 2.0f - q1 * q3 * 2.0f) * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[2][1] = -k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][2] = k1 * k2 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + + Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2]; + Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2]; + Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2]; + if (!(IS_REAL(Z[6]) && IS_REAL(Z[7]) && IS_REAL(Z[8]))) { + SensorsUsed = SensorsUsed & ~MAG_SENSORS; + } + } + + // barometric altimeter in meters and in local NED frame + Z[9] = BaroAlt; + + // EKF correction step + LinearizeH(ekf.X, ekf.Be, ekf.H); + MeasurementEq(ekf.X, ekf.Be, Y); + SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); + invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + ekf.X[6] *= invqmag; + ekf.X[7] *= invqmag; + ekf.X[8] *= invqmag; + ekf.X[9] *= invqmag; + + INSLimitBias(); +} + +// ************* CovariancePrediction ************* +// Does the prediction step of the Kalman filter for the covariance matrix +// Output, Pnew, overwrites P, the input covariance +// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' +// Q is the discrete time covariance of process noise +// Q is vector of the diagonal for a square matrix with +// dimensions equal to the number of disturbance noise variables +// The General Method is very inefficient,not taking advantage of the sparse F and G +// The first Method is very specific to this implementation +// ************************************************ + +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], + float Q[NUMW], float dT, float P[NUMX][NUMX]) +{ + // Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')] + + const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu. + const float dTsq = dT * dT; + + float Dummy[NUMX][NUMX]; + int8_t i; + int8_t k; + + for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P) + float *Firow = F[i]; + float *Pirow = P[i]; + float *Dirow = Dummy[i]; + const int8_t Fistart = FrowMin[i]; + const int8_t Fiend = FrowMax[i]; + int8_t j; + + for (j = 0; j < NUMX; j++) { + Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ... + } + for (k = Fistart; k <= Fiend; k++) { + for (j = 0; j < NUMX; j++) { + Dirow[j] += Firow[k] * P[k][j]; // [] + F * P + } + } + } + for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G'] + float *Dirow = Dummy[i]; + float *Girow = G[i]; + float *Pirow = P[i]; + const int8_t Gistart = GrowMin[i]; + const int8_t Giend = GrowMax[i]; + int8_t j; + + + for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular + float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ... + + const float *Fjrow = F[j]; + int8_t Fjstart = FrowMin[j]; + int8_t Fjend = FrowMax[j]; + k = Fjstart; + + while (k <= Fjend - 3) { + Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ... + Ptmp += Dirow[k + 1] * Fjrow[k + 1]; + Ptmp += Dirow[k + 2] * Fjrow[k + 2]; + Ptmp += Dirow[k + 3] * Fjrow[k + 3]; + k += 4; + } + while (k <= Fjend) { + Ptmp += Dirow[k] * Fjrow[k]; + k++; + } + + float *Gjrow = G[j]; + const int8_t Gjstart = MAX(Gistart, GrowMin[j]); + const int8_t Gjend = MIN(Giend, GrowMax[j]); + k = Gjstart; + while (k <= Gjend - 2) { + Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ... + Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1]; + Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2]; + k += 3; + } + if (k <= Gjend) { + Ptmp += Q[k] * Girow[k] * Gjrow[k]; + if (k <= Gjend - 1) { + Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1]; + } + } + + P[j][i] = Pirow[j] = Ptmp * dTsq; // [] * (T^2) + } + } +} + +// ************* SerialUpdate ******************* +// Does the update step of the Kalman filter for the covariance and estimate +// Outputs are Xnew & Pnew, and are written over P and X +// Z is actual measurement, Y is predicted measurement +// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P, +// where K=P*H'*inv[H*P*H'+R] +// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal +// i.e. the measurment noises are uncorrelated. +// It therefore uses a serial update that requires no matrix inversion by +// processing the measurements one at a time. +// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253 +// - or see Simon, "Optimal State Estimation," 1st Ed, p.150 +// The SensorsUsed variable is a bitwise mask indicating which sensors +// should be used in the update. +// ************************************************ +void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed) +{ + float HP[NUMX], HPHR, Error; + uint8_t i, j, k, m; + float Km[NUMX]; + + // Iterate through all the possible measurements and apply the + // appropriate corrections + for (m = 0; m < NUMV; m++) { + if (SensorsUsed & (0x01 << m)) { // use this sensor for update + for (j = 0; j < NUMX; j++) { // Find Hp = H*P + HP[j] = 0; + } + + for (k = HrowMin[m]; k <= HrowMax[m]; k++) { + for (j = 0; j < NUMX; j++) { // Find Hp = H*P + HP[j] += H[m][k] * P[k][j]; + } + } + HPHR = R[m]; // Find HPHR = H*P*H' + R + for (k = HrowMin[m]; k <= HrowMax[m]; k++) { + HPHR += HP[k] * H[m][k]; + } + float invHPHR = 1.0f / HPHR; + for (k = 0; k < NUMX; k++) { + Km[k] = HP[k] * invHPHR; // find K = HP/HPHR + } + for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP + for (j = i; j < NUMX; j++) { + P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j]; + } + } + + Error = Z[m] - Y[m]; + for (i = 0; i < NUMX; i++) { // Find X(m)= X(m-1) + K*Error + X[i] = X[i] + Km[i] * Error; + } + } + } +} + +// ************* RungeKutta ********************** +// Does a 4th order Runge Kutta numerical integration step +// Output, Xnew, is written over X +// NOTE the algorithm assumes time invariant state equations and +// constant inputs over integration step +// ************************************************ + +void RungeKutta(float X[NUMX], float U[NUMU], float dT) +{ + const float dT2 = dT / 2.0f; + float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; + uint8_t i; + + for (i = 0; i < NUMX; i++) { + Xlast[i] = X[i]; // make a working copy + } + StateEq(X, U, K1); // k1 = f(x,u) + for (i = 0; i < NUMX; i++) { + X[i] = Xlast[i] + dT2 * K1[i]; + } + StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u) + for (i = 0; i < NUMX; i++) { + X[i] = Xlast[i] + dT2 * K2[i]; + } + StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u) + for (i = 0; i < NUMX; i++) { + X[i] = Xlast[i] + dT * K3[i]; + } + StateEq(X, U, K4); // k4 = f(x+dT*k3,u) + + // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6 + for (i = 0; i < NUMX; i++) { + X[i] = + Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] + + K4[i]) * (1.0f / 6.0f); + } +} + +// ************* Model Specific Stuff *************************** +// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ******** +// +// State Variables = [Pos Vel Quaternion GyroBias AccelBias] +// Deterministic Inputs = [AngularVel Accel] +// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise] +// +// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter] +// Inputs to Measurement = [EarthFrameMagField] +// +// Notes: Pos and Vel in earth frame +// AngularVel and Accel in body frame +// MagFields are unit vectors +// Xdot is output of StateEq() +// F and G are outputs of LinearizeFG(), all elements not set should be zero +// y is output of OutputEq() +// H is output of LinearizeH(), all elements not set should be zero +// ************************************************ + +void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) +{ + const float wx = U[0] - X[10]; + const float wy = U[1] - X[11]; + const float wz = U[2] - X[12]; // subtract the biases on gyros + const float ax = U[3]; + const float ay = U[4]; + const float az = U[5] - X[13]; // subtract the biases on accels + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // Pdot = V + Xdot[0] = X[3]; + Xdot[1] = X[4]; + Xdot[2] = X[5]; + + // Vdot = Reb*a + Xdot[3] = + (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 - + q0 * q3) * + ay + 2.0f * (q1 * q3 + q0 * q2) * az; + Xdot[4] = + 2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 - + q3 * q3) * ay + 2.0f * (q2 * q3 - + q0 * q1) * + az; + Xdot[5] = + 2.0f * (q1 * q3 - q0 * q2) * ax + 2.0f * (q2 * q3 + q0 * q1) * ay + + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + PIOS_CONST_MKS_GRAV_ACCEL_F; + + // qdot = Q*w + Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f; + Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f; + Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f; + Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f; + + // best guess is that bias stays constant + Xdot[10] = Xdot[11] = Xdot[12] = 0; + + // For accels to make sure things stay stable, assume bias always walks weakly + // towards zero for the horizontal axis. This prevents drifting around an + // unobservable manifold of possible attitudes and gyro biases. The z-axis + // we assume no drift because this is the one we want to estimate most accurately. + Xdot[13] = 0.0f; +} + +/** + * Linearize the state equations around the current state estimate. + * @param[in] X the current state estimate + * @param[in] U the control inputs + * @param[out] F the linearized natural dynamics + * @param[out] G the linearized influence of disturbance model + * + * so the prediction of the next state is + * Xdot = F * X + G * U + * where X is the current state and U is the current input + * + * For reference the state order (in F) is pos, vel, attitude, gyro bias, accel bias + * and the input order is gyro, bias + */ +void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], + float G[NUMX][NUMW]) +{ + const float wx = U[0] - X[10]; + const float wy = U[1] - X[11]; + const float wz = U[2] - X[12]; // subtract the biases on gyros + const float ax = U[3]; + const float ay = U[4]; + const float az = U[5] - X[13]; // subtract the biases on accels + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // Pdot = V + F[0][3] = F[1][4] = F[2][5] = 1.0f; + + // dVdot/dq + F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az); + F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az); + F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az); + F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az); + F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az); + F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az); + F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az); + F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az); + F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az); + F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az); + F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az); + F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az); + + // dVdot/dabias & dVdot/dna - the equations for how the accel input and accel bias influence velocity are the same + F[3][13] = G[3][5] = -2.0f * (q1 * q3 + q0 * q2); + F[4][13] = G[4][5] = 2.0f * (-q2 * q3 + q0 * q1); + F[5][13] = G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3; + + // dqdot/dq + F[6][6] = 0; + F[6][7] = -wx / 2.0f; + F[6][8] = -wy / 2.0f; + F[6][9] = -wz / 2.0f; + F[7][6] = wx / 2.0f; + F[7][7] = 0; + F[7][8] = wz / 2.0f; + F[7][9] = -wy / 2.0f; + F[8][6] = wy / 2.0f; + F[8][7] = -wz / 2.0f; + F[8][8] = 0; + F[8][9] = wx / 2.0f; + F[9][6] = wz / 2.0f; + F[9][7] = wy / 2.0f; + F[9][8] = -wx / 2.0f; + F[9][9] = 0; + + // dqdot/dwbias + F[6][10] = q1 / 2.0f; + F[6][11] = q2 / 2.0f; + F[6][12] = q3 / 2.0f; + F[7][10] = -q0 / 2.0f; + F[7][11] = q3 / 2.0f; + F[7][12] = -q2 / 2.0f; + F[8][10] = -q3 / 2.0f; + F[8][11] = -q0 / 2.0f; + F[8][12] = q1 / 2.0f; + F[9][10] = q2 / 2.0f; + F[9][11] = -q1 / 2.0f; + F[9][12] = -q0 / 2.0f; + + // dVdot/dna + G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3; + G[3][4] = 2 * (-q1 * q2 + q0 * q3); + // G[3][5] = -2 * (q1 * q3 + q0 * q2); // already assigned above + G[4][3] = -2 * (q1 * q2 + q0 * q3); + G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3; + // G[4][5] = 2 * (-q2 * q3 + q0 * q1); // already assigned above + G[5][3] = 2 * (-q1 * q3 + q0 * q2); + G[5][4] = -2 * (q2 * q3 + q0 * q1); + // G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3; // already assigned above + + // dqdot/dnw + G[6][0] = q1 / 2.0f; + G[6][1] = q2 / 2.0f; + G[6][2] = q3 / 2.0f; + G[7][0] = -q0 / 2.0f; + G[7][1] = q3 / 2.0f; + G[7][2] = -q2 / 2.0f; + G[8][0] = -q3 / 2.0f; + G[8][1] = -q0 / 2.0f; + G[8][2] = q1 / 2.0f; + G[9][0] = q2 / 2.0f; + G[9][1] = -q1 / 2.0f; + G[9][2] = -q0 / 2.0f; +} + +/** + * Predicts the measurements from the current state. Note + * that this is very similar to @ref LinearizeH except this + * directly computes the outputs instead of a matrix that + * you transform the state by + */ +void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]) +{ + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // first six outputs are P and V + Y[0] = X[0]; + Y[1] = X[1]; + Y[2] = X[2]; + Y[3] = X[3]; + Y[4] = X[4]; + Y[5] = X[5]; + + // Rotate Be by only the yaw heading + const float a1 = 2 * q0 * q3 + 2 * q1 * q2; + const float a2 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; + const float r = sqrtf(a1 * a1 + a2 * a2); + const float cP = a2 / r; + const float sP = a1 / r; + Y[6] = Be[0] * cP + Be[1] * sP; + Y[7] = -Be[0] * sP + Be[1] * cP; + Y[8] = 0; // don't care + + // Alt = -Pz + Y[9] = X[2] * -1.0f; +} + +/** + * Linearize the measurement around the current state estiamte + * so the predicted measurements are + * Z = H * X + */ +void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]) +{ + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // dP/dP=I; (expect position to measure the position) + H[0][0] = H[1][1] = H[2][2] = 1.0f; + // dV/dV=I; (expect velocity to measure the velocity) + H[3][3] = H[4][4] = H[5][5] = 1.0f; + + // dBb/dq (expected magnetometer readings in the horizontal plane) + // these equations were generated by Rhb(q)*Be which is the matrix that + // rotates the earth magnetic field into the horizontal plane, and then + // taking the partial derivative wrt each term in q. Maniuplated in + // matlab symbolic toolbox + const float Be_0 = Be[0]; + const float Be_1 = Be[1]; + const float a1 = q0 * q3 * 2.0f + q1 * q2 * 2.0f; + const float a1s = a1 * a1; + const float a2 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; + const float a2s = a2 * a2; + const float a3 = 1.0f / powf(a1s + a2s, 3.0f / 2.0f) * (1.0f / 2.0f); + + const float k1 = 1.0f / sqrtf(a1s + a2s); + const float k3 = a3 * a2; + const float k4 = a2 * 4.0f; + const float k5 = a1 * 4.0f; + const float k6 = a3 * a1; + + H[6][6] = Be_0 * q0 * k1 * 2.0f + Be_1 * q3 * k1 * 2.0f - Be_0 * (q0 * k4 + q3 * k5) * k3 - Be_1 * (q0 * k4 + q3 * k5) * k6; + H[6][7] = Be_0 * q1 * k1 * 2.0f + Be_1 * q2 * k1 * 2.0f - Be_0 * (q1 * k4 + q2 * k5) * k3 - Be_1 * (q1 * k4 + q2 * k5) * k6; + H[6][8] = Be_0 * q2 * k1 * -2.0f + Be_1 * q1 * k1 * 2.0f + Be_0 * (q2 * k4 - q1 * k5) * k3 + Be_1 * (q2 * k4 - q1 * k5) * k6; + H[6][9] = Be_1 * q0 * k1 * 2.0f - Be_0 * q3 * k1 * 2.0f + Be_0 * (q3 * k4 - q0 * k5) * k3 + Be_1 * (q3 * k4 - q0 * k5) * k6; + H[7][6] = Be_1 * q0 * k1 * 2.0f - Be_0 * q3 * k1 * 2.0f - Be_1 * (q0 * k4 + q3 * k5) * k3 + Be_0 * (q0 * k4 + q3 * k5) * k6; + H[7][7] = Be_0 * q2 * k1 * -2.0f + Be_1 * q1 * k1 * 2.0f - Be_1 * (q1 * k4 + q2 * k5) * k3 + Be_0 * (q1 * k4 + q2 * k5) * k6; + H[7][8] = Be_0 * q1 * k1 * -2.0f - Be_1 * q2 * k1 * 2.0f + Be_1 * (q2 * k4 - q1 * k5) * k3 - Be_0 * (q2 * k4 - q1 * k5) * k6; + H[7][9] = Be_0 * q0 * k1 * -2.0f - Be_1 * q3 * k1 * 2.0f + Be_1 * (q3 * k4 - q0 * k5) * k3 - Be_0 * (q3 * k4 - q0 * k5) * k6; + H[8][6] = 0.0f; + H[8][7] = 0.0f; + H[8][9] = 0.0f; + + // dAlt/dPz = -1 (expected baro readings) + H[9][2] = -1.0f; +} + +/** + * @} + * @} + */ diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index 6e962c306f..c97af3a6c4 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -1,12 +1,8 @@ /** ****************************************************************************** - * @addtogroup OpenPilot Math Utilities - * @{ - * @addtogroup Sine and cosine methods that use a cached lookup table - * @{ - * * @file pid.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Methods to work with PID structure * * @see The GNU Public License (GPL) Version 3 @@ -159,7 +155,7 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) * @param[in] kp proportional gain * @param[in] ki integral gain. Time constant Ti = kp/ki * @param[in] kd derivative gain. Time constant Td = kd/kp - * @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20 + * @param[in] Tf filtering time = (kd/kp)/N, N is in the range of 2 to 20 * @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2 * @param[in] dt delta time increment * @param[in] beta setpoint weight on setpoint in P component. beta=1 error feedback. beta=0 smoothes out response to changes in setpoint @@ -220,7 +216,20 @@ float pid2_apply( pid->D = 0.0f; // t=0, u=u0, y=y0, v=u - pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y); + // pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y); + // this is dangerous, if pid->I starts nonzero with very low or zero kI, then + // it will never settle! + // pid->I = 0.0f; + // whether ki is zero or non-zero, when doing 'reconfigure' we start with setpoint==actual + pid->I = (pid->u0 - pid->va) / pid->vb; + // if ki is non-zero + if (pid->bi > 5e-7f) { + // then the output can wind up/down to get rid of a difference between setpoint and actual + // so we can generate the full bumpless transfer here + // else leave off the part that can vary due to difference between setpoint and actual + // so that it is repeatable (for tuning purposes at least) + pid->I -= pid->kp * (pid->beta * r - y); + } } // compute proportional part diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index b6d9e2ae66..a6ef5dcfe6 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -1,12 +1,8 @@ /** ****************************************************************************** - * @addtogroup OpenPilot Math Utilities - * @{ - * @addtogroup Sine and cosine methods that use a cached lookup table - * @{ - * * @file pid.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Methods to work with PID structure * * @see The GNU Public License (GPL) Version 3 @@ -46,17 +42,17 @@ struct pid { // pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control struct pid2 { - float u0; - float va; + float u0; // Initial value of r & y - for bumpless transfer + float va; // Actuator settings for scaling float vb; - float kp; - float bi; - float ad; - float bd; - float br; - float beta; - float yold; - float P; + float kp; // proportional gain + float bi; // Integral gain . dT + float ad; // Filtered factor for derivative calculation + float bd; // Constant for derivative calculation + float br; // Time constant for integral calculation + float beta; // Scalar for proportional factor + float yold; // t-1 y value for Integral calculation + float P; // Latest calculated P, I & D values float I; float D; uint8_t reconfigure; diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index f68504048e..b60fd4d8d0 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -42,6 +42,18 @@ #define ALARM_LED_OFF() #endif +#ifdef PIOS_BUZZER_ALARM +#define ALARM_BUZZER_ON() \ + { if (buzzer_enabled) { PIOS_LED_On(PIOS_BUZZER_ALARM); } \ + } +#define ALARM_BUZZER_OFF() \ + { if (buzzer_enabled) { PIOS_LED_Off(PIOS_BUZZER_ALARM); } \ + } +#else +#define ALARM_BUZZER_ON() +#define ALARM_BUZZER_OFF() +#endif + #ifdef PIOS_LED_HEARTBEAT #define HEARTBEAT_LED_ON() PIOS_LED_On(PIOS_LED_HEARTBEAT) #define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT) @@ -135,9 +147,10 @@ static volatile FlightStatusData currentFlightStatus; static volatile bool started = false; static volatile pios_notify_notification nextNotification = NOTIFY_NONE; -#ifdef PIOS_LED_ALARM + +#if defined(PIOS_LED_ALARM) || defined(PIOS_BUZZER_ALARM) static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern); -#endif // PIOS_LED_ALARM +#endif // PIOS_LED_ALARM || PIOS_BUZZER_ALARM static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern); static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern); static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern); @@ -163,6 +176,10 @@ void NotificationOnboardLedsRun() static uint8_t lastFlightMode = -1; static bool forceShowFlightMode = false; static pios_notify_notification runningNotification = NOTIFY_NONE; + +#ifdef PIOS_BUZZER_ALARM + static bool buzzer_enabled = false; +#endif static enum { STATUS_NOTIFY, STATUS_ALARM, @@ -206,6 +223,7 @@ void NotificationOnboardLedsRun() if (cycleCount & 0x10) { HEARTBEAT_LED_OFF(); ALARM_LED_OFF(); + ALARM_BUZZER_OFF(); cycleCount = 0x0; forceShowFlightMode = false; // Notification has been just shown, cleanup @@ -213,6 +231,9 @@ void NotificationOnboardLedsRun() runningNotification = NOTIFY_NONE; } status = (status + 1) % STATUS_LENGHT; +#ifdef PIOS_BUZZER_ALARM + buzzer_enabled = true; /* This is the place where we update buzzer_enabled status based on (not yet implemented) NotificationSettings.Buzzer config */ +#endif } if (status == STATUS_NOTIFY) { @@ -224,7 +245,7 @@ void NotificationOnboardLedsRun() // Handles Alarm display if (status == STATUS_ALARM) { -#ifdef PIOS_LED_ALARM +#if defined(PIOS_LED_ALARM) || defined(PIOS_BUZZER_ALARM) if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) { // no alarms, advance status++; @@ -252,14 +273,16 @@ void NotificationOnboardLedsRun() } if (r_pattern & 0x1) { ALARM_LED_ON(); + ALARM_BUZZER_ON(); } else { ALARM_LED_OFF(); + ALARM_BUZZER_OFF(); } r_pattern >>= 1; b_pattern >>= 1; } -#if defined(PIOS_LED_ALARM) +#if defined(PIOS_LED_ALARM) || defined(PIOS_BUZZER_ALARM) static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern) { if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) { @@ -269,7 +292,7 @@ static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern) *r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel); return true; } -#endif /* PIOS_LED_ALARM */ +#endif /* PIOS_LED_ALARM || PIOS_BUZZER_ALARM */ static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern) diff --git a/flight/libraries/op_dfu.c b/flight/libraries/op_dfu.c index d10b351e61..3cfb03a5b1 100644 --- a/flight/libraries/op_dfu.c +++ b/flight/libraries/op_dfu.c @@ -157,7 +157,8 @@ void processComand(uint8_t *xReceive_Buffer) switch (Command) { case EnterDFU: if (((DeviceState == BLidle) && (Data0 < numberOfDevices)) - || (DeviceState == DFUidle)) { + || (DeviceState == DFUidle) + || (DeviceState == failed_jump)) { if (Data0 > 0) { OPDfuIni(true); } diff --git a/flight/libraries/pid/pidcontroldown.cpp b/flight/libraries/pid/pidcontroldown.cpp index de86bba5b4..0a1455776b 100644 --- a/flight/libraries/pid/pidcontroldown.cpp +++ b/flight/libraries/pid/pidcontroldown.cpp @@ -7,7 +7,8 @@ * @{ * * @file pidcontroldown.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief Executes PID control for down direction * * @see The GNU Public License (GPL) Version 3 @@ -31,8 +32,6 @@ extern "C" { #include -#include - #include #include #include @@ -92,25 +91,35 @@ void PIDControlDown::Activate() void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax) { - // pid_configure(&PID, kp, ki, kd, ilimit); - float Ti = kp / ki; - float Td = kd / kp; - float Tt = (Ti + Td) / 2.0f; - float kt = 1.0f / Tt; - float N = 10.0f; - float Tf = Td / N; - - if (ki < 1e-6f) { - // Avoid Ti being infinite - Ti = 1e6f; - // Tt antiwindup time constant - we don't need antiwindup with no I term - Tt = 1e6f; + float Td; // Derivative time constant + float Ti; // Integral time constant + float kt; // Feedback gain for integral windup avoidance + float N = 10.0f; // N is the factor used to determine the + // time constant for derivative filtering + // Why 10? Maybe should be configurable? + float Tf; // Low pass filter time constant for derivative filtering + + // Define Td, handling zero kp term (for I or ID controller) + if (kp < 1e-6f) { + Td = 1e6f; + } else { + Td = kd / kp; + } + + // Define Ti, Tt and kt, handling zero ki term (for P or PD controller) + if (ki < 1e-6f) { // Avoid Ti being infinite kt = 0.0f; + } else { + Ti = kp / ki; + kt = 1.0f / Ti; } + // Define Tf, according to controller type if (kd < 1e-6f) { // PI Controller or P Controller - Tf = Ti / N; + Tf = 0; + } else { + Tf = Td / N; } if (beta > 1.0f) { diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 6e5db84425..f2c9d23b75 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -80,15 +80,12 @@ static float applyExpo(float value, float expo) */ void plan_initialize() { - TakeOffLocationInitialize(); PositionStateInitialize(); PathDesiredInitialize(); - FlightModeSettingsInitialize(); FlightStatusInitialize(); AttitudeStateInitialize(); ManualControlCommandInitialize(); VelocityStateInitialize(); - VtolPathFollowerSettingsInitialize(); StabilizationBankInitialize(); StabilizationDesiredInitialize(); } diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 8c019e38e1..7bd0f11fab 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -41,7 +41,6 @@ #include #include #include -#include // a number of useful macros #define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL)) @@ -74,13 +73,13 @@ int32_t configuration_check() // Classify navigation capability #ifdef REVOLUTION - RevoSettingsInitialize(); RevoSettingsFusionAlgorithmOptions revoFusion; RevoSettingsFusionAlgorithmGet(&revoFusion); bool navCapableFusion; switch (revoFusion) { case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: + case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: navCapableFusion = true; break; default: @@ -93,7 +92,7 @@ int32_t configuration_check() } } } -#else +#else /* ifdef REVOLUTION */ const bool navCapableFusion = false; #endif /* ifdef REVOLUTION */ @@ -363,6 +362,8 @@ FrameType_t GetCurrentFrameType() case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR: case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL: case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE: + case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEBOAT: + case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIALBOAT: return FRAME_TYPE_GROUND; case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL: diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index d4d89ce31f..77620dabf1 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -54,14 +54,16 @@ include $(FREERTOS_DIR)/library.mk OPTESTS = $(TOPDIR)/Tests ## PIOS Hardware -ifeq ($(MCU),cortex-m3) +ifneq (,$(findstring STM32F10,$(CHIP))) include $(PIOS)/stm32f10x/library.mk -else ifeq ($(MCU),cortex-m4) +else ifneq (,$(findstring STM32F4,$(CHIP))) include $(PIOS)/stm32f4xx/library.mk -else ifeq ($(MCU),cortex-m0) +else ifneq (,$(findstring STM32F30,$(CHIP))) + include $(PIOS)/stm32f30x/library.mk +else ifneq (,$(findstring STM32F0,$(CHIP))) include $(PIOS)/stm32f0x/library.mk else - $(error Unsupported MCU: $(MCU)) + $(error Unsupported CHIP: $(CHIP)) endif # List C source files here (C dependencies are automatically generated). @@ -82,13 +84,15 @@ SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_mpu9250.c SRC += $(PIOSCOMMON)/pios_mpxv.c SRC += $(PIOSCOMMON)/pios_ms4525do.c -SRC += $(PIOSCOMMON)/pios_ms5611.c +SRC += $(PIOSCOMMON)/pios_ms56xx.c +SRC += $(PIOSCOMMON)/pios_bmp280.c SRC += $(PIOSCOMMON)/pios_oplinkrcvr.c SRC += $(PIOSCOMMON)/pios_video.c 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 @@ -96,8 +100,11 @@ SRC += $(PIOSCOMMON)/pios_exbus.c SRC += $(PIOSCOMMON)/pios_ibus.c SRC += $(PIOSCOMMON)/pios_sdcard.c 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 +SRC += $(PIOSCOMMON)/pios_board_sensors.c ## Misc library functions SRC += $(FLIGHTLIB)/sanitycheck.c @@ -146,6 +153,11 @@ ifeq ($(USE_CXX),YES) CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp endif +ifeq ($(DEBUG), YES) +SRC += $(FLIGHTLIB)/dcc_stdio.c +SRC += $(FLIGHTLIB)/cm3_fault_handlers.c +endif + ## Modules SRC += $(foreach mod, $(MODULES), $(sort $(wildcard $(OPMODULEDIR)/$(mod)/*.c))) CPPSRC += $(foreach mod, $(MODULES), $(sort $(wildcard $(OPMODULEDIR)/$(mod)/*.cpp))) @@ -157,6 +169,10 @@ MODNAMES := $(notdir $(subst /revolution,,$(MODULES))) MODULES_BUILTIN := $(foreach mod, $(MODNAMES), -DMODULE_$(shell echo $(mod) | tr '[:lower:]' '[:upper:]')_BUILTIN) CDEFS += $(MODULES_BUILTIN) +MODNAMES_ALL := $(notdir $(subst /revolution,,$(OPTMODULES) $(MODULES))) +MODULES_ALL := $(foreach mod, $(MODNAMES_ALL), -DHAS_$(shell echo $(mod) | tr '[:lower:]' '[:upper:]')_MODULE) +CDEFS += $(MODULES_ALL) + # List C source files here which must be compiled in ARM-Mode (no -mthumb). # Use file-extension c for "c-only"-files SRCARM += diff --git a/flight/make/boot-defs.mk b/flight/make/boot-defs.mk index 42629af9c4..588f6e2436 100644 --- a/flight/make/boot-defs.mk +++ b/flight/make/boot-defs.mk @@ -34,14 +34,16 @@ FLIGHTLIBINC = $(FLIGHTLIB)/inc override USE_DSP_LIB := NO ## PIOS Hardware -ifeq ($(MCU),cortex-m3) +ifneq (,$(findstring STM32F10,$(CHIP))) include $(PIOS)/stm32f10x/library.mk -else ifeq ($(MCU),cortex-m4) +else ifneq (,$(findstring STM32F4,$(CHIP))) include $(PIOS)/stm32f4xx/library.mk -else ifeq ($(MCU),cortex-m0) +else ifneq (,$(findstring STM32F30,$(CHIP))) + include $(PIOS)/stm32f30x/library.mk +else ifneq (,$(findstring STM32F0,$(CHIP))) include $(PIOS)/stm32f0x/library.mk else - $(error Unsupported MCU: $(MCU)) + $(error Unsupported CHIP: $(CHIP)) endif # List C source files here (C dependencies are automatically generated). diff --git a/flight/make/common-defs.mk b/flight/make/common-defs.mk index 76b26e9616..933aedc8fb 100644 --- a/flight/make/common-defs.mk +++ b/flight/make/common-defs.mk @@ -152,6 +152,14 @@ ifdef USER_EE_BANK_BASE BOARD_CDEFS += -DUSER_EE_BANK_BASE=$(USER_EE_BANK_BASE) BOARD_CDEFS += -DUSER_EE_BANK_SIZE=$(USER_EE_BANK_SIZE) endif +ifdef SRAM_BANK_BASE +BOARD_CDEFS += -DSRAM_BANK_BASE=$(SRAM_BANK_BASE) +BOARD_CDEFS += -DSRAM_BANK_SIZE=$(SRAM_BANK_SIZE) +endif +ifdef CCSRAM_BANK_BASE +BOARD_CDEFS += -DCCSRAM_BANK_BASE=$(CCSRAM_BANK_BASE) +BOARD_CDEFS += -DCCSRAM_BANK_SIZE=$(CCSRAM_BANK_SIZE) +endif CDEFS += $(BOARD_CDEFS) ifeq ($(DEBUG), YES) @@ -214,6 +222,8 @@ ALLSRCBASE = $(notdir $(basename $(ALLSRC))) # Define all object files. ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE))) $(EXTRAOBJ) +ALLLD = $(addprefix $(OUTDIR)/, $(addsuffix .ld, $(notdir $(basename $(LDSRC))))) + # Define all listing files (used for make clean). LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE))) # Define all depedency-files (used for make clean). @@ -241,9 +251,9 @@ endif # Link: create ELF output file from object files. ifeq ($(USE_CXX), YES) -$(eval $(call LINK_CXX_TEMPLATE,$(OUTDIR)/$(TARGET).elf,$(ALLOBJ),$(ALLLIB))) +$(eval $(call LINK_CXX_TEMPLATE,$(OUTDIR)/$(TARGET).elf,$(ALLOBJ),$(ALLLIB),$(ALLLD))) else -$(eval $(call LINK_TEMPLATE,$(OUTDIR)/$(TARGET).elf,$(ALLOBJ),$(ALLLIB))) +$(eval $(call LINK_TEMPLATE,$(OUTDIR)/$(TARGET).elf,$(ALLOBJ),$(ALLLIB),$(ALLLD))) endif # Assemble: create object files from assembler source files. @@ -272,6 +282,9 @@ $(eval $(call PARTIAL_COMPILE_TEMPLATE,SRC)) # Compile: create assembler files from C source files. ARM only $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE,SRCARM)) +# Preprocess: create linker scripts from .lds files. +$(foreach src, $(LDSRC), $(eval $(call PREPROCESS_LDS_TEMPLATE,$(src)))) + # Add opfw target $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) diff --git a/flight/make/firmware-defs.mk b/flight/make/firmware-defs.mk index 14873835ee..a77932ede3 100644 --- a/flight/make/firmware-defs.mk +++ b/flight/make/firmware-defs.mk @@ -41,32 +41,33 @@ else endif # Define Messages -MSG_FORMATERROR = $(QUOTE) Can not handle output-format$(QUOTE) -MSG_MODINIT = $(QUOTE) MODINIT $(MSG_EXTRA) $(QUOTE) -MSG_SIZE = $(QUOTE) SIZE $(MSG_EXTRA) $(QUOTE) -MSG_LOAD_FILE = $(QUOTE) BIN/HEX $(MSG_EXTRA) $(QUOTE) -MSG_BIN_OBJ = $(QUOTE) BINO $(MSG_EXTRA) $(QUOTE) -MSG_STRIP_FILE = $(QUOTE) STRIP $(MSG_EXTRA) $(QUOTE) -MSG_EXTENDED_LISTING = $(QUOTE) LIS $(MSG_EXTRA) $(QUOTE) -MSG_SYMBOL_TABLE = $(QUOTE) NM $(MSG_EXTRA) $(QUOTE) -MSG_ARCHIVING = $(QUOTE) AR $(MSG_EXTRA) $(QUOTE) -MSG_LINKING = $(QUOTE) LD $(MSG_EXTRA) $(QUOTE) -MSG_COMPILING = $(QUOTE) CC $(MSG_EXTRA) $(QUOTE) -MSG_COMPILING_ARM = $(QUOTE) CC-ARM $(MSG_EXTRA) $(QUOTE) -MSG_COMPILINGCXX = $(QUOTE) CXX $(MSG_EXTRA) $(QUOTE) -MSG_COMPILINGCXX_ARM = $(QUOTE) CXX-ARM $(MSG_EXTRA) $(QUOTE) -MSG_ASSEMBLING = $(QUOTE) AS $(MSG_EXTRA) $(QUOTE) -MSG_ASSEMBLING_ARM = $(QUOTE) AS-ARM $(MSG_EXTRA) $(QUOTE) -MSG_CLEANING = $(QUOTE) CLEAN $(MSG_EXTRA) $(QUOTE) -MSG_ASMFROMC = $(QUOTE) AS(C) $(MSG_EXTRA) $(QUOTE) -MSG_ASMFROMC_ARM = $(QUOTE) AS(C)-ARM $(MSG_EXTRA) $(QUOTE) -MSG_PYMITEINIT = $(QUOTE) PY $(MSG_EXTRA) $(QUOTE) -MSG_OPFIRMWARE = $(QUOTE) OPFW $(MSG_EXTRA) $(QUOTE) -MSG_FWINFO = $(QUOTE) FWINFO $(MSG_EXTRA) $(QUOTE) -MSG_JTAG_PROGRAM = $(QUOTE) JTAG-PGM $(MSG_EXTRA) $(QUOTE) -MSG_JTAG_WIPE = $(QUOTE) JTAG-WIPE $(MSG_EXTRA) $(QUOTE) -MSG_PADDING = $(QUOTE) PADDING $(MSG_EXTRA) $(QUOTE) -MSG_FLASH_IMG = $(QUOTE) FLASH_IMG $(MSG_EXTRA) $(QUOTE) +MSG_FORMATERROR = $(QUOTE) Can not handle output-format$(QUOTE) +MSG_MODINIT = $(QUOTE) MODINIT $(MSG_EXTRA) $(QUOTE) +MSG_SIZE = $(QUOTE) SIZE $(MSG_EXTRA) $(QUOTE) +MSG_LOAD_FILE = $(QUOTE) BIN/HEX $(MSG_EXTRA) $(QUOTE) +MSG_BIN_OBJ = $(QUOTE) BINO $(MSG_EXTRA) $(QUOTE) +MSG_STRIP_FILE = $(QUOTE) STRIP $(MSG_EXTRA) $(QUOTE) +MSG_EXTENDED_LISTING = $(QUOTE) LIS $(MSG_EXTRA) $(QUOTE) +MSG_SYMBOL_TABLE = $(QUOTE) NM $(MSG_EXTRA) $(QUOTE) +MSG_ARCHIVING = $(QUOTE) AR $(MSG_EXTRA) $(QUOTE) +MSG_LINKING = $(QUOTE) LD $(MSG_EXTRA) $(QUOTE) +MSG_COMPILING = $(QUOTE) CC $(MSG_EXTRA) $(QUOTE) +MSG_COMPILING_ARM = $(QUOTE) CC-ARM $(MSG_EXTRA) $(QUOTE) +MSG_COMPILINGCXX = $(QUOTE) CXX $(MSG_EXTRA) $(QUOTE) +MSG_COMPILINGCXX_ARM = $(QUOTE) CXX-ARM $(MSG_EXTRA) $(QUOTE) +MSG_ASSEMBLING = $(QUOTE) AS $(MSG_EXTRA) $(QUOTE) +MSG_ASSEMBLING_ARM = $(QUOTE) AS-ARM $(MSG_EXTRA) $(QUOTE) +MSG_CLEANING = $(QUOTE) CLEAN $(MSG_EXTRA) $(QUOTE) +MSG_ASMFROMC = $(QUOTE) AS(C) $(MSG_EXTRA) $(QUOTE) +MSG_ASMFROMC_ARM = $(QUOTE) AS(C)-ARM $(MSG_EXTRA) $(QUOTE) +MSG_PYMITEINIT = $(QUOTE) PY $(MSG_EXTRA) $(QUOTE) +MSG_OPFIRMWARE = $(QUOTE) OPFW $(MSG_EXTRA) $(QUOTE) +MSG_FWINFO = $(QUOTE) FWINFO $(MSG_EXTRA) $(QUOTE) +MSG_JTAG_PROGRAM = $(QUOTE) JTAG-PGM $(MSG_EXTRA) $(QUOTE) +MSG_JTAG_WIPE = $(QUOTE) JTAG-WIPE $(MSG_EXTRA) $(QUOTE) +MSG_PADDING = $(QUOTE) PADDING $(MSG_EXTRA) $(QUOTE) +MSG_FLASH_IMG = $(QUOTE) FLASH_IMG $(MSG_EXTRA) $(QUOTE) +MSG_PREPROCESSING_LDS = $(QUOTE) PP $(MSG_EXTRA) $(QUOTE) # Function for converting an absolute path to one relative # to the top of the source tree. @@ -175,6 +176,13 @@ $(OUTDIR)/$(notdir $(basename $(1))).o : $(1) $(V1) $(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$(CPPFLAGS) $$< -o $$@ endef +# Preprocess: create linker scripts from .lds files. +define PREPROCESS_LDS_TEMPLATE +$(OUTDIR)/$(notdir $(basename $(1))).ld : $(1) + @echo $(MSG_PREPROCESSING_LDS) $$(call toprel, $$<) + $(V1) $(CXX) -E -P -x c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@ +endef + # Compile: create object files from C source files. ARM-only define COMPILE_C_ARM_TEMPLATE $(OUTDIR)/$(notdir $(basename $(1))).o : $(1) @@ -224,13 +232,16 @@ endef # $1 = elf file to produce # $2 = list of object files that make up the elf file # $3 = optional list of libraries to build and link +# $4 = optional list of linker scripts to build and link define LINK_TEMPLATE .SECONDARY : $(1) -.PRECIOUS : $(2) $(3) -$(1).input_files: $(2) $(3) +.PRECIOUS : $(2) $(3) $(4) +$(1).input_files: $(2) $(3) $(4) $(V1) rm -rf $(1).input_files $(foreach file,$(2) $(3), $(V1) echo $(file) >> $$@) + $(foreach file,$(4), + $(V1) echo -T$(file) >> $$@) $(1): $(1).input_files @echo $(MSG_LINKING) $$(call toprel, $$@) @@ -240,13 +251,17 @@ endef # Link: create ELF output file from object files. # $1 = elf file to produce # $2 = list of object files that make up the elf file +# $3 = optional list of libraries to build and link +# $4 = optional list of linker scripts to build and link define LINK_CXX_TEMPLATE .SECONDARY : $(1) -.PRECIOUS : $(2) $(3) -$(1).input_files: $(2) $(3) +.PRECIOUS : $(2) $(3) $(4) +$(1).input_files: $(2) $(3) $(4) $(V1) rm -rf $(1).input_files $(foreach file,$(2) $(3), $(V1) echo $(file) >> $$@) + $(foreach file,$(4), + $(V1) echo -T$(file) >> $$@) $(1): $(1).input_files @echo $(MSG_LINKING) $$(call toprel, $$@) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index a2a66838f5..3cc35e19b8 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -89,6 +89,7 @@ static xTaskHandle taskHandle; static FrameType_t frameType = FRAME_TYPE_MULTIROTOR; static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE; static bool camStabEnabled; +static bool camControlEnabled; static uint8_t pinsMode[MAX_MIX_ACTUATORS]; // used to inform the actuator thread that actuator update rate is changed @@ -146,11 +147,9 @@ int32_t ActuatorStart() int32_t ActuatorInitialize() { // Register for notification of changes to ActuatorSettings - ActuatorSettingsInitialize(); ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); // Register for notification of changes to MixerSettings - MixerSettingsInitialize(); MixerSettingsConnectCallback(MixerSettingsUpdatedCb); // Listen for ActuatorDesired updates (Primary input to this module) @@ -163,10 +162,9 @@ int32_t ActuatorInitialize() // Check if CameraStab module is enabled HwSettingsOptionalModulesData optionalModules; - HwSettingsInitialize(); HwSettingsOptionalModulesGet(&optionalModules); - camStabEnabled = (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED); - + camStabEnabled = (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED); + camControlEnabled = (optionalModules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED); // Primary output of this module ActuatorCommandInitialize(); @@ -176,10 +174,8 @@ int32_t ActuatorInitialize() #endif #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES - VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); #endif - SystemSettingsInitialize(); SystemSettingsConnectCallback(&SettingsUpdatedCb); return 0; @@ -483,6 +479,14 @@ static void actuatorTask(__attribute__((unused)) void *parameters) command.Channel[ct] = 0; } } + + if (mixer_type == MIXERSETTINGS_MIXER1TYPE_CAMERATRIGGER) { + if (camControlEnabled) { + CameraDesiredTriggerGet(&status[ct]); + } else { + status[ct] = 0; + } + } } // If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value. @@ -951,6 +955,13 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) // Remap 1000-2000 range to 5-25µs PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], (value * ACTUATOR_MULTISHOT_PULSE_FACTOR) - 180); break; + case ACTUATORSETTINGS_BANKMODE_DSHOT: + // Remap 0-2000 range to: 0 = disarmed, 1 to 47 = Reserved for special commands, 48 to 2047 = Active throttle control. + if (value > 0) { + value += 47; /* skip over reserved values */ + } + PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value); + break; default: PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value); break; @@ -981,9 +992,15 @@ static void actuator_update_rate_if_changed(bool force_update) { static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM]; + static uint16_t prevDShotMode; bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0); bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0); + if (force_update || (prevDShotMode != actuatorSettings.DShotMode)) { + PIOS_Servo_DSHot_Rate(actuatorSettings.DShotMode); + prevDShotMode = actuatorSettings.DShotMode; + } + // check if any setting is changed if (updateMode || updateFreq) { /* Something has changed, apply the settings to HW */ @@ -991,30 +1008,36 @@ static void actuator_update_rate_if_changed(bool force_update) uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 }; for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { - if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) { - PIOS_Servo_SetBankMode(i, - actuatorSettings.BankMode[i] == - ACTUATORSETTINGS_BANKMODE_PWM ? - PIOS_SERVO_BANK_MODE_PWM : - PIOS_SERVO_BANK_MODE_SINGLE_PULSE - ); - } + enum pios_servo_bank_mode servo_bank_mode = PIOS_SERVO_BANK_MODE_PWM; + switch (actuatorSettings.BankMode[i]) { case ACTUATORSETTINGS_BANKMODE_ONESHOT125: case ACTUATORSETTINGS_BANKMODE_ONESHOT42: case ACTUATORSETTINGS_BANKMODE_MULTISHOT: freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered clock[i] = ACTUATOR_ONESHOT_CLOCK; // Setup an 12MHz timer clock + servo_bank_mode = PIOS_SERVO_BANK_MODE_SINGLE_PULSE; break; case ACTUATORSETTINGS_BANKMODE_PWMSYNC: freq[i] = 100; clock[i] = ACTUATOR_PWM_CLOCK; + servo_bank_mode = PIOS_SERVO_BANK_MODE_SINGLE_PULSE; + break; + case ACTUATORSETTINGS_BANKMODE_DSHOT: + freq[i] = 100; + clock[i] = ACTUATOR_PWM_CLOCK; + servo_bank_mode = PIOS_SERVO_BANK_MODE_DSHOT; break; default: // PWM freq[i] = actuatorSettings.BankUpdateFreq[i]; clock[i] = ACTUATOR_PWM_CLOCK; + servo_bank_mode = PIOS_SERVO_BANK_MODE_PWM; break; } + + if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) { + PIOS_Servo_SetBankMode(i, servo_bank_mode); + } } memcpy(prevBankMode, @@ -1034,6 +1057,25 @@ static void actuator_update_rate_if_changed(bool force_update) } } +static void update_servo_active() +{ + /* For each mixer output that is not disabled, + * figure out servo address and send allocation map to pios_servo driver. + * We need to execute this when either ActuatorSettings or MixerSettings change. + */ + uint32_t servo_active = 0; + + Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; + + for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { + if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { + servo_active |= 1 << actuatorSettings.ChannelAddr[ct]; + } + } + + PIOS_Servo_SetActive(servo_active); +} + static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { ActuatorSettingsGet(&actuatorSettings); @@ -1042,6 +1084,8 @@ static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) spinWhileArmed = false; } actuator_update_rate_if_changed(false); + + update_servo_active(); } static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) @@ -1054,6 +1098,8 @@ static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) mixer_settings_count++; } } + + update_servo_active(); } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index e92c97c2f3..c8f7c40f12 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -93,16 +93,16 @@ int32_t AirspeedStart() */ int32_t AirspeedInitialize() { + HwSettingsOptionalModulesData optionalModules; + + HwSettingsOptionalModulesGet(&optionalModules); + #ifdef MODULE_AIRSPEED_BUILTIN airspeedEnabled = true; + optionalModules.Airspeed = HWSETTINGS_OPTIONALMODULES_ENABLED; + HwSettingsOptionalModulesSet(&optionalModules); #else - - HwSettingsInitialize(); - HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesArrayGet(optionalModules); - - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (optionalModules.Airspeed == HWSETTINGS_OPTIONALMODULES_ENABLED) { airspeedEnabled = true; } else { airspeedEnabled = false; @@ -121,7 +121,6 @@ int32_t AirspeedInitialize() } AirspeedSensorInitialize(); - AirspeedSettingsInitialize(); AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index 181f07c472..6c93b07d84 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -41,7 +41,6 @@ #include "airspeedsettings.h" #include "airspeedsensor.h" // object that will be updated by the module #include "airspeedalarm.h" -#include "taskinfo.h" #if defined(PIOS_INCLUDE_MS4525DO) diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c index 5514763f4b..8a5cc56119 100644 --- a/flight/modules/Altitude/altitude.c +++ b/flight/modules/Altitude/altitude.c @@ -103,7 +103,6 @@ int32_t AltitudeInitialize() #ifdef MODULE_ALTITUDE_BUILTIN altitudeEnabled = 1; #else - HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index beed0308f0..8a9719efc3 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -197,8 +197,6 @@ int32_t AttitudeStart(void) int32_t AttitudeInitialize(void) { AttitudeStateInitialize(); - AttitudeSettingsInitialize(); - AccelGyroSettingsInitialize(); AccelStateInitialize(); GyroStateInitialize(); diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index d84933c222..f03f720631 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -9,7 +9,7 @@ * @{ * * @file AutoTune/autotune.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017. * dRonin, http://dRonin.org/, Copyright (C) 2015-2016 * Tau Labs, http://taulabs.org, Copyright (C) 2013-2014 * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. @@ -69,43 +69,39 @@ #undef STACK_SIZE_BYTES // Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340 // Beware that Nano needs 156 bytes more stack than Sparky2 -#define STACK_SIZE_BYTES 1340 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define STACK_SIZE_BYTES 1340 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define AF_NUMX 13 -#define AF_NUMP 43 +#define AF_NUMX 13 +#define AF_NUMP 43 #if !defined(AT_QUEUE_NUMELEM) -#define AT_QUEUE_NUMELEM 18 +#define AT_QUEUE_NUMELEM 18 #endif -#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */ -#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */ -#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */ -#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */ -#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */ +#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */ +#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */ +#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */ +#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */ +#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */ -#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */ -#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */ -#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */ -#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */ -#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */ +#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */ +#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */ +#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */ +#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */ +#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */ // CheckSettings() returned error bits -#define TAU_NAN 1 -#define BETA_NAN 2 -#define ROLL_BETA_LOW 4 -#define PITCH_BETA_LOW 8 -#define YAW_BETA_LOW 16 -#define TAU_TOO_LONG 32 -#define TAU_TOO_SHORT 64 -#define CPU_TOO_SLOW 128 - -// smooth-quick modes -#define SMOOTH_QUICK_DISABLED 0 -#define SMOOTH_QUICK_ACCESSORY_BASE 10 -#define SMOOTH_QUICK_TOGGLE_BASE 20 +#define TAU_NAN 1 +#define BETA_NAN 2 +#define ROLL_BETA_LOW 4 +#define PITCH_BETA_LOW 8 +#define YAW_BETA_LOW 16 +#define TAU_TOO_LONG 32 +#define TAU_TOO_SHORT 64 +#define CPU_TOO_SLOW 128 +#define FMS_TOGGLE_STEP_DISABLED 0.0f // Private types enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; @@ -136,11 +132,11 @@ static float gyroReadTimeAverageAlpha; static float gyroReadTimeAverageAlphaAlpha; static float alpha; static float smoothQuickValue; +static float flightModeSwitchToggleStepValue; static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; static int8_t accessoryToUse; -static int8_t flightModeSwitchTogglePosition; static bool moduleEnabled; @@ -156,6 +152,7 @@ static uint8_t CheckSettingsRaw(); static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise); static void FlightModeSettingsUpdatedCb(UAVObjEvent *ev); static void InitSystemIdent(bool loadDefaults); +static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults); static void ProportionPidsSmoothToQuick(); static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle); static void UpdateStabilizationDesired(bool doingIdent); @@ -167,17 +164,15 @@ static void UpdateStabilizationDesired(bool doingIdent); */ int32_t AutoTuneInitialize(void) { - // do this here since module can become disabled for several reasons - // even for MODULE_AutoTune_BUILTIN - FlightModeSettingsInitialize(); + HwSettingsOptionalModulesData optionalModules; + + HwSettingsOptionalModulesGet(&optionalModules); -#if defined(MODULE_AutoTune_BUILTIN) +#if defined(MODULE_AUTOTUNE_BUILTIN) moduleEnabled = true; + optionalModules.AutoTune = HWSETTINGS_OPTIONALMODULES_ENABLED; + HwSettingsOptionalModulesSet(&optionalModules); #else - // HwSettings is only used right here, so init here - HwSettingsInitialize(); - HwSettingsOptionalModulesData optionalModules; - HwSettingsOptionalModulesGet(&optionalModules); if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) { // even though the AutoTune module is automatically enabled // (below, when the flight mode switch is configured to use autotune) @@ -189,7 +184,7 @@ int32_t AutoTuneInitialize(void) // do it for them if they have autotune on their flight mode switch moduleEnabled = AutoTuneFoundInFMS(); } -#endif /* defined(MODULE_AutoTune_BUILTIN) */ +#endif /* defined(MODULE_AUTOTUNE_BUILTIN) */ if (moduleEnabled) { AccessoryDesiredInitialize(); @@ -198,10 +193,6 @@ int32_t AutoTuneInitialize(void) GyroStateInitialize(); ManualControlCommandInitialize(); StabilizationBankInitialize(); - StabilizationSettingsBank1Initialize(); - StabilizationSettingsBank2Initialize(); - StabilizationSettingsBank3Initialize(); - SystemIdentSettingsInitialize(); SystemIdentStateInitialize(); atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data)); @@ -249,6 +240,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) uint32_t measureTime = 0; uint32_t updateCounter = 0; enum AUTOTUNE_STATE state = AT_INIT; + uint8_t currentSmoothQuickSource = 0; bool saveSiNeeded = false; bool savePidNeeded = false; @@ -286,13 +278,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) savePidNeeded = false; // Save PIDs to permanent settings switch (systemIdentSettings.DestinationPidBank) { - case 1: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1: UAVObjSave(StabilizationSettingsBank1Handle(), 0); break; - case 2: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2: UAVObjSave(StabilizationSettingsBank2Handle(), 0); break; - case 3: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3: UAVObjSave(StabilizationSettingsBank3Handle(), 0); break; } @@ -304,16 +296,20 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // and the autotune data gathering is complete // and the autotune data gathered is good // note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune - if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) + if (flightModeSwitchToggleStepValue > FMS_TOGGLE_STEP_DISABLED && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentSettings.Complete && !CheckSettings()) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // if user toggled while armed set PID's to next in sequence // if you assume that smoothest is -1 and quickest is +1 // this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle) - smoothQuickValue += 1.0f / (float)flightModeSwitchTogglePosition; + smoothQuickValue += flightModeSwitchToggleStepValue; if (smoothQuickValue > 1.001f) { smoothQuickValue = -1.0f; } + // Assume the value is 0 + if (fabsf(smoothQuickValue) < 0.001f) { + smoothQuickValue = 0.0f; + } } else { // if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick smoothQuickValue = 0.0f; @@ -327,6 +323,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) saveSiNeeded = true; } + // Check if the SmoothQuickSource changed, + // allow config changes without reboot or reinit + uint8_t smoothQuickSource; + SystemIdentSettingsSmoothQuickSourceGet(&smoothQuickSource); + if (smoothQuickSource != currentSmoothQuickSource) { + UpdateSmoothQuickSource(smoothQuickSource, true); + currentSmoothQuickSource = smoothQuickSource; + } + ////////////////////////////////////////////////////////////////////////////////////// // if configured to use a slider for smooth-quick and the autotune module is running // (note that the module can be automatically or manually enabled) @@ -355,12 +360,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) { AccessoryDesiredData accessoryValue; AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); - // if the accessory changed more than some percent of total range - // some old PPM receivers use a low resolution chip which only allows about 180 steps out of a range of 2.0 - // a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0 - // what we are doing here does not need any higher precision than that - // user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed - if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) { + // if the accessory changed more than 2 percent of total range (~20µs) + // the smoothQuickValue will be changed + if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > 0.02f) { smoothQuickValue = accessoryValue.AccessoryVal; // calculate PIDs based on new smoothQuickValue and save to the PID bank ProportionPidsSmoothToQuick(); @@ -727,40 +729,52 @@ static void InitSystemIdent(bool loadDefaults) gyroReadTimeAverageAlpha = 0.99999988f; gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; - uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; - switch (SmoothQuickSource) { - case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 - case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 - case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 - case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 - // leave smoothQuickValue alone since it is always controlled by knob - // disable PID changing with flight mode switch - flightModeSwitchTogglePosition = -1; - // enable PID changing with accessory0-3 - accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; + UpdateSmoothQuickSource(systemIdentSettings.SmoothQuickSource, loadDefaults); +} + + +// Update SmoothQuickSource to be used +static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults) +{ + // disable PID changing with accessory0-3 and flight mode switch toggle + accessoryToUse = -1; + flightModeSwitchToggleStepValue = FMS_TOGGLE_STEP_DISABLED; + + switch (smoothQuickSource) { + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY0: + accessoryToUse = 0; break; - case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points - case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points - case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points - // don't allow init of current toggle position in the middle of 3x fms toggle - if (loadDefaults) { - // set toggle to middle of range - smoothQuickValue = 0.0f; - } - // enable PID changing with flight mode switch - flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; - // disable PID changing with accessory0-3 - accessoryToUse = -1; + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY1: + accessoryToUse = 1; + break; + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY2: + accessoryToUse = 2; + break; + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY3: + accessoryToUse = 3; + break; + // enable PID changing with flight mode switch + // -1 to +1 give a range = 2, define step value for desired positions: 3, 5, 7 + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE3POS: + flightModeSwitchToggleStepValue = 1.0f; break; - case SMOOTH_QUICK_DISABLED: + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE5POS: + flightModeSwitchToggleStepValue = 0.5f; + break; + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE7POS: + flightModeSwitchToggleStepValue = 0.33f; + break; + case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_DISABLED: default: - // leave smoothQuickValue alone so user can set it to a different value and have it stay that value - // disable PID changing with flight mode switch - flightModeSwitchTogglePosition = -1; - // disable PID changing with accessory0-3 accessoryToUse = -1; + flightModeSwitchToggleStepValue = FMS_TOGGLE_STEP_DISABLED; break; } + // don't allow init of current toggle position in the middle of 3x fms toggle + if (loadDefaults && (flightModeSwitchToggleStepValue > FMS_TOGGLE_STEP_DISABLED)) { + // set toggle to middle of range + smoothQuickValue = 0.0f; + } } @@ -821,9 +835,14 @@ static void UpdateStabilizationDesired(bool doingIdent) } else { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + } + + if (systemIdentSettings.ThrustControl == SYSTEMIDENTSETTINGS_THRUSTCONTROL_ALTITUDEVARIO) { + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO; + } else { + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; } - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } @@ -905,13 +924,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float _Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)"); StabilizationBankData volatile stabSettingsBank; switch (systemIdentSettings.DestinationPidBank) { - case 1: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1: StabilizationSettingsBank1Get((void *)&stabSettingsBank); break; - case 2: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2: StabilizationSettingsBank2Get((void *)&stabSettingsBank); break; - case 3: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3: StabilizationSettingsBank3Get((void *)&stabSettingsBank); break; } @@ -1138,13 +1157,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // Save PIDs to UAVO RAM (not permanently yet) switch (systemIdentSettings.DestinationPidBank) { - case 1: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1: StabilizationSettingsBank1Set((void *)&stabSettingsBank); break; - case 2: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2: StabilizationSettingsBank2Set((void *)&stabSettingsBank); break; - case 3: + case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3: StabilizationSettingsBank3Set((void *)&stabSettingsBank); break; } diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index 436480bce9..f6aa706544 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -8,7 +8,8 @@ * @{ * * @file battery.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Module to read the battery Voltage and Current periodically and set alarms appropriately. * * @see The GNU Public License (GPL) Version 3 @@ -51,26 +52,35 @@ #include "flightbatterystate.h" #include "flightbatterysettings.h" #include "hwsettings.h" +#include "systemstats.h" // // Configuration // -#define SAMPLE_PERIOD_MS 500 -// Private types +#define SAMPLE_PERIOD_MS 500 + +// Time since power on the cells detection is active +#define DETECTION_TIMEFRAME 60000 + +#ifndef PIOS_ADC_VOLTAGE_PIN +#define PIOS_ADC_VOLTAGE_PIN -1 +#endif + +#ifndef PIOS_ADC_CURRENT_PIN +#define PIOS_ADC_CURRENT_PIN -1 +#endif -// Private variables -static bool batteryEnabled = false; // THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH // PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL // WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE // CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS. -static int8_t voltageADCPin = -1; // ADC pin for voltage -static int8_t currentADCPin = -1; // ADC pin for current +static int8_t voltageADCPin = PIOS_ADC_VOLTAGE_PIN; // ADC pin for voltage +static int8_t currentADCPin = PIOS_ADC_CURRENT_PIN; // ADC pin for current // Private functions static void onTimer(UAVObjEvent *ev); -static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData); +static void GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData); /** * Initialise the module, called on startup @@ -78,14 +88,18 @@ static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, Fligh */ int32_t BatteryInitialize(void) { + bool batteryEnabled; + + HwSettingsOptionalModulesData optionalModules; + + HwSettingsOptionalModulesGet(&optionalModules); + #ifdef MODULE_BATTERY_BUILTIN batteryEnabled = true; + optionalModules.Battery = HWSETTINGS_OPTIONALMODULES_ENABLED; + HwSettingsOptionalModulesSet(&optionalModules); #else - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - - HwSettingsOptionalModulesGet(optionalModules); - - if ((optionalModules[HWSETTINGS_OPTIONALMODULES_BATTERY] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { + if (optionalModules.Battery == HWSETTINGS_OPTIONALMODULES_ENABLED) { batteryEnabled = true; } else { batteryEnabled = false; @@ -113,9 +127,7 @@ int32_t BatteryInitialize(void) // Start module if (batteryEnabled) { FlightBatteryStateInitialize(); - FlightBatterySettingsInitialize(); - - // FlightBatterySettingsConnectCallback(FlightBatterySettingsUpdatedCb); + SystemStatsInitialize(); static UAVObjEvent ev; @@ -144,17 +156,19 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) batterySettings.ResetConsumedEnergy = false; FlightBatterySettingsSet(&batterySettings); } - +#ifdef PIOS_INCLUDE_ADC // calculate the battery parameters if (voltageADCPin >= 0) { flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts } else { flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } - +#else + flightBatteryData.Voltage = 0; +#endif /* PIOS_INCLUDE_ADC */ // voltage available: get the number of cells if possible, desired and not armed GetNbCells(&batterySettings, &flightBatteryData); - +#ifdef PIOS_INCLUDE_ADC // ad a plausibility check: zero voltage => zero current if (currentADCPin >= 0 && flightBatteryData.Voltage > 0.f) { flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps @@ -164,6 +178,9 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) } else { // If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } +#else + flightBatteryData.Current = -0; +#endif /* PIOS_INCLUDE_ADC */ // For safety reasons consider only positive currents in energy comsumption, i.e. no charging up. // necesary when sensor are not perfectly calibrated @@ -204,7 +221,7 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ - if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * flightBatteryData.NbCells) { + if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Critical * flightBatteryData.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); @@ -217,31 +234,37 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) } -static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData) +static void GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData) { // get flight status to check for armed uint8_t armed = 0; + static bool detected = false; - FlightStatusArmedGet(&armed); + // prevent the cell number to change once the board is armed at least once + if (detected) { + return; + } + FlightStatusArmedGet(&armed); // check only if not armed if (armed == FLIGHTSTATUS_ARMED_ARMED) { - return -2; + detected = true; + return; } // prescribed number of cells? if (batterySettings->NbCells != 0) { flightBatteryData->NbCells = batterySettings->NbCells; flightBatteryData->NbCellsAutodetected = 0; - return -3; + return; } // plausibility check if (flightBatteryData->Voltage <= 0.5f) { // cannot detect number of cells flightBatteryData->NbCellsAutodetected = 0; - return -1; + return; } float voltageMin = 0.f, voltageMax = 0.f; @@ -255,6 +278,10 @@ static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, Fligh voltageMin = 3.6f; voltageMax = 4.2f; break; + case FLIGHTBATTERYSETTINGS_TYPE_LIHV: + voltageMin = 3.6f; + voltageMax = 4.35f; + break; case FLIGHTBATTERYSETTINGS_TYPE_A123: voltageMin = 2.01f; voltageMax = 3.59f; @@ -262,7 +289,7 @@ static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, Fligh case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4: default: flightBatteryData->NbCellsAutodetected = 0; - return -1; + return; } // uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin @@ -271,14 +298,19 @@ static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, Fligh // checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin) if (flightBatteryData->Voltage > voltageMin * voltageMax / (voltageMax - voltageMin)) { flightBatteryData->NbCellsAutodetected = 0; - return -1; + return; } + // Prevent the battery discharging on the ground to change the detected number of cells: + // Detection is enabled in the first 60 seconds from powerup + uint32_t flightTime; + SystemStatsFlightTimeGet(&flightTime); + if (flightTime > DETECTION_TIMEFRAME) { + detected = true; + } flightBatteryData->NbCells = (int8_t)(flightBatteryData->Voltage / voltageMin); flightBatteryData->NbCellsAutodetected = 1; - - return flightBatteryData->NbCells; } /** diff --git a/flight/modules/CameraControl/cameracontrol.c b/flight/modules/CameraControl/cameracontrol.c new file mode 100644 index 0000000000..27f2cfa90a --- /dev/null +++ b/flight/modules/CameraControl/cameracontrol.c @@ -0,0 +1,361 @@ +/** + ****************************************************************************** + * + * @file cameracontrol.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @brief camera control module. triggers cameras with multiple options + * + * @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 + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#include +#include "inc/cameracontrol.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Private variables + +typedef enum { + CAMERASTATUS_Idle = 0, + CAMERASTATUS_Shot, + CAMERASTATUS_Video +} CameraStatus; + +static struct CameraControl_data { + int32_t lastTriggerTimeRaw; + float lastTriggerNEDPosition[3]; + CameraControlSettingsData settings; + CameraControlActivityData activity; + DelayedCallbackInfo *callbackHandle; + CameraStatus outputStatus; + CameraStatus lastOutputStatus; + CameraStatus manualInput; + CameraStatus lastManualInput; + bool autoTriggerEnabled; + uint16_t ImageId; + float HomeECEF[3]; + float HomeRne[3][3]; +} *ccd; + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR +#define CBTASK_PRIORITY CALLBACK_TASK_AUXILIARY +#define STACK_SIZE_BYTES 512 +#define CALLBACK_STD_PERIOD 50 +#define INPUT_DEADBAND 0.1f + +static void CameraControlTask(); +static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev); +static void HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev); +static void UpdateOutput(); +static void PublishActivity(); +static bool checkActivation(); +static void FillActivityInfo(); + + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t CameraControlInitialize(void) +{ + ccd = 0; + HwSettingsOptionalModulesData modules; + HwSettingsOptionalModulesGet(&modules); + if (modules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED) { + ccd = (struct CameraControl_data *)pios_malloc(sizeof(struct CameraControl_data)); + memset(ccd, 0, sizeof(struct CameraControl_data)); + ccd->callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&CameraControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_CAMERACONTROL, STACK_SIZE_BYTES); + CameraControlActivityInitialize(); + CameraDesiredInitialize(); + CameraControlSettingsConnectCallback(SettingsUpdateCb); + HomeLocationConnectCallback(HomeLocationUpdateCb); + GPSTimeInitialize(); + PositionStateInitialize(); + AttitudeStateInitialize(); + AccessoryDesiredInitialize(); + FlightStatusInitialize(); + + + SettingsUpdateCb(NULL); + HomeLocationUpdateCb(NULL); + + // init output: + ccd->outputStatus = CAMERASTATUS_Idle; + UpdateOutput(); + } + return 0; +} + +/* stub: module has no module thread */ +int32_t CameraControlStart(void) +{ + if (!ccd) { + return 0; + } + + PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_LATER); + ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw(); + return 0; +} + +MODULE_INITCALL(CameraControlInitialize, CameraControlStart); + +static void CameraControlTask() +{ + bool trigger = false; + PositionStateData pos; + uint32_t timeSinceLastShot = PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw); + CameraStatus newStatus; + + if (checkActivation()) { + if (ccd->manualInput != ccd->lastManualInput && ccd->manualInput != CAMERASTATUS_Idle) { + // Manual override + trigger = true; + newStatus = ccd->manualInput; + ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_MANUAL; + } else { + // MinimumTimeInterval sets a hard limit on time between two consecutive shots, i.e. the minimum time between shots the camera can achieve + if (ccd->autoTriggerEnabled && + timeSinceLastShot > (ccd->settings.MinimumTimeInterval * 1000 * 1000)) { + // check trigger conditions + if (ccd->settings.TimeInterval > 0) { + if (timeSinceLastShot > ccd->settings.TimeInterval * (1000 * 1000)) { + trigger = true; + newStatus = CAMERASTATUS_Shot; + ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTOTIME; + } + } + + if (ccd->settings.SpaceInterval > 0) { + PositionStateGet(&pos); + float dn = pos.North - ccd->lastTriggerNEDPosition[0]; + float de = pos.East - ccd->lastTriggerNEDPosition[1]; + float distance = sqrtf((dn * dn) + (de * de)); + ccd->activity.TriggerMillisecond = (int16_t)distance * 10.0f; + if (distance > ccd->settings.SpaceInterval) { + trigger = true; + newStatus = CAMERASTATUS_Shot; + ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTODISTANCE; + } + } + } + } + } + if (trigger) { + ccd->outputStatus = newStatus; + ccd->ImageId++; + ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw(); + ccd->lastTriggerNEDPosition[0] = pos.North; + ccd->lastTriggerNEDPosition[1] = pos.East; + ccd->lastTriggerNEDPosition[2] = pos.Down; + } else { + ccd->outputStatus = CAMERASTATUS_Idle; + ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE; + } + + ccd->lastManualInput = ccd->manualInput; + PublishActivity(); + UpdateOutput(); + PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_SOONER); +} + +static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev) +{ + CameraControlSettingsGet(&ccd->settings); +} + +static bool checkActivation() +{ + if (ccd->settings.ManualTriggerInput != CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_NONE) { + uint8_t accessory = ccd->settings.ManualTriggerInput - CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_ACCESSORY0; + + AccessoryDesiredData accessoryDesired; + AccessoryDesiredInstGet(accessory, &accessoryDesired); + + if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Shot) < INPUT_DEADBAND) { + ccd->manualInput = CAMERASTATUS_Shot; + } else if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Video) < INPUT_DEADBAND) { + ccd->manualInput = CAMERASTATUS_Video; + } else { + ccd->manualInput = CAMERASTATUS_Idle; + } + } + + switch (ccd->settings.AutoTriggerMode) { + case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_DISABLED: + ccd->autoTriggerEnabled = false; + break; + case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_WHENARMED: + { + FlightStatusArmedOptions armed; + + FlightStatusArmedGet(&armed); + ccd->autoTriggerEnabled = (armed == FLIGHTSTATUS_ARMED_ARMED); + } + break; + case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_ALWAYS: + ccd->autoTriggerEnabled = true; + break; + case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_INPUT: + { + uint8_t accessory = ccd->settings.AutoTriggerInput - CAMERACONTROLSETTINGS_AUTOTRIGGERINPUT_ACCESSORY0; + AccessoryDesiredData accessoryDesired; + AccessoryDesiredInstGet(accessory, &accessoryDesired); + + ccd->autoTriggerEnabled = (accessoryDesired.AccessoryVal > INPUT_DEADBAND); + } + break; + case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_MISSION: + { + FlightStatusFlightModeOptions flightmode; + FlightStatusFlightModeGet(&flightmode); + ccd->autoTriggerEnabled = (flightmode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER); + } + break; + } + return ccd->autoTriggerEnabled || (ccd->manualInput != CAMERASTATUS_Idle); +} + +static void UpdateOutput() +{ + if (ccd->outputStatus != ccd->lastOutputStatus) { + switch (ccd->outputStatus) { + case CAMERASTATUS_Idle: + if (CAMERASTATUS_Shot == ccd->lastOutputStatus) { + if (PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw) > ccd->settings.TriggerPulseWidth * 1000) { + CameraDesiredTriggerSet(&ccd->settings.OutputValues.Idle); + } else { + // skip updating lastOutputStatus until TriggerPulseWidth elapsed + return; + } + } + break; + case CAMERASTATUS_Shot: + CameraDesiredTriggerSet(&ccd->settings.OutputValues.Shot); + break; + case CAMERASTATUS_Video: + CameraDesiredTriggerSet(&ccd->settings.OutputValues.Video); + break; + } + } + ccd->lastOutputStatus = ccd->outputStatus; +} + +static void PublishActivity() +{ + if (ccd->outputStatus != ccd->lastOutputStatus) { + switch (ccd->outputStatus) { + case CAMERASTATUS_Idle: + if (ccd->lastOutputStatus == CAMERASTATUS_Video) { + ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STOPVIDEO; + } else { + ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE; + } + break; + case CAMERASTATUS_Shot: + ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_TRIGGERPICTURE; + break; + case CAMERASTATUS_Video: + if (ccd->lastOutputStatus != CAMERASTATUS_Video) { + ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STARTVIDEO; + } else { + ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE; + } + break; + } + if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_IDLE + || ccd->lastOutputStatus != CAMERASTATUS_Shot) { + FillActivityInfo(); + CameraControlActivitySet(&ccd->activity); + } + } +} + +static void FillActivityInfo() +{ + CameraControlActivityData *activity = &ccd->activity; + { + PositionStateData position; + PositionStateGet(&position); + int32_t LLAi[3]; + const float pos[3] = { + position.North, + position.East, + position.Down + }; + Base2LLA(pos, ccd->HomeECEF, ccd->HomeRne, LLAi); + + activity->Latitude = LLAi[0]; + activity->Longitude = LLAi[1]; + activity->Altitude = ((float)LLAi[2]) * 1e-4f; + } + { + GPSTimeData time; + GPSTimeGet(&time); + + activity->TriggerYear = time.Year; + activity->TriggerMonth = time.Month; + activity->TriggerDay = time.Day; + activity->TriggerHour = time.Hour; + activity->TriggerMinute = time.Minute; + activity->TriggerSecond = time.Second; + activity->TriggerMillisecond = time.Millisecond; + } + + activity->ImageId = ccd->ImageId; + activity->SystemTS = xTaskGetTickCount() * portTICK_RATE_MS; + { + AttitudeStateData attitude; + AttitudeStateGet(&attitude); + + activity->Roll = attitude.Roll; + activity->Pitch = attitude.Pitch; + activity->Yaw = attitude.Yaw; + } +} + +static void HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HomeLocationData home; + + HomeLocationGet(&home); + + int32_t LLAi[3] = { + home.Latitude, + home.Longitude, + home.Altitude + }; + LLA2ECEF(LLAi, ccd->HomeECEF); + RneFromLLA(LLAi, ccd->HomeRne); +} diff --git a/flight/modules/CameraControl/inc/cameracontrol.h b/flight/modules/CameraControl/inc/cameracontrol.h new file mode 100644 index 0000000000..50fe3e9e2e --- /dev/null +++ b/flight/modules/CameraControl/inc/cameracontrol.h @@ -0,0 +1,35 @@ +/** + ****************************************************************************** + * + * @file cameracontrol.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @brief camera control module. triggers cameras with multiple options + * + * @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 + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#ifndef FLIGHT_MODULES_CAMERACONTROL_INC_CAMERACONTROL_H +#define FLIGHT_MODULES_CAMERACONTROL_INC_CAMERACONTROL_H + + +#endif /* FLIGHT_MODULES_CAMERACONTROL_INC_CAMERACONTROL_H */ diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 27ba36d283..43b5742780 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -49,6 +49,8 @@ #include "accessorydesired.h" #include "attitudestate.h" +#include "mixersettings.h" +#include "actuatorcommand.h" #include "camerastabsettings.h" #include "cameradesired.h" #include "hwsettings.h" @@ -61,6 +63,8 @@ // Private types // Private variables +static bool gimbalOutputEnabled = false; + static struct CameraStab_data { portTickType lastSysTime; float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM]; @@ -83,6 +87,12 @@ static void attitudeUpdated(UAVObjEvent *ev); static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab); #endif +// this structure is equivalent to the UAVObjects for one mixer. +typedef struct { + uint8_t type; + int8_t matrix[5]; +} __attribute__((packed)) Mixer_t; + /** * Initialise the module, called on startup @@ -92,14 +102,15 @@ int32_t CameraStabInitialize(void) { bool cameraStabEnabled; -#ifdef MODULE_CAMERASTAB_BUILTIN - cameraStabEnabled = true; -#else HwSettingsOptionalModulesData optionalModules; - HwSettingsInitialize(); HwSettingsOptionalModulesGet(&optionalModules); +#ifdef MODULE_CAMERASTAB_BUILTIN + cameraStabEnabled = true; + optionalModules.CameraStab = HWSETTINGS_OPTIONALMODULES_ENABLED; + HwSettingsOptionalModulesSet(&optionalModules); +#else if (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED) { cameraStabEnabled = true; } else { @@ -119,7 +130,6 @@ int32_t CameraStabInitialize(void) csd->lastSysTime = xTaskGetTickCount(); AttitudeStateInitialize(); - CameraStabSettingsInitialize(); CameraDesiredInitialize(); UAVObjEvent ev = { @@ -150,6 +160,20 @@ static void attitudeUpdated(UAVObjEvent *ev) return; } + if (!gimbalOutputEnabled) { + MixerSettingsData mixerSettings; + MixerSettingsGet(&mixerSettings); + Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; + for (int ct = 0; ct < ACTUATORCOMMAND_CHANNEL_NUMELEM; ct++) { + uint8_t mixer_type = mixers[ct].type; + if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && + (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { + gimbalOutputEnabled = true; + } + } + return; + } + AccessoryDesiredData accessory; CameraStabSettingsData cameraStab; diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 15f9f5db97..fdc9a01440 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/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 73e2a37dbc..a8a93d4df5 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -98,7 +98,6 @@ int32_t MagBaroInitialize() #ifdef MODULE_MAGBARO_BUILTIN magbaroEnabled = 1; #else - HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); if (optionalModules[HWSETTINGS_OPTIONALMODULES_MAGBARO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { diff --git a/flight/modules/Fault/Fault.c b/flight/modules/Fault/Fault.c index 42faa781f3..7eb7801bf1 100644 --- a/flight/modules/Fault/Fault.c +++ b/flight/modules/Fault/Fault.c @@ -43,7 +43,6 @@ static int32_t fault_initialize(void) #ifdef MODULE_FAULT_BUILTIN module_enabled = true; #else - HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); @@ -61,7 +60,6 @@ static int32_t fault_initialize(void) * we've booted in BootFault recovery mode with all optional * modules disabled. */ - FaultSettingsInitialize(); if (module_enabled) { FaultSettingsActivateFaultGet(&active_fault); diff --git a/flight/modules/FirmwareIAP/firmwareiap.c b/flight/modules/FirmwareIAP/firmwareiap.c index 0d8b1c9005..3164f3281d 100644 --- a/flight/modules/FirmwareIAP/firmwareiap.c +++ b/flight/modules/FirmwareIAP/firmwareiap.c @@ -2,7 +2,8 @@ ****************************************************************************** * * @file firmwareiap.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief In Application Programming module to support firmware upgrades by * providing a means to enter the bootloader. * @@ -47,7 +48,7 @@ #define IAP_STATE_STEP_2 2 #define IAP_STATE_RESETTING 3 -#define RESET_DELAY 500 /* delay between sending reset ot INS */ +#define RESET_DELAY 500 #define TICKS2MS(t) ((t) / portTICK_RATE_MS) #define MS2TICKS(m) ((m) * portTICK_RATE_MS) @@ -96,13 +97,15 @@ int32_t FirmwareIAPInitialize() FirmwareIAPObjData data; FirmwareIAPObjGet(&data); - data.BoardType = bdinfo->board_type; PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); - data.BoardRevision = bdinfo->board_rev; + if (data.BoardType == 0) { + data.BoardRevision = bdinfo->board_rev; + data.BoardType = bdinfo->board_type; + } data.BootloaderRevision = bdinfo->bl_rev; data.ArmReset = 0; - data.crc = 0; + data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); FirmwareIAPObjSet(&data); if (bdinfo->magic == PIOS_BOARD_INFO_BLOB_MAGIC) { FirmwareIAPObjConnectCallback(&FirmwareIAPCallback); @@ -126,7 +129,6 @@ int32_t FirmwareIAPStart() static uint8_t iap_state = IAP_STATE_READY; static void FirmwareIAPCallback(UAVObjEvent *ev) { - const struct pios_board_info *bdinfo = &pios_board_info_blob; static uint32_t last_time = 0; uint32_t this_time; uint32_t delta; @@ -136,7 +138,6 @@ static void FirmwareIAPCallback(UAVObjEvent *ev) } FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); if (ev->obj == FirmwareIAPObjHandle()) { // Get the input object data @@ -144,18 +145,6 @@ static void FirmwareIAPCallback(UAVObjEvent *ev) this_time = get_time(); delta = this_time - last_time; last_time = this_time; - if ((data.BoardType == bdinfo->board_type) && (data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) { - PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); - data.BoardRevision = bdinfo->board_rev; - data.BootloaderRevision = bdinfo->bl_rev; - data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); - FirmwareIAPObjSet(&data); - } - if ((data.ArmReset == 1) && (iap_state != IAP_STATE_RESETTING)) { - data.ArmReset = 0; - FirmwareIAPObjSet(&data); - } switch (iap_state) { case IAP_STATE_READY: if (data.Command == IAP_CMD_STEP_1) { @@ -238,7 +227,7 @@ static uint32_t get_time(void) } /** - * Executed by event dispatcher callback to reset INS before resetting OP + * Executed by event dispatcher callback to reset Board */ static void resetTask(__attribute__((unused)) UAVObjEvent *ev) { @@ -250,18 +239,8 @@ static void resetTask(__attribute__((unused)) UAVObjEvent *ev) PIOS_LED_Toggle(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ - FirmwareIAPObjData data; - FirmwareIAPObjGet(&data); - if ((portTickType)(xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { lastResetSysTime = xTaskGetTickCount(); - data.BoardType = 0xFF; - data.ArmReset = 1; - data.crc = reset_count; /* Must change a value for this to get to INS */ - FirmwareIAPObjSet(&data); - ++reset_count; - if (reset_count > 3) { - PIOS_SYS_Reset(); - } + PIOS_SYS_Reset(); } } diff --git a/flight/modules/FirmwareIAP/inc/firmwareiap.h b/flight/modules/FirmwareIAP/inc/firmwareiap.h index 81c39d880c..08841d89ca 100644 --- a/flight/modules/FirmwareIAP/inc/firmwareiap.h +++ b/flight/modules/FirmwareIAP/inc/firmwareiap.h @@ -1,9 +1,10 @@ /** ****************************************************************************** * - * @file firmwareiap.c + * @file firmwareiap.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Example module to be used as a template for actual modules. + * @brief In Application Programming module to support firmware upgrades by + * providing a means to enter the bootloader. * * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/modules/FlightPlan/flightplan.c b/flight/modules/FlightPlan/flightplan.c index b53b76ec9a..94c72f1f97 100644 --- a/flight/modules/FlightPlan/flightplan.c +++ b/flight/modules/FlightPlan/flightplan.c @@ -79,7 +79,6 @@ int32_t FlightPlanInitialize() FlightPlanStatusInitialize(); FlightPlanControlInitialize(); - FlightPlanSettingsInitialize(); // Listen for object updates FlightPlanControlConnectCallback(&objectUpdatedCb); diff --git a/flight/modules/FlightPlan/lib/uavobject.pyt.template b/flight/modules/FlightPlan/lib/uavobject.pyt.template index ff6bab315d..1326d46a8f 100644 --- a/flight/modules/FlightPlan/lib/uavobject.pyt.template +++ b/flight/modules/FlightPlan/lib/uavobject.pyt.template @@ -2,7 +2,8 @@ ############################################################################## # # @file $(NAMELC).py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. +# The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. # @brief Implementation of the $(NAME) object. This file has been # automatically generated by the UAVObjectGenerator. For use with # the PyMite VM of the FlightPlan module. @@ -30,14 +31,14 @@ # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA # -from uavobject import * +from librepilot.uavtalk.uavobject import * $(DATAFIELDS) # Object $(NAME) definition class $(NAME)(UAVObject): # Object constants - OBJID = $(UOBJID) + OBJID = $(OBJIDHEX) # Constructor def __init__(self): diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 305fb7148f..fd7eb9a055 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" @@ -104,30 +105,52 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); // the new location with Set = true. #define GPS_HOMELOCATION_SET_DELAY 5000 -#define GPS_LOOP_DELAY_MS 6 +// PIOS serial port receive buffer for GPS is set to 32 bytes for the minimal and 128 bytes for the full version. +// GPS_READ_BUFFER is defined a few lines below in this file. +// +// 57600 bps = 5760 bytes per second +// +// For 32 bytes buffer: this is a maximum of 5760/32 = 180 buffers per second +// that is 1/180 = 0.0056 seconds per packet +// We must never wait more than 5ms since packet was last drained or it may overflow +// +// For 128 bytes buffer: this is a maximum of 5760/128 = 45 buffers per second +// that is 1/45 = 0.022 seconds per packet +// We must never wait more than 22ms since packet was last drained or it may overflow + +// There are two delays in play for the GPS task: +// - GPS_BLOCK_ON_NO_DATA_MS is the time to block while waiting to receive serial data from the GPS +// - GPS_LOOP_DELAY_MS is used for a context switch initiated by calling vTaskDelayUntil() to let other tasks run +// +// The delayMs time is not that critical. It should not be too high so that maintenance actions within this task +// are run often enough. +// GPS_LOOP_DELAY_MS on the other hand, should be less then 5.55 ms. A value set too high will cause data to be dropped. + +#define GPS_LOOP_DELAY_MS 5 +#define GPS_BLOCK_ON_NO_DATA_MS 20 #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 1024 + #define STACK_SIZE_BYTES 1024 #else #if defined(PIOS_GPS_MINIMAL) - #define GPS_READ_BUFFER 32 + #define GPS_READ_BUFFER 32 #ifdef PIOS_INCLUDE_GPS_NMEA_PARSER - #define STACK_SIZE_BYTES 580 // NMEA -#else - #define STACK_SIZE_BYTES 440 // UBX + #define STACK_SIZE_BYTES 580 // NMEA +#else // PIOS_INCLUDE_GPS_NMEA_PARSER + #define STACK_SIZE_BYTES 440 // UBX #endif // PIOS_INCLUDE_GPS_NMEA_PARSER -#else - #define STACK_SIZE_BYTES 650 +#else // PIOS_GPS_MINIMAL + #define STACK_SIZE_BYTES 650 #endif // PIOS_GPS_MINIMAL #endif // PIOS_GPS_SETS_HOMELOCATION #ifndef GPS_READ_BUFFER -#define GPS_READ_BUFFER 128 +#define GPS_READ_BUFFER 128 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // **************** // Private variables @@ -173,14 +196,15 @@ int32_t GPSStart(void) int32_t GPSInitialize(void) { - HwSettingsInitialize(); -#ifdef MODULE_GPS_BUILTIN - gpsEnabled = true; -#else HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); +#ifdef MODULE_GPS_BUILTIN + gpsEnabled = true; + optionalModules.GPS = HWSETTINGS_OPTIONALMODULES_ENABLED; + HwSettingsOptionalModulesSet(&optionalModules); +#else if (optionalModules.GPS == HWSETTINGS_OPTIONALMODULES_ENABLED) { gpsEnabled = true; } else { @@ -196,17 +220,14 @@ int32_t GPSInitialize(void) GPSVelocitySensorInitialize(); GPSTimeInitialize(); GPSSatellitesInitialize(); - HomeLocationInitialize(); #if defined(ANY_FULL_MAG_PARSER) AuxMagSensorInitialize(); - AuxMagSettingsInitialize(); GPSExtendedStatusInitialize(); // Initialize mag parameters AuxMagSettingsUpdatedCb(NULL); AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb); #endif - GPSSettingsInitialize(); // updateHwSettings() uses gpsSettings GPSSettingsGet(&gpsSettings); // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running @@ -219,10 +240,6 @@ int32_t GPSInitialize(void) GPSTimeInitialize(); GPSSatellitesInitialize(); #endif -#if defined(PIOS_GPS_SETS_HOMELOCATION) - HomeLocationInitialize(); -#endif - GPSSettingsInitialize(); // updateHwSettings() uses gpsSettings GPSSettingsGet(&gpsSettings); // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running @@ -283,14 +300,7 @@ MODULE_INITCALL(GPSInitialize, GPSStart); static void gpsTask(__attribute__((unused)) void *parameters) { - // 57600 baud = 5760 bytes per second - // PIOS serial buffers appear to be 32 bytes long - // that is a maximum of 5760/32 = 180 buffers per second - // that is 1/180 = 0.005555556 seconds per packet - // we must never wait more than 5ms since packet was last drained or it may overflow - // 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible - portTickType xDelay = 5 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; #ifdef PIOS_GPS_SETS_HOMELOCATION portTickType homelocationSetDelay = 0; @@ -352,8 +362,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) uint16_t cnt; int res; - // This blocks the task until there is something on the buffer (or 100ms? passes) - cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay); + // This blocks the task until there is something in the buffer (or GPS_BLOCK_ON_NO_DATA_MS passes) + cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, GPS_BLOCK_ON_NO_DATA_MS); res = PARSER_INCOMPLETE; if (cnt > 0) { PERF_TIMED_SECTION_START(counterParse); @@ -415,7 +425,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) // // if (the fix is good) { if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) && - (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) && (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION @@ -435,7 +445,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) } #endif // else if (we are at least getting what might be usable GPS data to finish a flight with) { - } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + } else if (((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) && (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); // else data is probably not good enough to fly @@ -727,31 +737,49 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) newconfig.enableGPS = true; newconfig.enableGLONASS = true; newconfig.enableBeiDou = false; + newconfig.enableGalileo = false; break; case GPSSETTINGS_UBXGNSSMODE_GLONASS: newconfig.enableGPS = false; newconfig.enableGLONASS = true; newconfig.enableBeiDou = false; + newconfig.enableGalileo = false; break; case GPSSETTINGS_UBXGNSSMODE_GPS: newconfig.enableGPS = true; newconfig.enableGLONASS = false; newconfig.enableBeiDou = false; + newconfig.enableGalileo = false; break; case GPSSETTINGS_UBXGNSSMODE_GPSBEIDOU: newconfig.enableGPS = true; newconfig.enableGLONASS = false; newconfig.enableBeiDou = true; + newconfig.enableGalileo = false; break; case GPSSETTINGS_UBXGNSSMODE_GLONASSBEIDOU: newconfig.enableGPS = false; newconfig.enableGLONASS = true; newconfig.enableBeiDou = true; + newconfig.enableGalileo = false; + break; + case GPSSETTINGS_UBXGNSSMODE_GPSGALILEO: + newconfig.enableGPS = true; + newconfig.enableGLONASS = false; + newconfig.enableBeiDou = false; + newconfig.enableGalileo = true; + break; + case GPSSETTINGS_UBXGNSSMODE_GPSGLONASSGALILEO: + newconfig.enableGPS = true; + newconfig.enableGLONASS = true; + newconfig.enableBeiDou = false; + newconfig.enableGalileo = true; break; default: newconfig.enableGPS = false; newconfig.enableGLONASS = false; newconfig.enableBeiDou = false; + newconfig.enableGalileo = false; break; } diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 19ad461273..e8a79a3b51 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -173,8 +173,35 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition #if defined(PIOS_GPS_MINIMAL) restart_state = RESTART_NO_ERROR; #else - restart_state = RESTART_WITH_ERROR; + /* UBX-NAV-SVINFO (id 30) and UBX-NAV-SAT (id 35) packets are NOT critical to the navigation. + Their only use is to update the nice GPS constellation widget in the GCS. + These packets become very large when a lot of SV (Space Vehicles) are tracked. (8 + 12 * ) bytes + + In the case of 3 simultaneously enabled GNSS, it is easy to reach the currently defined tracking limit of 32 SV. + The memory taken up by this is 8 + 12 * 32 = 392 bytes + + The NEO-M8N has been seen to send out information for more than 32 SV. This causes overflow errors. + We will ignore these informational packets if they become too large. + The downside of this is no more constellation updates in the GCS when we reach the limit. + + An alternative fix could be to increment the maximum number of satellites: MAX_SVS and UBX_CFG_GNSS_NUMCH_VER8 in UBX.h + This would use extra memory for little gain. Both fixes can be combined. + + Tests indicate that, once we reach this amount of tracked SV, the NEO-M8N module positioning output + becomes jittery (in time) and therefore less accurate. + + The recommendation is to limit the maximum number of simultaneously used GNSS to a value of 2. + This will help keep the number of tracked satellites in line. + */ + if ((ubx->header.class == 0x01) && ((ubx->header.id == 0x30) || (ubx->header.id == 0x35))) { + restart_state = RESTART_NO_ERROR; + } else { + restart_state = RESTART_WITH_ERROR; + } #endif + // We won't see the end of the packet. Which means it is useless to do any further processing. + // Therefore scan for the start of the next packet. + proto_state = START; break; } else { if (ubx->header.len == 0) { @@ -337,7 +364,7 @@ static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsP GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D; break; case STATUS_GPSFIX_3DFIX: - GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX3D; + GpsPosition->Status = (sol->flags & STATUS_FLAGS_DIFFSOLN) ? GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS : GPSPOSITIONSENSOR_STATUS_FIX3D; break; default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; } @@ -399,10 +426,18 @@ static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsP GpsPosition->Longitude = pvt->lon; GpsPosition->Satellites = pvt->numSV; GpsPosition->PDOP = pvt->pDOP * 0.01f; + if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) { - GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? - GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; - } else { + switch (pvt->fixType) { + case PVT_FIX_TYPE_2D: + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D; + break; + case PVT_FIX_TYPE_3D: + GpsPosition->Status = (pvt->flags & PVT_FLAGS_DIFFSOLN) ? GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS : GPSPOSITIONSENSOR_STATUS_FIX3D; + break; + default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; + } + } else { // fix is not valid so we make sure to treat is as NOFIX GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; } @@ -439,7 +474,7 @@ static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, __attribute__((unused)) GpsTime.Hour = timeutc->hour; GpsTime.Minute = timeutc->min; GpsTime.Second = timeutc->sec; - + GpsTime.Millisecond = (int16_t)(timeutc->nano / 1000000); GPSTimeSet(&GpsTime); } else { // Time is not valid, nothing to do diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 4615a4a155..f622ea87a0 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -460,8 +460,8 @@ struct UBX_CFG_CFG { // ------------------------------------10987654321098765432109876543210 // WAAS 122, 133, 134, 135, 138---------|---------|---------|---------| #define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100 -// EGNOS 120, 124, 126, 131-------------|---------|---------|---------| -#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001 +// EGNOS 120, 123, 136------------------|---------|---------|---------| +#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000010000000000001001 // MSAS 129, 137------------------------|---------|---------|---------| #define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000 // GAGAN 127, 128-----------------------|---------|---------|---------| @@ -485,6 +485,7 @@ struct UBX_CFG_SBAS { #define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000 #define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000 #define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000 +#define UBX_CFG_GNSS_FLAGS_GALILEO_E1 0x010000 #define UBX_CFG_GNSS_NUMCH_VER7 22 #define UBX_CFG_GNSS_NUMCH_VER8 32 diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 8578f1d6b0..44045840c0 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -102,6 +102,7 @@ typedef struct { bool enableGPS; bool enableGLONASS; bool enableBeiDou; + bool enableGalileo; } ubx_autoconfig_settings_t; // Sent messages for configuration support diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 9cfb079d33..1f692b6d1d 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -34,7 +34,6 @@ #include "inc/ubx_autoconfig.h" #include -#include "taskinfo.h" // private type definitions @@ -244,7 +243,9 @@ void gps_ubx_reset_sensor_type() ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; GPSPositionSensorSensorTypeSet(&ubxSensorType); // make the sensor type / autobaud code time out immediately to send the request immediately - status->lastStepTimestampRaw += 0x8000000UL; + if (status) { + status->lastStepTimestampRaw += 0x8000000UL; + } } --mutex; } @@ -397,6 +398,13 @@ static void config_gnss(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8; } break; + case UBX_GNSS_ID_GALILEO: + if (status->currentSettings.enableGalileo) { + status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GALILEO_E1; + status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 10; + status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8; + } + break; default: break; } diff --git a/flight/modules/Logging/Logging.c b/flight/modules/Logging/Logging.c index 7529420d26..09e1860b1d 100644 --- a/flight/modules/Logging/Logging.c +++ b/flight/modules/Logging/Logging.c @@ -52,7 +52,6 @@ static void FlightStatusUpdatedCb(UAVObjEvent *ev); int32_t LoggingInitialize(void) { - DebugLogSettingsInitialize(); DebugLogControlInitialize(); DebugLogStatusInitialize(); DebugLogEntryInitialize(); diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index a93586da44..6d067e2db2 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -41,8 +41,9 @@ #endif // Private constants -#define ARMED_THRESHOLD 0.20f -#define GROUND_LOW_THROTTLE 0.01f +#define ARMED_THRESHOLD_SWITCH 0.20f +#define ARMED_THRESHOLD_STICK 0.80f +#define GROUND_LOW_THROTTLE 0.01f // Private types typedef enum { @@ -116,7 +117,7 @@ void armHandler(bool newinit, FrameType_t frameType) } // immediate disarm on switch - if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) { + if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD_SWITCH) { lowThrottle = true; } @@ -209,9 +210,11 @@ void armHandler(bool newinit, FrameType_t frameType) previousArmingInputLevel = 0.0f; } - if ((armingInputLevel <= -ARMED_THRESHOLD) && (previousArmingInputLevel > -ARMED_THRESHOLD)) { + float armedThreshold = armSwitch ? ARMED_THRESHOLD_SWITCH : ARMED_THRESHOLD_STICK; + + if ((armingInputLevel <= -armedThreshold) && (previousArmingInputLevel > -armedThreshold)) { manualArm = true; - } else if ((armingInputLevel >= +ARMED_THRESHOLD) && (previousArmingInputLevel < +ARMED_THRESHOLD)) { + } else if ((armingInputLevel >= +armedThreshold) && (previousArmingInputLevel < +armedThreshold)) { manualDisarm = true; } diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b18f8d8cb1..4e89f10e4b 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -46,6 +46,7 @@ #include #include #include +#include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ @@ -119,9 +120,9 @@ static void commandUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings); +static void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings); #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ static void SettingsUpdatedCb(UAVObjEvent *ev); - #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) /** @@ -166,14 +167,10 @@ int32_t ManualControlInitialize() ManualControlCommandInitialize(); FlightStatusInitialize(); - ManualControlSettingsInitialize(); - FlightModeSettingsInitialize(); - SystemSettingsInitialize(); - StabilizationSettingsInitialize(); AccessoryDesiredInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + SystemAlarmsInitialize(); VtolSelfTuningStatsInitialize(); - VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); SystemSettingsConnectCallback(&SettingsUpdatedCb); #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ @@ -239,6 +236,11 @@ static void manualControlTask(void) uint8_t newFlightModeAssist = flightStatus.FlightModeAssist; uint8_t newAssistedControlState = flightStatus.AssistedControlState; uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState; + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + HandleBatteryFailsafe(&position, &modeSettings); +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ + if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { newMode = modeSettings.FlightModePosition[position]; } @@ -249,7 +251,6 @@ static void manualControlTask(void) newMode = flightStatus.FlightMode; position = lastPosition; } - // if a mode change occurs we default the assist mode and states here // to avoid having to add it to all of the below modes that are // otherwise unrelated @@ -493,7 +494,8 @@ static void manualControlTask(void) if (alwaysStabilizedSwitch) { if (acc.AccessoryVal <= -ALWAYSTABILIZEACCESSORY_THRESHOLD) { newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE; - } else if (acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) { + } else if ((acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) && + (cmd.Throttle >= modeSettings.AlwaysStabilizeWhenArmedThrottleThreshold)) { newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE; } } else { @@ -526,6 +528,87 @@ static void manualControlTask(void) } } +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings) +{ + // static uint8_t lastInputPosition = -1; + typedef enum { BATTERYFAILSAFE_NONE = 0, BATTERYFAILSAFE_WARNING = 1, BATTERYFAILSAFE_CRITICAL = 2 } batteryfailsafemode_t; + static batteryfailsafemode_t lastFailsafeStatus = BATTERYFAILSAFE_NONE; + static bool failsafeOverridden; + static uint8_t lastFlightPosition; + static uint32_t changeTimestamp; + SystemAlarmsAlarmData alarms; + batteryfailsafemode_t failsafeStatus; + FlightStatusArmedOptions armed; + FlightStatusArmedGet(&armed); + + // reset the status and do not change anything when not armed + if (armed != FLIGHTSTATUS_ARMED_ARMED) { + lastFailsafeStatus = BATTERYFAILSAFE_NONE; + failsafeOverridden = false; + changeTimestamp = PIOS_DELAY_GetRaw(); + lastFlightPosition = *position; + return; + } + + SystemAlarmsAlarmGet(&alarms); + + switch (alarms.Battery) { + case SYSTEMALARMS_ALARM_WARNING: + failsafeStatus = BATTERYFAILSAFE_WARNING; + break; + case SYSTEMALARMS_ALARM_CRITICAL: + failsafeStatus = BATTERYFAILSAFE_CRITICAL; + break; + default: + failsafeStatus = BATTERYFAILSAFE_NONE; + break; + } + uint32_t debounceTimerms = PIOS_DELAY_DiffuS(changeTimestamp) / 1000; + + if (failsafeStatus == lastFailsafeStatus) { + changeTimestamp = PIOS_DELAY_GetRaw(); + } else if ((debounceTimerms < modeSettings->BatteryFailsafeDebounceTimer) || failsafeStatus < lastFailsafeStatus) { + // do not change within the "grace" period and do not "downgrade" the failsafe mode + failsafeStatus = lastFailsafeStatus; + } else { + // a higher failsafe status was met and grace period elapsed. Trigger the new state + lastFailsafeStatus = failsafeStatus; + lastFlightPosition = *position; + failsafeOverridden = false; + } + + if ((failsafeStatus == BATTERYFAILSAFE_NONE) || failsafeOverridden) { + return; + } + + // failsafe has been triggered. Check for override + if (lastFlightPosition != *position) { + // flag the override and reset the grace period + failsafeOverridden = true; + changeTimestamp = PIOS_DELAY_GetRaw(); + return; + } + + switch (failsafeStatus) { + case BATTERYFAILSAFE_CRITICAL: + // if critical is not set, jump to the other case to use the warning setting. + if (modeSettings->BatteryFailsafeSwitchPositions.Critical != -1) { + *position = modeSettings->BatteryFailsafeSwitchPositions.Critical; + break; + } + case BATTERYFAILSAFE_WARNING: + if (modeSettings->BatteryFailsafeSwitchPositions.Warning != -1) { + *position = modeSettings->BatteryFailsafeSwitchPositions.Warning; + } + break; + default: + break; + } +} + +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ + /** * Called whenever a critical configuration component changes */ diff --git a/flight/modules/ManualControl/takeofflocationhandler.c b/flight/modules/ManualControl/takeofflocationhandler.c index 93332f2b11..90ed43ce2a 100644 --- a/flight/modules/ManualControl/takeofflocationhandler.c +++ b/flight/modules/ManualControl/takeofflocationhandler.c @@ -40,10 +40,10 @@ static void SetTakeOffLocation(); void takeOffLocationHandlerInit() { - TakeOffLocationInitialize(); // check whether there is a preset/valid takeoff location TakeOffLocationModeOptions mode; TakeOffLocationStatusOptions status; + TakeOffLocationModeGet(&mode); TakeOffLocationStatusGet(&status); // preset with invalid location will actually behave like FirstTakeoff diff --git a/flight/modules/Notify/notify.c b/flight/modules/Notify/notify.c index ed3f7ded28..56bde610d5 100644 --- a/flight/modules/Notify/notify.c +++ b/flight/modules/Notify/notify.c @@ -37,7 +37,6 @@ #include "inc/notify.h" #include "inc/sequences.h" #include -#include #define SAMPLE_PERIOD_MS 250 // private types @@ -54,13 +53,7 @@ static void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_ti static AlarmStatus_t *alarmStatus; int32_t NotifyInitialize(void) { - uint8_t ws281xOutStatus; - - HwSettingsWS2811LED_OutGet(&ws281xOutStatus); - // Todo: Until further applications exists for WS2811 notify enabled status is tied to ws281x output configuration - bool enabled = ws281xOutStatus != HWSETTINGS_WS2811LED_OUT_DISABLED; - - if (enabled) { + if (PIOS_WS2811_DEVICE) { alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize); for (uint8_t i = 0; i < alarmsMapSize; i++) { alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK; diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index 6727cdde34..dd27fdcb9e 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -16,7 +16,8 @@ * @{ * * @file oplinkmod.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief System module * * @see The GNU Public License (GPL) Version 3 @@ -67,9 +68,11 @@ static xTaskHandle systemTaskHandle; static bool stackOverflow; static bool mallocFailed; volatile int initTaskDone = 0; +static uint8_t previousRFXtalCap; // Private functions static void systemTask(void *parameters); +static void oplinkSettingsUpdatedCb(UAVObjEvent *ev); /** * Create the module task. @@ -119,6 +122,11 @@ static void systemTask(__attribute__((unused)) void *parameters) while (!initTaskDone) { vTaskDelay(10); } + +#ifndef PIOS_INCLUDE_WDG +// if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop +#define PIOS_WDG_Clear() +#endif /* create all modules thread */ MODULE_TASKCREATE_ALL; @@ -133,6 +141,10 @@ static void systemTask(__attribute__((unused)) void *parameters) PIOS_SYS_Reset(); } + // Initialize previousRFXtalCap used by callback + OPLinkSettingsRFXtalCapGet(&previousRFXtalCap); + OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb); + // Initialize vars lastSysTime = xTaskGetTickCount(); @@ -142,9 +154,6 @@ static void systemTask(__attribute__((unused)) void *parameters) OPLinkStatusData oplinkStatus; OPLinkStatusGet(&oplinkStatus); - // Get the other device stats. - PIOS_RFM22B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); - // Get the stats from the radio device struct rfm22b_stats radio_stats; PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); @@ -152,18 +161,19 @@ static void systemTask(__attribute__((unused)) void *parameters) if (pios_rfm22b_id) { // Update the status - oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - oplinkStatus.RxGood = radio_stats.rx_good; - oplinkStatus.RxCorrected = radio_stats.rx_corrected; - oplinkStatus.RxErrors = radio_stats.rx_error; - oplinkStatus.RxMissed = radio_stats.rx_missed; - oplinkStatus.RxFailure = radio_stats.rx_failure; - oplinkStatus.TxDropped = radio_stats.tx_dropped; - oplinkStatus.TxFailure = radio_stats.tx_failure; - oplinkStatus.Resets = radio_stats.resets; - oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; + oplinkStatus.TxFailure = radio_stats.tx_failure; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; - oplinkStatus.LinkQuality = radio_stats.link_quality; + oplinkStatus.LinkQuality = radio_stats.link_quality; + oplinkStatus.AFCCorrection = radio_stats.afc_correction; if (first_time) { first_time = false; } else { @@ -241,6 +251,23 @@ void vApplicationMallocFailedHook(void) #endif } +/** + * Called whenever OPLink settings changed + */ +static void oplinkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + uint8_t currentRFXtalCap; + + OPLinkSettingsRFXtalCapGet(¤tRFXtalCap); + + // Check if RFXtalCap value changed + if (currentRFXtalCap != previousRFXtalCap) { + PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, currentRFXtalCap); + PIOS_RFM22B_Reinit(pios_rfm22b_id); + previousRFXtalCap = currentRFXtalCap; + } +} + /** * @} * @} diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index fdf74f072a..3aa0b822df 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -450,7 +450,7 @@ static void Run(void) // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude); // GPS Status - if (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) { + if ((positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) { msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX; } else { msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX; diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index fd8e8e67d8..65dbdbe052 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -2426,11 +2426,7 @@ int32_t osdgenInitialize(void) GPSTimeInitialize(); GPSSatellitesInitialize(); #endif -#ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationInitialize(); #endif -#endif - OsdSettingsInitialize(); BaroSensorInitialize(); FlightStatusInitialize(); diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c index f46c61a577..09d5e2c95a 100644 --- a/flight/modules/Osd/osdinput/osdinput.c +++ b/flight/modules/Osd/osdinput/osdinput.c @@ -35,7 +35,6 @@ #include "osdinput.h" #include "attitudestate.h" -#include "taskinfo.h" #include "flightstatus.h" #include "fifo_buffer.h" diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 88f84d8f72..76de3d5f8a 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 { @@ -275,7 +277,6 @@ static int32_t osdoutputInitialize(void) #ifdef MODULE_OSDOUTPUT_BUILTIN osdoutputEnabled = 1; #else - HwSettingsInitialize(); HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); if (optionalModules.OsdHk == HWSETTINGS_OPTIONALMODULES_ENABLED) { @@ -284,6 +285,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/modules/OveroSync/overosync.c b/flight/modules/OveroSync/overosync.c index 38e0be1c3d..ed55207f5e 100644 --- a/flight/modules/OveroSync/overosync.c +++ b/flight/modules/OveroSync/overosync.c @@ -81,7 +81,6 @@ int32_t OveroSyncInitialize(void) overoEnabled = true; #else - HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); diff --git a/flight/modules/PathFollower/grounddrivecontroller.cpp b/flight/modules/PathFollower/grounddrivecontroller.cpp index 3397a630ed..f511db3043 100644 --- a/flight/modules/PathFollower/grounddrivecontroller.cpp +++ b/flight/modules/PathFollower/grounddrivecontroller.cpp @@ -27,8 +27,6 @@ extern "C" { #include -#include - #include #include #include diff --git a/flight/modules/PathFollower/inc/vtolautotakeofffsm.h b/flight/modules/PathFollower/inc/vtolautotakeofffsm.h index 3f8badb704..adbc76a380 100644 --- a/flight/modules/PathFollower/inc/vtolautotakeofffsm.h +++ b/flight/modules/PathFollower/inc/vtolautotakeofffsm.h @@ -3,13 +3,13 @@ * @addtogroup LibrePilotModules LibrePilot Modules * @{ * @addtogroup PathFollower FSM - * @brief Executes landing sequence via an FSM + * @brief Executes auto takeoff sequence via an FSM * @{ * - * @file vtollandfsm.h - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @file vtolautotakeofffsm.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018 * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. - * @brief Executes FSM for landing sequence + * @brief Executes FSM for auto takeoff sequence * * @see The GNU Public License (GPL) Version 3 * @@ -144,7 +144,7 @@ class VtolAutoTakeoffFSM : public PathFollowerFSM { int32_t runAlways(); void updateVtolAutoTakeoffFSMStatus(); - void assessAltitude(void); + float assessAltitude(void); void setStateTimeout(int32_t count); diff --git a/flight/modules/PathFollower/inc/vtollandfsm.h b/flight/modules/PathFollower/inc/vtollandfsm.h index 4c582a00aa..5c3fe8e754 100644 --- a/flight/modules/PathFollower/inc/vtollandfsm.h +++ b/flight/modules/PathFollower/inc/vtollandfsm.h @@ -7,7 +7,8 @@ * @{ * * @file vtollandfsm.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief Executes FSM for landing sequence * * @see The GNU Public License (GPL) Version 3 @@ -133,7 +134,7 @@ class VtolLandFSM : public PathFollowerFSM { int32_t runState(); int32_t runAlways(); void updateVtolLandFSMStatus(); - void assessAltitude(void); + float assessAltitude(void); void setStateTimeout(int32_t count); diff --git a/flight/modules/PathFollower/pathfollower.cpp b/flight/modules/PathFollower/pathfollower.cpp index 562b2e9b3f..b7f45328f4 100644 --- a/flight/modules/PathFollower/pathfollower.cpp +++ b/flight/modules/PathFollower/pathfollower.cpp @@ -154,12 +154,8 @@ extern "C" int32_t PathFollowerStart() extern "C" int32_t PathFollowerInitialize() { // initialize objects - GroundPathFollowerSettingsInitialize(); - FixedWingPathFollowerSettingsInitialize(); FixedWingPathFollowerStatusInitialize(); - VtolPathFollowerSettingsInitialize(); FlightStatusInitialize(); - FlightModeSettingsInitialize(); PathStatusInitialize(); PathSummaryInitialize(); PathDesiredInitialize(); @@ -169,10 +165,8 @@ extern "C" int32_t PathFollowerInitialize() StabilizationDesiredInitialize(); AirspeedStateInitialize(); AttitudeStateInitialize(); - TakeOffLocationInitialize(); PoiLocationInitialize(); ManualControlCommandInitialize(); - SystemSettingsInitialize(); StabilizationBankInitialize(); VtolSelfTuningStatsInitialize(); PIDStatusInitialize(); @@ -181,7 +175,6 @@ extern "C" int32_t PathFollowerInitialize() StatusVtolAutoTakeoffInitialize(); // VtolLandFSM additional objects - HomeLocationInitialize(); AccelStateInitialize(); // Init references to controllers @@ -484,7 +477,7 @@ static void updateFixedAttitude(float *attitude) stabDesired.Thrust = attitude[3]; stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } diff --git a/flight/modules/PathFollower/pidcontrolne.cpp b/flight/modules/PathFollower/pidcontrolne.cpp index 5aa6f7460d..b947e69526 100644 --- a/flight/modules/PathFollower/pidcontrolne.cpp +++ b/flight/modules/PathFollower/pidcontrolne.cpp @@ -7,7 +7,8 @@ * @{ * * @file PIDControlNE.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief Executes PID control loops for NE directions * * @see The GNU Public License (GPL) Version 3 @@ -31,8 +32,6 @@ extern "C" { #include -#include - #include #include #include @@ -81,26 +80,37 @@ void PIDControlNE::Activate() void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax) { - // pid_configure(&PID, kp, ki, kd, ilimit); - float Ti = kp / ki; - float Td = kd / kp; - float Tt = (Ti + Td) / 2.0f; - float kt = 1.0f / Tt; + float Td; // Derivative time constant + float Ti; // Integral time constant + float kt; // Feedback gain for integral windup avoidance + float N = 10.0f; // N is the factor used to determine the + // time constant for derivative filtering + // Why 10? Maybe should be configurable? + float Tf; // Low pass filter time constant for derivative filtering + float u0 = 0.0f; - float N = 10.0f; - float Tf = Td / N; - - if (ki < 1e-6f) { - // Avoid Ti being infinite - Ti = 1e6f; - // Tt antiwindup time constant - we don't need antiwindup with no I term - Tt = 1e6f; + + // Define Td, handling zero kp term (for I or ID controller) + if (kp < 1e-6f) { + Td = 1e6f; + } else { + Td = kd / kp; + } + + // Define Ti, Tt and kt, handling zero ki term (for P or PD controller) + if (ki < 1e-6f) { // Avoid Ti being infinite kt = 0.0f; + } else { + Ti = kp / ki; + kt = 1.0f / Ti; } + // Define Tf, according to controller type if (kd < 1e-6f) { - // PI Controller - Tf = Ti / N; + // PI Controller or P Controller + Tf = 0; + } else { + Tf = Td / N; } if (beta > 1.0f) { diff --git a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp index 0c0bc64901..d4e6240f51 100644 --- a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp +++ b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp @@ -1,10 +1,10 @@ /* ****************************************************************************** * - * @file vtollandcontroller.cpp - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @file vtolautotakeoffcontroller.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018 * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. - * @brief Vtol landing controller loop + * @brief Vtol auto takeoff controller loop * @see The GNU Public License (GPL) Version 3 * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation * @@ -28,8 +28,6 @@ extern "C" { #include -#include - #include #include #include @@ -134,11 +132,11 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void) if (mOverride) { // override pathDesired from PathPlanner with current position, // as we deliberately don't care about the location of the waypoints on the map - float velocity_down; + float autotakeoff_velocity; float autotakeoff_height; PositionStateData positionState; PositionStateGet(&positionState); - FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down); + FlightModeSettingsAutoTakeOffVelocityGet(&autotakeoff_velocity); FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height); autotakeoff_height = fabsf(autotakeoff_height); if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) { @@ -146,7 +144,7 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void) } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) { autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX; } - controlDown.UpdateVelocitySetpoint(velocity_down); + controlDown.UpdateVelocitySetpoint(-autotakeoff_velocity); controlNE.UpdateVelocitySetpoint(0.0f, 0.0f); controlNE.UpdatePositionSetpoint(positionState.North, positionState.East); @@ -322,9 +320,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot() // 1. Arming must be done whilst in the AutoTakeOff flight mode // 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first // 3. Wait for armed state - // 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff. - // 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored. - // 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated. + // 4. Once the user increases the throttle position to above 30%, then and only then initiate auto-takeoff. + // 5. Whilst the throttle is < 30% before takeoff, all stick inputs are being ignored. + // 6. If during the autotakeoff sequence, at any stage, the throttle stick position reduces to less than 10%, landing is initiated. switch (autotakeoffState) { case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST: diff --git a/flight/modules/PathFollower/vtolautotakeofffsm.cpp b/flight/modules/PathFollower/vtolautotakeofffsm.cpp index 524686d1fe..372e01af60 100644 --- a/flight/modules/PathFollower/vtolautotakeofffsm.cpp +++ b/flight/modules/PathFollower/vtolautotakeofffsm.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file vtolautotakeofffsm.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief This autotakeoff state machine is a helper state machine to the * VtolAutoTakeoffController. * @see The GNU Public License (GPL) Version 3 @@ -27,8 +28,6 @@ extern "C" { #include -#include - #include #include #include @@ -62,11 +61,11 @@ extern "C" { // Private constants -#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod) -#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND) -#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND) -#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND) -#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f +#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod) +#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND) +#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND) +#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND) +#define AUTOTAKEOFF_SLOWDOWN_HEIGHT -5.0f VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = { [AUTOTAKEOFF_STATE_INACTIVE] = { .setup = &VtolAutoTakeoffFSM::setup_inactive, .run = 0 }, @@ -248,14 +247,8 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St mAutoTakeoffData->currentState = newState; if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) { - PositionStateData positionState; - PositionStateGet(&positionState); - float takeOffDown = 0.0f; - if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) { - takeOffDown = mAutoTakeoffData->takeOffLocation.Down; - } - mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown; - assessAltitude(); + float altitudeAboveTakeoff = assessAltitude(); + mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = altitudeAboveTakeoff; } // Restart state timer counter @@ -268,6 +261,7 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St (this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)(); } + // Update StatusVtolAutoTakeoff UAVObject with new status updateVtolAutoTakeoffFSMStatus(); } @@ -278,6 +272,7 @@ void VtolAutoTakeoffFSM::setStateTimeout(int32_t count) mAutoTakeoffData->stateTimeoutCount = count; } +// Update StatusVtolAutoTakeoff UAVObject void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus() { mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState; @@ -290,7 +285,7 @@ void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus() } -void VtolAutoTakeoffFSM::assessAltitude(void) +float VtolAutoTakeoffFSM::assessAltitude(void) { float positionDown; @@ -300,11 +295,13 @@ void VtolAutoTakeoffFSM::assessAltitude(void) takeOffDown = mAutoTakeoffData->takeOffLocation.Down; } float positionDownRelativeToTakeoff = positionDown - takeOffDown; - if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) { + if (positionDownRelativeToTakeoff < AUTOTAKEOFF_SLOWDOWN_HEIGHT) { mAutoTakeoffData->flLowAltitude = false; } else { mAutoTakeoffData->flLowAltitude = true; } + // Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff + return -positionDownRelativeToTakeoff; } // Action the required state from plans.c @@ -349,10 +346,10 @@ void VtolAutoTakeoffFSM::setup_checkstate(void) // Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF // 1. Already armed // 2. Not in flight. This was checked in plans.c - // 3. User has placed throttle position to more than 50% to allow autotakeoff + // 3. User has placed throttle position to more than 30% to allow autotakeoff // If pathplanner, we need additional checks - // E.g. if inflight, this mode is just positon hol + // E.g. if inflight, this mode is just position hold StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); @@ -409,15 +406,13 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout } } -// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect. +// STATE: THRUSTUP spools up motors to vtol min over 1 second for effect. // PID loops may be cumulating I terms but that problem needs to be solved #define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f void VtolAutoTakeoffFSM::setup_thrustup(void) { setStateTimeout(TIMEOUT_THRUSTUP); mAutoTakeoffData->flZeroStabiHorizontal = false; - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max; mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP; mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp index 960e330de2..4c763541c7 100644 --- a/flight/modules/PathFollower/vtolbrakecontroller.cpp +++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp @@ -31,8 +31,6 @@ extern "C" { #include -#include - #include #include #include @@ -291,7 +289,7 @@ int8_t VtolBrakeController::UpdateStabilizationDesired(void) ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; // default thrust mode to cruise control diff --git a/flight/modules/PathFollower/vtolbrakefsm.cpp b/flight/modules/PathFollower/vtolbrakefsm.cpp index 543f526fbb..a8e92e7d26 100644 --- a/flight/modules/PathFollower/vtolbrakefsm.cpp +++ b/flight/modules/PathFollower/vtolbrakefsm.cpp @@ -27,8 +27,6 @@ extern "C" { #include -#include - #include #include #include diff --git a/flight/modules/PathFollower/vtolflycontroller.cpp b/flight/modules/PathFollower/vtolflycontroller.cpp index 2ba3592a9e..f3c7f2ad70 100644 --- a/flight/modules/PathFollower/vtolflycontroller.cpp +++ b/flight/modules/PathFollower/vtolflycontroller.cpp @@ -26,8 +26,6 @@ extern "C" { #include -#include - #include #include #include @@ -276,7 +274,9 @@ int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float ya } #endif // if 0 - if (yaw_attitude) { + // Yaw Attitude will be disabled without velocity requested. + // PositionHold, AutoTakeoff or AutoCruise still using manual Yaw. + if (yaw_attitude && ((fabsf(pathDesired->StartingVelocity) > 0.0f) && (fabsf(pathDesired->EndingVelocity) > 0.0f))) { stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Yaw = yaw_direction; } else { @@ -343,7 +343,7 @@ void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback() stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; StabilizationDesiredSet(&stabDesired); } diff --git a/flight/modules/PathFollower/vtollandcontroller.cpp b/flight/modules/PathFollower/vtollandcontroller.cpp index 7362526bf1..31740669f5 100644 --- a/flight/modules/PathFollower/vtollandcontroller.cpp +++ b/flight/modules/PathFollower/vtollandcontroller.cpp @@ -28,8 +28,6 @@ extern "C" { #include -#include - #include #include #include diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp index 267299bc6c..236acdc72c 100644 --- a/flight/modules/PathFollower/vtollandfsm.cpp +++ b/flight/modules/PathFollower/vtollandfsm.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file vtollandfsm.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief This landing state machine is a helper state machine to the * VtolLandController. * @see The GNU Public License (GPL) Version 3 @@ -27,8 +28,6 @@ extern "C" { #include -#include - #include #include #include @@ -277,14 +276,8 @@ void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandSt mLandData->currentState = newState; if (newState != STATUSVTOLLAND_STATE_INACTIVE) { - PositionStateData positionState; - PositionStateGet(&positionState); - float takeOffDown = 0.0f; - if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) { - takeOffDown = mLandData->takeOffLocation.Down; - } - mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown; - assessAltitude(); + float altitudeAboveTakeoff = assessAltitude(); + mLandData->fsmLandStatus.AltitudeAtState[newState] = altitudeAboveTakeoff; } // Restart state timer counter @@ -334,7 +327,7 @@ float VtolLandFSM::BoundVelocityDown(float velocity_down) } } -void VtolLandFSM::assessAltitude(void) +float VtolLandFSM::assessAltitude(void) { float positionDown; @@ -349,6 +342,8 @@ void VtolLandFSM::assessAltitude(void) } else { mLandData->flLowAltitude = true; } + // Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff + return -positionDownRelativeToTakeoff; } diff --git a/flight/modules/PathFollower/vtolvelocitycontroller.cpp b/flight/modules/PathFollower/vtolvelocitycontroller.cpp index 3934335ce2..ae6e4e7c18 100644 --- a/flight/modules/PathFollower/vtolvelocitycontroller.cpp +++ b/flight/modules/PathFollower/vtolvelocitycontroller.cpp @@ -26,8 +26,6 @@ extern "C" { #include -#include - #include #include #include @@ -182,7 +180,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused) ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; // default thrust mode to altvario diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 0faeeafe2c..833c208c8f 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -222,7 +222,8 @@ static void pathPlannerTask() } // the transition from pathplanner to another flightmode back to pathplanner - // triggers a reset back to 0 index in the waypoint list + // triggers a reset back either to 0 index in the waypoint list, + // or to current index in the waypoint list, depending on FlightModeChangeRestartsPathPlan setting if (pathplanner_active == false) { pathplanner_active = true; @@ -230,8 +231,10 @@ static void pathPlannerTask() FlightModeSettingsFlightModeChangeRestartsPathPlanGet(&restart); if (restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE) { setWaypoint(0); - return; + } else { + setWaypoint(waypointActive.Index); } + return; } WaypointInstGet(waypointActive.Index, &waypoint); @@ -601,8 +604,7 @@ static uint8_t conditionBelowError() /** * the AboveAltitude measures the flight altitude relative to home position * returns true if above critical altitude - * WARNING! Altitudes are always negative (down coordinate) - * Parameter 0: altitude in meters (negative!) + * Parameter 0: altitude in meters */ static uint8_t conditionAboveAltitude() { @@ -610,7 +612,7 @@ static uint8_t conditionAboveAltitude() PositionStateGet(&positionState); - if (positionState.Down <= pathAction.ConditionParameters[0]) { + if (-positionState.Down >= pathAction.ConditionParameters[0]) { return true; } return false; diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index a9b4c70aeb..55e926c4b5 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -41,24 +41,23 @@ #include #include #include -#if defined(PIOS_INCLUDE_FLASH_EEPROM) -#include -#endif - +#include #include +#include // **************** // Private constants -#define STACK_SIZE_BYTES 150 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define MAX_RETRIES 2 -#define RETRY_TIMEOUT_MS 20 -#define EVENT_QUEUE_SIZE 10 -#define MAX_PORT_DELAY 200 -#define SERIAL_RX_BUF_LEN 100 -#define PPM_INPUT_TIMEOUT 100 +#define TELEM_STACK_SIZE_WORDS 150 +#define PPM_STACK_SIZE_WORDS 150 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_RETRIES 2 +#define RETRY_TIMEOUT_MS 20 +#define EVENT_QUEUE_SIZE 10 +#define MAX_PORT_DELAY 200 +#define PPM_INPUT_TIMEOUT 100 +#define PIOS_PPM_RECEIVER pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] // **************** // Private types @@ -67,10 +66,9 @@ typedef struct { // The task handles. xTaskHandle telemetryTxTaskHandle; xTaskHandle telemetryRxTaskHandle; - xTaskHandle radioTxTaskHandle; - xTaskHandle radioRxTaskHandle; + xTaskHandle telemRadioTxTaskHandle; + xTaskHandle telemRadioRxTaskHandle; xTaskHandle PPMInputTaskHandle; - xTaskHandle serialRxTaskHandle; // The UAVTalk connection on the com side. UAVTalkConnection telemUAVTalkCon; @@ -80,21 +78,12 @@ typedef struct { xQueueHandle uavtalkEventQueue; xQueueHandle radioEventQueue; - // The raw serial Rx buffer - uint8_t serialRxBuf[SERIAL_RX_BUF_LEN]; - // Error statistics. - uint32_t telemetryTxRetries; - uint32_t radioTxRetries; + uint32_t telemetryTxRetries; + uint32_t radioTxRetries; // Is this modem the coordinator - bool isCoordinator; - - // Should we parse UAVTalk? - bool parseUAVTalk; - - // The current configured uart speed - OPLinkSettingsComSpeedOptions comSpeed; + bool isCoordinator; } RadioComBridgeData; // **************** @@ -102,9 +91,8 @@ typedef struct { static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); -static void serialRxTask(void *parameters); -static void radioTxTask(void *parameters); -static void radioRxTask(void *parameters); +static void telemRadioTxTask(void *parameters); +static void telemRadioRxTask(void *parameters); static void PPMInputTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); @@ -133,11 +121,6 @@ static int32_t RadioComBridgeStart(void) // Check if this is the coordinator modem data->isCoordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR); - // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); - // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: @@ -186,34 +169,37 @@ static int32_t RadioComBridgeStart(void) ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. - xTaskCreate(telemetryTxTask, "telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); - xTaskCreate(telemetryRxTask, "telemetryRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle)); - if (PIOS_PPM_RECEIVER != 0) { - xTaskCreate(PPMInputTask, "PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->PPMInputTaskHandle)); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); + // These tasks are always needed at least for configuration over HID. + xTaskCreate(telemetryTxTask, "telemetryTxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); + xTaskCreate(telemetryRxTask, "telemetryRxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle)); +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYTX) + PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYTX); #endif - } - if (!data->parseUAVTalk) { - // If the user wants raw serial communication, we need to spawn another thread to handle it. - xTaskCreate(serialRxTask, "serialRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->serialRxTaskHandle)); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_SERIALRX); +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYRX) + PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYRX); +#endif + // Start the tasks for sending/receiving telemetry over a radio link if a telemetry (GCS) link configured. + if (PIOS_COM_GCS_OUT) { + xTaskCreate(telemRadioTxTask, "telemRadioTxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemRadioTxTaskHandle)); + xTaskCreate(telemRadioRxTask, "telemRadioRxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemRadioRxTaskHandle)); +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIOTX) + PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMRADIOTX); +#endif +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIORX) + PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMRADIORX); #endif } - xTaskCreate(radioTxTask, "radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); - xTaskCreate(radioRxTask, "radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); - // Register the watchdog timers. -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYTX); - PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYRX); - PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOTX); - PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX); + // Start the task for reading and processing PPM data if it's configured. + if (PIOS_PPM_RECEIVER) { + xTaskCreate(PPMInputTask, "PPMInputTask", PPM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->PPMInputTaskHandle)); +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_PPMINPUT) + PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); #endif + } + return 0; } - return -1; } @@ -248,10 +234,6 @@ static int32_t RadioComBridgeInitialize(void) data->telemetryTxRetries = 0; data->radioTxRetries = 0; - data->parseUAVTalk = true; - data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; - PIOS_COM_RADIO = PIOS_COM_RFM22B; - return 0; } MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart); @@ -323,9 +305,48 @@ static void updateRadioComBridgeStats() } /** - * @brief Telemetry transmit task, regular priority + * @brief Reads the PPM input device and sends out OPLinkReceiver objects. * - * @param[in] parameters The task parameters + * @param[in] parameters The task parameters (unused) + */ +static void PPMInputTask(__attribute__((unused)) void *parameters) +{ + xSemaphoreHandle sem = PIOS_RCVR_GetSemaphore(PIOS_PPM_RECEIVER, 1); + int16_t channels[RFM22B_PPM_NUM_CHANNELS]; + + while (1) { +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_PPMINPUT) + PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); +#endif + + // Wait for the receiver semaphore. + if (xSemaphoreTake(sem, PPM_INPUT_TIMEOUT) == pdTRUE) { + // Read the receiver inputs. + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); + } + } else { + // Failsafe + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + channels[i] = PIOS_RCVR_INVALID; + } + } + + // Pass the channel values to the radio device. + PIOS_RFM22B_PPMSet(pios_rfm22b_id, channels, RFM22B_PPM_NUM_CHANNELS); + } +} + + +/**************************************************************************** +* Telemetry tasks and functions +****************************************************************************/ + +/** + * @brief Receives events on the Radio->GCS telemetry stream and sends + * the requested object(s) to the GCS. + * + * @param[in] parameters None. */ static void telemetryTxTask(__attribute__((unused)) void *parameters) { @@ -333,7 +354,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) // Loop forever while (1) { -#ifdef PIOS_INCLUDE_WDG +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYTX) PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYTX); #endif // Wait for queue message @@ -357,16 +378,17 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) } /** - * @brief Radio tx task. Receive data packets from the com port and send to the radio. + * @brief Receives events on the GCS->Radio telemetry stream and sends + * the requested object(s) over the radio telenetry stream. * - * @param[in] parameters The task parameters + * @param[in] parameters none. */ -static void radioTxTask(__attribute__((unused)) void *parameters) +static void telemRadioTxTask(__attribute__((unused)) void *parameters) { // Task loop while (1) { -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX); +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIOTX) + PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMRADIOTX); #endif // Process the radio event queue, sending UAVObjects over the radio link as necessary. @@ -391,34 +413,24 @@ static void radioTxTask(__attribute__((unused)) void *parameters) } /** - * @brief Radio rx task. Receive data packets from the radio and pass them on. + * @brief Reads data from the radio telemetry port and processes it + * through the Radio->GCS telemetry stream. * * @param[in] parameters The task parameters */ -static void radioRxTask(__attribute__((unused)) void *parameters) +static void telemRadioRxTask(__attribute__((unused)) void *parameters) { // Task loop while (1) { -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIORX) + PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMRADIORX); #endif - if (PIOS_COM_RADIO) { + if (PIOS_COM_GCS_OUT) { uint8_t serial_data[16]; - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_GCS_OUT, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - if (data->parseUAVTalk) { - // Pass the data through the UAVTalk parser. - ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process); - } else if (PIOS_COM_TELEMETRY) { - // Send the data straight to the telemetry port. - // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex) - // It is the caller responsibility to retry in such cases... - int32_t ret = -2; - uint8_t count = 5; - while (count-- > 0 && ret < -1) { - ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); - } - } + // Pass the data through the UAVTalk parser. + ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process); } } else { vTaskDelay(5); @@ -427,7 +439,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters) } /** - * @brief Receive telemetry from the USB/COM port. + * @brief Reads data from the GCS telemetry port and processes it + * through the GCS->Radio telemetry stream. * * @param[in] parameters The task parameters */ @@ -435,14 +448,15 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) { // Task loop while (1) { - uint32_t inputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; -#ifdef PIOS_INCLUDE_WDG + // uint32_t inputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; + uint32_t inputPort = PIOS_COM_GCS; +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYRX) PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYRX); #endif #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB_HID) { - inputPort = PIOS_COM_TELEM_USB_HID; + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_HID) { + inputPort = PIOS_COM_HID; } #endif /* PIOS_INCLUDE_USB */ if (inputPort) { @@ -458,73 +472,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } /** - * @brief Reads the PPM input device and sends out OPLinkReceiver objects. - * - * @param[in] parameters The task parameters (unused) - */ -static void PPMInputTask(__attribute__((unused)) void *parameters) -{ - xSemaphoreHandle sem = PIOS_RCVR_GetSemaphore(PIOS_PPM_RECEIVER, 1); - int16_t channels[RFM22B_PPM_NUM_CHANNELS]; - - while (1) { - #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); - #endif - - // Wait for the receiver semaphore. - if (xSemaphoreTake(sem, PPM_INPUT_TIMEOUT) == pdTRUE) { - // Read the receiver inputs. - for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { - channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); - } - } else { - // Failsafe - for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { - channels[i] = PIOS_RCVR_INVALID; - } - } - - // Pass the channel values to the radio device. - PIOS_RFM22B_PPMSet(pios_rfm22b_id, channels, RFM22B_PPM_NUM_CHANNELS); - } -} - -/** - * @brief Receive raw serial data from the USB/COM port. - * - * @param[in] parameters The task parameters - */ -static void serialRxTask(__attribute__((unused)) void *parameters) -{ - // Task loop - while (1) { - uint32_t inputPort = PIOS_COM_TELEMETRY; -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_SERIALRX); -#endif - if (inputPort && PIOS_COM_RADIO) { - // Receive some data. - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY); - - if (bytes_to_process > 0) { - // Send the data over the radio link. - // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex) - // It is the caller responsibility to retry in such cases... - int32_t ret = -2; - uint8_t count = 5; - while (count-- > 0 && ret < -1) { - ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process); - } - } - } else { - vTaskDelay(5); - } - } -} - -/** - * @brief Transmit data buffer to the com port. + * @brief Send data from the Radio->GCS telemetry stream to the GCS port. * * @param[in] buf Data buffer to send * @param[in] length Length of buffer @@ -534,12 +482,12 @@ static void serialRxTask(__attribute__((unused)) void *parameters) static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { int32_t ret; - uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; + uint32_t outputPort = PIOS_COM_GCS; #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) - if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) { - outputPort = PIOS_COM_TELEM_USB_HID; + if (PIOS_COM_HID && PIOS_COM_Available(PIOS_COM_HID)) { + outputPort = PIOS_COM_HID; } #endif /* PIOS_INCLUDE_USB */ if (outputPort) { @@ -557,7 +505,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) } /** - * Transmit data buffer to the com port. + * @brief Send data from the GCS telemetry stream to the Radio->GCS port. * * @param[in] buf Data buffer to send * @param[in] length Length of buffer @@ -566,10 +514,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) */ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { - if (!data->parseUAVTalk) { - return length; - } - uint32_t outputPort = PIOS_COM_RADIO; + uint32_t outputPort = PIOS_COM_GCS_OUT; // Don't send any data unless the radio port is available. if (outputPort && PIOS_COM_Available(outputPort)) { @@ -606,10 +551,8 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk switch (objId) { case OPLINKSTATUS_OBJID: case OPLINKSETTINGS_OBJID: - case OPLINKRECEIVER_OBJID: case MetaObjectId(OPLINKSTATUS_OBJID): case MetaObjectId(OPLINKSETTINGS_OBJID): - case MetaObjectId(OPLINKRECEIVER_OBJID): UAVTalkReceiveObject(inConnectionHandle); break; case OBJECTPERSISTENCE_OBJID: @@ -668,10 +611,9 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn case OPLINKRECEIVER_OBJID: case MetaObjectId(OPLINKRECEIVER_OBJID): // Receive object locally - // These objects are received by the modem and are not transmitted to the telemetry port - // - OPLINKRECEIVER_OBJID : not sure why - // some objects will send back a response to the remote modem UAVTalkReceiveObject(inConnectionHandle); + // Also send the packet to the telemetry point. + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); break; default: // all other packets are relayed to the telemetry port diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 900e1194c7..8b5643c5b8 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -10,7 +10,7 @@ * pass it to ManualControl * * @file receiver.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Receiver module. Handles safety R/C link and flight mode. * @@ -64,12 +64,11 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control #define UPDATE_PERIOD_MS 20 -#define THROTTLE_FAILSAFE -0.1f -#define ARMED_THRESHOLD 0.50f + // safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 250 -#define ASSISTEDCONTROL_DEADBAND_MINIMUM 2 // minimum value for a well bahaved Tx, in percent. +#define ASSISTEDCONTROL_DEADBAND_MINIMUM 2 // minimum value for a well behaved Tx, in percent. // Private types @@ -114,7 +113,8 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); static void resetRcvrStatus(struct rcvr_activity_fsm *fsm); static bool updateRcvrStatus( struct rcvr_activity_fsm *fsm, - ManualControlSettingsChannelGroupsOptions group); + ManualControlSettingsChannelGroupsOptions group, + int8_t rssiValue); #define assumptions \ ( \ @@ -151,13 +151,9 @@ int32_t ReceiverInitialize() ManualControlCommandInitialize(); ReceiverActivityInitialize(); ReceiverStatusInitialize(); - ManualControlSettingsInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES - StabilizationSettingsInitialize(); - VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); #endif - SystemSettingsInitialize(); SystemSettingsConnectCallback(&SettingsUpdatedCb); @@ -198,21 +194,21 @@ static void receiverTask(__attribute__((unused)) void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; - FlightStatusData flightStatus; + uint8_t armed; uint8_t disconnected_count = 0; uint8_t connected_count = 0; // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // this includes not even registering it if not used - AccessoryDesiredCreateInstance(); - AccessoryDesiredCreateInstance(); - AccessoryDesiredCreateInstance(); + AccessoryDesiredCreateInstance(); // Accessory1 + AccessoryDesiredCreateInstance(); // Accessory2 + AccessoryDesiredCreateInstance(); // Accessory3 - // Whenever the configuration changes, make sure it is safe to fly + // Rssi input + AccessoryDesiredCreateInstance(); ManualControlCommandGet(&cmd); - FlightStatusGet(&flightStatus); /* Initialize the RcvrActivty FSM */ portTickType lastActivityTime = xTaskGetTickCount(); @@ -232,20 +228,26 @@ static void receiverTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); #endif + int8_t rssiValue = -1; + // Read settings ManualControlSettingsGet(&settings); SystemSettingsThrustControlGet(&thrustType); + FlightStatusArmedGet(&armed); /* Update channel activity monitor */ - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { + if (armed == FLIGHTSTATUS_ARMED_DISARMED) { if (updateRcvrActivity(&activity_fsm)) { /* Reset the aging timer because activity was detected */ lastActivityTime = lastSysTime; } - /* Read signal quality from the group used for the throttle */ - (void)updateRcvrStatus(&activity_fsm, - settings.ChannelGroups.Throttle); } + + /* Read signal quality from the group used for the throttle */ + (void)updateRcvrStatus(&activity_fsm, + settings.ChannelGroups.Throttle, + rssiValue); + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { resetRcvrActivity(&activity_fsm); resetRcvrStatus(&activity_fsm); @@ -267,6 +269,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) } bool valid_input_detected = true; + bool valid_rssi_input = false; // Read channel values in us for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { @@ -285,16 +288,27 @@ static void receiverTask(__attribute__((unused)) void *parameters) if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { valid_input_detected = false; } else { + int16_t neutralValue = ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n]; + + if (n == MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI) { + // Rssi neutral is ignored and set to min, for 0 - 100% output range + neutralValue = ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n]; + valid_rssi_input = true; + } scaledChannel[n] = scaleChannel(cmd.Channel[n], ManualControlSettingsChannelMaxToArray(settings.ChannelMax)[n], ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n], - ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n]); + neutralValue); } } + if (valid_rssi_input) { + rssiValue = (int8_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI] * 100); + } /* Read signal quality from the group used for the throttle */ (void)updateRcvrStatus(&activity_fsm, - settings.ChannelGroups.Throttle); + settings.ChannelGroups.Throttle, + rssiValue); // Sanity Check Throttle and Yaw if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE @@ -383,6 +397,10 @@ static void receiverTask(__attribute__((unused)) void *parameters) valid_input_detected &= validInputRange(settings.ChannelMin.Accessory3, settings.ChannelMax.Accessory3, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY3]); } + if (settings.ChannelGroups.Rssi != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Rssi, + settings.ChannelMax.Rssi, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI]); + } // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { @@ -569,6 +587,18 @@ static void receiverTask(__attribute__((unused)) void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } + // Set Rssi input + if (settings.ChannelGroups.Rssi != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI]; +#ifdef USE_INPUT_LPF + // Allow some Rssi filtering without deadband + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_RSSI, &settings.ResponseTime, 0.0f, dT); +#endif + + if (AccessoryDesiredInstSet(4, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); + } + } } // Update cmd object @@ -626,6 +656,12 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm { bool activity_updated = false; + ManualControlSettingsChannelGroupsData channelGroups; + ManualControlSettingsChannelNumberData channelNumber; + + ManualControlSettingsChannelGroupsGet(&channelGroups); + ManualControlSettingsChannelNumberGet(&channelNumber); + /* Compare the current value to the previous sampled value */ for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { uint16_t delta; @@ -637,6 +673,12 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm delta = prev - curr; } + // Ignore activity from this Group/Channel because already used/set for Rssi input + // Without that, the ReceiverActivity will be saturated just with Rssi value activity. + if (channelGroups.Rssi == fsm->group && channelNumber.Rssi == channel) { + delta = 0; + } + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { /* Mark this channel as active */ ReceiverActivityActiveGroupOptions group; @@ -748,16 +790,18 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) return activity_updated; } -/* Read signal quality from the specified group */ +/** + * Read signal quality from the specified group or use Rssi input value if any + */ static bool updateRcvrStatus( struct rcvr_activity_fsm *fsm, - ManualControlSettingsChannelGroupsOptions group) + ManualControlSettingsChannelGroupsOptions group, int8_t rssiValue) { extern uint32_t pios_rcvr_group_map[]; bool activity_updated = false; int8_t quality; - quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]); + quality = (rssiValue > 0) ? rssiValue : PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]); /* If no driver is detected or any other error then return */ if (quality < 0) { diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index a08e74b64f..2d16307e40 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -63,7 +63,6 @@ #include #include #include -#include #include #include @@ -211,14 +210,9 @@ int32_t SensorsInitialize(void) AccelSensorInitialize(); MagSensorInitialize(); BaroSensorInitialize(); - RevoCalibrationInitialize(); - RevoSettingsInitialize(); - AttitudeSettingsInitialize(); - AccelGyroSettingsInitialize(); #if defined(PIOS_INCLUDE_HMC5X83) // for auxmagsupport.c helpers - AuxMagSettingsInitialize(); AuxMagSensorInitialize(); #endif @@ -284,7 +278,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) count++; } - PIOS_Assert(count); +// PIOS_Assert(count); RELOAD_WDG(); if (!sensors_test) { AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); diff --git a/flight/modules/Sensors/simulated/Sensors/sensors.c b/flight/modules/Sensors/simulated/Sensors/sensors.c index 0e583f0169..3cbc855b2d 100644 --- a/flight/modules/Sensors/simulated/Sensors/sensors.c +++ b/flight/modules/Sensors/simulated/Sensors/sensors.c @@ -115,7 +115,6 @@ int32_t SensorsInitialize(void) GPSPositionSensorInitialize(); GPSVelocitySensorInitialize(); MagSensorInitialize(); - RevoCalibrationInitialize(); return 0; } diff --git a/flight/modules/Stabilization/altitudeloop.cpp b/flight/modules/Stabilization/altitudeloop.cpp index 1b43962060..69f40d34bb 100644 --- a/flight/modules/Stabilization/altitudeloop.cpp +++ b/flight/modules/Stabilization/altitudeloop.cpp @@ -191,7 +191,6 @@ static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) */ void stabilizationAltitudeloopInit() { - AltitudeHoldSettingsInitialize(); AltitudeHoldStatusInitialize(); PositionStateInitialize(); VelocityStateInitialize(); diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index fbf8dbf9b4..6884bfe6f7 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -63,6 +63,7 @@ typedef struct { struct pid innerPids[3], outerPids[3]; // TPS [Roll,Pitch,Yaw][P,I,D] bool thrust_pid_scaling_enabled[3][3]; + float feedForward_alpha[3]; } StabilizationData; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index a50e6e6ec8..58228728c7 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -449,10 +449,15 @@ static void stabilizationInnerloopTask() { FlightStatusArmedOptions armed; FlightStatusArmedGet(&armed); + FlightStatusAlwaysStabilizeWhenArmedOptions alwaysStabilizeWhenArmed; + FlightStatusAlwaysStabilizeWhenArmedGet(&alwaysStabilizeWhenArmed); + float throttleDesired; ManualControlCommandThrottleGet(&throttleDesired); if (armed != FLIGHTSTATUS_ARMED_ARMED || - ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) { + ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && + (throttleDesired < 0) && + (alwaysStabilizeWhenArmed != FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE))) { // Force all axes to reinitialize when engaged for (t = 0; t < AXES; t++) { previous_mode[t] = 255; diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 9fc3c6e4ae..8949f36856 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -63,6 +64,7 @@ static DelayedCallbackInfo *callbackHandle; static AttitudeStateData attitude; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; +static float gyro_filtered[3] = { 0, 0, 0 }; static PiOSDeltatimeConfig timeval; static bool pitchMin = false; static bool pitchMax = false; @@ -71,6 +73,7 @@ static bool rollMax = false; // Private functions static void stabilizationOuterloopTask(); +static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); void stabilizationOuterloopInit() @@ -78,6 +81,7 @@ void stabilizationOuterloopInit() RateDesiredInitialize(); StabilizationDesiredInitialize(); AttitudeStateInitialize(); + GyroStateInitialize(); StabilizationStatusInitialize(); FlightStatusInitialize(); ManualControlCommandInitialize(); @@ -85,6 +89,7 @@ void stabilizationOuterloopInit() PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES); + GyroStateConnectCallback(GyroStateUpdatedCb); AttitudeStateConnectCallback(AttitudeStateUpdatedCb); } @@ -146,8 +151,8 @@ static void stabilizationOuterloopTask() float local_error[3]; - { #if defined(PIOS_QUATERNION_STABILIZATION) + if (stabSettings.settings.ForceRollPitchDuringYawTransition == STABILIZATIONSETTINGS_FORCEROLLPITCHDURINGYAWTRANSITION_FALSE) { // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; float q_desired[4]; @@ -173,8 +178,10 @@ static void stabilizationOuterloopTask() quat_mult(q_desired, &attitudeState.q1, q_error); quat_inverse(q_error); Quaternion2RPY(q_error, local_error); - + } else { #else /* if defined(PIOS_QUATERNION_STABILIZATION) */ + { +#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ // Simpler algorithm for CC, less memory local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll; local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch; @@ -187,9 +194,12 @@ static void stabilizationOuterloopTask() } else { local_error[2] = modulo - 180.0f; } -#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ } + // Feed forward: Assume things always get worse before they get better + local_error[0] = local_error[0] - (gyro_filtered[0] * stabSettings.stabBank.AttitudeFeedForward.Roll); + local_error[1] = local_error[1] - (gyro_filtered[1] * stabSettings.stabBank.AttitudeFeedForward.Pitch); + local_error[2] = local_error[2] - (gyro_filtered[2] * stabSettings.stabBank.AttitudeFeedForward.Yaw); for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) { reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); @@ -380,6 +390,18 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) #endif } +static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + GyroStateData gyroState; + + GyroStateGet(&gyroState); + + gyro_filtered[0] = gyro_filtered[0] * stabSettings.feedForward_alpha[0] + gyroState.x * (1 - stabSettings.feedForward_alpha[0]); + gyro_filtered[1] = gyro_filtered[1] * stabSettings.feedForward_alpha[1] + gyroState.y * (1 - stabSettings.feedForward_alpha[1]); + gyro_filtered[2] = gyro_filtered[2] * stabSettings.feedForward_alpha[2] + gyroState.z * (1 - stabSettings.feedForward_alpha[2]); +} + + /** * @} * @} diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 24f257d12d..0d29dd63f6 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -95,12 +95,8 @@ int32_t StabilizationInitialize() { // Initialize variables StabilizationDesiredInitialize(); - StabilizationSettingsInitialize(); StabilizationStatusInitialize(); StabilizationBankInitialize(); - StabilizationSettingsBank1Initialize(); - StabilizationSettingsBank2Initialize(); - StabilizationSettingsBank3Initialize(); RateDesiredInitialize(); ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch sin_lookup_initalize(); @@ -148,7 +144,7 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e if (t <= 1) { StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; } - // else yaw (other modes don't worry about invalid thrust mode either) + // yaw else { StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; } @@ -368,6 +364,21 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; + + // The dT has some jitter iteration to iteration that we don't want to + // make thie result unpredictable. Still, it's nicer to specify the constant + // based on a time (in ms) rather than a fixed multiplier. The error between + // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this + // calculation + const float fakeDt = 0.0025f; + for (int t = 0; t < STABILIZATIONBANK_ATTITUDEFEEDFORWARD_NUMELEM; t++) { + float tau = StabilizationBankAttitudeFeedForwardToArray(stabSettings.stabBank.AttitudeFeedForward)[t] * 0.1f; + if (tau < 0.0001f) { + stabSettings.feedForward_alpha[t] = 0.0f; // not trusting this to resolve to 0 + } else { + stabSettings.feedForward_alpha[t] = expf(-fakeDt / tau); + } + } } diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index a20a1be53c..3ea68e397f 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -83,11 +83,9 @@ int32_t filterAltitudeInitialize(stateFilter *handle) handle->init = &init; handle->filter = &filter; handle->localdata = pios_malloc(sizeof(struct data)); - HomeLocationInitialize(); AttitudeStateInitialize(); - AltitudeFilterSettingsInitialize(); AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb); - reloadSettings = true; + reloadSettings = true; return STACK_REQUIRED; } diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 1a804649b3..6aefdb7573 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -7,7 +7,8 @@ * @{ * * @file filterbaro.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Barometric altitude filter, calculates altitude offset based on * GPS altitude offset if available * @@ -36,8 +37,9 @@ // Private constants -#define STACK_REQUIRED 128 -#define INIT_CYCLES 100 +#define STACK_REQUIRED 128 +#define INIT_CYCLES 500 +#define BARO_OFFSET_ALPHA 0.02f // Private types struct data { @@ -99,7 +101,6 @@ static int32_t maininit(stateFilter *self) this->gpsAlt = 0.0f; this->first_run = INIT_CYCLES; - RevoSettingsInitialize(); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); return 0; @@ -120,7 +121,11 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (this->first_run < INIT_CYCLES || !this->useGPS) { - this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt); + if (this->first_run > INIT_CYCLES - 2) { + this->baroOffset = (state->baro[0] + this->gpsAlt); + } + // Set baroOffset using filtering, this allow better altitude zeroing + this->baroOffset = ((1.0f - BARO_OFFSET_ALPHA) * this->baroOffset) + (BARO_OFFSET_ALPHA * (state->baro[0] + this->gpsAlt)); this->baroAlt = state->baro[0]; this->first_run--; } diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index fae8d1f8c7..1bad774d0f 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -7,7 +7,8 @@ * @{ * * @file filtercf.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Complementary filter to calculate Attitude from Accels and Gyros * and optionally magnetometers: * WARNING: Will drift if the mean acceleration force doesn't point @@ -49,7 +50,14 @@ #define CALIBRATION_DELAY_MS 4000 #define CALIBRATION_DURATION_MS 6000 +#define RELOADSETTINGS_DELAY_MS 1000 +#define CONVERGENCE_MAGKP 20.0f #define VARIANCE_WINDOW_SIZE 40 + +#define UNDONE 0 +#define DONE 1 +#define RUN 2 + // Private types struct data { AttitudeSettingsData attitudeSettings; @@ -68,6 +76,7 @@ struct data { float rollPitchBiasRate; int32_t timeval; int32_t starttime; + int32_t inittime; uint8_t init; bool magCalibrated; pw_variance_t gyro_var[3]; @@ -95,8 +104,6 @@ static void globalInit(void) if (!initialized) { initialized = 1; FlightStatusInitialize(); - HomeLocationInitialize(); - RevoCalibrationInitialize(); FlightStatusConnectCallback(&flightStatusUpdatedCb); flightStatusUpdatedCb(NULL); } @@ -248,7 +255,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float AttitudeStateData attitudeState; // base on previous state AttitudeStateGet(&attitudeState); - this->init = 0; + this->init = UNDONE; // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level @@ -288,7 +295,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion } // check whether copter is steady - if (this->init == 0 && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) { + if (this->init == UNDONE && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) { pseudo_windowed_variance_push_sample(&this->gyro_var[0], gyro[0]); pseudo_windowed_variance_push_sample(&this->gyro_var[1], gyro[1]); pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]); @@ -304,18 +311,18 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float } - if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { + if (this->init == UNDONE && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { // wait 4 seconds for the user to get his hands off in case the board was just powered this->timeval = PIOS_DELAY_GetRaw(); return FILTERRESULT_ERROR; - } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { + } else if (this->init == UNDONE && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { // For first 6 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.YawBiasRate = 0.23f; this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; - this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f; + this->attitudeSettings.MagKp = this->magCalibrated ? CONVERGENCE_MAGKP : 0.0f; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { this->attitudeSettings.AccelKp = 1.0f; @@ -323,17 +330,21 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->attitudeSettings.YawBiasRate = 0.23f; this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; - this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f; - this->init = 0; + this->attitudeSettings.MagKp = this->magCalibrated ? CONVERGENCE_MAGKP : 0.0f; + this->init = UNDONE; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); - } else if (this->init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&this->attitudeSettings); + } else if (this->init == UNDONE) { this->rollPitchBiasRate = 0.0f; if (this->accel_alpha > 0.0f) { this->accel_filter_enabled = true; } - this->init = 1; + this->inittime = xTaskGetTickCount(); + this->init = DONE; + // Allow running filter with custom MagKp for some time before reload settings + } else if (this->init == DONE && (!this->magCalibrated || (xTaskGetTickCount() - this->inittime > RELOADSETTINGS_DELAY_MS / portTICK_RATE_MS))) { + // Reload settings (all the rates) + AttitudeSettingsGet(&this->attitudeSettings); + this->init = RUN; } // Compute the dT using the cpu clock @@ -423,7 +434,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate; this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate; if (this->useMag) { - this->gyroBias[2] -= -mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; + this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; } else { this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate; } @@ -473,7 +484,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float return FILTERRESULT_WARNING; } - if (this->init) { + if (this->init != UNDONE) { return FILTERRESULT_OK; } else { return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 1fd036afd1..92f3f7364b 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -68,90 +68,70 @@ struct data { stateEstimation work; - bool inited; + bool inited; PiOSDeltatimeConfig dtconfig; + bool navOnly; + float magLockAlpha; }; -// Private variables -static bool initialized = 0; - // Private functions -static int32_t init13i(stateFilter *self); -static int32_t init13(stateFilter *self); -static int32_t maininit(stateFilter *self); +static int32_t init(stateFilter *self); static filterResult filter(stateFilter *self, stateEstimation *state); static inline bool invalid_var(float data); -static void globalInit(void); +static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly); -static void globalInit(void) +static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly) { - if (!initialized) { - initialized = 1; - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - HomeLocationInitialize(); - } + handle->init = &init; + handle->filter = &filter; + handle->localdata = pios_malloc(sizeof(struct data)); + struct data *this = (struct data *)handle->localdata; + this->usePos = usePos; + this->navOnly = navOnly; + EKFStateVarianceInitialize(); + return STACK_REQUIRED; } int32_t filterEKF13iInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13i; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, false, false); } + int32_t filterEKF13Initialize(stateFilter *handle) { - globalInit(); - handle->init = &init13; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, true, false); } -// XXX -// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through -// XXX -int32_t filterEKF16iInitialize(stateFilter *handle) + +int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13i; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, false, true); } -int32_t filterEKF16Initialize(stateFilter *handle) + +int32_t filterEKF13NavOnlyInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, true, true); } - -static int32_t init13i(stateFilter *self) +#ifdef FALSE +// XXX +// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through +// XXX +int32_t filterEKF16iInitialize(stateFilter *handle) { - struct data *this = (struct data *)self->localdata; - - this->usePos = 0; - return maininit(self); + return filterEKFi13Initialize(handle); } - -static int32_t init13(stateFilter *self) +int32_t filterEKF16Initialize(stateFilter *handle) { - struct data *this = (struct data *)self->localdata; - - this->usePos = 1; - return maininit(self); + return filterEKF13Initialize(handle); } +#endif -static int32_t maininit(stateFilter *self) +static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; @@ -185,6 +165,15 @@ static int32_t maininit(stateFilter *self) return 2; } + // calculate Angle between Down vector and homeLocation.Be + float cross[3]; + float magnorm[3] = { this->homeLocation.Be[0], this->homeLocation.Be[1], this->homeLocation.Be[2] }; + vector_normalizef(magnorm, 3); + const float down[3] = { 0, 0, 1 }; + CrossProduct(down, magnorm, cross); + // VectorMagnitude(cross) = sin(Alpha) + // [0,0,1] dot magnorm = magnorm[2] = cos(Alpha) + this->magLockAlpha = atan2f(VectorMagnitude(cross), magnorm[2]); return 0; } @@ -202,8 +191,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state) float dT; uint16_t sensors = 0; + INSSetArmed(state->armed); + INSSetMagNorth(this->homeLocation.Be); + state->navUsed = (this->usePos || this->navOnly); this->work.updated |= state->updated; - // check magnetometer alarm, discard any magnetometer readings if not OK // during initialization phase (but let them through afterwards) SystemAlarmsAlarmData alarms; @@ -303,13 +294,16 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true - state->attitude[0] = Nav.q[0]; - state->attitude[1] = Nav.q[1]; - state->attitude[2] = Nav.q[2]; - state->attitude[3] = Nav.q[3]; - state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); - state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); - state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + if (!this->navOnly) { + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + } state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; @@ -321,6 +315,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) this->init_stage++; if (this->init_stage > 10) { + state->navOk = true; this->inited = true; } @@ -328,7 +323,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) } if (!this->inited) { - return FILTERRESULT_CRITICAL; + return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_CRITICAL; } float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; @@ -338,13 +333,20 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true - state->attitude[0] = Nav.q[0]; - state->attitude[1] = Nav.q[1]; - state->attitude[2] = Nav.q[2]; - state->attitude[3] = Nav.q[3]; - state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); - state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); - state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + if (!this->navOnly) { + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + } + { + float tmp[3]; + Quaternion2RPY(Nav.q, tmp); + state->debugNavYaw = tmp[2]; + } state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; @@ -358,14 +360,48 @@ static filterResult filter(stateFilter *self, stateEstimation *state) if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS; - } + if (this->ekfConfiguration.MapMagnetometerToHorizontalPlane == EKFCONFIGURATION_MAPMAGNETOMETERTOHORIZONTALPLANE_TRUE) { + // Map Magnetometer vector to correspond to the Roll+Pitch of the current Attitude State Estimate (no conflicting gravity) + // Idea: Alpha between Local Down and Mag is invariant of orientation, and identical to Alpha between [0,0,1] and HomeLocation.Be + // which is measured in init() + float R[3][3]; + + // 1. rotate down vector into body frame + Quaternion2R(Nav.q, R); + float local_down[3]; + rot_mult(R, (float[3]) { 0, 0, 1 }, local_down); + // 2. create a rotation vector that is perpendicular to rotated down vector, magnetic field vector and of size magLockAlpha + float rotvec[3]; + CrossProduct(local_down, this->work.mag, rotvec); + vector_normalizef(rotvec, 3); + rotvec[0] *= -this->magLockAlpha; + rotvec[1] *= -this->magLockAlpha; + rotvec[2] *= -this->magLockAlpha; + // 3. rotate artificial magnetometer reading from straight down to correct roll+pitch + Rv2Rot(rotvec, R); + float MagStrength = VectorMagnitude(this->homeLocation.Be); + local_down[0] *= MagStrength; + local_down[1] *= MagStrength; + local_down[2] *= MagStrength; + rot_mult(R, local_down, this->work.mag); + } + // From Eric: "exporting it in MagState was meant for debugging, but I think it makes a + // lot of sense to have a "corrected" magnetometer reading available in the system." + // TODO: Should move above calc to filtermag, updating from here cause trouble with the state->MagStatus (LP-534) + // debug rotated mags + // state->mag[0] = this->work.mag[0]; + // state->mag[1] = this->work.mag[1]; + // state->mag[2] = this->work.mag[2]; + // state->updated |= SENSORUPDATES_mag; + } // else { + // mag state is delayed until EKF processed it, allows overriding/debugging magnetometer estimate + // UNSET_MASK(state->updated, SENSORUPDATES_mag); + // } if (IS_SET(this->work.updated, SENSORUPDATES_baro)) { sensors |= BARO_SENSOR; } - INSSetMagNorth(this->homeLocation.Be); - if (!this->usePos) { // position and velocity variance used in indoor mode INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor, @@ -421,7 +457,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); - INSGetP(EKFStateVariancePToArray(vardata.P)); + INSGetVariance(EKFStateVariancePToArray(vardata.P)); EKFStateVarianceSet(&vardata); int t; for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { @@ -436,7 +472,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) this->work.updated = 0; if (this->init_stage < 0) { - return FILTERRESULT_WARNING; + return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_WARNING; } else { return FILTERRESULT_OK; } diff --git a/flight/modules/StateEstimation/filterlla.c b/flight/modules/StateEstimation/filterlla.c index 352e0000c3..ca1619dd17 100644 --- a/flight/modules/StateEstimation/filterlla.c +++ b/flight/modules/StateEstimation/filterlla.c @@ -44,7 +44,7 @@ struct data { GPSSettingsData settings; HomeLocationData home; - double HomeECEF[3]; + float HomeECEF[3]; float HomeRne[3][3]; }; @@ -61,9 +61,7 @@ int32_t filterLLAInitialize(stateFilter *handle) handle->init = &init; handle->filter = &filter; handle->localdata = pios_malloc(sizeof(struct data)); - GPSSettingsInitialize(); GPSPositionSensorInitialize(); - HomeLocationInitialize(); return STACK_REQUIRED; } @@ -104,7 +102,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim // check if we have a valid GPS signal (not checked by StateEstimation istelf) if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) && - (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + ((gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) && (gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) { int32_t LLAi[3] = { gpsdata.Latitude, diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 4fa5afa5c8..299af3d305 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -72,7 +72,6 @@ int32_t filterMagInitialize(stateFilter *handle) handle->init = &init; handle->filter = &filter; handle->localdata = pios_malloc(sizeof(struct data)); - HomeLocationInitialize(); return STACK_REQUIRED; } diff --git a/flight/modules/StateEstimation/filtervelocity.c b/flight/modules/StateEstimation/filtervelocity.c index ec6e29867b..618bed9092 100644 --- a/flight/modules/StateEstimation/filtervelocity.c +++ b/flight/modules/StateEstimation/filtervelocity.c @@ -83,7 +83,6 @@ static int32_t init(stateFilter *self) this->oldPos[2] = 0.0f; this->inited = 0; - RevoSettingsInitialize(); RevoSettingsVelocityPostProcessingLowPassAlphaGet(&this->alpha); PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA); diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index 38d21e599b..ad71c7c182 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -70,8 +70,12 @@ typedef struct { float airspeed[2]; float baro[1]; float auxMag[3]; - uint8_t magStatus; float boardMag[3]; + float debugNavYaw; + uint8_t magStatus; + bool navOk; + bool navUsed; + bool armed; sensorUpdates updated; } stateEstimation; @@ -94,6 +98,8 @@ int32_t filterCFInitialize(stateFilter *handle); int32_t filterCFMInitialize(stateFilter *handle); int32_t filterEKF13iInitialize(stateFilter *handle); int32_t filterEKF13Initialize(stateFilter *handle); +int32_t filterEKF13NavOnlyInitialize(stateFilter *handle); +int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle); int32_t filterEKF16iInitialize(stateFilter *handle); int32_t filterEKF16Initialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 72d28fb007..a8fbd86656 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -157,12 +157,20 @@ static stateFilter cfFilter; static stateFilter cfmFilter; static stateFilter ekf13iFilter; static stateFilter ekf13Filter; +static stateFilter ekf13iNavFilter; +static stateFilter ekf13NavFilter; // this is a hack to provide a computational shortcut for faster gyro state progression static float gyroRaw[3]; static float gyroDelta[3]; // preconfigured filter chains selectable via revoSettings.FusionAlgorithm + +static const filterPipeline *acroQueue = &(filterPipeline) { + .filter = &cfFilter, + .next = NULL, +}; + static const filterPipeline *cfQueue = &(filterPipeline) { .filter = &airFilter, .next = &(filterPipeline) { @@ -251,6 +259,52 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) { } }; +static const filterPipeline *ekf13NavCFAttQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &llaFilter, + .next = &(filterPipeline) { + .filter = &baroFilter, + .next = &(filterPipeline) { + .filter = &ekf13NavFilter, + .next = &(filterPipeline) { + .filter = &velocityFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } + } + } + } + } + } +}; + +static const filterPipeline *ekf13iNavCFAttQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &baroiFilter, + .next = &(filterPipeline) { + .filter = &stationaryFilter, + .next = &(filterPipeline) { + .filter = &ekf13iNavFilter, + .next = &(filterPipeline) { + .filter = &velocityFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } + } + } + } + } + } +}; + // Private functions static void settingsUpdatedCb(UAVObjEvent *objEv); @@ -272,8 +326,6 @@ static inline int32_t maxint32_t(int32_t a, int32_t b) */ int32_t StateEstimationInitialize(void) { - RevoSettingsInitialize(); - GyroSensorInitialize(); MagSensorInitialize(); AuxMagSensorInitialize(); @@ -282,15 +334,12 @@ int32_t StateEstimationInitialize(void) GPSVelocitySensorInitialize(); GPSPositionSensorInitialize(); - HomeLocationInitialize(); - GyroStateInitialize(); AccelStateInitialize(); MagStateInitialize(); AirspeedStateInitialize(); PositionStateInitialize(); VelocityStateInitialize(); - AuxMagSettingsInitialize(); RevoSettingsConnectCallback(&settingsUpdatedCb); @@ -320,6 +369,8 @@ int32_t StateEstimationInitialize(void) stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter)); + stack_required = maxint32_t(stack_required, filterEKF13NavOnlyInitialize(&ekf13NavFilter)); + stack_required = maxint32_t(stack_required, filterEKF13iNavOnlyInitialize(&ekf13iNavFilter)); stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required); @@ -348,9 +399,11 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart); */ static void StateEstimationCb(void) { - static filterResult alarm = FILTERRESULT_OK; - static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; - static uint16_t alarmcounter = 0; + static filterResult alarm = FILTERRESULT_OK; + static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; + static bool lastNavStatus = false; + static uint16_t alarmcounter = 0; + static uint16_t navstatuscounter = 0; static const filterPipeline *current; static stateEstimation states; static uint32_t last_time; @@ -373,14 +426,18 @@ static void StateEstimationCb(void) } else { last_time = PIOS_DELAY_GetRaw(); } + FlightStatusArmedOptions fsarmed; + FlightStatusArmedGet(&fsarmed); + states.armed = fsarmed != FLIGHTSTATUS_ARMED_DISARMED; // check if a new filter chain should be initialized if (fusionAlgorithm != revoSettings.FusionAlgorithm) { - FlightStatusData fs; - FlightStatusGet(&fs); - if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) { + if (fsarmed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) { const filterPipeline *newFilterChain; switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) { + case REVOSETTINGS_FUSIONALGORITHM_ACRONOSENSORS: + newFilterChain = acroQueue; + break; case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY: newFilterChain = cfQueue; // reinit Mag alarm @@ -398,12 +455,21 @@ static void StateEstimationCb(void) case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: newFilterChain = ekf13Queue; break; + case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: + newFilterChain = ekf13NavCFAttQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_TESTINGINSINDOORCF: + newFilterChain = ekf13iNavCFAttQueue; + break; default: newFilterChain = NULL; } // initialize filters in chain current = newFilterChain; bool error = 0; + states.debugNavYaw = 0; + states.navOk = false; + states.navUsed = false; while (current != NULL) { int32_t result = current->filter->init((stateFilter *)current->filter); if (result != 0) { @@ -493,11 +559,12 @@ static void StateEstimationCb(void) if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ AttitudeStateData s; AttitudeStateGet(&s); - s.q1 = states.attitude[0]; - s.q2 = states.attitude[1]; - s.q3 = states.attitude[2]; - s.q4 = states.attitude[3]; + s.q1 = states.attitude[0]; + s.q2 = states.attitude[1]; + s.q3 = states.attitude[2]; + s.q4 = states.attitude[3]; Quaternion2RPY(&s.q1, &s.Roll); + s.NavYaw = states.debugNavYaw; AttitudeStateSet(&s); } // throttle alarms, raise alarm flags immediately @@ -515,6 +582,18 @@ static void StateEstimationCb(void) } } + if (lastNavStatus < states.navOk) { + lastNavStatus = states.navOk; + navstatuscounter = 0; + } else { + if (navstatuscounter < 100) { + navstatuscounter++; + } else { + lastNavStatus = states.navOk; + navstatuscounter = 0; + } + } + // clear alarms if everything is alright, then schedule callback execution after timeout if (lastAlarm == FILTERRESULT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); @@ -526,6 +605,16 @@ static void StateEstimationCb(void) AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } + if (!states.navUsed) { + AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_UNINITIALISED); + } else { + if (states.navOk) { + AlarmsClear(SYSTEMALARMS_ALARM_NAV); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_CRITICAL); + } + } + if (updatedSensors) { PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); } else { diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 2cf86e7511..ce172a07f5 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -16,7 +16,7 @@ * @{ * * @file systemmod.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015. * @brief System module * @@ -62,7 +62,7 @@ #include #include #include - +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -129,6 +129,12 @@ static void updateWDGstats(); static uint8_t i2c_error_activity[PIOS_I2C_ERROR_COUNT_NUMELEM]; #endif +#ifdef PIOS_INCLUDE_RFM22B +static uint8_t previousRFXtalCap; +static uint8_t protocol; +static void oplinkSettingsUpdatedCb(UAVObjEvent *ev); +#endif + extern uintptr_t pios_uavo_settings_fs_id; extern uintptr_t pios_user_fs_id; @@ -154,7 +160,6 @@ int32_t SystemModStart(void) int32_t SystemModInitialize(void) { // Must registers objects here for system thread because ObjectManager started in OpenPilotInit - SystemSettingsInitialize(); SystemStatsInitialize(); FlightStatusInitialize(); ObjectPersistenceInitialize(); @@ -231,9 +236,19 @@ static void systemTask(__attribute__((unused)) void *parameters) HwSettingsConnectCallback(checkSettingsUpdatedCb); SystemSettingsConnectCallback(checkSettingsUpdatedCb); +#ifdef PIOS_INCLUDE_RFM22B + // Initialize previousRFXtalCap used by callback + OPLinkSettingsRFXtalCapGet(&previousRFXtalCap); + OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb); + // Get protocol + OPLinkSettingsProtocolGet(&protocol); +#endif + #ifdef DIAG_TASKS TaskInfoData taskInfoData; + memset(&taskInfoData, 0, sizeof(TaskInfoData)); CallbackInfoData callbackInfoData; + memset(&callbackInfoData, 0, sizeof(CallbackInfoData)); #endif // Main system loop while (1) { @@ -259,13 +274,9 @@ static void systemTask(__attribute__((unused)) void *parameters) PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData); TaskInfoSet(&taskInfoData); // Update the callback status object -// if(FALSE){ PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData); CallbackInfoSet(&callbackInfoData); -// } #endif -// } - UAVObjEvent ev; int delayTime = SYSTEM_UPDATE_PERIOD_MS; @@ -278,9 +289,6 @@ static void systemTask(__attribute__((unused)) void *parameters) oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); if (pios_rfm22b_id) { - // Get the other device stats. - PIOS_RFM22B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); - // Get the stats from the radio device struct rfm22b_stats radio_stats; PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); @@ -292,18 +300,19 @@ static void systemTask(__attribute__((unused)) void *parameters) static uint16_t prev_tx_seq = 0; static uint16_t prev_rx_seq = 0; - oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - oplinkStatus.RxGood = radio_stats.rx_good; - oplinkStatus.RxCorrected = radio_stats.rx_corrected; - oplinkStatus.RxErrors = radio_stats.rx_error; - oplinkStatus.RxMissed = radio_stats.rx_missed; - oplinkStatus.RxFailure = radio_stats.rx_failure; - oplinkStatus.TxDropped = radio_stats.tx_dropped; - oplinkStatus.TxFailure = radio_stats.tx_failure; - oplinkStatus.Resets = radio_stats.resets; - oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; + oplinkStatus.TxFailure = radio_stats.tx_failure; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; - oplinkStatus.LinkQuality = radio_stats.link_quality; + oplinkStatus.LinkQuality = radio_stats.link_quality; + oplinkStatus.AFCCorrection = radio_stats.afc_correction; if (first_time) { first_time = false; } else { @@ -326,7 +335,7 @@ static void systemTask(__attribute__((unused)) void *parameters) oplinkStatus.RXSeq = radio_stats.rx_seq; oplinkStatus.LinkState = radio_stats.link_state; - } else { + } else if (protocol != OPLINKSETTINGS_PROTOCOL_OPENLRS) { oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; } OPLinkStatusSet(&oplinkStatus); @@ -453,6 +462,25 @@ static void checkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) } } +#ifdef PIOS_INCLUDE_RFM22B +/** + * Called whenever OPLink settings changed + */ +static void oplinkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + uint8_t currentRFXtalCap; + + OPLinkSettingsRFXtalCapGet(¤tRFXtalCap); + + // Check if RFXtalCap value changed + if (currentRFXtalCap != previousRFXtalCap) { + PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, currentRFXtalCap); + PIOS_RFM22B_Reinit(pios_rfm22b_id); + previousRFXtalCap = currentRFXtalCap; + } +} +#endif /* ifdef PIOS_INCLUDE_RFM22B */ + #ifdef DIAG_TASKS static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context) { @@ -553,7 +581,11 @@ static uint16_t GetFreeIrqStackSize(void) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) extern uint32_t _irq_stack_top; extern uint32_t _irq_stack_end; - uint32_t pattern = 0x0000A5A5; +#ifdef STM32F3 + uint32_t pattern = 0xA5A5A5A5; +#else + uint32_t pattern = 0xA5A5; +#endif uint32_t *ptr = &_irq_stack_end; #if 1 /* the ugly way accurate but takes more time, useful for debugging */ diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 9796e8f599..0a30f55689 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) @@ -265,10 +267,7 @@ void TelemetryInitializeChannel(channelContext *channel) */ int32_t TelemetryInitialize(void) { - HwSettingsInitialize(); - #ifdef PIOS_INCLUDE_RFM22B - OPLinkSettingsInitialize(); OPLinkSettingsData data; OPLinkSettingsGet(&data); @@ -775,6 +774,7 @@ static int32_t setLoggingPeriod( UAVObjHandle obj, int32_t updatePeriodMs) { +#ifdef HAS_LOGGING_MODULE UAVObjEvent ev; int32_t ret; @@ -796,6 +796,14 @@ static int32_t setLoggingPeriod( ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs); } return ret; + +#else /* HAS_LOGGING_MODULE */ + (void)channel; + (void)obj; + (void)updatePeriodMs; + return 0; + +#endif /* ifdef HAS_LOGGING_MODULE */ } /** diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 8452727c24..47569bed33 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -100,23 +100,24 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM int32_t TxPIDInitialize(void) { bool txPIDEnabled; - HwSettingsOptionalModulesData optionalModules; -#ifdef REVOLUTION - AltitudeHoldSettingsInitialize(); -#endif + HwSettingsOptionalModulesData optionalModules; - HwSettingsInitialize(); HwSettingsOptionalModulesGet(&optionalModules); +#ifdef MODULE_TXPID_BUILTIN + txPIDEnabled = true; + optionalModules.TxPID = HWSETTINGS_OPTIONALMODULES_ENABLED; + HwSettingsOptionalModulesSet(&optionalModules); +#else if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) { txPIDEnabled = true; } else { txPIDEnabled = false; } +#endif if (txPIDEnabled) { - TxPIDSettingsInitialize(); TxPIDStatusInitialize(); AccessoryDesiredInitialize(); @@ -136,14 +137,12 @@ int32_t TxPIDInitialize(void) // StabilizationSettings update rate permanently. Use Metadata via // browser to reset to defaults (telemetryAcked=true, OnChange). UAVObjMetadata metadata; - StabilizationSettingsInitialize(); StabilizationSettingsGetMetadata(&metadata); metadata.telemetryAcked = 0; metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; StabilizationSettingsSetMetadata(&metadata); - AttitudeSettingsInitialize(); AttitudeSettingsGetMetadata(&metadata); metadata.telemetryAcked = 0; metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; diff --git a/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c b/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c new file mode 100644 index 0000000000..964697d8c9 --- /dev/null +++ b/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c @@ -0,0 +1,763 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotModules LibrePilot Modules + * @{ + * @addtogroup UAVOFrSKYSensorHubBridge UAVO to FrSKY Bridge Module + * @{ + * + * @file UAVOFrSKYSensorHubBridge.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * dRonin, http://dRonin.org/, Copyright (C) 2016 + * Tau Labs, http://taulabs.org, Copyright (C) 2014 + * @brief Bridges selected UAVObjects to FrSKY Sensor Hub + * @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, see + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +// **************** +#include "pios.h" +#include "pios_board_io.h" +#include "openpilot.h" +#include "flightbatterysettings.h" +#include "flightbatterystate.h" +#include "homelocation.h" +#include "flightstatus.h" +#include "pios_thread.h" +#include "barosensor.h" +#include "accelsensor.h" +#include "gpspositionsensor.h" +#include "taskinfo.h" + +// **************** +// Private functions + +static void uavoFrSKYSensorHubBridgeTask(void *parameters); + +static uint16_t frsky_pack_altitude( + float altitude, + uint8_t *serial_buf); + +static uint16_t frsky_pack_temperature_01( + float temperature_01, + uint8_t *serial_buf); + +static uint16_t frsky_pack_temperature_02( + float temperature_02, + uint8_t *serial_buf); + +static uint16_t frsky_pack_accel( + float accels_x, + float accels_y, + float accels_z, + uint8_t *serial_buf); + +static uint16_t frsky_pack_cellvoltage( + uint8_t cell, + float cell_voltage, + uint8_t *serial_buf); + +static uint16_t frsky_pack_fas( + float voltage, + float current, + uint8_t *serial_buf); + +static uint16_t frsky_pack_rpm( + uint16_t rpm, + uint8_t *serial_buf); + +static uint16_t frsky_pack_gps( + float course, + int32_t latitude, + int32_t longitude, + float altitude, + float speed, + uint8_t *serial_buf); + +static uint16_t frsky_pack_fuel( + float fuel_level, + uint8_t *serial_buf); + +static uint16_t frsky_pack_stop( + uint8_t *serial_buf); + +static int16_t frsky_acceleration_unit(float accel); + +static void frsky_serialize_value(uint8_t valueid, + uint8_t *value, + uint8_t *serial_buf, + uint8_t *index); + +static void frsky_write_userdata_byte(uint8_t byte, + uint8_t *serial_buf, + uint8_t *index); + +static bool frame_trigger(uint8_t frame_num); +// **************** +// Private constants + + +#if defined(PIOS_FRSKY_SENSOR_HUB_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_FRSKY_SENSOR_HUB_STACK_SIZE +#else +#define STACK_SIZE_BYTES 672 +#endif + +#define TASK_PRIORITY (tskIDLE_PRIORITY) +#define TASK_RATE_HZ 10 + +#define FRSKY_MAX_PACKET_LEN 106 +#define FRSKY_BAUD_RATE 9600 + +#define FRSKY_FRAME_DATA_HEADER 0x5E +#define FRSKY_FRAME_STOP 0x5E + +enum FRSKY_VALUE_ID { + FRSKY_GPS_ALTITUDE_INTEGER = 0x01, + FRSKY_GPS_ALTITUDE_DECIMAL = FRSKY_GPS_ALTITUDE_INTEGER + 8, + FRSKY_TEMPERATURE_1 = 0x02, + FRSKY_RPM = 0x03, + FRSKY_FUEL_LEVEL = 0x04, + FRSKY_TEMPERATURE_2 = 0x05, + FRSKY_VOLTAGE = 0x06, + FRSKY_ALTITUDE_INTEGER = 0x10, + FRSKY_ALTITUDE_DECIMAL = 0x21, + FRSKY_GPS_SPEED_INTEGER = 0x11, + FRSKY_GPS_SPEED_DECIMAL = 0x11 + 8, + FRSKY_GPS_LONGITUDE_INTEGER = 0x12, + FRSKY_GPS_LONGITUDE_DECIMAL = FRSKY_GPS_LONGITUDE_INTEGER + 8, + FRSKY_GPS_E_W = 0x1A + 8, + FRSKY_GPS_LATITUDE_INTEGER = 0x13, + FRSKY_GPS_LATITUDE_DECIMAL = FRSKY_GPS_LATITUDE_INTEGER + 8, + FRSKY_GPS_N_S = 0x1B + 8, + FRSKY_GPS_COURSE_INTEGER = 0x14, + FRSKY_GPS_COURSE_DECIMAL = FRSKY_GPS_COURSE_INTEGER + 8, + FRSKY_DATE_MONTH = 0x15, + FRSKY_DATE_YEAR = 0x16, + FRSKY_HOUR_MINUTE = 0x17, + FRSKY_SECOND = 0x18, + FRSKY_ACCELERATION_X = 0x24, + FRSKY_ACCELERATION_Y = 0x25, + FRSKY_ACCELERATION_Z = 0x26, + FRSKY_VOLTAGE_AMPERE_SENSOR_INTEGER = 0x3A, + FRSKY_VOLTAGE_AMPERE_SENSOR_DECIMAL = 0x3B, + FRSKY_CURRENT = 0x28, +}; + +enum FRSKY_FRAME { + FRSKY_FRAME_VARIO, + FRSKY_FRAME_BATTERY, + FRSKY_FRAME_GPS, +}; + +static const uint8_t frsky_rates[] = { + [FRSKY_FRAME_VARIO] = 5, // 5Hz + [FRSKY_FRAME_BATTERY] = 5, // 5Hz + [FRSKY_FRAME_GPS] = 2, // 1Hz +}; + +#define MAXSTREAMS sizeof(frsky_rates) +#define KNOTS2M_PER_SECOND 0.514444F +#define GRAVITY 9.8F + +// **************** +// Private variables + +static struct { + uint32_t frsky_port; + uint8_t frame_ticks[MAXSTREAMS]; + uint8_t serial_buf[FRSKY_MAX_PACKET_LEN]; +} *shub_global; + +/** + * Start the module + * \return -1 if start failed + * \return 0 on success + */ +static int32_t uavoFrSKYSensorHubBridgeStart(void) +{ + if (shub_global) { + xTaskHandle taskHandle; + xTaskCreate(uavoFrSKYSensorHubBridgeTask, "uavoFrSKYSensorHubBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOFRSKYSENSORHUBBRIDGE, taskHandle); + return 0; + } + return -1; +} + +/** + * Initialize the module + * \return -1 if initialization failed + * \return 0 on success + */ +static int32_t uavoFrSKYSensorHubBridgeInitialize(void) +{ + uintptr_t frsky_port = PIOS_COM_FRSKY_SENSORHUB; + + if (frsky_port) { + shub_global = pios_malloc(sizeof(*shub_global)); + + if (shub_global == 0) { + return -1; + } + + PIOS_COM_ChangeBaud(frsky_port, FRSKY_BAUD_RATE); + + shub_global->frsky_port = frsky_port; + + for (uint32_t x = 0; x < MAXSTREAMS; ++x) { + shub_global->frame_ticks[x] = + (TASK_RATE_HZ / frsky_rates[x]); + } + + return 0; + } + + + return -1; +} +MODULE_INITCALL(uavoFrSKYSensorHubBridgeInitialize, uavoFrSKYSensorHubBridgeStart); + +/** + * Main task. It does not return. + */ +static void uavoFrSKYSensorHubBridgeTask(__attribute__((unused)) void *parameters) +{ + uint32_t lastSysTime = PIOS_Thread_Systime(); + + // Main task loop + while (1) { + PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ); + + if (frame_trigger(FRSKY_FRAME_VARIO)) { + uint16_t msg_length = 0; + + // Get the accelerometer values + float accX = 0, accY = 0, accZ = 0; + if (AccelSensorHandle() != NULL) { + AccelSensorxGet(&accX); + AccelSensoryGet(&accY); + AccelSensorzGet(&accZ); + } + msg_length += frsky_pack_accel( + accX, + accY, + accZ, + shub_global->serial_buf + msg_length); + + // Get the altritude from the barometer. + float altitude; + if (BaroSensorHandle() != NULL) { + BaroSensorAltitudeGet(&altitude); + } + msg_length += frsky_pack_altitude( + altitude, + shub_global->serial_buf + msg_length); + msg_length += frsky_pack_stop(shub_global->serial_buf + msg_length); + + PIOS_COM_SendBuffer(shub_global->frsky_port, + shub_global->serial_buf, msg_length); + } + + if (frame_trigger(FRSKY_FRAME_BATTERY)) { + uint16_t msg_length = 0; + + FlightBatteryStateData batState; + if (FlightBatteryStateHandle() != NULL) { + FlightBatteryStateGet(&batState); + } + + float voltage = voltage = batState.Voltage; + + float current = current = batState.Current; + + // As long as there is no voltage for each cell + // all cells will have the same voltage. + // Receiver will know number of cells. + FlightBatterySettingsData batSettings; + if (FlightBatterySettingsHandle() != NULL) { + FlightBatterySettingsGet(&batSettings); + } + if (batState.NbCellsAutodetected == FLIGHTBATTERYSTATE_NBCELLSAUTODETECTED_TRUE) { + batSettings.NbCells = batState.NbCells; + } + + if (batSettings.NbCells > 0) { + float cell_v = voltage / batSettings.NbCells; + for (uint8_t i = 0; i < batSettings.NbCells; ++i) { + msg_length += frsky_pack_cellvoltage( + i, + cell_v, + shub_global->serial_buf + msg_length); + } + } + + msg_length += frsky_pack_fas( + voltage, + current, + shub_global->serial_buf + msg_length); + + if (batSettings.Capacity > 0) { + float fuel = 1.0f - batState.ConsumedEnergy / batSettings.Capacity; + msg_length += frsky_pack_fuel( + fuel, + shub_global->serial_buf + msg_length); + } + + msg_length += frsky_pack_stop(shub_global->serial_buf + msg_length); + + PIOS_COM_SendBuffer(shub_global->frsky_port, + shub_global->serial_buf, msg_length); + } + + if (frame_trigger(FRSKY_FRAME_GPS)) { + uint16_t msg_length = 0; + + /** + * Encodes ARM status and flight mode number as RPM value + * Since there is no RPM information in any UAVO available, + * we will intentionally misuse this item to encode other useful information. + * It will encode flight status as three-digit number as follow: + * most left digit encodes arm status (200=armed, 100=disarmed) + * two most right digits encode flight mode number (see FlightStatus UAVO FlightMode enum) + * To work properly on Taranis, you have to set Blades to "60" in telemetry setting + */ + FlightStatusData flight_status; + FlightStatusGet(&flight_status); + + uint16_t status = 0; + float hdop, vdop; + + status = (flight_status.Armed == FLIGHTSTATUS_ARMED_ARMED) ? 200 : 100; + status += flight_status.FlightMode; + + msg_length += frsky_pack_rpm(status, shub_global->serial_buf + msg_length); + + uint8_t hl_set = HOMELOCATION_SET_FALSE; + + GPSPositionSensorData gpsPosData; + if (GPSPositionSensorHandle() != NULL) { + GPSPositionSensorGet(&gpsPosData); + } + + if (HomeLocationHandle() != NULL) { + HomeLocationSetGet(&hl_set); + } + + /** + * Encode GPS status and visible satellites as T1 value + * We will intentionally misuse this item to encode other useful information. + * Right-most two digits encode visible satellite count, left-most digit has following meaning: + * 1 - no GPS connected + * 2 - no fix + * 3 - 2D fix + * 4 - 3D fix + * 5 - 3D fix and HomeLocation is SET - should be safe for navigation + */ + switch (gpsPosData.Status) { + case GPSPOSITIONSENSOR_STATUS_NOGPS: + status = 100; + break; + case GPSPOSITIONSENSOR_STATUS_NOFIX: + status = 200; + break; + case GPSPOSITIONSENSOR_STATUS_FIX2D: + status = 300; + break; + case GPSPOSITIONSENSOR_STATUS_FIX3D: + case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS: + if (hl_set == HOMELOCATION_SET_TRUE) { + status = 500; + } else { + status = 400; + } + break; + } + + if (gpsPosData.Satellites > 0) { + status += gpsPosData.Satellites; + } + + msg_length += frsky_pack_temperature_01((float)status, shub_global->serial_buf + msg_length); + + /** + * Encode GPS HDOP and VDOP as T2 value + * We will intentionally misuse this item to encode other useful information. + * VDOP in the upper 16 bits, max 256 (2.56 * 100) + * HDOP in the lower 16 bits, max 256 (2.56 * 100) + */ + hdop = gpsPosData.HDOP * 100.0f; + hdop = 0; + + if (hdop > 255.0f) { + hdop = 255.0f; + } + + vdop = gpsPosData.VDOP * 100.0f; + vdop = 0; + + if (vdop > 255.0f) { + vdop = 255.0f; + } + + msg_length += frsky_pack_temperature_02((vdop * 256 + hdop), shub_global->serial_buf + msg_length); + + if (gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX2D || + gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D || + gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS) { + msg_length += frsky_pack_gps( + gpsPosData.Heading, + gpsPosData.Latitude, + gpsPosData.Longitude, + gpsPosData.Altitude, + gpsPosData.Groundspeed, + shub_global->serial_buf + msg_length); + } else { + msg_length += frsky_pack_gps(0, 0, 0, 0, 0, shub_global->serial_buf + msg_length); + } + + msg_length += frsky_pack_stop(shub_global->serial_buf + msg_length); + + PIOS_COM_SendBuffer(shub_global->frsky_port, + shub_global->serial_buf, msg_length); + } + } +} + +static bool frame_trigger(uint8_t frame_num) +{ + uint8_t rate = frsky_rates[frame_num]; + + if (rate == 0) { + return false; + } + + if (shub_global->frame_ticks[frame_num] == 0) { + // we're triggering now, setup the next trigger point + if (rate > TASK_RATE_HZ) { + rate = TASK_RATE_HZ; + } + shub_global->frame_ticks[frame_num] = (TASK_RATE_HZ / rate); + return true; + } + + // count down at 50Hz + shub_global->frame_ticks[frame_num]--; + return false; +} + +/** + * Writes altitude to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_altitude(float altitude, uint8_t *serial_buf) +{ + uint8_t index = 0; + + float altitudeInteger = 0.0f; + + uint16_t decimalValue = lroundf(modff(altitude, &altitudeInteger) * 100); + int16_t integerValue = lroundf(altitude); + + frsky_serialize_value(FRSKY_ALTITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_ALTITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + + return index; +} + +/** + * Writes baro temperature to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_temperature_01( + float temperature_01, + uint8_t *serial_buf) +{ + uint8_t index = 0; + + int16_t temperature = lroundf(temperature_01); + + frsky_serialize_value(FRSKY_TEMPERATURE_1, (uint8_t *)&temperature, serial_buf, &index); + + return index; +} + +/** + * Writes accel temperature to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_temperature_02( + float temperature_02, + uint8_t *serial_buf) +{ + uint8_t index = 0; + + int16_t temperature = lroundf(temperature_02); + + frsky_serialize_value(FRSKY_TEMPERATURE_2, (uint8_t *)&temperature, serial_buf, &index); + + return index; +} + +/** + * Writes acceleration values to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_accel( + float accels_x, + float accels_y, + float accels_z, + uint8_t *serial_buf) +{ + uint8_t index = 0; + + int16_t accel = frsky_acceleration_unit(accels_x); + + frsky_serialize_value(FRSKY_ACCELERATION_X, (uint8_t *)&accel, serial_buf, &index); + + accel = frsky_acceleration_unit(accels_y); + frsky_serialize_value(FRSKY_ACCELERATION_Y, (uint8_t *)&accel, serial_buf, &index); + + accel = frsky_acceleration_unit(accels_z); + frsky_serialize_value(FRSKY_ACCELERATION_Z, (uint8_t *)&accel, serial_buf, &index); + + return index; +} + +static uint16_t frsky_pack_cellvoltage( + uint8_t cell, + float cell_voltage, + uint8_t *serial_buf) +{ + // its not possible to address more than 15 cells + if (cell > 15) { + return 0; + } + + uint8_t index = 0; + uint16_t v = lroundf((cell_voltage / 4.2f) * 2100); + if (v > 2100) { + v = 2100; + } + + uint16_t voltage = ((v & 0x00ff) << 8) | (cell << 4) | ((v & 0x0f00) >> 8); + frsky_serialize_value(FRSKY_VOLTAGE, (uint8_t *)&voltage, serial_buf, &index); + + return index; +} +/** + * Writes battery values to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_fas( + float voltage, + float current, + uint8_t *serial_buf) +{ + uint8_t index = 0; + + voltage = (voltage * 110.0f) / 21.0f; + + uint16_t integerValue = lroundf(voltage) / 100; + uint16_t decimalValue = lroundf(voltage - integerValue); + + frsky_serialize_value(FRSKY_VOLTAGE_AMPERE_SENSOR_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_VOLTAGE_AMPERE_SENSOR_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + + integerValue = lroundf(current * 10); + frsky_serialize_value(FRSKY_CURRENT, (uint8_t *)&integerValue, serial_buf, &index); + + return index; +} + +/** + * Writes rpm value to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_rpm(uint16_t rpm, uint8_t *serial_buf) +{ + uint8_t index = 0; + + frsky_serialize_value(FRSKY_RPM, (uint8_t *)&rpm, serial_buf, &index); + + return index; +} + +/** + * Writes fuel value [0.0 ... 1.0] to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_fuel( + float fuel_level, + uint8_t *serial_buf) +{ + uint8_t index = 0; + + uint16_t level = lroundf(abs(fuel_level * 100)); + + // Use fixed levels here because documentation says so. + if (level > 94) { + level = 100; + } else if (level > 69) { + level = 75; + } else if (level > 44) { + level = 50; + } else if (level > 19) { + level = 25; + } else { + level = 0; + } + + frsky_serialize_value(FRSKY_FUEL_LEVEL, (uint8_t *)&level, serial_buf, &index); + + return index; +} + +/** + * Writes gps values to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_gps( + float course, + int32_t latitude, + int32_t longitude, + float altitude, + float speed, + uint8_t *serial_buf) +{ + uint8_t index = 0; + + { + float courseInteger = 0.0f; + uint16_t decimalValue = lroundf(modff(course, &courseInteger) * 100); + uint16_t integerValue = lroundf(courseInteger); + + frsky_serialize_value(FRSKY_GPS_COURSE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_GPS_COURSE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + } + + // latitude + { + uint16_t integerValue = (abs(latitude) / 100000); + uint16_t decimalValue = (abs(latitude) / 10) % 10000; + + frsky_serialize_value(FRSKY_GPS_LATITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_GPS_LATITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + + uint16_t hemisphere = 'N'; + if (latitude < 0) { + hemisphere = 'S'; + } + frsky_serialize_value(FRSKY_GPS_N_S, (uint8_t *)&hemisphere, serial_buf, &index); + } + + // longitude + { + uint16_t integerValue = (abs(longitude) / 100000); + uint16_t decimalValue = (abs(longitude) / 10) % 10000; + + uint16_t hemisphere = 'E'; + if (longitude < 0) { + hemisphere = 'W'; + } + + frsky_serialize_value(FRSKY_GPS_LONGITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_GPS_LONGITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + + frsky_serialize_value(FRSKY_GPS_E_W, (uint8_t *)&hemisphere, serial_buf, &index); + } + + // speed + { + float knots = speed / KNOTS2M_PER_SECOND; + + float knotsInteger = 0.0f; + uint16_t decimalValue = lroundf(modff(knots, &knotsInteger) * 100); + int16_t integerValue = lroundf(knotsInteger); + + frsky_serialize_value(FRSKY_GPS_SPEED_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_GPS_SPEED_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + } + + // altitude + { + float altitudeInteger = 0.0F; + uint16_t decimalValue = lroundf(modff(altitude, &altitudeInteger) * 100); + int16_t integerValue = lroundf(altitudeInteger); + + frsky_serialize_value(FRSKY_GPS_ALTITUDE_INTEGER, (uint8_t *)&integerValue, serial_buf, &index); + frsky_serialize_value(FRSKY_GPS_ALTITUDE_DECIMAL, (uint8_t *)&decimalValue, serial_buf, &index); + } + + return index; +} + +/** + * Writes stop byte to buffer + * \return number of bytes written to the buffer + */ +static uint16_t frsky_pack_stop(uint8_t *serial_buf) +{ + serial_buf[0] = FRSKY_FRAME_STOP; + + return 1; +} + +/** + * Convert acceleration value to frsky acceleration unit + * \return Acceleration value + */ +static int16_t frsky_acceleration_unit(float accel) +{ + accel = accel / GRAVITY; // convert to gravity + accel *= 1000; + return lroundf(accel); +} + +/** + * Write value to buffer + */ +static void frsky_serialize_value(uint8_t valueid, uint8_t *value, uint8_t *serial_buf, uint8_t *index) +{ + serial_buf[(*index)++] = FRSKY_FRAME_DATA_HEADER; + serial_buf[(*index)++] = valueid; + + frsky_write_userdata_byte(value[0], serial_buf, index); + frsky_write_userdata_byte(value[1], serial_buf, index); +} + +/** + * Write a byte to the buffer and do byte stuffing if needed. + */ +static void frsky_write_userdata_byte(uint8_t byte, uint8_t *serial_buf, uint8_t *index) +{ + // ** byte stuffing + if ((byte == 0x5E) || (byte == 0x5D)) { + serial_buf[(*index)++] = 0x5D; + serial_buf[(*index)++] = ~(byte ^ 0x60); + } else { + serial_buf[(*index)++] = byte; + } +} + +/** + * @} + * @} + */ diff --git a/flight/modules/UAVOHottBridge/inc/uavohottbridge.h b/flight/modules/UAVOHottBridge/inc/uavohottbridge.h new file mode 100644 index 0000000000..1af2d09e9c --- /dev/null +++ b/flight/modules/UAVOHottBridge/inc/uavohottbridge.h @@ -0,0 +1,380 @@ +/** + ****************************************************************************** + * @addtogroup Modules + * @{ + * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module + * @{ + * + * @file uavohottbridge.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @brief sends telemery data on HoTT request + * + * @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 + */ + +// timing variables +#define IDLE_TIME 10 // idle line delay to prevent data crashes on telemetry line. +#define DATA_TIME 3 // time between 2 transmitted bytes + +// sizes and lengths +#define climbratesize 50 // defines size of ring buffer for climbrate calculation +#define statussize 21 // number of characters in status line +#define HOTT_MAX_MESSAGE_LENGTH 200 + +// scale factors +#define M_TO_CM 100 // scale m to cm or m/s to cm/s +#define MS_TO_KMH 3.6f // scale m/s to km/h +#define DEG_TO_UINT 0.5f // devide degrees by 2. then the value fits into a byte. + +// offsets. used to make transmitted values unsigned. +#define OFFSET_ALTITUDE 500 // 500m +#define OFFSET_CLIMBRATE 30000 // 30000cm/s or 300m/s +#define OFFSET_CLIMBRATE3S 120 // 120m/s +#define OFFSET_TEMPERATURE 20 // 20 degrees + +// Bits to invert display areas or values +#define VARIO_INVERT_ALT (1 << 0) // altitude +#define VARIO_INVERT_MAX (1 << 1) // max altitude +#define VARIO_INVERT_MIN (1 << 2) // min altitude +#define VARIO_INVERT_CR1S (1 << 3) // climbrate 1s +#define VARIO_INVERT_CR3S (1 << 4) // climbrate 3s +#define VARIO_INVERT_CR10S (1 << 5) // climbrate 10s + +#define GPS_INVERT_HDIST (1 << 0) // home distance +#define GPS_INVERT_SPEED (1 << 1) // speed (kmh) +#define GPS_INVERT_ALT (1 << 2) // altitude +#define GPS_INVERT_CR1S (1 << 3) // climbrate 1s +#define GPS_INVERT_CR3S (1 << 4) // climbrate 3s +#define GPS_INVERT2_POS (1 << 0) // GPS postion values + +#define GAM_INVERT_CELL (1 << 0) // cell voltage +#define GAM_INVERT_BATT1 (1 << 1) // battery 1 voltage +#define GAM_INVERT_BATT2 (1 << 2) // battery 1 voltage +#define GAM_INVERT_TEMP1 (1 << 3) // temperature 1 +#define GAM_INVERT_TEMP2 (1 << 4) // temperature 2 +#define GAM_INVERT_FUEL (1 << 5) // fuel +#define GAM_INVERT2_CURRENT (1 << 0) // current +#define GAM_INVERT2_VOLTAGE (1 << 1) // voltage +#define GAM_INVERT2_ALT (1 << 2) // altitude +#define GAM_INVERT2_CR1S (1 << 3) // climbrate 1s +#define GAM_INVERT2_CR3S (1 << 4) // climbrate 3s + +#define EAM_INVERT_CAPACITY (1 << 0) // capacity +#define EAM_INVERT_BATT1 (1 << 1) // battery 1 voltage +#define EAM_INVERT_BATT2 (1 << 2) // battery 1 voltage +#define EAM_INVERT_TEMP1 (1 << 3) // temperature 1 +#define EAM_INVERT_TEMP2 (1 << 4) // temperature 2 +#define EAM_INVERT_ALT (1 << 5) // altitude +#define EAM_INVERT_CURRENT (1 << 6) // current +#define EAM_INVERT_VOLTAGE (1 << 7) // voltage +#define EAM_INVERT2_ALT (1 << 2) // altitude +#define EAM_INVERT2_CR1S (1 << 3) // climbrate 1s +#define EAM_INVERT2_CR3S (1 << 4) // climbrate 3s + +#define ESC_INVERT_VOLTAGE (1 << 0) // voltage +#define ESC_INVERT_TEMP1 (1 << 1) // temperature 1 +#define ESC_INVERT_TEMP2 (1 << 2) // temperature 2 +#define ESC_INVERT_CURRENT (1 << 3) // current +#define ESC_INVERT_RPM (1 << 4) // rpm +#define ESC_INVERT_CAPACITY (1 << 5) // capacity +#define ESC_INVERT_MAXCURRENT (1 << 6) // maximum current + +// message codes +#define HOTT_TEXT_ID 0x7f // Text request +#define HOTT_BINARY_ID 0x80 // Binary request +#define HOTT_VARIO_ID 0x89 // Vario Module ID +#define HOTT_VARIO_TEXT_ID 0x90 // Vario Module TEXT ID +#define HOTT_GPS_ID 0x8a // GPS Module ID +#define HOTT_GPS_TEXT_ID 0xa0 // GPS Module TEXT ID +#define HOTT_ESC_ID 0x8c // ESC Module ID +#define HOTT_ESC_TEXT_ID 0xc0 // ESC Module TEXT ID +#define HOTT_GAM_ID 0x8d // General Air Module ID +#define HOTT_GAM_TEXT_ID 0xd0 // General Air Module TEXT ID +#define HOTT_EAM_ID 0x8e // Electric Air Module ID +#define HOTT_EAM_TEXT_ID 0xe0 // Electric Air Module TEXT ID +#define HOTT_TEXT_START 0x7b // Start byte Text mode +#define HOTT_START 0x7c // Start byte Binary mode +#define HOTT_STOP 0x7d // End byte +#define HOTT_BUTTON_DEC 0xEB // minus button +#define HOTT_BUTTON_INC 0xED // plus button +#define HOTT_BUTTON_SET 0xE9 // set button +#define HOTT_BUTTON_NIL 0x0F // esc button +#define HOTT_BUTTON_NEXT 0xEE // next button +#define HOTT_BUTTON_PREV 0xE7 // previous button + +// prefined signal tones or spoken announcments +#define HOTT_TONE_A 1 // minimum speed +#define HOTT_TONE_B 2 // sink rate 3 seconds +#define HOTT_TONE_C 3 // sink rate 1 second +#define HOTT_TONE_D 4 // maximum distance +#define HOTT_TONE_E 5 // - +#define HOTT_TONE_F 6 // minimum temperature sensor 1 +#define HOTT_TONE_G 7 // minimum temperature sensor 2 +#define HOTT_TONE_H 8 // maximum temperature sensor 1 +#define HOTT_TONE_I 9 // maximum temperature sensor 2 +#define HOTT_TONE_J 10 // overvoltage sensor 1 +#define HOTT_TONE_K 11 // overvoltage sensor 2 +#define HOTT_TONE_L 12 // maximum speed +#define HOTT_TONE_M 13 // climb rate 3 seconds +#define HOTT_TONE_N 14 // climb rate 1 second +#define HOTT_TONE_O 15 // minimum height +#define HOTT_TONE_P 16 // minimum input voltage +#define HOTT_TONE_Q 17 // minimum cell voltage +#define HOTT_TONE_R 18 // undervoltage sensor 1 +#define HOTT_TONE_S 19 // undervoltage sensor 2 +#define HOTT_TONE_T 20 // minimum rpm +#define HOTT_TONE_U 21 // fuel reserve +#define HOTT_TONE_V 22 // capacity +#define HOTT_TONE_W 23 // maximum current +#define HOTT_TONE_X 24 // maximum input voltage +#define HOTT_TONE_Y 25 // maximum rpm +#define HOTT_TONE_Z 26 // maximum height +#define HOTT_TONE_20M 37 // 20 meters +#define HOTT_TONE_40M 38 // 40 meters +#define HOTT_TONE_60M 39 // 60 meters +#define HOTT_TONE_80M 40 // 80 meters +#define HOTT_TONE_100M 41 // 100 meters +#define HOTT_TONE_42 42 // receiver voltage +#define HOTT_TONE_43 43 // receiver temperature +#define HOTT_TONE_200M 46 // 200 meters +#define HOTT_TONE_400M 47 // 400 meters +#define HOTT_TONE_600M 48 // 600 meters +#define HOTT_TONE_800M 49 // 800 meters +#define HOTT_TONE_1000M 50 // 10000 meters +#define HOTT_TONE_51 51 // maximum servo temperature +#define HOTT_TONE_52 52 // maximum servo position difference + + +// Private types +typedef struct { + uint8_t l; + uint8_t h; +} uword_t; + +// Private structures +struct telemetrydata { + HoTTBridgeSettingsData Settings; + AttitudeStateData Attitude; + BaroSensorData Baro; + FlightBatteryStateData Battery; + FlightStatusData FlightStatus; + GPSPositionSensorData GPS; + AirspeedStateData Airspeed; + GPSTimeData GPStime; + GyroSensorData Gyro; + HomeLocationData Home; + PositionStateData Position; + SystemAlarmsData SysAlarms; + VelocityStateData Velocity; + int16_t climbratebuffer[climbratesize]; + uint8_t climbrate_pointer; + float altitude; + float min_altitude; + float max_altitude; + float altitude_last; + float climbrate1s; + float climbrate3s; + float climbrate10s; + float homedistance; + float homecourse; + uint8_t last_armed; + char statusline[statussize]; +}; + +// VARIO Module message structure +struct hott_vario_message { + uint8_t start; // start byte + uint8_t sensor_id; // VARIO sensor ID + uint8_t warning; // 0…= warning beeps + uint8_t sensor_text_id; // VARIO sensor text ID + uint8_t alarm_inverse; // this inverts specific parts of display + uword_t altitude; // altitude (meters), offset 500, 500 == 0m + uword_t max_altitude; // max. altitude (meters) + uword_t min_altitude; // min. altitude (meters) + uword_t climbrate; // climb rate (0.01m/s), offset 30000, 30000 == 0.00m/s + uword_t climbrate3s; // climb rate (0.01m/3s) + uword_t climbrate10s; // climb rate (0.01m/10s) + uint8_t ascii[21]; // 21 chars of text + uint8_t ascii1; // ASCII Free character [1] (next to Alt) + uint8_t ascii2; // ASCII Free character [2] (next to Dir) + uint8_t ascii3; // ASCII Free character [3] (next to I) + int8_t compass; // 1=2°, -1=-2° + uint8_t version; // version number + uint8_t stop; // stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed +}; + +// GPS Module message structure +struct hott_gps_message { + uint8_t start; // start byte + uint8_t sensor_id; // GPS sensor ID + uint8_t warning; // 0…= warning beeps + uint8_t sensor_text_id; // GPS Sensor text mode ID + uint8_t alarm_inverse1; // this inverts specific parts of display + uint8_t alarm_inverse2; + uint8_t flight_direction; // flight direction (1 = 2°; 0° = north, 90° = east , 180° = south , 270° west) + uword_t gps_speed; // GPS speed (km/h) + uint8_t latitude_ns; // GPS latitude north/south (0 = N) + uword_t latitude_min; // GPS latitude (min) + uword_t latitude_sec; // GPS latitude (sec) + uint8_t longitude_ew; // GPS longitude east/west (0 = E) + uword_t longitude_min; // GPS longitude (min) + uword_t longitude_sec; // GPS longitude (sec) + uword_t distance; // distance from home location (meters) + uword_t altitude; // altitude (meters), offset 500, 500 == 0m + uword_t climbrate; // climb rate (0.01m/s), offset of 30000, 30000 = 0.00 m/s + uint8_t climbrate3s; // climb rate in (m/3s). offset of 120, 120 == 0m/3sec + uint8_t gps_num_sat; // GPS number of satelites + uint8_t gps_fix_char; // GPS FixChar ('D'=DGPS, '2'=2D, '3'=3D) + uint8_t home_direction; // home direction (1=2°, direction from starting point to model position) + int8_t angle_roll; // angle x-direction (roll 1=2°, 255=-2°) + int8_t angle_nick; // angle y-direction (nick) + int8_t angle_compass; // angle z-direction (yaw) + uint8_t gps_hour; // GPS time hours + uint8_t gps_min; // GPS time minutes + uint8_t gps_sec; // GPS time seconds + uint8_t gps_msec; // GPS time .sss + uword_t msl; // MSL or NN altitude + uint8_t vibration; // vibration + uint8_t ascii4; // ASCII Free Character [4] (next to home distance) + uint8_t ascii5; // ASCII Free Character [5] (next to home direction) + uint8_t ascii6; // ASCII Free Character [6] (next to number of sats) + uint8_t version; // version number (0=gps, 1=gyro, 255=multicopter) + uint8_t stop; // stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed +}; + +// General Air Module message structure +struct hott_gam_message { + uint8_t start; // start byte + uint8_t sensor_id; // GAM sensor ID + uint8_t warning; // 0…= warning beeps + uint8_t sensor_text_id; // EAM Sensor text mode ID + uint8_t alarm_inverse1; // this inverts specific parts of display + uint8_t alarm_inverse2; + uint8_t cell1; // cell voltages in 0.02V steps, 210 == 4.2V + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uword_t batt1_voltage; // battery sensor 1 in 0.1V steps, 50 == 5.5V + uword_t batt2_voltage; // battery sensor 2 voltage + uint8_t temperature1; // temperature 1 in °C, offset of 20, 20 == 0°C + uint8_t temperature2; // temperature 2 + uint8_t fuel_procent; // fuel capacity in %, values from 0..100 + uword_t fuel_ml; // fuel capacity in ml, values from 0..65535 + uword_t rpm; // rpm, scale factor 10, 300 == 3000rpm + uword_t altitude; // altitude in meters, offset of 500, 500 == 0m + uword_t climbrate; // climb rate (0.01m/s), offset of 30000, 30000 = 0.00 m/s + uint8_t climbrate3s; // climb rate (m/3sec). offset of 120, 120 == 0m/3sec + uword_t current; // current in 0.1A steps + uword_t voltage; // main power voltage in 0.1V steps + uword_t capacity; // used battery capacity in 10mAh steps + uword_t speed; // speed in km/h + uint8_t min_cell_volt; // lowest cell voltage in 20mV steps. 124 == 2.48V + uint8_t min_cell_volt_num; // number of the cell with the lowest voltage + uword_t rpm2; // rpm2 in 10 rpm steps, 300 == 3000rpm + uint8_t g_error_number; // general error number (Voice error == 12) + uint8_t pressure; // pressure up to 15bar, 0.1bar steps + uint8_t version; // version number + uint8_t stop; // stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed +}; + +// Electric Air Module message structure +struct hott_eam_message { + uint8_t start; // Start byte + uint8_t sensor_id; // EAM sensor id + uint8_t warning; + uint8_t sensor_text_id; // EAM Sensor text mode ID + uint8_t alarm_inverse1; // this inverts specific parts of display + uint8_t alarm_inverse2; + uint8_t cell1_L; // cell voltages of the lower battery + uint8_t cell2_L; + uint8_t cell3_L; + uint8_t cell4_L; + uint8_t cell5_L; + uint8_t cell6_L; + uint8_t cell7_L; + uint8_t cell1_H; // cell voltages of higher battery + uint8_t cell2_H; + uint8_t cell3_H; + uint8_t cell4_H; + uint8_t cell5_H; + uint8_t cell6_H; + uint8_t cell7_H; + uword_t batt1_voltage; // battery sensor 1 voltage, in steps of 0.02V + uword_t batt2_voltage; // battery sensor 2 voltage, in steps of 0.02V + uint8_t temperature1; // temperature sensor 1. 20 = 0 degrees + uint8_t temperature2; // temperature sensor 2. 20 = 0 degrees + uword_t altitude; // altitude (meters). 500 = 0 meters + uword_t current; // current (A) in steps of 0.1A + uword_t voltage; // main power voltage in steps of 0.1V + uword_t capacity; // used battery capacity in steps of 10mAh + uword_t climbrate; // climb rate in 0.01m/s. 0m/s = 30000 + uint8_t climbrate3s; // climb rate in m/3sec. 0m/3sec = 120 + uword_t rpm; // rpm in steps of 10 rpm + uint8_t electric_min; // estaminated flight time in minutes. + uint8_t electric_sec; // estaminated flight time in seconds. + uword_t speed; // speed in km/h in steps of 1 km/h + uint8_t stop; // Stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed. +}; + +// ESC Module message structure +struct hott_esc_message { + uint8_t start; // Start byte + uint8_t sensor_id; // EAM sensor id + uint8_t warning; + uint8_t sensor_text_id; // ESC Sensor text mode ID + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uword_t batt_voltage; // battery voltage in steps of 0.1V + uword_t min_batt_voltage; // min battery voltage + uword_t batt_capacity; // used battery capacity in steps of 10mAh + uint8_t temperatureESC; // temperature of ESC . 20 = 0 degrees + uint8_t max_temperatureESC; // max temperature of ESC + uword_t current; // Current in steps of 0.1A + uword_t max_current; // maximal current + uword_t rpm; // rpm in steps of 10 rpm + uword_t max_rpm; // max rpm + uint8_t temperatureMOT; // motor temperature + uint8_t max_temperatureMOT; // maximal motor temperature + uint8_t dummy[19]; // 19 dummy bytes + uint8_t stop; // Stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed. +}; + +// TEXT message structure +struct hott_text_message { + uint8_t start; // Start byte + uint8_t sensor_id; // TEXT id + uint8_t warning; + uint8_t text[21][8]; // text field 21 columns and 8 rows (bit 7=1 for inverse display) + uint8_t stop; // Stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed. +}; + + +/** + * @} + * @} + */ diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c new file mode 100644 index 0000000000..60023b759d --- /dev/null +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -0,0 +1,1008 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module + * @{ + * + * @file uavohottbridge.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * Tau Labs, http://taulabs.org, Copyright (C) 2013-2014 + * @brief sends telemery data on HoTT request + * + * @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 + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#include "openpilot.h" +#include "hwsettings.h" +#include "taskinfo.h" +#include "callbackinfo.h" +#include "hottbridgesettings.h" +#include "attitudestate.h" +#include "barosensor.h" +#include "flightbatterystate.h" +#include "flightstatus.h" +#include "gyrosensor.h" +#include "gpspositionsensor.h" +#include "gpstime.h" +#include "airspeedstate.h" +#include "homelocation.h" +#include "positionstate.h" +#include "systemalarms.h" +#include "velocitystate.h" +#include "hottbridgestatus.h" +#include "hottbridgesettings.h" +#include "objectpersistence.h" +#include "pios_sensors.h" +#include "uavohottbridge.h" + +#include "pios_board_io.h" + +#if defined(PIOS_INCLUDE_HOTT_BRIDGE) + +#if defined(PIOS_HoTT_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE +#else +#define STACK_SIZE_BYTES 2048 +#endif +#define TASK_PRIORITY CALLBACK_TASK_AUXILIARY + +static bool module_enabled = false; + +// Private variables +static bool module_enabled; +static struct telemetrydata *telestate; +static HoTTBridgeStatusData status; + +// Private functions +static void uavoHoTTBridgeTask(void *parameters); +static uint16_t build_VARIO_message(struct hott_vario_message *msg); +static uint16_t build_GPS_message(struct hott_gps_message *msg); +static uint16_t build_GAM_message(struct hott_gam_message *msg); +static uint16_t build_EAM_message(struct hott_eam_message *msg); +static uint16_t build_ESC_message(struct hott_esc_message *msg); +static uint16_t build_TEXT_message(struct hott_text_message *msg); +static uint8_t calc_checksum(uint8_t *data, uint16_t size); +static uint8_t generate_warning(); +static void update_telemetrydata(); +static void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec); +static uint8_t scale_float2uint8(float value, float scale, float offset); +static int8_t scale_float2int8(float value, float scale, float offset); +static uword_t scale_float2uword(float value, float scale, float offset); + +/** + * Module start routine automatically called after initialization routine + * @return 0 when was successful + */ +static int32_t uavoHoTTBridgeStart(void) +{ + status.TxPackets = 0; + status.RxPackets = 0; + status.TxFail = 0; + status.RxFail = 0; + + + // Start task + if (module_enabled) { + xTaskHandle taskHandle; + + xTaskCreate(uavoHoTTBridgeTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHOTTBRIDGE, taskHandle); + } + + return 0; +} + +/** + * Module initialization routine + * @return 0 when initialization was successful + */ +static int32_t uavoHoTTBridgeInitialize(void) +{ + if (PIOS_COM_HOTT) { + module_enabled = true; + // HoTT telemetry baudrate is fixed to 19200 + + PIOS_COM_ChangeBaud(PIOS_COM_HOTT, 19200); + bool param = true; + PIOS_COM_Ioctl(PIOS_COM_HOTT, PIOS_IOCTL_USART_SET_HALFDUPLEX, ¶m); + HoTTBridgeStatusInitialize(); + + // allocate memory for telemetry data + telestate = (struct telemetrydata *)pios_malloc(sizeof(*telestate)); + + if (telestate == NULL) { + // there is not enough free memory. the module could not run. + module_enabled = false; + return -1; + } + } else { + module_enabled = false; + } + return 0; +} +MODULE_INITCALL(uavoHoTTBridgeInitialize, uavoHoTTBridgeStart); + +/** + * Main task. It does not return. + */ +static void uavoHoTTBridgeTask(__attribute__((unused)) void *parameters) +{ + uint8_t rx_buffer[2]; + uint8_t tx_buffer[HOTT_MAX_MESSAGE_LENGTH]; + uint16_t message_size; + + // clear all state values + memset(telestate, 0, sizeof(*telestate)); + + // initialize timer variables + portTickType lastSysTime = xTaskGetTickCount(); + // idle delay between telemetry request and answer + uint32_t idledelay = IDLE_TIME; + // data delay between transmitted bytes + uint32_t datadelay = DATA_TIME; + + // work on hott telemetry. endless loop. + while (1) { + // clear message size on every loop before processing + message_size = 0; + + // shift receiver buffer. make room for one byte. + rx_buffer[1] = rx_buffer[0]; + + // wait for a byte of telemetry request in data delay interval + while (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) { + vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS); + } + // set start trigger point + lastSysTime = xTaskGetTickCount(); + + // examine received data stream + if (rx_buffer[1] == HOTT_BINARY_ID) { + // first received byte looks like a binary request. check second received byte for a sensor id. + switch (rx_buffer[0]) { + case HOTT_VARIO_ID: + message_size = build_VARIO_message((struct hott_vario_message *)tx_buffer); + break; + case HOTT_GPS_ID: + message_size = build_GPS_message((struct hott_gps_message *)tx_buffer); + break; + case HOTT_GAM_ID: + message_size = build_GAM_message((struct hott_gam_message *)tx_buffer); + break; + case HOTT_EAM_ID: + message_size = build_EAM_message((struct hott_eam_message *)tx_buffer); + break; + case HOTT_ESC_ID: + message_size = build_ESC_message((struct hott_esc_message *)tx_buffer); + break; + default: + message_size = 0; + } + } else if (rx_buffer[1] == HOTT_TEXT_ID) { + // first received byte looks like a text request. check second received byte for a valid button. + switch (rx_buffer[0]) { + case HOTT_BUTTON_DEC: + case HOTT_BUTTON_INC: + case HOTT_BUTTON_SET: + case HOTT_BUTTON_NIL: + case HOTT_BUTTON_NEXT: + case HOTT_BUTTON_PREV: + message_size = build_TEXT_message((struct hott_text_message *)tx_buffer); + break; + default: + message_size = 0; + } + } + + // check if a message is in the transmit buffer. + if (message_size > 0) { + status.RxPackets++; + + // check idle line before transmit. pause, then check receiver buffer + vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS); + + if (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) { + // nothing received means idle line. ready to transmit the requested message + for (int i = 0; i < message_size; i++) { + // send message content with pause between each byte + PIOS_COM_SendCharNonBlocking(PIOS_COM_HOTT, tx_buffer[i]); + // grab possible incoming loopback data and throw it away + PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, sizeof(rx_buffer), 0); + vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS); + } + status.TxPackets++; + + + // after transmitting the message, any loopback data needs to be cleaned up. + vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS); + PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, tx_buffer, message_size, 0); + } else { + status.RxFail++; + } + HoTTBridgeStatusSet(&status); + } + } +} + +/** + * Build requested answer messages. + * \return value sets message size + */ +uint16_t build_VARIO_message(struct hott_vario_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.VARIO == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_VARIO_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_VARIO_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_ALT : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_ALT : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_MAX : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_MIN : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0; + + // altitude relative to ground + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + msg->min_altitude = scale_float2uword(telestate->min_altitude, 1, OFFSET_ALTITUDE); + msg->max_altitude = scale_float2uword(telestate->max_altitude, 1, OFFSET_ALTITUDE); + + // climbrate + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uword(telestate->climbrate3s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate10s = scale_float2uword(telestate->climbrate10s, M_TO_CM, OFFSET_CLIMBRATE); + + // compass + msg->compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0); + + // statusline + memcpy(msg->ascii, telestate->statusline, sizeof(msg->ascii)); + + // free display characters + msg->ascii1 = 0; + msg->ascii2 = 0; + msg->ascii3 = 0; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_GPS_message(struct hott_gps_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.GPS == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_GPS_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_GPS_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxDistance < telestate->homedistance) ? GPS_INVERT_HDIST : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GPS_INVERT_ALT : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GPS_INVERT_ALT : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GPS_INVERT_CR1S : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GPS_INVERT_CR1S : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GPS_INVERT_CR3S : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GPS_INVERT_CR3S : 0; + msg->alarm_inverse2 |= (telestate->SysAlarms.Alarm.GPS != SYSTEMALARMS_ALARM_OK) ? GPS_INVERT2_POS : 0; + + // gps direction, groundspeed and postition + msg->flight_direction = scale_float2uint8(telestate->GPS.Heading, DEG_TO_UINT, 0); + msg->gps_speed = scale_float2uword(telestate->GPS.Groundspeed, MS_TO_KMH, 0); + convert_long2gps(telestate->GPS.Latitude, &msg->latitude_ns, &msg->latitude_min, &msg->latitude_sec); + convert_long2gps(telestate->GPS.Longitude, &msg->longitude_ew, &msg->longitude_min, &msg->longitude_sec); + + // homelocation distance, course and state + msg->distance = scale_float2uword(telestate->homedistance, 1, 0); + msg->home_direction = scale_float2uint8(telestate->homecourse, DEG_TO_UINT, 0); + msg->ascii5 = (telestate->Home.Set ? 'H' : '-'); + + // altitude relative to ground and climb rate + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S); + + // number of satellites,gps fix and state + msg->gps_num_sat = telestate->GPS.Satellites; + switch (telestate->GPS.Status) { + case GPSPOSITIONSENSOR_STATUS_FIX2D: + msg->gps_fix_char = '2'; + break; + case GPSPOSITIONSENSOR_STATUS_FIX3D: + case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS: + msg->gps_fix_char = '3'; + break; + default: + msg->gps_fix_char = 0; + } + switch (telestate->SysAlarms.Alarm.GPS) { + case SYSTEMALARMS_ALARM_UNINITIALISED: + msg->ascii6 = 0; + // if there is no gps, show compass flight direction + msg->flight_direction = scale_float2int8((telestate->Attitude.Yaw > 0) ? telestate->Attitude.Yaw : 360 + telestate->Attitude.Yaw, DEG_TO_UINT, 0); + break; + case SYSTEMALARMS_ALARM_OK: + msg->ascii6 = '.'; + break; + case SYSTEMALARMS_ALARM_WARNING: + msg->ascii6 = '?'; + break; + case SYSTEMALARMS_ALARM_ERROR: + case SYSTEMALARMS_ALARM_CRITICAL: + msg->ascii6 = '!'; + break; + default: + msg->ascii6 = 0; + } + + // model angles + msg->angle_roll = scale_float2int8(telestate->Attitude.Roll, DEG_TO_UINT, 0); + msg->angle_nick = scale_float2int8(telestate->Attitude.Pitch, DEG_TO_UINT, 0); + msg->angle_compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0); + + // gps time + msg->gps_hour = telestate->GPStime.Hour; + msg->gps_min = telestate->GPStime.Minute; + msg->gps_sec = telestate->GPStime.Second; + msg->gps_msec = 0; + + // gps MSL (NN) altitude MSL + msg->msl = scale_float2uword(telestate->GPS.Altitude, 1, 0); + + // free display chararacter + msg->ascii4 = 0; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_GAM_message(struct hott_gam_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.GAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_GAM_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_GAM_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? GAM_INVERT2_CURRENT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0; + + // temperatures + msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE); + msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE); + + // altitude + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + + // climbrate + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S); + + // main battery + float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0; + float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0; + float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0; + msg->voltage = scale_float2uword(voltage, 10, 0); + msg->current = scale_float2uword(current, 10, 0); + msg->capacity = scale_float2uword(energy, 0.1f, 0); + + // simulate individual cell voltage + uint8_t cell_voltage = (telestate->Battery.Voltage > 0) ? scale_float2uint8(telestate->Battery.Voltage / telestate->Battery.NbCells, 50, 0) : 0; + msg->cell1 = (telestate->Battery.NbCells >= 1) ? cell_voltage : 0; + msg->cell2 = (telestate->Battery.NbCells >= 2) ? cell_voltage : 0; + msg->cell3 = (telestate->Battery.NbCells >= 3) ? cell_voltage : 0; + msg->cell4 = (telestate->Battery.NbCells >= 4) ? cell_voltage : 0; + msg->cell5 = (telestate->Battery.NbCells >= 5) ? cell_voltage : 0; + msg->cell6 = (telestate->Battery.NbCells >= 6) ? cell_voltage : 0; + + msg->min_cell_volt = cell_voltage; + msg->min_cell_volt_num = telestate->Battery.NbCells; + + // apply main voltage to batt1 voltage + msg->batt1_voltage = msg->voltage; + + // AirSpeed + float airspeed = (telestate->Airspeed.TrueAirspeed > 0) ? telestate->Airspeed.TrueAirspeed : 0; + msg->speed = scale_float2uword(airspeed, MS_TO_KMH, 0); + + // pressure kPa to 0.1Bar + msg->pressure = scale_float2uint8(telestate->Baro.Pressure, 0.1f, 0); + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_EAM_message(struct hott_eam_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.EAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_EAM_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_EAM_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy) ? EAM_INVERT_CAPACITY : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? EAM_INVERT_CURRENT : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? EAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? EAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0; + + // main battery + float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0; + float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0; + float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0; + msg->voltage = scale_float2uword(voltage, 10, 0); + msg->current = scale_float2uword(current, 10, 0); + msg->capacity = scale_float2uword(energy, 0.1f, 0); + + // simulate individual cell voltage + uint8_t cell_voltage = (telestate->Battery.Voltage > 0) ? scale_float2uint8(telestate->Battery.Voltage / telestate->Battery.NbCells, 50, 0) : 0; + msg->cell1_H = (telestate->Battery.NbCells >= 1) ? cell_voltage : 0; + msg->cell2_H = (telestate->Battery.NbCells >= 2) ? cell_voltage : 0; + msg->cell3_H = (telestate->Battery.NbCells >= 3) ? cell_voltage : 0; + msg->cell4_H = (telestate->Battery.NbCells >= 4) ? cell_voltage : 0; + msg->cell5_H = (telestate->Battery.NbCells >= 5) ? cell_voltage : 0; + msg->cell6_H = (telestate->Battery.NbCells >= 6) ? cell_voltage : 0; + msg->cell7_H = (telestate->Battery.NbCells >= 7) ? cell_voltage : 0; + + // apply main voltage to batt1 voltage + msg->batt1_voltage = msg->voltage; + + // AirSpeed + float airspeed = (telestate->Airspeed.TrueAirspeed > 0) ? telestate->Airspeed.TrueAirspeed : 0; + msg->speed = scale_float2uword(airspeed, MS_TO_KMH, 0); + + // temperatures + msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE); + msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE); + + // altitude + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + + // climbrate + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S); + + // flight time + float flighttime = (telestate->Battery.EstimatedFlightTime <= 5999) ? telestate->Battery.EstimatedFlightTime : 5999; + msg->electric_min = flighttime / 60; + msg->electric_sec = flighttime - 60 * msg->electric_min; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_ESC_message(struct hott_esc_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.ESC == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_ESC_ID; + msg->warning = 0; + msg->sensor_text_id = HOTT_ESC_TEXT_ID; + + // main batterie + float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0; + float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0; + float max_current = (telestate->Battery.PeakCurrent > 0) ? telestate->Battery.PeakCurrent : 0; + float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0; + msg->batt_voltage = scale_float2uword(voltage, 10, 0); + msg->current = scale_float2uword(current, 10, 0); + msg->max_current = scale_float2uword(max_current, 10, 0); + msg->batt_capacity = scale_float2uword(energy, 0.1f, 0); + + // temperatures + msg->temperatureESC = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE); + msg->max_temperatureESC = scale_float2uint8(0, 1, OFFSET_TEMPERATURE); + msg->temperatureMOT = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE); + msg->max_temperatureMOT = scale_float2uint8(0, 1, OFFSET_TEMPERATURE); + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_TEXT_message(struct hott_text_message *msg) +{ + update_telemetrydata(); + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_TEXT_ID; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +/** + * update telemetry data + * this is called on every telemetry request + * calling interval is 200ms depending on TX + * 200ms telemetry request is used as time base for timed calculations (5Hz interval) + */ +void update_telemetrydata() +{ + // update all available data + if (HoTTBridgeSettingsHandle() != NULL) { + HoTTBridgeSettingsGet(&telestate->Settings); + } + if (AttitudeStateHandle() != NULL) { + AttitudeStateGet(&telestate->Attitude); + } + if (BaroSensorHandle() != NULL) { + BaroSensorGet(&telestate->Baro); + } + if (FlightBatteryStateHandle() != NULL) { + FlightBatteryStateGet(&telestate->Battery); + } + if (FlightStatusHandle() != NULL) { + FlightStatusGet(&telestate->FlightStatus); + } + if (GPSPositionSensorHandle() != NULL) { + GPSPositionSensorGet(&telestate->GPS); + } + if (AirspeedStateHandle() != NULL) { + AirspeedStateGet(&telestate->Airspeed); + } + if (GPSTimeHandle() != NULL) { + GPSTimeGet(&telestate->GPStime); + } + if (GyroSensorHandle() != NULL) { + GyroSensorGet(&telestate->Gyro); + } + if (HomeLocationHandle() != NULL) { + HomeLocationGet(&telestate->Home); + } + if (PositionStateHandle() != NULL) { + PositionStateGet(&telestate->Position); + } + if (SystemAlarmsHandle() != NULL) { + SystemAlarmsGet(&telestate->SysAlarms); + } + if (VelocityStateHandle() != NULL) { + VelocityStateGet(&telestate->Velocity); + } + + // send actual climbrate value to ring buffer as mm per 0.2s values + uint8_t n = telestate->climbrate_pointer; + telestate->climbratebuffer[telestate->climbrate_pointer++] = -telestate->Velocity.Down * 200; + telestate->climbrate_pointer %= climbratesize; + + // calculate avarage climbrates in meters per 1, 3 and 10 second(s) based on 200ms interval + telestate->climbrate1s = 0; + telestate->climbrate3s = 0; + telestate->climbrate10s = 0; + for (uint8_t i = 0; i < climbratesize; i++) { + telestate->climbrate1s += (i < 5) ? telestate->climbratebuffer[n] : 0; + telestate->climbrate3s += (i < 15) ? telestate->climbratebuffer[n] : 0; + telestate->climbrate10s += (i < 50) ? telestate->climbratebuffer[n] : 0; + n += climbratesize - 1; + n %= climbratesize; + } + telestate->climbrate1s = telestate->climbrate1s / 1000; + telestate->climbrate3s = telestate->climbrate3s / 1000; + telestate->climbrate10s = telestate->climbrate10s / 1000; + + // set altitude offset and clear min/max values when arming + if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING) || ((telestate->last_armed != FLIGHTSTATUS_ARMED_ARMED) && (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED))) { + telestate->min_altitude = 0; + telestate->max_altitude = 0; + } + telestate->last_armed = telestate->FlightStatus.Armed; + + // calculate altitude relative to start position + telestate->altitude = -telestate->Position.Down; + + // check and set min/max values when armed + // and without receiver input for standalone board used as sensor + if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) || ((telestate->SysAlarms.Alarm.Attitude == SYSTEMALARMS_ALARM_OK) && (telestate->SysAlarms.Alarm.Receiver != SYSTEMALARMS_ALARM_OK))) { + if (telestate->min_altitude > telestate->altitude) { + telestate->min_altitude = telestate->altitude; + } + if (telestate->max_altitude < telestate->altitude) { + telestate->max_altitude = telestate->altitude; + } + } + + // gps home position and course + telestate->homedistance = sqrtf(telestate->Position.North * telestate->Position.North + telestate->Position.East * telestate->Position.East); + telestate->homecourse = acosf(-telestate->Position.North / telestate->homedistance) / 3.14159265f * 180; + if (telestate->Position.East > 0) { + telestate->homecourse = 360 - telestate->homecourse; + } + + // statusline + const char *txt_unknown = "unknown"; + const char *txt_manual = "Manual"; + const char *txt_stabilized1 = "Stabilized1"; + const char *txt_stabilized2 = "Stabilized2"; + const char *txt_stabilized3 = "Stabilized3"; + const char *txt_stabilized4 = "Stabilized4"; + const char *txt_stabilized5 = "Stabilized5"; + const char *txt_stabilized6 = "Stabilized6"; + const char *txt_positionhold = "PositionHold"; + const char *txt_courselock = "CourseLock"; + const char *txt_velocityroam = "VelocityRoam"; + const char *txt_homeleash = "HomeLeash"; + const char *txt_absoluteposition = "AbsolutePosition"; + const char *txt_returntobase = "ReturnToBase"; + const char *txt_land = "Land"; + const char *txt_pathplanner = "PathPlanner"; + const char *txt_poi = "PointOfInterest"; + const char *txt_autocruise = "AutoCruise"; + const char *txt_autotakeoff = "AutoTakeOff"; + const char *txt_autotune = "Autotune"; + const char *txt_disarmed = "Disarmed"; + const char *txt_arming = "Arming"; + const char *txt_armed = "Armed"; + + const char *txt_flightmode; + switch (telestate->FlightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + txt_flightmode = txt_manual; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + txt_flightmode = txt_stabilized1; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + txt_flightmode = txt_stabilized2; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + txt_flightmode = txt_stabilized3; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: + txt_flightmode = txt_stabilized4; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + txt_flightmode = txt_stabilized5; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: + txt_flightmode = txt_stabilized6; + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + txt_flightmode = txt_positionhold; + break; + case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: + txt_flightmode = txt_courselock; + break; + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: + txt_flightmode = txt_velocityroam; + break; + case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: + txt_flightmode = txt_homeleash; + break; + case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION: + txt_flightmode = txt_absoluteposition; + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + txt_flightmode = txt_returntobase; + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + txt_flightmode = txt_land; + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + txt_flightmode = txt_pathplanner; + break; + case FLIGHTSTATUS_FLIGHTMODE_POI: + txt_flightmode = txt_poi; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: + txt_flightmode = txt_autocruise; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: + txt_flightmode = txt_autotakeoff; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: + txt_flightmode = txt_autotune; + break; + default: + txt_flightmode = txt_unknown; + } + + const char *txt_armstate; + switch (telestate->FlightStatus.Armed) { + case FLIGHTSTATUS_ARMED_DISARMED: + txt_armstate = txt_disarmed; + break; + case FLIGHTSTATUS_ARMED_ARMING: + txt_armstate = txt_arming; + break; + case FLIGHTSTATUS_ARMED_ARMED: + txt_armstate = txt_armed; + break; + default: + txt_armstate = txt_unknown; + } + + snprintf(telestate->statusline, sizeof(telestate->statusline), "%12s,%8s", txt_flightmode, txt_armstate); +} + +/** + * generate warning beeps or spoken announcements + */ +uint8_t generate_warning() +{ + bool gps_ok = (telestate->SysAlarms.Alarm.GPS == SYSTEMALARMS_ALARM_OK); + + // set warning tone with hardcoded priority + if ((telestate->Settings.Warning.MinSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed * MS_TO_KMH) && gps_ok) { + return HOTT_TONE_A; // minimum speed + } + if ((telestate->Settings.Warning.NegDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s)) { + return HOTT_TONE_B; // sink rate 3s + } + if ((telestate->Settings.Warning.NegDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s)) { + return HOTT_TONE_C; // sink rate 1s + } + if ((telestate->Settings.Warning.MaxDistance == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxDistance < telestate->homedistance) && gps_ok) { + return HOTT_TONE_D; // maximum distance + } + if ((telestate->Settings.Warning.MinSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinSensor1Temp > telestate->Gyro.temperature)) { + return HOTT_TONE_F; // minimum temperature sensor 1 + } + if ((telestate->Settings.Warning.MinSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinSensor2Temp > telestate->Baro.Temperature)) { + return HOTT_TONE_G; // minimum temperature sensor 2 + } + if ((telestate->Settings.Warning.MaxSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxSensor1Temp < telestate->Gyro.temperature)) { + return HOTT_TONE_H; // maximum temperature sensor 1 + } + if ((telestate->Settings.Warning.MaxSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxSensor2Temp < telestate->Baro.Temperature)) { + return HOTT_TONE_I; // maximum temperature sensor 2 + } + if ((telestate->Settings.Warning.MaxSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed * MS_TO_KMH) && gps_ok) { + return HOTT_TONE_L; // maximum speed + } + if ((telestate->Settings.Warning.PosDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.PosDifference2 > telestate->climbrate3s)) { + return HOTT_TONE_M; // climb rate 3s + } + if ((telestate->Settings.Warning.PosDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.PosDifference1 > telestate->climbrate1s)) { + return HOTT_TONE_N; // climb rate 1s + } + if ((telestate->Settings.Warning.MinHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinHeight > telestate->altitude)) { + return HOTT_TONE_O; // minimum height + } + if ((telestate->Settings.Warning.MinPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage)) { + return HOTT_TONE_P; // minimum input voltage + } + if ((telestate->Settings.Warning.MaxUsedCapacity == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy)) { + return HOTT_TONE_V; // capacity + } + if ((telestate->Settings.Warning.MaxCurrent == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current)) { + return HOTT_TONE_W; // maximum current + } + if ((telestate->Settings.Warning.MaxPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage)) { + return HOTT_TONE_X; // maximum input voltage + } + if ((telestate->Settings.Warning.MaxHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxHeight < telestate->altitude)) { + return HOTT_TONE_Z; // maximum height + } + // altitude beeps when crossing altitude limits at 20,40,60,80,100,200,400,600,800 and 1000 meters + if (telestate->Settings.Warning.AltitudeBeep == HOTTBRIDGESETTINGS_WARNING_ENABLED) { + // update altitude when checked for beeps + float last = telestate->altitude_last; + float actual = telestate->altitude; + telestate->altitude_last = telestate->altitude; + if (((last < 20) && (actual >= 20)) || ((last > 20) && (actual <= 20))) { + return HOTT_TONE_20M; + } + if (((last < 40) && (actual >= 40)) || ((last > 40) && (actual <= 40))) { + return HOTT_TONE_40M; + } + if (((last < 60) && (actual >= 60)) || ((last > 60) && (actual <= 60))) { + return HOTT_TONE_60M; + } + if (((last < 80) && (actual >= 80)) || ((last > 80) && (actual <= 80))) { + return HOTT_TONE_80M; + } + if (((last < 100) && (actual >= 100)) || ((last > 100) && (actual <= 100))) { + return HOTT_TONE_100M; + } + if (((last < 200) && (actual >= 200)) || ((last > 200) && (actual <= 200))) { + return HOTT_TONE_200M; + } + if (((last < 400) && (actual >= 400)) || ((last > 400) && (actual <= 400))) { + return HOTT_TONE_400M; + } + if (((last < 600) && (actual >= 600)) || ((last > 600) && (actual <= 600))) { + return HOTT_TONE_600M; + } + if (((last < 800) && (actual >= 800)) || ((last > 800) && (actual <= 800))) { + return HOTT_TONE_800M; + } + if (((last < 1000) && (actual >= 1000)) || ((last > 1000) && (actual <= 1000))) { + return HOTT_TONE_1000M; + } + } + + // there is no warning + return 0; +} + +/** + * calculate checksum of data buffer + */ +uint8_t calc_checksum(uint8_t *data, uint16_t size) +{ + uint16_t sum = 0; + + for (int i = 0; i < size; i++) { + sum += data[i]; + } + return sum; +} + +/** + * scale float value with scale and offset to unsigned byte + */ +uint8_t scale_float2uint8(float value, float scale, float offset) +{ + uint16_t temp = (uint16_t)roundf(value * scale + offset); + uint8_t result; + + result = (uint8_t)temp & 0xff; + return result; +} + +/** + * scale float value with scale and offset to signed byte (int8_t) + */ +int8_t scale_float2int8(float value, float scale, float offset) +{ + int8_t result = (int8_t)roundf(value * scale + offset); + + return result; +} + +/** + * scale float value with scale and offset to word + */ +uword_t scale_float2uword(float value, float scale, float offset) +{ + uint16_t temp = (uint16_t)roundf(value * scale + offset); + uword_t result; + + result.l = (uint8_t)temp & 0xff; + result.h = (uint8_t)(temp >> 8) & 0xff; + return result; +} + +/** + * convert dword gps value into HoTT gps format and write result to given pointers + */ +void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec) +{ + // convert gps decigrad value into degrees, minutes and seconds + uword_t temp; + uint32_t absvalue = abs(value); + uint16_t degrees = (absvalue / 10000000); + uint32_t seconds = (absvalue - degrees * 10000000) * 6; + uint16_t minutes = seconds / 1000000; + + seconds %= 1000000; + seconds = seconds / 100; + uint16_t degmin = degrees * 100 + minutes; + // write results + *dir = (value < 0) ? 1 : 0; + temp.l = (uint8_t)degmin & 0xff; + temp.h = (uint8_t)(degmin >> 8) & 0xff; + *min = temp; + temp.l = (uint8_t)seconds & 0xff; + temp.h = (uint8_t)(seconds >> 8) & 0xff; + *sec = temp; +} + +#endif // PIOS_INCLUDE_HOTT_BRIDGE +/** + * @} + * @} + */ diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index cb0c3fb147..16130085ea 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -49,6 +49,7 @@ #include "airspeedstate.h" #include "actuatorsettings.h" #include "actuatordesired.h" +#include "actuatorcommand.h" #include "flightstatus.h" #include "systemstats.h" #include "systemalarms.h" @@ -68,6 +69,8 @@ #include "pios_sensors.h" +#include + #define PIOS_INCLUDE_MSP_BRIDGE @@ -476,7 +479,8 @@ static void msp_send_analog(struct msp_bridge *m) ManualControlSettingsChannelGroupsGet(&channelGroups); #ifdef PIOS_INCLUDE_OPLINKRCVR - if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) { + if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) || + (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) { int8_t rssi; OPLinkStatusRSSIGet(&rssi); @@ -531,7 +535,7 @@ static void msp_send_raw_gps(struct msp_bridge *m) data.raw_gps.lat = gps_data.Latitude; data.raw_gps.lon = gps_data.Longitude; data.raw_gps.alt = (uint16_t)gps_data.Altitude; - data.raw_gps.speed = (uint16_t)gps_data.Groundspeed; + data.raw_gps.speed = (uint16_t)(gps_data.Groundspeed * 100.0f); data.raw_gps.ground_course = (int16_t)(gps_data.Heading * 10.0f); msp_send(m, MSP_RAW_GPS, data.buf, sizeof(data)); @@ -663,6 +667,20 @@ static void msp_send_channels(struct msp_bridge *m) msp_send(m, MSP_RC, data.buf, sizeof(data)); } +static void msp_send_servo(struct msp_bridge *m) +{ + ActuatorCommandData ac; + + ActuatorCommandGet(&ac); + + // Only the first 8 channels are supported. + // Channels are 16 bits (2 bytes). + uint8_t channels[16]; + memcpy(channels, (uint8_t *)ac.Channel, 16); + + msp_send(m, MSP_SERVO, channels, sizeof(channels)); +} + static void msp_send_boxids(struct msp_bridge *m) // This is actually sending a map of MSP_STATUS.flag bits to BOX ids. { msp_send(m, MSP_BOXIDS, msp_boxes, sizeof(msp_boxes)); @@ -676,14 +694,18 @@ static void msp_send_pidnames(struct msp_bridge *m) static void pid_native2msp(const float *native, msp_pid_t *piditem, float scale, unsigned numelem) { for (unsigned i = 0; i < numelem; ++i) { - piditem->values[i] = lroundf(native[i] * scale); + // Handle Dterm scale + float s = (i == 2) ? scale * 100 : scale; + piditem->values[i] = lroundf(native[i] * s); } } static void pid_msp2native(const msp_pid_t *piditem, float *native, float scale, unsigned numelem) { for (unsigned i = 0; i < numelem; ++i) { - native[i] = (float)piditem->values[i] / scale; + // Handle Dterm scale + float s = (i == 2) ? scale * 100 : scale; + native[i] = (float)piditem->values[i] / s; } } @@ -865,6 +887,9 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) case MSP_RC: msp_send_channels(m); break; + case MSP_SERVO: + msp_send_servo(m); + break; case MSP_BOXIDS: msp_send_boxids(m); break; diff --git a/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c b/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c index bca1ab6fad..2575356e35 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 @@ -245,7 +248,8 @@ static void mavlink_send_rc_channels() ManualControlSettingsChannelGroupsGet(&channelGroups); #ifdef PIOS_INCLUDE_OPLINKRCVR - if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) { + if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) || + (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) { int8_t rssi; OPLinkStatusRSSIGet(&rssi); @@ -317,6 +321,7 @@ static void mavlink_send_position() gps_fix_type = 2; break; case GPSPOSITIONSENSOR_STATUS_FIX3D: + case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS: gps_fix_type = 3; break; } diff --git a/flight/modules/gpsp/gpsdsysmod.c b/flight/modules/gpsp/gpsdsysmod.c index d7f009658e..91480078ab 100644 --- a/flight/modules/gpsp/gpsdsysmod.c +++ b/flight/modules/gpsp/gpsdsysmod.c @@ -44,6 +44,7 @@ #include "inc/gps9flashhandler.h" #include "inc/gps9protocol.h" #include "pios_board_info.h" +#include "pios_board_init.h" extern uint32_t pios_com_main_id; @@ -93,8 +94,7 @@ int32_t GPSPSystemModStart(void) */ int32_t GPSPSystemModInitialize(void) { - GPSPSystemModStart(); - + // Module started in main return 0; } @@ -107,9 +107,17 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters) #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM); #endif + + /* board driver init */ + PIOS_Board_Init(); + + /* Initialize all modules */ + MODULE_INITIALISE_ALL; + while (!initTaskDone) { vTaskDelay(10); } + #ifndef PIOS_INCLUDE_WDG // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop #define PIOS_WDG_Clear() @@ -127,6 +135,7 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_IAP) PIOS_IAP_WriteBootCount(0); #endif + /* Right now there is no configuration and uart speed is fixed at 57600. * TODO: * 1) add a tiny ubx parser on gps side to intercept CFG-RINV and use that for config storage; @@ -143,7 +152,7 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM); #endif uint32_t ledPeriod = PIOS_DELAY_DiffuS(ledTimer) / 1000; - if (ledPeriod < HB_LED_BLINK_ON_PERIOD_MS) { + if (ledPeriod < HB_LED_BLINK_OFF_PERIOD_MS) { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else { PIOS_LED_On(PIOS_LED_HEARTBEAT); diff --git a/flight/pios/common/libraries/FreeRTOS/Source/openocd.c b/flight/pios/common/libraries/FreeRTOS/Source/openocd.c new file mode 100644 index 0000000000..a595d1d3ff --- /dev/null +++ b/flight/pios/common/libraries/FreeRTOS/Source/openocd.c @@ -0,0 +1,20 @@ +/* + * Since at least FreeRTOS V7.5.3 uxTopUsedPriority is no longer + * present in the kernel, so it has to be supplied by other means for + * OpenOCD's threads awareness. + * + * Add this file to your project, and, if you're using --gc-sections, + * ``--undefined=uxTopUsedPriority'' (or + * ``-Wl,--undefined=uxTopUsedPriority'' when using gcc for final + * linking) to your LDFLAGS; same with all the other symbols you need. + */ + +#include "FreeRTOS.h" + +#ifdef __GNUC__ +#define USED __attribute__((section(".keep"))) +#else +#define USED +#endif + +const int USED uxTopUsedPriority = configMAX_PRIORITIES; diff --git a/flight/pios/common/libraries/msheap/pios_msheap.c b/flight/pios/common/libraries/msheap/pios_msheap.c index e7e03f8c65..f28d2f644b 100644 --- a/flight/pios/common/libraries/msheap/pios_msheap.c +++ b/flight/pios/common/libraries/msheap/pios_msheap.c @@ -81,6 +81,9 @@ pios_general_malloc(void *ptr, size_t s, bool use_fast_heap) vPortEnterCritical(); if(use_fast_heap){ p = msheap_alloc(&fast_heap, ptr, s); + if(!p) { + p = msheap_alloc(&sram_heap, ptr, s); + } } else { p = msheap_alloc(&sram_heap, ptr, s); } diff --git a/flight/pios/common/pios_bmp280.c b/flight/pios/common/pios_bmp280.c new file mode 100644 index 0000000000..1a7c3fc380 --- /dev/null +++ b/flight/pios/common/pios_bmp280.c @@ -0,0 +1,407 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BMP280 BMP280 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_bmp280.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * dRonin, http://dRonin.org/, Copyright (C) 2015-2016 + * Tau Labs, http://taulabs.org, Copyright (C) 2012-2014 + * @brief BMP280 Pressure Sensor Routines + * @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_BMP280 +#include +#include + + +#define BMP280_I2C_ADDR 0x76 +#define BMP280_ID 0xD0 +#define BMP280_RESET 0xE0 +#define BMP280_STATUS 0xF3 +#define BMP280_CTRL_MEAS 0xF4 +#define BMP280_CONFIG 0xF5 +#define BMP280_PRESS_MSB 0xF7 +#define BMP280_PRESS_LSB 0xF8 +#define BMP280_PRESS_XLSB 0xF9 +#define BMP280_TEMP_MSB 0xFA +#define BMP280_TEMP_LSB 0xFB +#define BMP280_TEMP_XLSB 0xFC + +#define BMP280_CAL_ADDR 0x88 + +#define BMP280_P0 101.3250f + +#define BMP280_MODE_CONTINUOUS 0x03 +#define BMP280_MODE_STANDBY 0x00 +#define BMP280_MODE_FORCED 0x01 + +#define BMP280_T_STANDBY 500 + +#define BMP280_DEFAULT_CHIP_ID 0x58 +#define BMP280_RESET_MAGIC 0xB6 + +#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH 24 +#define BMP280_DATA_FRAME_SIZE 6 + +#define PIOS_BMP280_I2C_RETRIES 5 +#define PIOS_BMP280_I2C_CONFIG_RETRY_DELAY 1000000 + + +enum pios_bmp280_dev_magic { + PIOS_BMP280_DEV_MAGIC = 0x42323830 +}; + +struct pios_bmp280_dev { + enum pios_bmp280_dev_magic magic; + uintptr_t i2c_id; + + bool sensorIsAlive; + uint32_t conversionDelayUs; + uint32_t configTime; + uint32_t conversionStart; + + uint8_t oversampling; + + // compensation parameters + uint16_t digT1; + int16_t digT2; + int16_t digT3; + uint16_t digP1; + int16_t digP2; + int16_t digP3; + int16_t digP4; + int16_t digP5; + int16_t digP6; + int16_t digP7; + int16_t digP8; + int16_t digP9; + + PIOS_SENSORS_1Axis_SensorsWithTemp results; +}; + +static int32_t PIOS_BMP280_Read(uintptr_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len); +static int32_t PIOS_BMP280_Write(uintptr_t i2c_id, uint8_t address, uint8_t buffer); +static int32_t PIOS_BMP280_Configure(struct pios_bmp280_dev *dev); +static int32_t PIOS_BMP280_ReadPTCompensated(struct pios_bmp280_dev *dev, + uint32_t *compensatedPressure, + int32_t *compensatedTemperature); + +// sensor driver interface +static bool PIOS_BMP280_driver_Test(uintptr_t context); +static void PIOS_BMP280_driver_Reset(uintptr_t context); +static void PIOS_BMP280_driver_get_scale(float *scales, uint8_t size, uintptr_t context); +static void PIOS_BMP280_driver_fetch(void *, uint8_t size, uintptr_t context); +static bool PIOS_BMP280_driver_poll(uintptr_t context); + +const PIOS_SENSORS_Driver PIOS_BMP280_Driver = { + .test = PIOS_BMP280_driver_Test, + .poll = PIOS_BMP280_driver_poll, + .fetch = PIOS_BMP280_driver_fetch, + .reset = PIOS_BMP280_driver_Reset, + .get_queue = NULL, + .get_scale = PIOS_BMP280_driver_get_scale, + .is_polled = true, +}; + +static bool PIOS_BMP280_Validate(struct pios_bmp280_dev *dev) +{ + return dev && (dev->magic == PIOS_BMP280_DEV_MAGIC); +} + +/** + * Initialise the BMP280 sensor + */ + +void PIOS_BMP280_Init(const struct pios_bmp280_cfg *cfg, uint32_t i2c_device) +{ + struct pios_bmp280_dev *dev = (struct pios_bmp280_dev *)pios_malloc(sizeof(*dev)); + + PIOS_Assert(dev); + + dev->magic = PIOS_BMP280_DEV_MAGIC; + dev->i2c_id = i2c_device; + dev->sensorIsAlive = false; + dev->oversampling = cfg->oversampling; + + switch (cfg->oversampling) { + case BMP280_STANDARD_RESOLUTION: + dev->conversionDelayUs = 13300 + BMP280_T_STANDBY; + break; + case BMP280_HIGH_RESOLUTION: + dev->conversionDelayUs = 22500 + BMP280_T_STANDBY; + break; + default: + case BMP280_ULTRA_HIGH_RESOLUTION: + dev->conversionDelayUs = 43200 + BMP280_T_STANDBY; + break; + } + + PIOS_BMP280_Configure(dev); + + PIOS_SENSORS_Register(&PIOS_BMP280_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, (uintptr_t)dev); +} + +static int32_t PIOS_BMP280_Configure(struct pios_bmp280_dev *dev) +{ + // read chip id? + uint8_t chip_id; + + if (dev->sensorIsAlive) { + return 0; + } + + if (PIOS_DELAY_DiffuS(dev->configTime) < PIOS_BMP280_I2C_CONFIG_RETRY_DELAY) { // Do not reinitialize too often + return -1; + } + + dev->configTime = PIOS_DELAY_GetRaw(); + + dev->sensorIsAlive = (PIOS_BMP280_Read(dev->i2c_id, BMP280_ID, &chip_id, sizeof(chip_id)) == 0); + if (!dev->sensorIsAlive) { + return -1; + } + + if (chip_id != BMP280_DEFAULT_CHIP_ID) { + return -2; + } + + + uint8_t data[BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH]; + + dev->sensorIsAlive = (PIOS_BMP280_Read(dev->i2c_id, BMP280_CAL_ADDR, data, BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH) == 0); + + if (!dev->sensorIsAlive) { + return -1; + } + + dev->digT1 = (data[1] << 8) | data[0]; + dev->digT2 = (data[3] << 8) | data[2]; + dev->digT3 = (data[5] << 8) | data[4]; + dev->digP1 = (data[7] << 8) | data[6]; + dev->digP2 = (data[9] << 8) | data[8]; + dev->digP3 = (data[11] << 8) | data[10]; + dev->digP4 = (data[13] << 8) | data[12]; + dev->digP5 = (data[15] << 8) | data[14]; + dev->digP6 = (data[17] << 8) | data[16]; + dev->digP7 = (data[19] << 8) | data[18]; + dev->digP8 = (data[21] << 8) | data[20]; + dev->digP9 = (data[23] << 8) | data[22]; + + dev->sensorIsAlive = (PIOS_BMP280_Write(dev->i2c_id, BMP280_RESET, BMP280_RESET_MAGIC) == 0); + if (!dev->sensorIsAlive) { + return -1; + } + + /* start conversion */ + + dev->sensorIsAlive = (PIOS_BMP280_Write(dev->i2c_id, BMP280_CTRL_MEAS, dev->oversampling | BMP280_MODE_CONTINUOUS) == 0); + + dev->conversionStart = PIOS_DELAY_GetRaw(); + + return 0; +} + +static int32_t PIOS_BMP280_ReadPTCompensated(struct pios_bmp280_dev *dev, + uint32_t *compensatedPressure, + int32_t *compensatedTemperature) +{ + uint8_t data[BMP280_DATA_FRAME_SIZE]; + + /* Read and store results */ + + if (PIOS_BMP280_Read(dev->i2c_id, BMP280_PRESS_MSB, data, BMP280_DATA_FRAME_SIZE) != 0) { + return -1; + } + + static int32_t T = 0; + + int32_t raw_temperature = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); + + int32_t varT1, varT2; + + varT1 = ((((raw_temperature >> 3) - ((int32_t)dev->digT1 << 1))) * ((int32_t)dev->digT2)) >> 11; + varT2 = (((((raw_temperature >> 4) - ((int32_t)dev->digT1)) * ((raw_temperature >> 4) - ((int32_t)dev->digT1))) >> 12) * ((int32_t)dev->digT3)) >> 14; + + /* Filter T ourselves */ + if (!T) { + T = (varT1 + varT2) * 5; + } else { + T = (varT1 + varT2) + (T * 4) / 5; // IIR Gain=5 + } + + *compensatedTemperature = T; + + int32_t raw_pressure = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); + + if (raw_pressure == 0x80000) { + return 1; + } + + int64_t varP1, varP2, P; + + varP1 = ((int64_t)T / 5) - 128000; + varP2 = varP1 * varP1 * (int64_t)dev->digP6; + varP2 = varP2 + ((varP1 * (int64_t)dev->digP5) << 17); + varP2 = varP2 + (((int64_t)dev->digP4) << 35); + varP1 = ((varP1 * varP1 * (int64_t)dev->digP3) >> 8) + ((varP1 * (int64_t)dev->digP2) << 12); + varP1 = (((((int64_t)1) << 47) + varP1)) * ((int64_t)dev->digP1) >> 33; + if (varP1 == 0) { + return 1; // avoid exception caused by division by zero + } + P = 1048576 - raw_pressure; + P = (((P << 31) - varP2) * 3125) / varP1; + varP1 = (((int64_t)dev->digP9) * (P >> 13) * (P >> 13)) >> 25; + varP2 = (((int64_t)dev->digP8) * P) >> 19; + *compensatedPressure = (uint32_t)((P + varP1 + varP2) >> 8) + (((int64_t)dev->digP7) << 4); + + return 0; +} + +/** + * Reads one or more bytes into a buffer + * \param[in] the command indicating the address to read + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ +static int32_t PIOS_BMP280_Read(uintptr_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = BMP280_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &address, + } + , + { + .info = __func__, + .addr = BMP280_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + for (uint8_t retry = PIOS_BMP280_I2C_RETRIES; retry > 0; --retry) { + if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { + return 0; + } + } + + return -1; +} + +static int32_t PIOS_BMP280_Write(uintptr_t i2c_id, uint8_t address, uint8_t value) +{ + uint8_t data[] = { address, value }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = BMP280_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + }; + + for (uint8_t retry = PIOS_BMP280_I2C_RETRIES; retry > 0; --retry) { + if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { + return 0; + } + } + + return -1; +} + +bool PIOS_BMP280_driver_Test(__attribute__((unused)) uintptr_t context) +{ + return true; +} + +static void PIOS_BMP280_driver_Reset(__attribute__((unused)) uintptr_t context) +{} + +static void PIOS_BMP280_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context) +{ + PIOS_Assert(size > 0); + scales[0] = 1; +} + +static void PIOS_BMP280_driver_fetch(void *data, __attribute__((unused)) uint8_t size, uintptr_t context) +{ + struct pios_bmp280_dev *dev = (struct pios_bmp280_dev *)context; + + PIOS_Assert(PIOS_BMP280_Validate(dev)); + PIOS_Assert(data); + + memcpy(data, (void *)&dev->results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp)); +} + +static bool PIOS_BMP280_driver_poll(uintptr_t context) +{ + struct pios_bmp280_dev *dev = (struct pios_bmp280_dev *)context; + + PIOS_Assert(PIOS_BMP280_Validate(dev)); + + if (!dev->sensorIsAlive) { + if (PIOS_BMP280_Configure(dev) < 0) { + return false; + } + } + + if (PIOS_DELAY_DiffuS(dev->conversionStart) < dev->conversionDelayUs) { + return false; + } + + dev->conversionStart = PIOS_DELAY_GetRaw(); + + uint32_t cP = 0; + int32_t cT = 0; + + int32_t res = PIOS_BMP280_ReadPTCompensated(dev, &cP, &cT); + + dev->sensorIsAlive = (res >= 0); + + if (res != 0) { + return false; + } + + dev->results.temperature = ((float)cT) / 256.0f / 100.0f; + dev->results.sample = ((float)cP) / 256.0f; + + return true; +} + + +#endif /* PIOS_INCLUDE_BMP280 */ + +/** + * @} + * @} + */ diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c new file mode 100644 index 0000000000..2e447e15b5 --- /dev/null +++ b/flight/pios/common/pios_board_io.c @@ -0,0 +1,738 @@ +/** + ****************************************************************************** + * + * @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 +# ifdef PIOS_INCLUDE_OPLINKRCVR +# include +# include +# endif +#endif /* PIOS_INCLUDE_RCVR */ + +#ifdef PIOS_INCLUDE_RFM22B +# include +# include +# ifdef PIOS_INCLUDE_RCVR +# include +# include +# endif /* PIOS_INCLUDE_RCVR */ +#endif /* PIOS_INCLUDE_RFM22B */ + +#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 "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_pri_radio_id; /* oplink primary com stream */ +uint32_t pios_com_aux_radio_id; /* oplink aux com stream */ +#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 */ +uint32_t pios_com_frsky_sensorhub_id; /* Frsky Sensorhub */ +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_HOTT_BRIDGE +uint32_t pios_com_hott_id; +#endif + +#ifdef PIOS_INCLUDE_USB + +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 + +#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 */ + +#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() +{ + 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) +{ + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + +#if defined(PIOS_INCLUDE_USB_CDC) + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } +#else + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } +#endif + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(pios_board_info_blob.board_rev)); + +#if defined(PIOS_INCLUDE_USB_CDC) + switch (vcp_function) { + case PIOS_BOARD_IO_USB_VCP_NONE: + break; + 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 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 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 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; + } +#endif /* PIOS_INCLUDE_USB_CDC */ + +#if defined(PIOS_INCLUDE_USB_HID) + /* Configure the usb HID port */ + + switch (hid_function) { + case PIOS_BOARD_IO_USB_HID_NONE: + break; + 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 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); + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + break; + } + +#endif /* PIOS_INCLUDE_USB_HID */ + +#if !defined(STM32F1) && !defined(STM32F3) + PIOS_USBHOOK_Activate(); +#endif +} + +#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) +{ + 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); +} +static int32_t PIOS_SBus_Normal_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id) +{ + struct pios_sbus_cfg sbus_cfg = { + .non_inverted = false, + }; + + return PIOS_SBus_Init(id, &sbus_cfg, driver, lower_id); +} +static int32_t PIOS_SBus_Not_Inverted_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id) +{ + struct pios_sbus_cfg sbus_cfg = { + .non_inverted = true, + }; + + return PIOS_SBus_Init(id, &sbus_cfg, driver, lower_id); +} +# endif /* ifdef PIOS_INCLUDE_SBUS */ +#endif /* ifdef PIOS_INCLUDE_RCVR */ + +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[] = { + [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, + }, +#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, + .com_tx_buf_len = PIOS_COM_HKOSD_TX_BUF_LEN, + }, +#ifdef PIOS_INCLUDE_FRSKY_SENSORHUB + [PIOS_BOARD_IO_UART_FRSKY_SENSORHUB] = { + .com_id = &pios_com_frsky_sensorhub_id, + .com_tx_buf_len = PIOS_COM_FRSKY_SENSORHUB_TX_BUF_LEN, + }, +#endif +#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, + .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] = { + .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, + }, + [PIOS_BOARD_IO_UART_SBUS_NORMAL] = { + .rcvr_init = &PIOS_SBus_Normal_Init_Helper, + .rcvr_driver = &pios_sbus_rcvr_driver, + .rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS, + }, + [PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED] = { + .rcvr_init = &PIOS_SBus_Not_Inverted_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_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)) { + return; + } + + if (uart_function_map[function].com_id) { + 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) { + 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); + } + + /* lock uart config */ + if (pios_usart_com_driver.ioctl) { + bool lock = true; + pios_usart_com_driver.ioctl(usart_id, PIOS_IOCTL_USART_LOCK_CONFIG, &lock); + } + + pios_rcvr_group_map[uart_function_map[function].rcvr_group] = rcvr_id; + } +#endif /* PIOS_INCLUDE_RCVR */ +} + +#ifdef PIOS_INCLUDE_PWM +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; + + 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_RCVR(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 */ + +#if defined(PIOS_INCLUDE_GCSRCVR) +void PIOS_BOARD_IO_Configure_GCS_RCVR() +{ + 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) && defined(PIOS_INCLUDE_RCVR) +void PIOS_BOARD_IO_Configure_OPLink_RCVR() +{ + uint32_t pios_oplinkrcvr_id; + + OPLinkReceiverInitialize(); +#if defined(PIOS_INCLUDE_RFM22B) + PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id); +#else /* PIOS_INCLUDE_RFM22B */ + PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id); +#endif /* PIOS_INCLUDE_RFM22B */ + 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 */ + +#ifdef PIOS_INCLUDE_RFM22B + +void PIOS_BOARD_IO_Configure_RFM22B() +{ +#if defined(PIOS_INCLUDE_RFM22B) + 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 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) + const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(pios_board_info_blob.board_rev); + + PIOS_OpenLRS_Init(&pios_openlrs_id, PIOS_SPI_RFM22B_ADAPTER, 0, openlrs_cfg); + + uint32_t openlrsrcvr_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)) { + 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_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); + } + + /* Configure the radio com interface */ + 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); + } + + 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; + } + + /* Initialize the OPLink Receiver */ +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + + /* Reinitialize the modem. */ + PIOS_RFM22B_Reinit(pios_rfm22b_id); + } + } else { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + } + + 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_board_sensors.c b/flight/pios/common/pios_board_sensors.c new file mode 100644 index 0000000000..1357c17989 --- /dev/null +++ b/flight/pios/common/pios_board_sensors.c @@ -0,0 +1,227 @@ +/** + ****************************************************************************** + * + * @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 + +# include + +#ifdef PIOS_INCLUDE_BMP280 +# include +#endif + +#ifdef PIOS_INCLUDE_ADXL345 +# include +#endif + +#ifdef PIOS_INCLUDE_MPU9250 +# include +# include +#endif + +#ifdef PIOS_INCLUDE_HMC5X83 +# include "pios_hmc5x83.h" +# 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 + +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) { +#ifdef PIOS_SPI_MPU6000_ADAPTER + PIOS_MPU6000_Init(PIOS_SPI_MPU6000_ADAPTER, 0, mpu6000_cfg); +#elif defined(PIOS_I2C_MPU6000_ADAPTER) + PIOS_MPU6000_Init(PIOS_I2C_MPU6000_ADAPTER, 0, mpu6000_cfg); +#else +#error PIOS_INCLUDE_MPU6000 requires one of PIOS_SPI_MPU6000_ADAPTER or PIOS_I2C_MPU6000_ADAPTER +#endif + PIOS_MPU6000_CONFIG_Configure(); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + PIOS_MPU6000_Register(); +#endif + } +#endif /* PIOS_INCLUDE_MPU6000 */ + +#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 */ + +#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 + 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 + + 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 +#ifdef PIOS_I2C_FLEXI_ADAPTER + i2c_id = PIOS_I2C_FLEXI_ADAPTER; +#endif + } else if (option == AUXMAGSETTINGS_TYPE_I2C) { + // i2c_internal (or Sparky2/F3 dedicated I2C port) +#ifdef PIOS_I2C_EXTERNAL_ADAPTER + i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; +#endif + } + + 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 +#ifdef PIOS_INCLUDE_MS56XX + const struct pios_ms56xx_cfg *ms56xx_cfg = PIOS_BOARD_HW_DEFS_GetMS56xxCfg(pios_board_info_blob.board_rev); + if (ms56xx_cfg) { + PIOS_MS56xx_Init(ms56xx_cfg, PIOS_I2C_MS56XX_INTERNAL_ADAPTER); + PIOS_MS56xx_Register(); + } +#endif /* PIOS_INCLUDE_MS56XX */ + +#ifdef PIOS_INCLUDE_BMP280 + const struct pios_bmp280_cfg *bmp280_cfg = PIOS_BOARD_HW_DEFS_GetBMP280Cfg(pios_board_info_blob.board_rev); + if (bmp280_cfg) { + PIOS_BMP280_Init(bmp280_cfg, PIOS_I2C_BMP280_INTERNAL_ADAPTER); + } +#endif + +#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) { + 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 /* PIOS_INCLUDE_ADC */ + + // external ETASV3 Eagletree Airspeed v3 + // external MS4525D PixHawk Airpeed based on MS4525DO + // bmp085/bmp180 baro + // hmc5843 mag + // i2c esc (?) + // UBX DCC +} diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index b0a394b6cd..6bef104547 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -7,7 +7,8 @@ * @{ * * @file pios_com.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief COM layer functions * @see The GNU Public License (GPL) Version 3 * @@ -71,7 +72,7 @@ static struct pios_com_dev *PIOS_COM_alloc(void) { struct pios_com_dev *com_dev; - com_dev = (struct pios_com_dev *)pios_malloc(sizeof(struct pios_com_dev)); + com_dev = (struct pios_com_dev *)pios_fastheapmalloc(sizeof(struct pios_com_dev)); if (!com_dev) { return NULL; } @@ -117,9 +118,23 @@ 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); - PIOS_Assert(has_rx || has_tx); + if ((rx_buffer_len > 0) && !rx_buffer) { +#if defined(PIOS_INCLUDE_FREERTOS) + rx_buffer = (uint8_t *)pios_fastheapmalloc(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_fastheapmalloc(tx_buffer_len); +#endif + 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); @@ -153,12 +168,10 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); #if defined(PIOS_INCLUDE_FREERTOS) vSemaphoreCreateBinary(com_dev->tx_sem); + com_dev->sendbuffer_sem = xSemaphoreCreateMutex(); #endif /* PIOS_INCLUDE_FREERTOS */ (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev); } -#if defined(PIOS_INCLUDE_FREERTOS) - com_dev->sendbuffer_sem = xSemaphoreCreateMutex(); -#endif /* PIOS_INCLUDE_FREERTOS */ *com_id = (uint32_t)com_dev; return 0; @@ -283,6 +296,23 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) return 0; } +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; + + 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_config) { + com_dev->driver->set_config(com_dev->lower_id, word_len, parity, stop_bits, baud_rate); + } + + return 0; +} + /** * Set control lines associated with the port * \param[in] port COM port @@ -358,6 +388,74 @@ int32_t PIOS_COM_RegisterBaudRateCallback(uint32_t com_id, pios_com_callback_bau return 0; } +int32_t PIOS_COM_ASYNC_TxStart(uint32_t com_id, uint16_t tx_bytes_avail) +{ + 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->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, tx_bytes_avail); + } + + return 0; +} + +int32_t PIOS_COM_ASYNC_RxStart(uint32_t com_id, uint16_t rx_bytes_avail) +{ + 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->rx_start) { + com_dev->driver->rx_start(com_dev->lower_id, rx_bytes_avail); + } + + return 0; +} + +int32_t PIOS_COM_ASYNC_RegisterRxCallback(uint32_t com_id, pios_com_callback rx_in_cb, uint32_t context) +{ + 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->bind_rx_cb) { + com_dev->driver->bind_rx_cb(com_dev->lower_id, rx_in_cb, context); + } + + return 0; +} + +int32_t PIOS_COM_ASYNC_RegisterTxCallback(uint32_t com_id, pios_com_callback tx_out_cb, uint32_t context) +{ + 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->bind_tx_cb) { + com_dev->driver->bind_tx_cb(com_dev->lower_id, tx_out_cb, context); + } + + return 0; +} + static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len) { PIOS_Assert(com_dev); @@ -411,6 +509,7 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u /* Undefined COM port for this board (see pios_board.c) */ return -1; } + PIOS_Assert(com_dev->has_tx); #if defined(PIOS_INCLUDE_FREERTOS) if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) { return -3; @@ -668,12 +767,98 @@ uint32_t PIOS_COM_Available(uint32_t com_id) return COM_AVAILABLE_TX; } - return COM_AVAILABLE_NONE; /* can this really happen? */ + return COM_AVAILABLE_RXTX; /* This is the case for com_dev without buffers - for async use only */ } return (com_dev->driver->available)(com_dev->lower_id); } +/* + * Set available callback + * \param[in] port COM port + * \param[in] available_cb Callback function + * \param[in] context context to pass to the callback function + * \return -1 if port not available + * \return 0 on success + */ +int32_t PIOS_COM_RegisterAvailableCallback(uint32_t com_id, pios_com_callback_available available_cb, uint32_t context) +{ + 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->bind_available_cb) { + com_dev->driver->bind_available_cb(com_dev->lower_id, available_cb, context); + } + + return 0; +} + +/* + * Callback used to pass data from one ocm port to another in PIOS_COM_LinkComPare. + * \param[in] context The "to" com port + * \param[in] buf The data buffer + * \param[in] buf_len The number of bytes received + * \param[in] headroom Not used + * \param[in] task_woken Not used + */ +static uint16_t PIOS_COM_LinkComPairRxCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, __attribute__((unused)) uint16_t *headroom, __attribute__((unused)) bool *task_woken) +{ + int32_t sent = PIOS_COM_SendBufferNonBlocking(context, buf, buf_len); + + if (sent > 0) { + return sent; + } + return 0; +} + +/* + * Link a pair of com ports so that any data arriving on one is sent out the other. + * \param[in] com1_id The first com port + * \param[in] com2_id The second com port + */ +void PIOS_COM_LinkComPair(uint32_t com1_id, uint32_t com2_id, bool link_ctrl_line, bool link_baud_rate) +{ + PIOS_COM_ASYNC_RegisterRxCallback(com1_id, PIOS_COM_LinkComPairRxCallback, com2_id); + PIOS_COM_ASYNC_RegisterRxCallback(com2_id, PIOS_COM_LinkComPairRxCallback, com1_id); + // Optionally link the control like and baudrate changes between the two. + if (link_ctrl_line) { + PIOS_COM_RegisterCtrlLineCallback(com1_id, (pios_com_callback_ctrl_line)PIOS_COM_SetCtrlLine, com2_id); + PIOS_COM_RegisterCtrlLineCallback(com2_id, (pios_com_callback_ctrl_line)PIOS_COM_SetCtrlLine, com1_id); + } + if (link_baud_rate) { + PIOS_COM_RegisterBaudRateCallback(com1_id, (pios_com_callback_baud_rate)PIOS_COM_ChangeBaud, com2_id); + PIOS_COM_RegisterBaudRateCallback(com2_id, (pios_com_callback_baud_rate)PIOS_COM_ChangeBaud, com1_id); + } +} + +/* + * 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/common/pios_dsm.c b/flight/pios/common/pios_dsm.c new file mode 100644 index 0000000000..a935e22c39 --- /dev/null +++ b/flight/pios/common/pios_dsm.c @@ -0,0 +1,460 @@ +/** + ****************************************************************************** + * @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) 2012. + * @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 + +#ifndef PIOS_INCLUDE_RTC +#error PIOS_INCLUDE_RTC must be used to use DSM +#endif + +/* 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; + 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 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); + + /* RX line, set high */ + 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(rxpin->gpio, rxpin->init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + /* RX line, drive high for 120us */ + GPIO_SetBits(rxpin->gpio, rxpin->init.GPIO_Pin); + PIOS_DELAY_WaituS(120); + } + + /* RX line, set input and wait for data */ + GPIO_Init(rxpin->gpio, (GPIO_InitTypeDef *)&rxpin->init); +} + +/* 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_com_driver *driver, + uint32_t lower_id, + uint8_t bind) +{ + PIOS_DEBUG_Assert(dsm_id); + 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 receiver if requested */ + if (bind) { + struct stm32_gpio rxpin; + + PIOS_DEBUG_Assert(driver->ioctl); + + if ((driver->ioctl)(lower_id, PIOS_IOCTL_USART_GET_DSMBIND, &rxpin) == 0) { + 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_Parity_No, PIOS_COM_StopBits_1, 115200); + + /* 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); + + 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/common/pios_exbus.c b/flight/pios/common/pios_exbus.c index a65333ea00..edb4162335 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_Parity_No, PIOS_COM_StopBits_1, 125000); + + /* 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_flash_jedec.c b/flight/pios/common/pios_flash_jedec.c index 1ebde0836f..abefbc1380 100644 --- a/flight/pios/common/pios_flash_jedec.c +++ b/flight/pios/common/pios_flash_jedec.c @@ -31,7 +31,7 @@ #include "pios.h" -#ifdef PIOS_INCLUDE_FLASH +#if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_SPI) #include "pios_flash_jedec_priv.h" #include "pios_flash_jedec_catalog.h" @@ -632,4 +632,4 @@ const struct pios_flash_driver pios_jedec_flash_driver = { .read_data = PIOS_Flash_Jedec_ReadData, }; -#endif /* PIOS_INCLUDE_FLASH */ +#endif /* PIOS_INCLUDE_FLASH && PIOS_INCLUDE_SPI */ diff --git a/flight/pios/common/pios_hott.c b/flight/pios/common/pios_hott.c index a3106edc4d..c823cc8fe8 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_Parity_No, PIOS_COM_StopBits_1, 115200); + + /* 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 4ba74fcf26..8dd4595e6c 100644 --- a/flight/pios/common/pios_ibus.c +++ b/flight/pios/common/pios_ibus.c @@ -1,7 +1,7 @@ /** ****************************************************************************** * @file pios_ibus.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017. * dRonin, http://dRonin.org/, Copyright (C) 2016 * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ @@ -33,11 +33,39 @@ #ifdef PIOS_INCLUDE_IBUS -// 1 sync byte, 1 unknown byte, 10x channels (uint16_t), 8 unknown bytes, 2 crc bytes -#define PIOS_IBUS_BUFLEN (1 + 1 + PIOS_IBUS_NUM_INPUTS * 2 + 8 + 2) -#define PIOS_IBUS_SYNCBYTE 0x20 +/* + iBus packet format learned from: https://bitbucket.org/daveborthwick/ibus_pic12f1572 + + An iBus packet consists of: + - the total packet length (1 byte) + - command / type field (1 byte) + - sequence of 16-bit (little endian) variables (2 bytes each) + - checksum (2 bytes) + + The most common packet length is 0x20 = 32 bytes + A packet of this type has enough room to transport data for 14 channels. + + The command / type field tells you the meaning of the next variables. + This field is set to 0x40 for RC command channels. + We must verify this field to be 0x40 so we can be sure that the next fields + can be interpreted as RC command channels. + + The checksum equals 0xFFFF minus the sum of all previous bytes in the packet. + + */ + +// 1 length byte, 1 type byte, 10x channels (uint16_t), 4 extra channels (unint16_t), 2 crc bytes +// We only decode the first 10 channels at this time. +#define PIOS_IBUS_BUFLEN (1 + 1 + PIOS_IBUS_NUM_INPUTS * 2 + (14 - PIOS_IBUS_NUM_INPUTS) * 2 + 2) +#define PIOS_IBUS_SYNCBYTE 0x20 // sync on the packet length byte +#define PIOS_IBUS_TYPEBYTE 0x40 #define PIOS_IBUS_MAGIC 0x84fd9a39 +// make sure to always allocate sufficient bufferspace: +#if (PIOS_IBUS_SYNCBYTE != PIOS_IBUS_BUFLEN) +#error PIOS_IBUS_BUFLEN must be updated to match the expected packet length +#endif + /** * @brief IBus receiver driver internal state data */ @@ -146,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_Parity_No, PIOS_COM_StopBits_1, 115200); + + /* 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; } @@ -182,9 +220,16 @@ static uint16_t PIOS_IBUS_Receive(uint32_t context, uint8_t *buf, uint16_t buf_l } for (int i = 0; i < buf_len; i++) { + // byte 0: sync byte (packet length) should match: if (ibus_dev->buf_pos == 0 && buf[i] != PIOS_IBUS_SYNCBYTE) { continue; } + // byte 1: type field should match: + if (ibus_dev->buf_pos == 1 && buf[i] != PIOS_IBUS_TYPEBYTE) { + // not the correct type of data, restart from byte 0 + ibus_dev->buf_pos = 0; + continue; + } ibus_dev->rx_buf[ibus_dev->buf_pos++] = buf[i]; if (ibus_dev->buf_pos <= PIOS_IBUS_BUFLEN - 2) { @@ -238,10 +283,12 @@ static void PIOS_IBUS_Supervisor(uint32_t context) PIOS_Assert(PIOS_IBUS_Validate(ibus_dev)); + // clear rx buffer after 4.8 ms without input if (++ibus_dev->rx_timer > 3) { PIOS_IBUS_ResetBuffer(ibus_dev); } + // mark all channels invalid after 51.2 ms without valid data if (++ibus_dev->failsafe_timer > 32) { PIOS_IBUS_SetAllChannels(ibus_dev, PIOS_RCVR_TIMEOUT); } diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index 97daddd68c..ace15cac50 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -35,6 +35,7 @@ #include #include #include +#include /* Global Variables */ @@ -42,6 +43,9 @@ enum pios_mpu6000_dev_magic { PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, }; +#define CALLBACK_TASK_STACKSIZE 256 + + // sensor driver interface bool PIOS_MPU6000_driver_Test(uintptr_t context); void PIOS_MPU6000_driver_Reset(uintptr_t context); @@ -58,13 +62,18 @@ const PIOS_SENSORS_Driver PIOS_MPU6000_Driver = { .is_polled = false, }; // - +struct pios_mpu6000_io_driver { + int32_t (*SetReg)(uint8_t address, uint8_t buffer); + int32_t (*GetReg)(uint8_t address); + bool (*ReadSensor)(bool *woken); +}; struct mpu6000_dev { - uint32_t spi_id; + uint32_t port_id; uint32_t slave_num; QueueHandle_t queue; const struct pios_mpu6000_cfg *cfg; + struct pios_mpu6000_io_driver *driver; enum pios_mpu6000_range gyro_range; enum pios_mpu6000_accel_range accel_range; enum pios_mpu6000_filter filter; @@ -101,6 +110,7 @@ typedef union { static struct mpu6000_dev *dev; volatile bool mpu6000_configured = false; static mpu6000_data_t mpu6000_data; +static uint32_t gyro_read_timestamp; static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0; #define SENSOR_COUNT 2 #define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT) @@ -109,12 +119,33 @@ static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0; static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg); static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev); static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg); -static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); -static int32_t PIOS_MPU6000_GetReg(uint8_t address); + +#ifdef PIOS_INCLUDE_SPI +static int32_t PIOS_MPU6000_SPI_SetReg(uint8_t address, uint8_t buffer); +static int32_t PIOS_MPU6000_SPI_GetReg(uint8_t address); +static bool PIOS_MPU6000_SPI_ReadSensor(bool *woken); static void PIOS_MPU6000_SetSpeed(const bool fast); -static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp); -static bool PIOS_MPU6000_ReadSensor(bool *woken); +struct pios_mpu6000_io_driver spi_io_driver = { + .SetReg = PIOS_MPU6000_SPI_SetReg, + .GetReg = PIOS_MPU6000_SPI_GetReg, + .ReadSensor = PIOS_MPU6000_SPI_ReadSensor, +}; + +#endif /* PIOS_INCLUDE_SPI */ + + +#ifdef PIOS_INCLUDE_I2C +static int32_t PIOS_MPU6000_I2C_SetReg(uint8_t address, uint8_t buffer); +static int32_t PIOS_MPU6000_I2C_GetReg(uint8_t address); +static bool PIOS_MPU6000_I2C_ReadSensor(bool *woken); +struct pios_mpu6000_io_driver i2c_io_driver = { + .SetReg = PIOS_MPU6000_I2C_SetReg, + .GetReg = PIOS_MPU6000_I2C_GetReg, + .ReadSensor = PIOS_MPU6000_I2C_ReadSensor, +}; +#endif /* PIOS_INCLUDE_I2C */ +static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp); static int32_t PIOS_MPU6000_Test(void); void PIOS_MPU6000_Register() @@ -133,6 +164,20 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; + if (cfg->i2c_addr == 0) { +#ifdef PIOS_INCLUDE_SPI + mpu6000_dev->driver = &spi_io_driver; +#else + PIOS_Assert(0) +#endif + } else { +#ifdef PIOS_INCLUDE_I2C + mpu6000_dev->driver = &i2c_io_driver; +#else + PIOS_Assert(0); +#endif + } + mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE); PIOS_Assert(mpu6000_dev->queue); @@ -154,7 +199,7 @@ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev) if (vdev->magic != PIOS_MPU6000_DEV_MAGIC) { return -2; } - if (vdev->spi_id == 0) { + if (vdev->port_id == 0) { return -3; } return 0; @@ -164,14 +209,14 @@ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev) * @brief Initialize the MPU6000 3-axis gyro sensor. * @return 0 for success, -1 for failure */ -int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg) +int32_t PIOS_MPU6000_Init(uint32_t port_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg) { dev = PIOS_MPU6000_alloc(cfg); if (dev == NULL) { return -1; } - dev->spi_id = spi_id; + dev->port_id = port_id; dev->slave_num = slave_num; dev->cfg = cfg; @@ -194,13 +239,13 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg) PIOS_MPU6000_Test(); // Reset chip - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0) { ; } PIOS_DELAY_WaitmS(50); // Reset chip and fifo - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, + while (dev->driver->SetReg(PIOS_MPU6000_USER_CTRL_REG, PIOS_MPU6000_USERCTL_GYRO_RST | PIOS_MPU6000_USERCTL_SIG_COND | PIOS_MPU6000_USERCTL_FIFO_RST) != 0) { @@ -208,7 +253,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg) } // Wait for reset to finish - while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & + while (dev->driver->GetReg(PIOS_MPU6000_USER_CTRL_REG) & (PIOS_MPU6000_USERCTL_GYRO_RST | PIOS_MPU6000_USERCTL_SIG_COND | PIOS_MPU6000_USERCTL_FIFO_RST)) { @@ -216,45 +261,45 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg) } PIOS_DELAY_WaitmS(10); // Power management configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { ; } // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { ; } // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { ; } // FIFO storage - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) { ; } PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter); // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) { ; } // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { ; } // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { ; } // Interrupt configuration - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { ; } - if ((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) { + if ((dev->driver->GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) { return; } @@ -274,12 +319,12 @@ int32_t PIOS_MPU6000_ConfigureRanges( } // update filter settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) { ; } // Sample rate divider, chosen upon digital filtering settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, + while (dev->driver->SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ? dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0) { ; @@ -288,13 +333,13 @@ int32_t PIOS_MPU6000_ConfigureRanges( dev->filter = filterSetting; // Gyro range - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0) { ; } dev->gyro_range = gyroRange; // Set the accel range - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) { + while (dev->driver->SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) { ; } @@ -302,6 +347,7 @@ int32_t PIOS_MPU6000_ConfigureRanges( return 0; } +#ifdef PIOS_INCLUDE_SPI /** * @brief Claim the SPI bus for the accel communications and select this chip * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus @@ -311,11 +357,11 @@ static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi) if (PIOS_MPU6000_Validate(dev) != 0) { return -1; } - if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { + if (PIOS_SPI_ClaimBus(dev->port_id) != 0) { return -2; } PIOS_MPU6000_SetSpeed(fast_spi); - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0); return 0; } @@ -323,9 +369,9 @@ static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi) static void PIOS_MPU6000_SetSpeed(const bool fast) { if (fast) { - PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->fast_prescaler); + PIOS_SPI_SetClockSpeed(dev->port_id, dev->cfg->fast_prescaler); } else { - PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->std_prescaler); + PIOS_SPI_SetClockSpeed(dev->port_id, dev->cfg->std_prescaler); } } @@ -340,11 +386,11 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken, bool fast_spi) if (PIOS_MPU6000_Validate(dev) != 0) { return -1; } - if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { + if (PIOS_SPI_ClaimBusISR(dev->port_id, woken) != 0) { return -2; } PIOS_MPU6000_SetSpeed(fast_spi); - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0); return 0; } @@ -357,8 +403,8 @@ static int32_t PIOS_MPU6000_ReleaseBus() if (PIOS_MPU6000_Validate(dev) != 0) { return -1; } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBus(dev->spi_id); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBus(dev->port_id); } /** @@ -372,8 +418,8 @@ static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) if (PIOS_MPU6000_Validate(dev) != 0) { return -1; } - PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1); - return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1); + return PIOS_SPI_ReleaseBusISR(dev->port_id, woken); } /** @@ -381,7 +427,7 @@ static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken) * @returns The register value or -1 if failure to get bus * @param reg[in] Register address to be read */ -static int32_t PIOS_MPU6000_GetReg(uint8_t reg) +static int32_t PIOS_MPU6000_SPI_GetReg(uint8_t reg) { uint8_t data; @@ -389,8 +435,8 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) return -1; } - PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte - data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response + PIOS_SPI_TransferByte(dev->port_id, (0x80 | reg)); // request byte + data = PIOS_SPI_TransferByte(dev->port_id, 0); // receive response PIOS_MPU6000_ReleaseBus(); return data; @@ -404,18 +450,18 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg) * \return -1 if unable to claim SPI bus * \return -2 if unable to claim i2c device */ -static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) +static int32_t PIOS_MPU6000_SPI_SetReg(uint8_t reg, uint8_t data) { if (PIOS_MPU6000_ClaimBus(false) != 0) { return -1; } - if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) { + if (PIOS_SPI_TransferByte(dev->port_id, 0x7f & reg) != 0) { PIOS_MPU6000_ReleaseBus(); return -2; } - if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) { + if (PIOS_SPI_TransferByte(dev->port_id, data) != 0) { PIOS_MPU6000_ReleaseBus(); return -3; } @@ -439,7 +485,7 @@ int32_t PIOS_MPU6000_DummyReadGyros() return -1; } - if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { + if (PIOS_SPI_TransferBlock(dev->port_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { return -2; } @@ -448,13 +494,127 @@ int32_t PIOS_MPU6000_DummyReadGyros() return 0; } +static bool PIOS_MPU6000_SPI_ReadSensor(bool *woken) +{ + const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_SENSOR_FIRST_REG | 0x80 }; + + if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) { + return false; + } + if (PIOS_SPI_TransferBlock(dev->port_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) { + PIOS_MPU6000_ReleaseBusISR(woken); + return false; + } + PIOS_MPU6000_ReleaseBusISR(woken); + return true; +} +#endif /* PIOS_INCLUDE_SPI */ + +#ifdef PIOS_INCLUDE_I2C +static int32_t PIOS_MPU6000_I2C_SetReg(uint8_t address, uint8_t buffer) +{ + uint8_t data[] = { + address, + buffer, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = dev->cfg->i2c_addr, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; + + return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list)); +} + +static int32_t PIOS_MPU6000_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) +{ + uint8_t addr_buffer[] = { + address, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = dev->cfg->i2c_addr, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = dev->cfg->i2c_addr, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list)); +} + +static int32_t PIOS_MPU6000_I2C_GetReg(uint8_t address) +{ + uint8_t data; + + if (PIOS_MPU6000_I2C_Read(address, &data, sizeof(data)) < 0) { + return -1; + } + + return data; +} + +static bool PIOS_MPU6000_I2C_Read_Completed(enum pios_i2c_transfer_result result) +{ + return (result == PIOS_I2C_TRANSFER_OK) ? PIOS_MPU6000_HandleData(gyro_read_timestamp) : false; +} + +static bool PIOS_MPU6000_I2C_ReadSensor(bool *woken) +{ + static uint8_t addr_buffer[] = { + PIOS_MPU6000_SENSOR_FIRST_REG, + }; + + static struct pios_i2c_txn txn_list[] = { + { + .info = __func__, +// .addr = dev->cfg->i2c_addr, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, +// .addr = dev->cfg->i2c_addr, + .rw = PIOS_I2C_TXN_READ, + .len = sizeof(mpu6000_data_t) - 1, + .buf = &mpu6000_data.buffer[1], + } + }; + + txn_list[0].addr = dev->cfg->i2c_addr; + txn_list[1].addr = dev->cfg->i2c_addr; + + PIOS_I2C_Transfer_CallbackFromISR(dev->port_id, txn_list, NELEMENTS(txn_list), PIOS_MPU6000_I2C_Read_Completed, woken); + + return false; /* we did not read anything now */ +} +#endif /* PIOS_INCLUDE_I2C */ + /* * @brief Read the identification bytes from the MPU6000 sensor * \return ID read from MPU6000 or -1 if failure */ int32_t PIOS_MPU6000_ReadID() { - int32_t mpu6000_id = PIOS_MPU6000_GetReg(PIOS_MPU6000_WHOAMI); + int32_t mpu6000_id = dev->driver->GetReg(PIOS_MPU6000_WHOAMI); if (mpu6000_id < 0) { return -1; @@ -541,21 +701,21 @@ static int32_t PIOS_MPU6000_Test(void) bool PIOS_MPU6000_IRQHandler(void) { - uint32_t gyro_read_timestamp = PIOS_DELAY_GetRaw(); + gyro_read_timestamp = PIOS_DELAY_GetRaw(); bool woken = false; if (!mpu6000_configured) { return false; } - if (PIOS_MPU6000_ReadSensor(&woken)) { + if (dev->driver->ReadSensor(&woken)) { woken |= PIOS_MPU6000_HandleData(gyro_read_timestamp); } return woken; } -static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp) +static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp_p) { if (!queue_data) { return false; @@ -564,8 +724,6 @@ static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp) // Rotate the sensor to OP convention. The datasheet defines X as towards the right // and Y as forward. OP convention transposes this. Also the Z is defined negatively // to our convention - - // Currently we only support rotations on top so switch X/Y accordingly switch (dev->cfg->orientation) { case PIOS_MPU6000_TOP_0DEG: queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X @@ -592,34 +750,49 @@ static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp) queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X break; + case PIOS_MPU6000_BOTTOM_0DEG: + queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X + queue_data->sample[0].x = -1 - GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y + queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X + queue_data->sample[1].x = -1 - GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y + break; + case PIOS_MPU6000_BOTTOM_90DEG: + queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y + queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X + queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y + queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X + break; + case PIOS_MPU6000_BOTTOM_180DEG: + queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X + queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y + queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X + queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y + break; + case PIOS_MPU6000_BOTTOM_270DEG: + queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y + queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X + queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y + queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X + break; + } + if ((dev->cfg->orientation & PIOS_MPU6000_LOCATION_MASK) == PIOS_MPU6000_LOCATION_TOP) { + queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z)); + queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z)); + } else { + queue_data->sample[0].z = GET_SENSOR_DATA(mpu6000_data, Accel_Z); + queue_data->sample[1].z = GET_SENSOR_DATA(mpu6000_data, Gyro_Z); } - queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z)); - queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z)); const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature); // Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 queue_data->temperature = 3653 + (temp * 100) / 340; - queue_data->timestamp = gyro_read_timestamp; + queue_data->timestamp = gyro_read_timestamp_p; + BaseType_t higherPriorityTaskWoken; xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken); return higherPriorityTaskWoken == pdTRUE; } -static bool PIOS_MPU6000_ReadSensor(bool *woken) -{ - const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_SENSOR_FIRST_REG | 0x80 }; - - if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) { - return false; - } - if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) { - PIOS_MPU6000_ReleaseBusISR(woken); - return false; - } - PIOS_MPU6000_ReleaseBusISR(woken); - return true; -} - // Sensor driver implementation bool PIOS_MPU6000_driver_Test(__attribute__((unused)) uintptr_t context) { @@ -628,7 +801,7 @@ bool PIOS_MPU6000_driver_Test(__attribute__((unused)) uintptr_t context) void PIOS_MPU6000_driver_Reset(__attribute__((unused)) uintptr_t context) { - PIOS_MPU6000_DummyReadGyros(); +// PIOS_MPU6000_DummyReadGyros(); } void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet) diff --git a/flight/pios/common/pios_mpu9250.c b/flight/pios/common/pios_mpu9250.c index cdf11c259c..b9da3f30d0 100644 --- a/flight/pios/common/pios_mpu9250.c +++ b/flight/pios/common/pios_mpu9250.c @@ -117,7 +117,7 @@ typedef union { } data; } __attribute__((__packed__)) mpu9250_data_t; -#define GET_SENSOR_DATA(mpudataptr, sensor) (mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l) +#define GET_SENSOR_DATA(mpudataptr, sensor) ((int16_t)((mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l))) static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0; static PIOS_SENSORS_3Axis_SensorsWithTemp *mag_data = 0; @@ -129,6 +129,7 @@ static volatile bool mag_ready = false; static struct mpu9250_dev *dev; volatile bool mpu9250_configured = false; static mpu9250_data_t mpu9250_data; +static int32_t mpu9250_id; // ! Private functions static struct mpu9250_dev *PIOS_MPU9250_alloc(const struct pios_mpu9250_cfg *cfg); @@ -186,7 +187,9 @@ void PIOS_MPU9250_MainRegister() void PIOS_MPU9250_MagRegister() { - PIOS_SENSORS_Register(&PIOS_MPU9250_Mag_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, 0); + if (mpu9250_id == PIOS_MPU9250_GYRO_ACC_ID) { + PIOS_SENSORS_Register(&PIOS_MPU9250_Mag_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, 0); + } } /** * @brief Allocate a new device @@ -320,7 +323,9 @@ static void PIOS_MPU9250_Config(struct pios_mpu9250_cfg const *cfg) } #ifdef PIOS_MPU9250_MAG - PIOS_MPU9250_Mag_Init(); + if (mpu9250_id == PIOS_MPU9250_GYRO_ACC_ID) { + PIOS_MPU9250_Mag_Init(); + } #endif // Interrupt enable @@ -514,12 +519,12 @@ static int32_t PIOS_MPU9250_SetReg(uint8_t reg, uint8_t data) */ int32_t PIOS_MPU9250_ReadID() { - int32_t mpu9250_id = PIOS_MPU9250_GetReg(PIOS_MPU9250_WHOAMI); + int32_t id = PIOS_MPU9250_GetReg(PIOS_MPU9250_WHOAMI); - if (mpu9250_id < 0) { + if (id < 0) { return -1; } - return mpu9250_id; + return id; } static float PIOS_MPU9250_GetScale() @@ -566,13 +571,13 @@ static float PIOS_MPU9250_GetAccelScale() static int32_t PIOS_MPU9250_Test(void) { /* Verify that ID matches */ - int32_t mpu9250_id = PIOS_MPU9250_ReadID(); + mpu9250_id = PIOS_MPU9250_ReadID(); if (mpu9250_id < 0) { return -1; } - if (mpu9250_id != PIOS_MPU9250_GYRO_ACC_ID) { + if ((mpu9250_id != PIOS_MPU9250_GYRO_ACC_ID) && (mpu9250_id != PIOS_MPU6500_GYRO_ACC_ID)) { return -2; } @@ -651,8 +656,6 @@ static int32_t PIOS_MPU9250_Mag_SetReg(uint8_t reg, uint8_t data) */ static int32_t PIOS_MPU9250_Mag_Sensitivity(void) { - int i; - /* Put mag in power down state before changing mode */ PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_POWER_DOWN_MODE); PIOS_DELAY_WaitmS(1); @@ -661,33 +664,32 @@ static int32_t PIOS_MPU9250_Mag_Sensitivity(void) PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_FUSE_ROM_MODE); PIOS_DELAY_WaitmS(1); - if (PIOS_MPU9250_ClaimBus(false) != 0) { - return -1; - } - /* Set addres and read flag */ - PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_ADDR); - PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_MAG_I2C_ADDR | PIOS_MPU9250_MAG_I2C_READ_FLAG); + PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_ADDR, PIOS_MPU9250_MAG_I2C_ADDR | PIOS_MPU9250_MAG_I2C_READ_FLAG); /* Set the address of the register to read. */ - PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_REG); - PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_ASAX); + PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_REG, PIOS_MPU9250_ASAX); /* Trigger the byte transfer. */ - PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_CTRL); - PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV_ENABLE | 0x3); + PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_CTRL, PIOS_MPU9250_I2C_SLV_ENABLE | 0x3); PIOS_DELAY_WaitmS(1); + if (PIOS_MPU9250_ClaimBus(false) != 0) { + return -1; + } + /* Read the mag data from SPI block */ - for (i = 0; i < 0x3; i++) { - PIOS_SPI_TransferByte(dev->spi_id, (PIOS_MPU9250_EXT_SENS_DATA_00 | 0x80) + i); - int32_t ret = PIOS_SPI_TransferByte(dev->spi_id, 0x0); - if (ret < 0) { - PIOS_MPU9250_ReleaseBus(); - return -1; + + uint8_t mpu9250_send_buf[4] = { PIOS_MPU9250_EXT_SENS_DATA_00 | 0x80 }; + + if (PIOS_SPI_TransferBlock(dev->spi_id, mpu9250_send_buf, mpu9250_send_buf, sizeof(mpu9250_send_buf), 0) == 0) { + for (int i = 0; i < 3; ++i) { + dev->mag_sens_adj[i] = 1.0f + ((float)((uint8_t)mpu9250_send_buf[i + 1] - 128)) / 256.0f; } - dev->mag_sens_adj[i] = 1.0f; // 1.0f + ((float)((uint8_t)ret - 128)) / 256.0f; + } else { + PIOS_MPU9250_ReleaseBus(); + return -1; } PIOS_MPU9250_ReleaseBus(); diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c deleted file mode 100644 index d526750791..0000000000 --- a/flight/pios/common/pios_ms5611.c +++ /dev/null @@ -1,531 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_MS5611 MS5611 Functions - * @brief Hardware functions to deal with the altitude pressure sensor - * @{ - * - * @file pios_ms5611.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief MS5611 Pressure Sensor Routines - * @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_MS5611 -#include -#define POW2(x) (1 << x) - -// TODO: Clean this up. Getting around old constant. -#define PIOS_MS5611_OVERSAMPLING oversampling - -// Option to change the interleave between Temp and Pressure conversions -// Undef for normal operation -#define PIOS_MS5611_SLOW_TEMP_RATE 20 -#ifndef PIOS_MS5611_SLOW_TEMP_RATE -#define PIOS_MS5611_SLOW_TEMP_RATE 1 -#endif -// Running moving average smoothing factor -#define PIOS_MS5611_TEMP_SMOOTHING 10 -// - -#define PIOS_MS5611_I2C_RETRIES 5 - -/* Local Types */ -typedef struct { - uint16_t C[6]; -} MS5611CalibDataTypeDef; - -typedef enum { - MS5611_CONVERSION_TYPE_None = 0, - MS5611_CONVERSION_TYPE_PressureConv, - MS5611_CONVERSION_TYPE_TemperatureConv -} ConversionTypeTypeDef; - -typedef enum { - MS5611_FSM_INIT = 0, - MS5611_FSM_TEMPERATURE, - MS5611_FSM_PRESSURE, - MS5611_FSM_CALCULATE, -} MS5611_FSM_State; - -/* Glocal Variables */ -ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None; - -/* Local Variables */ -MS5611CalibDataTypeDef CalibData; - -/* Straight from the datasheet */ -static uint32_t RawTemperature; -static uint32_t RawPressure; -static int64_t Pressure; -static int64_t Temperature; -static int64_t FilteredTemperature = INT32_MIN; -static int32_t lastConversionStart; - -static uint32_t conversionDelayMs; -static uint32_t conversionDelayUs; - -static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); -static int32_t PIOS_MS5611_WriteCommand(uint8_t command); -static uint32_t PIOS_MS5611_GetDelay(); -static uint32_t PIOS_MS5611_GetDelayUs(); - -// Second order temperature compensation. Temperature offset -static int64_t compensation_t2; - -// Move into proper driver structure with cfg stored -static uint32_t oversampling; -static const struct pios_ms5611_cfg *dev_cfg; -static int32_t i2c_id; -static PIOS_SENSORS_1Axis_SensorsWithTemp results; - -static bool sensorIsAlive = false; - -// sensor driver interface -bool PIOS_MS5611_driver_Test(uintptr_t context); -void PIOS_MS5611_driver_Reset(uintptr_t context); -void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, uintptr_t context); -void PIOS_MS5611_driver_fetch(void *, uint8_t size, uintptr_t context); -bool PIOS_MS5611_driver_poll(uintptr_t context); - -const PIOS_SENSORS_Driver PIOS_MS5611_Driver = { - .test = PIOS_MS5611_driver_Test, - .poll = PIOS_MS5611_driver_poll, - .fetch = PIOS_MS5611_driver_fetch, - .reset = PIOS_MS5611_driver_Reset, - .get_queue = NULL, - .get_scale = PIOS_MS5611_driver_get_scale, - .is_polled = true, -}; -/** - * Initialise the MS5611 sensor - */ -int32_t ms5611_read_flag; -void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) -{ - static uint32_t initTime; - - if (cfg) { - i2c_id = i2c_device; - - oversampling = cfg->oversampling; - conversionDelayMs = PIOS_MS5611_GetDelay(); - conversionDelayUs = PIOS_MS5611_GetDelayUs(); - - dev_cfg = cfg; // Store cfg before enabling interrupt - } else if (PIOS_DELAY_DiffuS(initTime) < 1000000) { // Do not reinitialize too often - return; - } - - initTime = PIOS_DELAY_GetRaw(); - - PIOS_MS5611_WriteCommand(MS5611_RESET); - PIOS_DELAY_WaitmS(20); - - uint8_t data[2]; - - // reset temperature compensation values - compensation_t2 = 0; - /* Calibration parameters */ - for (int i = 0; i < 6; i++) { - if (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2) != 0) { - return; - } - CalibData.C[i] = (data[0] << 8) | data[1]; - } - - sensorIsAlive = true; -} - -/** - * Start the ADC conversion - * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR - * \return 0 for success, -1 for failure (conversion completed and not read), -2 if failure occurred - */ -int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type) -{ - if (!sensorIsAlive) { /* if sensor is not alive, don't bother, wait for next poll to try to reinitialize */ - return -2; - } - - /* Start the conversion */ - - if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) { - if (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) { - return -2; - } - } else if (Type == MS5611_CONVERSION_TYPE_PressureConv) { - if (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) { - return -2; - } - } - lastConversionStart = PIOS_DELAY_GetRaw(); - CurrentRead = Type; - - return 0; -} - -/** - * @brief Return the delay for the current osr - */ -static uint32_t PIOS_MS5611_GetDelay() -{ - switch (oversampling) { - case MS5611_OSR_256: - return 1; - - case MS5611_OSR_512: - return 2; - - case MS5611_OSR_1024: - return 3; - - case MS5611_OSR_2048: - return 5; - - case MS5611_OSR_4096: - return 10; - - default: - break; - } - return 10; -} - -/** - * @brief Return the delay for the current osr in uS - */ -static uint32_t PIOS_MS5611_GetDelayUs() -{ - switch (oversampling) { - case MS5611_OSR_256: - return 600; - - case MS5611_OSR_512: - return 1170; - - case MS5611_OSR_1024: - return 2280; - - case MS5611_OSR_2048: - return 4540; - - case MS5611_OSR_4096: - return 9040; - - default: - break; - } - return 10; -} - -/** - * Read the ADC conversion value (once ADC conversion has completed) - * \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred - */ -int32_t PIOS_MS5611_ReadADC(void) -{ - if (!sensorIsAlive) { /* if sensor is not alive, don't bother, wait for next poll to try to reinitialize */ - return -2; - } - - uint8_t Data[3]; - - Data[0] = 0; - Data[1] = 0; - Data[2] = 0; - - if (CurrentRead == MS5611_CONVERSION_TYPE_None) { - return -2; - } - if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) { - return -1; - } - static int64_t deltaTemp; - - /* Read and store the 16bit result */ - if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) { - /* Read the temperature conversion */ - if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { - return -2; - } - - RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; - // Difference between actual and reference temperature - // dT = D2 - TREF = D2 - C5 * 2^8 - deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] * POW2(8)); - // Actual temperature (-40…85°C with 0.01°C resolution) - // TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23 - Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23)); - if (FilteredTemperature != INT32_MIN) { - FilteredTemperature = (FilteredTemperature * (PIOS_MS5611_TEMP_SMOOTHING - 1) - + Temperature) / PIOS_MS5611_TEMP_SMOOTHING; - } else { - FilteredTemperature = Temperature; - } - } else { - int64_t Offset; - int64_t Sens; - // used for second order temperature compensation - int64_t Offset2 = 0; - int64_t Sens2 = 0; - - /* Read the pressure conversion */ - if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { - return -2; - } - // check if temperature is less than 20°C - if (FilteredTemperature < 2000) { - // Apply compensation - // T2 = dT^2 / 2^31 - // OFF2 = 5 ⋅ (TEMP – 2000)^2/2 - // SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2 - - int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 2000); - Offset2 = (5 * tcorr) / 2; - Sens2 = (5 * tcorr) / 4; - compensation_t2 = (deltaTemp * deltaTemp) >> 31; - // Apply the "Very low temperature compensation" when temp is less than -15°C - if (FilteredTemperature < -1500) { - // OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2 - // SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2 - int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 1500); - Offset2 += 7 * tcorr2; - Sens2 += (11 * tcorr2) / 2; - } - } else { - compensation_t2 = 0; - Offset2 = 0; - Sens2 = 0; - } - RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); - // Offset at actual temperature - // OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7 - Offset = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2; - // Sensitivity at actual temperature - // SENS = SENST1 + TCS * dT = C1 * 2^15 + (C3 * dT) / 2^8 - Sens = ((int64_t)CalibData.C[0]) * POW2(15) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(8) - Sens2; - // Temperature compensated pressure (10…1200mbar with 0.01mbar resolution) - // P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15 - Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15); - } - return 0; -} - -/** - * Return the most recently computed temperature in kPa - */ -static float PIOS_MS5611_GetTemperature(void) -{ - // Apply the second order low and very low temperature compensation offset - return ((float)(FilteredTemperature - compensation_t2)) / 100.0f; -} - -/** - * Return the most recently computed pressure in Pa - */ -static float PIOS_MS5611_GetPressure(void) -{ - return (float)Pressure; -} - -/** - * Reads one or more bytes into a buffer - * \param[in] the command indicating the address to read - * \param[out] buffer destination buffer - * \param[in] len number of bytes which should be read - * \return 0 if operation was successful - * \return -1 if error during I2C transfer - */ -static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len) -{ - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = 1, - .buf = &address, - } - , - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - for (uint8_t retry = PIOS_MS5611_I2C_RETRIES; retry > 0; --retry) { - if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { - return 0; - } - } - - sensorIsAlive = false; - - return -1; -} - -/** - * Writes one or more bytes to the MS5611 - * \param[in] address Register address - * \param[in] buffer source buffer - * \return 0 if operation was successful - * \return -1 if error during I2C transfer - */ -static int32_t PIOS_MS5611_WriteCommand(uint8_t command) -{ - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS5611_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = 1, - .buf = &command, - } - , - }; - - for (uint8_t retry = PIOS_MS5611_I2C_RETRIES; retry > 0; --retry) { - if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { - return 0; - } - } - - sensorIsAlive = false; - - return -1; -} - -/** - * @brief Run self-test operation. - * \return 0 if self-test succeed, -1 if failed - */ -int32_t PIOS_MS5611_Test() -{ - // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? - int32_t cur_value = 0; - - cur_value = Temperature; - PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv); - PIOS_DELAY_WaitmS(10); - PIOS_MS5611_ReadADC(); - if (cur_value == Temperature) { - return -1; - } - - cur_value = Pressure; - PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv); - PIOS_DELAY_WaitmS(10); - PIOS_MS5611_ReadADC(); - if (cur_value == Pressure) { - return -1; - } - - return 0; -} - - -/* PIOS sensor driver implementation */ -void PIOS_MS5611_Register() -{ - PIOS_SENSORS_Register(&PIOS_MS5611_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0); -} - -bool PIOS_MS5611_driver_Test(__attribute__((unused)) uintptr_t context) -{ - return true; // !PIOS_MS5611_Test(); -} - -void PIOS_MS5611_driver_Reset(__attribute__((unused)) uintptr_t context) {} - -void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context) -{ - PIOS_Assert(size > 0); - scales[0] = 1; -} - -void PIOS_MS5611_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context) -{ - PIOS_Assert(data); - memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp)); -} - -bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context) -{ - static uint8_t temp_press_interleave_count = 1; - static MS5611_FSM_State next_state = MS5611_FSM_INIT; - - if (!sensorIsAlive) { // try to reinit - PIOS_MS5611_Init(0, 0); - } - - int32_t conversionResult = PIOS_MS5611_ReadADC(); - - if (__builtin_expect(conversionResult == -1, 1)) { - return false; // wait for conversion to complete - } else if (__builtin_expect(conversionResult == -2, 0)) { - temp_press_interleave_count = 1; - next_state = MS5611_FSM_INIT; - } - switch (next_state) { - case MS5611_FSM_INIT: - case MS5611_FSM_TEMPERATURE: - PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv); - next_state = MS5611_FSM_PRESSURE; - return false; - - case MS5611_FSM_PRESSURE: - PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv); - next_state = MS5611_FSM_CALCULATE; - return false; - - case MS5611_FSM_CALCULATE: - temp_press_interleave_count--; - if (!temp_press_interleave_count) { - temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE; - PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv); - next_state = MS5611_FSM_PRESSURE; - } else { - PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv); - next_state = MS5611_FSM_CALCULATE; - } - - results.temperature = PIOS_MS5611_GetTemperature(); - results.sample = PIOS_MS5611_GetPressure(); - return true; - - default: - // it should not be there - PIOS_Assert(0); - } - return false; -} - - -#endif /* PIOS_INCLUDE_MS5611 */ - -/** - * @} - * @} - */ diff --git a/flight/pios/common/pios_ms56xx.c b/flight/pios/common/pios_ms56xx.c new file mode 100644 index 0000000000..bf0237b12a --- /dev/null +++ b/flight/pios/common/pios_ms56xx.c @@ -0,0 +1,534 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MS56XX MS56XX Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_ms56xx.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief MS56XX Pressure Sensor Routines + * @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_MS56XX +#include +#define POW2(x) (1 << x) + +// Command addresses +#define MS56XX_RESET 0x1E +#define MS56XX_CALIB_ADDR 0xA2 /* First sample is factory stuff */ +#define MS56XX_CALIB_LEN 16 +#define MS56XX_ADC_READ 0x00 +#define MS56XX_PRES_ADDR 0x40 +#define MS56XX_TEMP_ADDR 0x50 + +// Option to change the interleave between Temp and Pressure conversions +// Undef for normal operation +#define PIOS_MS56XX_SLOW_TEMP_RATE 20 +#ifndef PIOS_MS56XX_SLOW_TEMP_RATE +#define PIOS_MS56XX_SLOW_TEMP_RATE 1 +#endif +// Running moving average smoothing factor +#define PIOS_MS56XX_TEMP_SMOOTHING 10 + +#define PIOS_MS56XX_INIT_DELAY_US 1000000 +#define PIOS_MS56XX_RESET_DELAY_US 20000 + +/* Local Types */ +typedef struct { + uint16_t C[6]; +} MS56XXCalibDataTypeDef; + +typedef enum { + MS56XX_CONVERSION_TYPE_None = 0, + MS56XX_CONVERSION_TYPE_PressureConv, + MS56XX_CONVERSION_TYPE_TemperatureConv +} ConversionTypeTypeDef; + +typedef enum { + MS56XX_FSM_INIT = 0, + MS56XX_FSM_CALIBRATION, + MS56XX_FSM_TEMPERATURE, + MS56XX_FSM_PRESSURE, + MS56XX_FSM_CALCULATE, +} MS56XX_FSM_State; + +static ConversionTypeTypeDef CurrentRead = MS56XX_CONVERSION_TYPE_None; + +static MS56XXCalibDataTypeDef CalibData; + +static uint8_t temp_press_interleave_count = 1; + +/* Straight from the datasheet */ +static uint32_t RawTemperature; +static uint32_t RawPressure; +static int64_t Pressure; +static int64_t Temperature; +static int64_t FilteredTemperature = INT32_MIN; + +static uint32_t lastCommandTimeRaw; +static uint32_t commandDelayUs; + +static uint32_t conversionDelayUs; + +// Second order temperature compensation. Temperature offset +static int64_t compensation_t2; + +// Move into proper driver structure with cfg stored +static enum pios_ms56xx_version version; +static uint8_t ms56xx_address; +static uint32_t oversampling; +static int32_t i2c_id; +static PIOS_SENSORS_1Axis_SensorsWithTemp results; +static bool hw_error = false; + +// private functions +static int32_t PIOS_MS56xx_Read_I2C(uint8_t address, uint8_t *buffer, uint8_t len); +static int32_t PIOS_MS56xx_WriteCommand(uint8_t command, uint32_t delayuS); +static uint32_t PIOS_MS56xx_GetDelayUs(void); +static void PIOS_MS56xx_ReadCalibrationData(void); + +// sensor driver interface +bool PIOS_MS56xx_driver_Test(uintptr_t context); +void PIOS_MS56xx_driver_Reset(uintptr_t context); +void PIOS_MS56xx_driver_get_scale(float *scales, uint8_t size, uintptr_t context); +void PIOS_MS56xx_driver_fetch(void *, uint8_t size, uintptr_t context); +bool PIOS_MS56xx_driver_poll(uintptr_t context); + +const PIOS_SENSORS_Driver PIOS_MS56xx_Driver = { + .test = PIOS_MS56xx_driver_Test, + .poll = PIOS_MS56xx_driver_poll, + .fetch = PIOS_MS56xx_driver_fetch, + .reset = PIOS_MS56xx_driver_Reset, + .get_queue = NULL, + .get_scale = PIOS_MS56xx_driver_get_scale, + .is_polled = true, +}; +/** + * Initialise the MS56XX sensor + */ +void PIOS_MS56xx_Init(const struct pios_ms56xx_cfg *cfg, int32_t i2c_device) +{ + i2c_id = i2c_device; + + ms56xx_address = cfg->address; + version = cfg->version; + oversampling = cfg->oversampling; + conversionDelayUs = PIOS_MS56xx_GetDelayUs(); +} +/** + * Start the ADC conversion + * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR + * \return 0 for success, -1 for failure (conversion completed and not read), -2 if failure occurred + */ +int32_t PIOS_MS56xx_StartADC(ConversionTypeTypeDef Type) +{ + /* Start the conversion */ + + if (Type == MS56XX_CONVERSION_TYPE_TemperatureConv) { + if (PIOS_MS56xx_WriteCommand(MS56XX_TEMP_ADDR + oversampling, conversionDelayUs) != 0) { + return -2; + } + } else if (Type == MS56XX_CONVERSION_TYPE_PressureConv) { + if (PIOS_MS56xx_WriteCommand(MS56XX_PRES_ADDR + oversampling, conversionDelayUs) != 0) { + return -2; + } + } + + CurrentRead = Type; + + return 0; +} + +/** + * @brief Return the delay for the current osr in uS + */ +static uint32_t PIOS_MS56xx_GetDelayUs() +{ + switch (oversampling) { + case MS56XX_OSR_256: + return 600; + + case MS56XX_OSR_512: + return 1170; + + case MS56XX_OSR_1024: + return 2280; + + case MS56XX_OSR_2048: + return 4540; + + case MS56XX_OSR_4096: + return 9040; + + case MS56XX_OSR_8192: + return 18080; + + default: + break; + } + return 10; +} + +/** + * Read the ADC conversion value (once ADC conversion has completed) + * \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred + */ +int32_t PIOS_MS56xx_ReadADC(void) +{ + uint8_t Data[3]; + + Data[0] = 0; + Data[1] = 0; + Data[2] = 0; + + if (CurrentRead == MS56XX_CONVERSION_TYPE_None) { + return -2; + } + + static int64_t deltaTemp; + + if (PIOS_MS56xx_Read_I2C(MS56XX_ADC_READ, Data, 3) != 0) { + return -2; + } + + /* Read and store the 16bit result */ + if (CurrentRead == MS56XX_CONVERSION_TYPE_TemperatureConv) { + RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; + // Difference between actual and reference temperature + // dT = D2 - TREF = D2 - C5 * 2^8 + deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] * POW2(8)); + // Actual temperature (-40…85°C with 0.01°C resolution) + // TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23 + Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23)); + if (FilteredTemperature != INT32_MIN) { + FilteredTemperature = (FilteredTemperature * (PIOS_MS56XX_TEMP_SMOOTHING - 1) + + Temperature) / PIOS_MS56XX_TEMP_SMOOTHING; + } else { + FilteredTemperature = Temperature; + } + } else { + int64_t Offset; + int64_t Sens; + // used for second order temperature compensation + int64_t Offset2 = 0; + int64_t Sens2 = 0; + + // check if temperature is less than 20°C + if (FilteredTemperature < 2000) { + // Apply compensation + // T2 = dT^2 / 2^31 + // OFF2 = 5 ⋅ (TEMP – 2000)^2/2 + // SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2 + + int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 2000); + Offset2 = (5 * tcorr) / 2; + Sens2 = (5 * tcorr) / 4; + compensation_t2 = (deltaTemp * deltaTemp) >> 31; + // Apply the "Very low temperature compensation" when temp is less than -15°C + if (FilteredTemperature < -1500) { + // OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2 + // SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2 + int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 1500); + Offset2 += 7 * tcorr2; + Sens2 += (11 * tcorr2) / 2; + } + } else { + compensation_t2 = 0; + Offset2 = 0; + Sens2 = 0; + } + RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); + + // Offset and sensitivity at actual temperature + if (version == MS56XX_VERSION_5611) { + // OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7 + Offset = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2; + // SENS = SENST1 + TCS * dT = C1 * 2^15 + (C3 * dT) / 2^8 + Sens = ((int64_t)CalibData.C[0]) * POW2(15) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(8) - Sens2; + } else { + // OFF = OFFT1 + TCO * dT = C2 * 2^17 + (C4 * dT) / 2^6 + Offset = ((int64_t)CalibData.C[1]) * POW2(17) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(6) - Offset2; + // SENS = SENST1 + TCS * dT = C1 * 2^16 + (C3 * dT) / 2^7 + Sens = ((int64_t)CalibData.C[0]) * POW2(16) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(7) - Sens2; + } + + // Temperature compensated pressure (10…1200mbar with 0.01mbar resolution) + // P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15 + Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15); + } + + CurrentRead = MS56XX_CONVERSION_TYPE_None; + + return 0; +} + +/** + * Return the most recently computed temperature in kPa + */ +static float PIOS_MS56xx_GetTemperature(void) +{ + // Apply the second order low and very low temperature compensation offset + return ((float)(FilteredTemperature - compensation_t2)) / 100.0f; +} + +/** + * Return the most recently computed pressure in Pa + */ +static float PIOS_MS56xx_GetPressure(void) +{ + return (float)Pressure; +} + +/** + * Reads one or more bytes into a buffer + * \param[in] the command indicating the address to read + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ +static int32_t PIOS_MS56xx_Read_I2C(uint8_t address, uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ms56xx_address, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &address, + } + , + { + .info = __func__, + .addr = ms56xx_address, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + enum pios_i2c_transfer_result i2c_result = PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); + + if (i2c_result == PIOS_I2C_TRANSFER_OK) { + return 0; + } + + if (i2c_result != PIOS_I2C_TRANSFER_BUSY) { + hw_error = true; + } + + return -1; +} + +/** + * Writes one or more bytes to the MS56XX + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ +static int32_t PIOS_MS56xx_WriteCommand(uint8_t command, uint32_t delayuS) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ms56xx_address, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &command, + } + , + }; + + lastCommandTimeRaw = PIOS_DELAY_GetRaw(); + commandDelayUs = delayuS; + + enum pios_i2c_transfer_result i2c_result = PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); + + if (i2c_result == PIOS_I2C_TRANSFER_OK) { + return 0; + } + + if (i2c_result != PIOS_I2C_TRANSFER_BUSY) { + hw_error = true; + } + + return -1; +} + +static void PIOS_MS56xx_ReadCalibrationData() +{ + uint8_t data[2]; + + /* reset temperature compensation values */ + compensation_t2 = 0; + /* Calibration parameters */ + for (int i = 0; i < 6; i++) { + if (PIOS_MS56xx_Read_I2C(MS56XX_CALIB_ADDR + i * 2, data, 2) != 0) { + break; + } + CalibData.C[i] = (data[0] << 8) | data[1]; + } +} + +static void PIOS_MS56xx_Reset() +{ + temp_press_interleave_count = 1; + hw_error = false; + + PIOS_MS56xx_WriteCommand(MS56XX_RESET, PIOS_MS56XX_RESET_DELAY_US); +} + + +/** + * @brief Run self-test operation. + * \return 0 if self-test succeed, -1 if failed + */ +int32_t PIOS_MS56xx_Test() +{ + // TODO: Is there a better way to test this than just checking that pressure/temperature has changed? + int32_t cur_value = 0; + + cur_value = Temperature; + PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv); + PIOS_DELAY_WaitmS(10); + PIOS_MS56xx_ReadADC(); + if (cur_value == Temperature) { + return -1; + } + + cur_value = Pressure; + PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_PressureConv); + PIOS_DELAY_WaitmS(10); + PIOS_MS56xx_ReadADC(); + if (cur_value == Pressure) { + return -1; + } + + return 0; +} + + +/* PIOS sensor driver implementation */ +void PIOS_MS56xx_Register() +{ + PIOS_SENSORS_Register(&PIOS_MS56xx_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0); +} + +bool PIOS_MS56xx_driver_Test(__attribute__((unused)) uintptr_t context) +{ + return true; // !PIOS_MS56xx_Test(); +} + +void PIOS_MS56xx_driver_Reset(__attribute__((unused)) uintptr_t context) {} + +void PIOS_MS56xx_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context) +{ + PIOS_Assert(size > 0); + scales[0] = 1; +} + +void PIOS_MS56xx_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context) +{ + PIOS_Assert(data); + memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp)); +} + +bool PIOS_MS56xx_driver_poll(__attribute__((unused)) uintptr_t context) +{ + static MS56XX_FSM_State next_state = MS56XX_FSM_INIT; + + if (PIOS_DELAY_DiffuS(lastCommandTimeRaw) < commandDelayUs) { + return false; + } + + commandDelayUs = 0; + + PIOS_MS56xx_ReadADC(); + + switch (next_state) { + case MS56XX_FSM_INIT: + PIOS_MS56xx_Reset(); + next_state = MS56XX_FSM_CALIBRATION; + break; + case MS56XX_FSM_CALIBRATION: + PIOS_MS56xx_ReadCalibrationData(); + /* fall through to MS56XX_FSM_TEMPERATURE */ + + case MS56XX_FSM_TEMPERATURE: + PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv); + next_state = MS56XX_FSM_PRESSURE; + break; + + case MS56XX_FSM_PRESSURE: + PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_PressureConv); + next_state = MS56XX_FSM_CALCULATE; + break; + + case MS56XX_FSM_CALCULATE: + temp_press_interleave_count--; + if (!temp_press_interleave_count) { + temp_press_interleave_count = PIOS_MS56XX_SLOW_TEMP_RATE; + PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv); + next_state = MS56XX_FSM_PRESSURE; + } else { + PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_PressureConv); + next_state = MS56XX_FSM_CALCULATE; + } + + results.temperature = PIOS_MS56xx_GetTemperature(); + results.sample = PIOS_MS56xx_GetPressure(); + return true; + + default: + // it should not be there + PIOS_Assert(0); + } + + if (hw_error) { + lastCommandTimeRaw = PIOS_DELAY_GetRaw(); + commandDelayUs = (next_state == MS56XX_FSM_CALIBRATION) ? PIOS_MS56XX_INIT_DELAY_US : 0; + CurrentRead = MS56XX_CONVERSION_TYPE_None; + next_state = MS56XX_FSM_INIT; + } + + return false; +} + +/* Poll the pressure sensor and return the temperature and pressure. */ +bool PIOS_MS56xx_Read(float *temperature, float *pressure) +{ + if (PIOS_MS56xx_driver_poll(0)) { + *temperature = results.temperature; + *pressure = results.sample; + return true; + } + return false; +} + +#endif /* PIOS_INCLUDE_MS56XX */ + +/** + * @} + * @} + */ diff --git a/flight/pios/common/pios_openlrs.c b/flight/pios/common/pios_openlrs.c index bab86b19e9..79a52f5b8a 100644 --- a/flight/pios/common/pios_openlrs.c +++ b/flight/pios/common/pios_openlrs.c @@ -465,6 +465,57 @@ static void tx_packet(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_ } } +#ifdef OPENLRS_SIMPLE_BEACON_TONE +// Basic tone +static void beacon_simpletone(struct pios_openlrs_dev *openlrs_dev, uint8_t tone, int16_t len __attribute__((unused))) +{ + DEBUG_PRINTF(2, "beacon_morse: %d\r\n", len); + +#if defined(PIOS_LED_LINK) + PIOS_LED_On(PIOS_LED_LINK); +#endif /* PIOS_LED_LINK */ + + rfm22_claimBus(openlrs_dev); + + GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio; + uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin; + uint8_t remap = openlrs_dev->cfg.spi_cfg->remap; + + GPIO_InitTypeDef init = { + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }; + init.GPIO_Pin = pin_source; + + // Set MOSI to digital out for bit banging + GPIO_PinAFConfig(gpio, pin_source, 0); + GPIO_Init(gpio, &init); + + int16_t cycles = (len * 50) / tone; + for (int16_t i = 0; i < cycles; i++) { + GPIO_SetBits(gpio, pin_source); + PIOS_Thread_Sleep(tone); + GPIO_ResetBits(gpio, pin_source); + PIOS_Thread_Sleep(tone); + } + GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init); + GPIO_PinAFConfig(gpio, pin_source, remap); + rfm22_releaseBus(openlrs_dev); + +#if defined(PIOS_LED_LINK) + PIOS_LED_Off(PIOS_LED_LINK); +#endif /* PIOS_LED_LINK */ + +#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B) + // Update the watchdog timer + PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B); +#endif /* PIOS_WDG_RFM22B */ +} + +#else /* OPENLRS_SIMPLE_BEACON_TONE */ + static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_t len __attribute__((unused))) // duration is now in half seconds. { DEBUG_PRINTF(2, "beacon_tone: %d %d\r\n", hz, len * 2); @@ -481,7 +532,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_ rfm22_claimBus(openlrs_dev); // This need fixed for F1 -#ifdef GPIO_Mode_OUT +#ifdef GPIO_MODE_OUT GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio; uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin; uint8_t remap = openlrs_dev->cfg.spi_cfg->remap; @@ -515,7 +566,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_ GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init); GPIO_PinAFConfig(gpio, pin_source, remap); -#endif /* ifdef GPIO_Mode_OUT */ +#endif /* ifdef GPIO_MODE_OUT */ rfm22_releaseBus(openlrs_dev); #if defined(PIOS_LED_LINK) @@ -528,6 +579,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_ #endif /* PIOS_WDG_RFM22B */ } +#endif /* OPENLRS_SIMPLE_BEACON_TONE */ static uint8_t beaconGetRSSI(struct pios_openlrs_dev *openlrs_dev) { @@ -588,9 +640,40 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone) if (static_tone) { uint8_t i = 0; while (i++ < 20) { +#ifdef OPENLRS_SIMPLE_BEACON_TONE + beacon_simpletone(openlrs_dev, 2, 3); +#else beacon_tone(openlrs_dev, 440, 1); +#endif } } else { +#ifdef OPENLRS_SIMPLE_BEACON_TONE + for (uint32_t tone = 1; tone < 4; tone++) { + if (tone == 1) { + rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set min power 25mW + } else { + rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW + } + // L - --- - - + beacon_simpletone(openlrs_dev, tone, 1); + PIOS_Thread_Sleep(100); + beacon_simpletone(openlrs_dev, tone, 3); + PIOS_Thread_Sleep(100); + beacon_simpletone(openlrs_dev, tone, 1); + PIOS_Thread_Sleep(100); + beacon_simpletone(openlrs_dev, tone, 1); + PIOS_Thread_Sleep(600); + // P - --- --- - + beacon_simpletone(openlrs_dev, tone, 1); + PIOS_Thread_Sleep(100); + beacon_simpletone(openlrs_dev, tone, 3); + PIOS_Thread_Sleep(100); + beacon_simpletone(openlrs_dev, tone, 3); + PIOS_Thread_Sleep(100); + beacon_simpletone(openlrs_dev, tone, 1); + PIOS_Thread_Sleep(2000); + } +#else /* ifdef OPENLRS_SIMPLE_BEACON_TONE */ // close encounters tune // G, A, F, F(lower octave), C // octave 3: 392 440 349 175 261 @@ -612,6 +695,7 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone) rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW PIOS_Thread_Sleep(10); beacon_tone(openlrs_dev, 261, 2); +#endif /* #ifdef OPENLRS_SIMPLE_BEACON_TONE */ } rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_READY); } @@ -829,9 +913,6 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) OPLinkStatusData oplink_status; OPLinkStatusGet(&oplink_status); - // Update the RSSI - oplink_status.RSSI = openlrs_dev->rssi; - timeUs = PIOS_DELAY_GetuS(); timeMs = PIOS_Thread_Systime(); @@ -860,8 +941,8 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) openlrs_dev->lastPacketTimeUs = timeUs; openlrs_dev->numberOfLostPackets = 0; - oplink_status.LinkQuality <<= 1; - oplink_status.LinkQuality |= 1; + openlrs_dev->link_quality <<= 1; + openlrs_dev->link_quality |= 1; if ((openlrs_dev->rx_buf[0] & 0x3e) == 0x00) { // This flag indicates receiving PPM data @@ -870,13 +951,8 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) rescaleChannels(openlrs_dev->ppm); // Call the PPM received callback if it's available. - if (openlrs_dev->openlrs_rcvr_id) { -#if defined(PIOS_INCLUDE_OPENLRS_RCVR) - PIOS_OpenLRS_Rcvr_UpdateChannels(openlrs_dev->openlrs_rcvr_id, openlrs_dev->ppm); -#endif - } if (openlrs_dev->ppm_callback) { - openlrs_dev->ppm_callback(openlrs_dev->ppm); + openlrs_dev->ppm_callback(openlrs_dev->ppm_context, openlrs_dev->ppm); } } else { // Not PPM data. Push into serial RX buffer. @@ -933,7 +1009,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) } tx_buf[4] = (openlrs_dev->lastAFCCvalue >> 8); tx_buf[5] = openlrs_dev->lastAFCCvalue & 0xff; - tx_buf[6] = countSetBits(oplink_status.LinkQuality & 0x7fff); + tx_buf[6] = countSetBits(openlrs_dev->link_quality & 0x7fff); } } @@ -954,7 +1030,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) if ((openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) + packet_timeout_us))) { DEBUG_PRINTF(2, "OLRS WARN: Lost packet: %d\r\n", openlrs_dev->numberOfLostPackets); // we lost packet, hop to next channel - oplink_status.LinkQuality <<= 1; + openlrs_dev->link_quality <<= 1; openlrs_dev->willhop = 1; if (openlrs_dev->numberOfLostPackets == 0) { openlrs_dev->linkLossTimeMs = timeMs; @@ -966,7 +1042,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) } else if ((openlrs_dev->numberOfLostPackets >= openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) * openlrs_dev->hopcount))) { DEBUG_PRINTF(2, "ORLS WARN: Trying to resync\r\n"); // hop slowly to allow resync with TX - oplink_status.LinkQuality = 0; + openlrs_dev->link_quality = 0; openlrs_dev->willhop = 1; openlrs_dev->lastPacketTimeUs = timeUs; } @@ -982,7 +1058,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) DEBUG_PRINTF(2, "Failsafe activated: %d %d\r\n", timeMs, openlrs_dev->linkLossTimeMs); oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED; // failsafeApply(); - openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // beacon activating... + openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_delay)) | 1; // beacon activating... } if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) && @@ -998,7 +1074,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) beacon_send(openlrs_dev, false); // play cool tune init_rfm(openlrs_dev, 0); // go back to normal RX rx_reset(openlrs_dev); - openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // avoid 0 in time + openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_period)) | 1; // avoid 0 in time } } } @@ -1034,6 +1110,19 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev) openlrs_dev->willhop = 0; } + if (oplink_status.LinkState > OPLINKSTATUS_LINKSTATE_DISCONNECTED) { + // Convert raw Rssi to dBm + oplink_status.RSSI = (int8_t)(openlrs_dev->rssi >> 1) - 122; + + // Count number of bits set in link_quality + uint8_t linkquality_bits = countSetBits(openlrs_dev->link_quality & 0x7fff); + // Translate link quality to 0 - 128 range + oplink_status.LinkQuality = (linkquality_bits + 1) * 8; + } else { + oplink_status.LinkQuality = 0; + oplink_status.RSSI = -127; + } + // Update UAVO OPLinkStatusSet(&oplink_status); } @@ -1084,31 +1173,13 @@ uint8_t PIOS_OpenLRS_RSSI_Get(void) * PPM Code *****************************************************************************/ -/** - * Register a OpenLRS_Rcvr interface to inform of PPM packets - * - * @param[in] rfm22b_dev The RFM22B device ID. - * @param[in] rfm22b_rcvr_id The receiver device to inform of PPM packets - */ -void PIOS_OpenLRS_RegisterRcvr(uint32_t openlrs_id, uint32_t openlrs_rcvr_id) -{ - struct pios_openlrs_dev *openlrs_dev = - (struct pios_openlrs_dev *)openlrs_id; - - if (!pios_openlrs_validate(openlrs_dev)) { - return; - } - - openlrs_dev->openlrs_rcvr_id = openlrs_rcvr_id; -} - /** * Register a OpenLRS_Rcvr interface to inform of PPM packets using a generic callback. * * @param[in] openlrs_id The OpenLRS device ID. * @param[in] callback The callback function. */ -void PIOS_OpenLRS_RegisterPPMCallback(uint32_t openlrs_id, PIOS_OpenLRS_PPMReceivedCallback callback) +void PIOS_OpenLRS_RegisterPPMCallback(uint32_t openlrs_id, PIOS_OpenLRS_PPMReceivedCallback callback, uint32_t context) { struct pios_openlrs_dev *openlrs_dev = (struct pios_openlrs_dev *)openlrs_id; @@ -1117,6 +1188,11 @@ void PIOS_OpenLRS_RegisterPPMCallback(uint32_t openlrs_id, PIOS_OpenLRS_PPMRecei return; } + /* + * Order is important in these assignments since openlrs_task uses ppm_callback + * field to determine if it's ok to dereference ppm_callback and ppm_context + */ + openlrs_dev->ppm_context = context; openlrs_dev->ppm_callback = callback; } @@ -1166,14 +1242,14 @@ int32_t PIOS_OpenLRS_Init(uint32_t *openlrs_id, uint32_t spi_id, } // Initialize the com callbacks. - openlrs_dev->rx_in_cb = NULL; - openlrs_dev->tx_out_cb = NULL; + openlrs_dev->rx_in_cb = NULL; + openlrs_dev->tx_out_cb = NULL; // Initialize the "PPM" callback. - openlrs_dev->openlrs_rcvr_id = 0; - openlrs_dev->ppm_callback = 0; + openlrs_dev->ppm_context = 0; + openlrs_dev->ppm_callback = 0; + - OPLinkSettingsInitialize(); OPLinkStatusInitialize(); DEBUG_PRINTF(2, "OpenLRS UAVOs Initialized\r\n"); OPLinkSettingsData binding; @@ -1294,13 +1370,13 @@ static void pios_openlrs_task(void *parameters) } } else { // We timed out because packet was missed - DEBUG_PRINTF(3, "ISR Timeout. Missed packet: %d %d %d\r\n", delay, getInterval(&openlrs_dev->bind_data), time_since_packet_us); + // DEBUG_PRINTF(3, "ISR Timeout. Missed packet: %d %d %d\r\n", delay_ms, getInterval(&openlrs_dev->bind_data), time_since_packet_us); pios_openlrs_rx_loop(openlrs_dev); } rssi_sampled = true; } else { - // DEBUG_PRINTF(3, "ISR %d %d %d\r\n", delay, getInterval(&openlrs_dev->bind_data), time_since_packet_us); + // DEBUG_PRINTF(3, "ISR %d %d %d\r\n", delay_ms, getInterval(&openlrs_dev->bind_data), time_since_packet_us); // Process incoming data pios_openlrs_rx_loop(openlrs_dev); diff --git a/flight/pios/common/pios_openlrs_rcvr.c b/flight/pios/common/pios_openlrs_rcvr.c index 3ffc734641..8f984dfee2 100644 --- a/flight/pios/common/pios_openlrs_rcvr.c +++ b/flight/pios/common/pios_openlrs_rcvr.c @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -44,9 +45,12 @@ /* Provide a RCVR driver */ static int32_t PIOS_OpenLRS_Rcvr_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t ppm_id); +static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16_t *channels); +static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id); const struct pios_rcvr_driver pios_openlrs_rcvr_driver = { - .read = PIOS_OpenLRS_Rcvr_Get, + .read = PIOS_OpenLRS_Rcvr_Get, + .get_quality = PIOS_OpenLRSRCVR_Quality_Get }; /* Local Variables */ @@ -57,6 +61,8 @@ enum pios_openlrs_rcvr_dev_magic { struct pios_openlrs_rcvr_dev { enum pios_openlrs_rcvr_dev_magic magic; int16_t channels[OPENLRS_PPM_NUM_CHANNELS]; + int8_t RSSI; + uint8_t LinkQuality; uint8_t supv_timer; bool fresh; }; @@ -102,7 +108,7 @@ extern int32_t PIOS_OpenLRS_Rcvr_Init(uint32_t *openlrs_rcvr_id, uintptr_t openl OPLinkReceiverInitialize(); *openlrs_rcvr_id = (uintptr_t)openlrs_rcvr_dev; - PIOS_OpenLRS_RegisterRcvr(openlrs_id, *openlrs_rcvr_id); + PIOS_OpenLRS_RegisterPPMCallback(openlrs_id, PIOS_OpenLRS_Rcvr_ppm_callback, *openlrs_rcvr_id); /* Register the failsafe timer callback. */ if (!PIOS_RTC_RegisterTickCallback @@ -118,7 +124,7 @@ extern int32_t PIOS_OpenLRS_Rcvr_Init(uint32_t *openlrs_rcvr_id, uintptr_t openl * PPM packet is received. This method stores the data locally as well * as sets the data into the OPLinkReceiver UAVO for visibility */ -int32_t PIOS_OpenLRS_Rcvr_UpdateChannels(uint32_t openlrs_rcvr_id, int16_t *channels) +static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16_t *channels) { /* Recover our device context */ struct pios_openlrs_rcvr_dev *openlrs_rcvr_dev = @@ -126,19 +132,26 @@ int32_t PIOS_OpenLRS_Rcvr_UpdateChannels(uint32_t openlrs_rcvr_id, int16_t *chan if (!PIOS_OpenLRS_Rcvr_Validate(openlrs_rcvr_dev)) { /* Invalid device specified */ - return -1; + return; } for (uint32_t i = 0; i < OPENLRS_PPM_NUM_CHANNELS; i++) { openlrs_rcvr_dev->channels[i] = channels[i]; } + // Update the RSSI and quality fields. + int8_t rssi; + OPLinkStatusRSSIGet(&rssi); + openlrs_rcvr_dev->RSSI = rssi; + uint16_t quality; + OPLinkStatusLinkQualityGet(&quality); + // Link quality is 0-128, so scale it down to 0-100 + openlrs_rcvr_dev->LinkQuality = quality * 100 / 128; + openlrs_rcvr_update_uavo(openlrs_rcvr_dev); // let supervisor know we have new data openlrs_rcvr_dev->fresh = true; - - return 0; } /** @@ -192,6 +205,8 @@ static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t openlrs_rcvr_id) i++) { openlrs_rcvr_dev->channels[i] = PIOS_RCVR_TIMEOUT; } + openlrs_rcvr_dev->RSSI = -127; + openlrs_rcvr_dev->LinkQuality = 0; } openlrs_rcvr_dev->fresh = false; @@ -212,9 +227,27 @@ static void openlrs_rcvr_update_uavo(struct pios_openlrs_rcvr_dev *rcvr_dev) for (int i = OPENLRS_PPM_NUM_CHANNELS - 1; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) { rcvr.Channel[i] = PIOS_RCVR_INVALID; } + + rcvr.RSSI = rcvr_dev->RSSI; + rcvr.LinkQuality = rcvr_dev->LinkQuality; + OPLinkReceiverSet(&rcvr); } + +static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id) +{ + /* Recover our device context */ + struct pios_openlrs_rcvr_dev *openlrs_rcvr_dev = (struct pios_openlrs_rcvr_dev *)openlrs_rcvr_id; + + if (!PIOS_OpenLRS_Rcvr_Validate(openlrs_rcvr_dev)) { + /* Invalid device specified */ + return 0; + } + + return openlrs_rcvr_dev->LinkQuality; +} + #endif /* PIOS_INCLUDE_OPENLRS */ /** diff --git a/flight/pios/common/pios_oplinkrcvr.c b/flight/pios/common/pios_oplinkrcvr.c index 1896a572e9..a9760e5027 100644 --- a/flight/pios/common/pios_oplinkrcvr.c +++ b/flight/pios/common/pios_oplinkrcvr.c @@ -37,7 +37,8 @@ #include #include -static OPLinkReceiverData oplinkreceiverdata; +// Put receiver in failsafe if not updated within timeout +#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 /* Provide a RCVR driver */ static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel); @@ -56,8 +57,8 @@ enum pios_oplinkrcvr_dev_magic { struct pios_oplinkrcvr_dev { enum pios_oplinkrcvr_dev_magic magic; - uint8_t supv_timer; + OPLinkReceiverData oplinkreceiverdata; bool Fresh; }; @@ -68,6 +69,36 @@ static bool PIOS_oplinkrcvr_validate(struct pios_oplinkrcvr_dev *oplinkrcvr_dev) return oplinkrcvr_dev->magic == PIOS_OPLINKRCVR_DEV_MAGIC; } +#if defined(PIOS_INCLUDE_RFM22B) +static void PIOS_oplinkrcvr_ppm_callback(uint32_t oplinkrcvr_id, const int16_t *channels) +{ + /* Recover our device context */ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)oplinkrcvr_id; + + if (!PIOS_oplinkrcvr_validate(oplinkrcvr_dev)) { + /* Invalid device specified */ + return; + } + + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { + oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = (i < RFM22B_PPM_NUM_CHANNELS) ? channels[i] : PIOS_RCVR_TIMEOUT; + } + + // Update the RSSI and quality fields. + int8_t rssi; + OPLinkStatusRSSIGet(&rssi); + oplinkrcvr_dev->oplinkreceiverdata.RSSI = rssi; + uint16_t quality; + OPLinkStatusLinkQualityGet(&quality); + // Link quality is 0-128, so scale it down to 0-100 + oplinkrcvr_dev->oplinkreceiverdata.LinkQuality = quality * 100 / 128; + + OPLinkReceiverSet(&oplinkrcvr_dev->oplinkreceiverdata); + + oplinkrcvr_dev->Fresh = true; +} +#endif /* PIOS_INCLUDE_RFM22B */ + #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void) { @@ -103,8 +134,6 @@ static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void) oplinkrcvr_dev->Fresh = false; oplinkrcvr_dev->supv_timer = 0; - global_oplinkrcvr_dev = oplinkrcvr_dev; - return oplinkrcvr_dev; } #endif /* if defined(PIOS_INCLUDE_FREERTOS) */ @@ -114,12 +143,16 @@ static void oplinkreceiver_updated(UAVObjEvent *ev) struct pios_oplinkrcvr_dev *oplinkrcvr_dev = global_oplinkrcvr_dev; if (ev->obj == OPLinkReceiverHandle()) { - OPLinkReceiverGet(&oplinkreceiverdata); + OPLinkReceiverGet(&oplinkrcvr_dev->oplinkreceiverdata); oplinkrcvr_dev->Fresh = true; } } -extern int32_t PIOS_OPLinkRCVR_Init(__attribute__((unused)) uint32_t *oplinkrcvr_id) +#if defined(PIOS_INCLUDE_RFM22B) +extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id, uint32_t rfm22b_id) +#else +extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id) +#endif { struct pios_oplinkrcvr_dev *oplinkrcvr_dev; @@ -131,10 +164,15 @@ extern int32_t PIOS_OPLinkRCVR_Init(__attribute__((unused)) uint32_t *oplinkrcvr for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) { /* Flush channels */ - oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; } - /* Register uavobj callback */ +#if defined(PIOS_INCLUDE_RFM22B) + /* Register ppm callback */ + PIOS_RFM22B_SetPPMCallback(rfm22b_id, PIOS_oplinkrcvr_ppm_callback, (uint32_t)oplinkrcvr_dev); +#endif + + /* Updates could come over the telemetry channel, so register uavobj callback */ OPLinkReceiverConnectCallback(oplinkreceiver_updated); /* Register the failsafe timer callback. */ @@ -142,6 +180,8 @@ extern int32_t PIOS_OPLinkRCVR_Init(__attribute__((unused)) uint32_t *oplinkrcvr PIOS_DEBUG_Assert(0); } + *oplinkrcvr_id = (uint32_t)oplinkrcvr_dev; + return 0; } @@ -152,14 +192,22 @@ extern int32_t PIOS_OPLinkRCVR_Init(__attribute__((unused)) uint32_t *oplinkrcvr * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver * \output >=0 channel value */ -static int32_t PIOS_OPLinkRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel) +static int32_t PIOS_OPLinkRCVR_Get(uint32_t oplinkrcvr_id, uint8_t channel) { + /* Recover our device context */ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)oplinkrcvr_id; + + if (!PIOS_oplinkrcvr_validate(oplinkrcvr_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } + if (channel >= OPLINKRECEIVER_CHANNEL_NUMELEM) { /* channel is out of range */ return PIOS_RCVR_INVALID; } - return oplinkreceiverdata.Channel[channel]; + return oplinkrcvr_dev->oplinkreceiverdata.Channel[channel]; } static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id) @@ -182,21 +230,26 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id) if (!oplinkrcvr_dev->Fresh) { for (int32_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) { - oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; } + oplinkrcvr_dev->oplinkreceiverdata.RSSI = -127; + oplinkrcvr_dev->oplinkreceiverdata.LinkQuality = 0; } oplinkrcvr_dev->Fresh = false; } -static uint8_t PIOS_OPLinkRCVR_Quality_Get(__attribute__((unused)) uint32_t oplinkrcvr_id) +static uint8_t PIOS_OPLinkRCVR_Quality_Get(uint32_t oplinkrcvr_id) { - uint16_t oplink_quality; + /* Recover our device context */ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)oplinkrcvr_id; - OPLinkStatusLinkQualityGet(&oplink_quality); + if (!PIOS_oplinkrcvr_validate(oplinkrcvr_dev)) { + /* Invalid device specified */ + return 0; + } - /* link_status is in the range 0-128, so scale to a % */ - return oplink_quality * 100 / 128; + return oplinkrcvr_dev->oplinkreceiverdata.LinkQuality; } #endif /* PIOS_INCLUDE_OPLINKRCVR */ diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 1bc83015a0..f160b034d7 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -66,54 +66,56 @@ #include /* Local Defines */ -#define STACK_SIZE_BYTES 200 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link) -#define ISR_TIMEOUT 1 // ms -#define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 -#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 -#define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 -#define RFM22B_LINK_QUALITY_THRESHOLD 20 -#define RFM22B_DEFAULT_MIN_CHANNEL 0 -#define RFM22B_DEFAULT_MAX_CHANNEL 250 -#define RFM22B_PPM_ONLY_DATARATE RFM22_datarate_9600 +#define STACK_SIZE_BYTES 200 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link) +#define ISR_TIMEOUT 1 // ms +#define EVENT_QUEUE_SIZE 5 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 +#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 +#define RFM22B_NOMINAL_CARRIER_FREQUENCY_433 430000000 +#define RFM22B_NOMINAL_CARRIER_FREQUENCY_868 860000000 +#define RFM22B_NOMINAL_CARRIER_FREQUENCY_915 900000000 +#define RFM22B_LINK_QUALITY_THRESHOLD 20 +#define RFM22B_DEFAULT_MIN_CHANNEL 0 +#define RFM22B_DEFAULT_MAX_CHANNEL 250 +#define RFM22B_PPM_ONLY_DATARATE RFM22_datarate_9600 // PPM encoding limits -#define RFM22B_PPM_MIN 1 -#define RFM22B_PPM_MAX 511 -#define RFM22B_PPM_INVALID 0 -#define RFM22B_PPM_SCALE 2 -#define RFM22B_PPM_MIN_US 990 -#define RFM22B_PPM_MAX_US (RFM22B_PPM_MIN_US + (RFM22B_PPM_MAX - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE) +#define RFM22B_PPM_MIN 1 +#define RFM22B_PPM_MAX 511 +#define RFM22B_PPM_INVALID 0 +#define RFM22B_PPM_SCALE 2 +#define RFM22B_PPM_MIN_US 990 +#define RFM22B_PPM_MAX_US (RFM22B_PPM_MIN_US + (RFM22B_PPM_MAX - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE) // The maximum amount of time without activity before initiating a reset. -#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 150 // ms +#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 150 // ms // this is too adjust the RF module so that it is on frequency -#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default +#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default -#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) -#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) -#define SYNC_BYTES 4 -#define HEADER_BYTES 4 -#define LENGTH_BYTES 1 +#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) +#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) +#define SYNC_BYTES 4 +#define HEADER_BYTES 4 +#define LENGTH_BYTES 1 // the size of the rf modules internal FIFO buffers -#define FIFO_SIZE 64 +#define FIFO_SIZE 64 -#define TX_FIFO_HI_WATERMARK 62 // 0-63 -#define TX_FIFO_LO_WATERMARK 32 // 0-63 +#define TX_FIFO_HI_WATERMARK 62 // 0-63 +#define TX_FIFO_LO_WATERMARK 32 // 0-63 -#define RX_FIFO_HI_WATERMARK 32 // 0-63 +#define RX_FIFO_HI_WATERMARK 32 // 0-63 // preamble byte (preceeds SYNC_BYTE's) -#define PREAMBLE_BYTE 0x55 +#define PREAMBLE_BYTE 0x55 // RF sync bytes (32-bit in all) -#define SYNC_BYTE_1 0x2D -#define SYNC_BYTE_2 0xD4 -#define SYNC_BYTE_3 0x4B -#define SYNC_BYTE_4 0x59 +#define SYNC_BYTE_1 0x2D +#define SYNC_BYTE_2 0xD4 +#define SYNC_BYTE_3 0x4B +#define SYNC_BYTE_4 0x59 #ifndef RX_LED_ON #define RX_LED_ON @@ -187,10 +189,9 @@ static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan); +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan, uint32_t frequency_hz); static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); static void rfm22_generateDeviceID(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev); static void rfm22_updateStats(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_checkTimeOut(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev); @@ -198,14 +199,15 @@ static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev); static uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev); -static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); +static uint32_t rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev); static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index); static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_clearLEDs(); // Utility functions. -static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time); +static uint32_t pios_rfm22_time_ms(); +static uint32_t pios_rfm22_time_difference_ms(uint32_t start_time, uint32_t end_time); static struct pios_rfm22b_dev *pios_rfm22_alloc(void); static void rfm22_hmac_sha1(const uint8_t *data, size_t len, uint8_t key[SHA1_DIGEST_LENGTH], uint8_t digest[SHA1_DIGEST_LENGTH]); @@ -337,14 +339,26 @@ static const uint32_t data_rate[] = { static const uint8_t channel_spacing[] = { 1, /* 9.6kbps */ - 2, /* 19.2kps */ - 2, /* 32kps */ - 2, /* 57.6kps */ - 2, /* 64kps */ - 3, /* 100kps */ - 4, /* 128kps */ - 4, /* 192kps */ - 4 /* 256kps */ + 2, /* 19.2kbps */ + 2, /* 32kbps */ + 2, /* 57.6kbps */ + 2, /* 64kbps */ + 3, /* 100kbps */ + 4, /* 128kbps */ + 4, /* 192kbps */ + 5, /* 256kbps */ +}; + +static const uint8_t channel_limits[] = { + 1, /* 9.6kbps */ + 1, /* 19.2kbps */ + 1, /* 32kbps */ + 1, /* 57.6kbps */ + 1, /* 64kbps */ + 1, /* 100kbps */ + 2, /* 128kbps */ + 2, /* 192kbps */ + 2, /* 256kbps */ }; static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth @@ -360,7 +374,7 @@ static const uint8_t reg_23[] = { 0xA5, 0x49, 0x25, 0x93, 0x86, 0x11, 0x86, 0x4A static const uint8_t reg_24[] = { 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x06, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 static const uint8_t reg_25[] = { 0x34, 0x88, 0x77, 0x29, 0xE2, 0x90, 0xE2, 0x1A, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x28, 0x3C, 0x3C, 0x50, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x28, 0x3C, 0x3C, 0x50, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = ±AFCLimiter[7:0] x (hbsel+1) x 625 Hz static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0xC0, 0xC0, 0xC0, 0xED }; // rfm22_cpcuu static const uint8_t reg_69[] = { 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60 }; // rfm22_agc_override1 @@ -370,7 +384,7 @@ static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0x62, 0x9A, 0xC5, 0x27 static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C }; // rfm22_modulation_mode_control1 static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD, 0x0F }; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD, 0xF0 }; // rfm22_frequency_deviation static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 }; static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 }; @@ -391,7 +405,7 @@ static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; * @param[in] slave_num The SPI bus slave number. * @param[in] cfg The device configuration. */ -int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg) +int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg, OPLinkSettingsRFBandOptions band) { PIOS_DEBUG_Assert(rfm22b_id); PIOS_DEBUG_Assert(cfg); @@ -405,36 +419,26 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu g_rfm22b_dev = rfm22b_dev; // Store the SPI handle - rfm22b_dev->slave_num = slave_num; - rfm22b_dev->spi_id = spi_id; + rfm22b_dev->slave_num = slave_num; + rfm22b_dev->spi_id = spi_id; // Initialize our configuration parameters - rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; - rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; - rfm22b_dev->coordinator = false; - rfm22b_dev->coordinatorID = 0; - - // Initialize the com callbacks. - rfm22b_dev->rx_in_cb = NULL; - rfm22b_dev->tx_out_cb = NULL; - - // Initialize the PPM callback. - rfm22b_dev->ppm_callback = NULL; - - // Initialize the stats. - rfm22b_dev->stats.packets_per_sec = 0; - rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; - rfm22b_dev->stats.tx_dropped = 0; - rfm22b_dev->stats.resets = 0; - rfm22b_dev->stats.timeouts = 0; - rfm22b_dev->stats.link_quality = 0; - rfm22b_dev->stats.rssi = 0; - rfm22b_dev->stats.tx_seq = 0; - rfm22b_dev->stats.rx_seq = 0; - rfm22b_dev->stats.tx_failure = 0; + rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + + // Set the frequency band + switch (band) { + case OPLINKSETTINGS_RFBAND_915MHZ: + rfm22b_dev->base_freq = RFM22B_NOMINAL_CARRIER_FREQUENCY_915; + break; + case OPLINKSETTINGS_RFBAND_868MHZ: + rfm22b_dev->base_freq = RFM22B_NOMINAL_CARRIER_FREQUENCY_868; + break; + case OPLINKSETTINGS_RFBAND_433MHZ: + default: + rfm22b_dev->base_freq = RFM22B_NOMINAL_CARRIER_FREQUENCY_433; + break; + } // Initialize the channels. PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_RX_DATARATE, RFM22B_DEFAULT_MIN_CHANNEL, @@ -635,6 +639,21 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar } } +/** + * Set a XtalCap + * + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] XtalCap Value. + */ +void PIOS_RFM22B_SetXtalCap(uint32_t rfm22b_id, uint8_t xtal_cap) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + rfm22b_dev->cfg.RFXtalCap = xtal_cap; + } +} + /** * Set a modem to be a coordinator or not. * @@ -686,31 +705,6 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) *stats = rfm22b_dev->stats; } -/** - * Get the stats of the oter radio devices that are in range. - * - * @param[out] device_ids A pointer to the array to store the device IDs. - * @param[out] RSSIs A pointer to the array to store the RSSI values in. - * @param[in] mx_pairs The length of the pdevice_ids and RSSIs arrays. - * @return The number of pair stats returned. - */ -uint8_t PIOS_RFM22B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return 0; - } - - uint8_t mp = (max_pairs >= OPLINKSTATUS_PAIRIDS_NUMELEM) ? max_pairs : OPLINKSTATUS_PAIRIDS_NUMELEM; - for (uint8_t i = 0; i < mp; ++i) { - device_ids[i] = rfm22b_dev->pair_stats[i].pairID; - RSSIs[i] = rfm22b_dev->pair_stats[i].rssi; - } - - return mp; -} - /** * Check the radio device for a valid connection * @@ -801,11 +795,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len) return false; } - rfm22b_dev->tx_packet_handle = p; - rfm22b_dev->stats.tx_byte_count += len; - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) { - rfm22b_dev->packet_start_ticks = 1; + 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; } // Claim the SPI bus. @@ -866,8 +859,6 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len) // We're in Tx mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_TX_MODE; - TX_LED_ON; - #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D1_LED_ON; #endif @@ -992,9 +983,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) // Increment the total byte received count. rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; - // Update the pair status with this packet. - rfm22_updatePairStatus(rfm22b_dev); - // We're finished with Rx mode rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; @@ -1043,9 +1031,9 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D2_LED_ON; #endif // PIOS_RFM22B_DEBUG_ON_TELEM - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) { - rfm22b_dev->packet_start_ticks = 1; + rfm22b_dev->packet_start_time = pios_rfm22_time_ms(); + if (rfm22b_dev->packet_start_time == 0) { + rfm22b_dev->packet_start_time = 1; } // We detected the preamble, now wait for sync. @@ -1056,13 +1044,13 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) // read the 10-bit signed afc correction value // bits 9 to 2 - uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; + int16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; // bits 1 & 0 - afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; + afc_correction |= (int16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; afc_correction >>= 6; // convert the afc value to Hz int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); - rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr); + rfm22b_dev->afc_correction_Hz = afc_corr; // read rx signal strength .. 45 = -100dBm, 205 = -20dBm uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi); @@ -1089,7 +1077,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) * @param[in] rfm22b_dev The RFM22B device ID. * @param[in] cb The callback function pointer. */ -void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb) +void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb, uint32_t cb_context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; @@ -1097,6 +1085,11 @@ void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb) return; } + /* + * Order is important in these assignments since rfm22_task uses ppm_callback + * field to determine if it's ok to dereference ppm_callback and ppm_context + */ + rfm22b_dev->ppm_context = cb_context; rfm22b_dev->ppm_callback = cb; } @@ -1175,7 +1168,7 @@ static void pios_rfm22_task(void *parameters) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - portTickType lastEventTicks = xTaskGetTickCount(); + uint32_t lastEventTime = pios_rfm22_time_ms(); while (1) { #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B) @@ -1185,7 +1178,7 @@ static void pios_rfm22_task(void *parameters) // Wait for a signal indicating an external interrupt or a pending send/receive request. if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { - lastEventTicks = xTaskGetTickCount(); + lastEventTime = pios_rfm22_time_ms(); // Process events through the state machine. enum pios_radio_event event; @@ -1198,14 +1191,14 @@ static void pios_rfm22_task(void *parameters) } } else { // Has it been too long since the last event? - portTickType curTicks = xTaskGetTickCount(); - if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) { + uint32_t curTime = pios_rfm22_time_ms(); + if (pios_rfm22_time_difference_ms(lastEventTime, curTime) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) { // Clear the event queue. enum pios_radio_event event; while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) { // Do nothing; } - lastEventTicks = xTaskGetTickCount(); + lastEventTime = pios_rfm22_time_ms(); // Transsition through an error event. rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); @@ -1217,11 +1210,10 @@ static void pios_rfm22_task(void *parameters) rfm22_process_event(rfm22b_dev, RADIO_EVENT_RX_MODE); } - portTickType curTicks = xTaskGetTickCount(); // Have we been sending / receiving this packet too long? - - if ((rfm22b_dev->packet_start_ticks > 0) && - (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->packet_time * 3))) { + uint32_t curTime = pios_rfm22_time_ms(); + if ((rfm22b_dev->packet_start_time > 0) && + (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_time, curTime) > (rfm22b_dev->packet_time * 3))) { rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT); } @@ -1354,14 +1346,6 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // Clean the LEDs rfm22_clearLEDs(); - // Initialize the detected device statistics. - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { - rfm22b_dev->pair_stats[i].pairID = 0; - rfm22b_dev->pair_stats[i].rssi = -127; - rfm22b_dev->pair_stats[i].afc_correction = 0; - rfm22b_dev->pair_stats[i].lastContact = 0; - } - // Initlize the link stats. for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) { rfm22b_dev->rx_packet_stats[i] = 0; @@ -1376,15 +1360,14 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->tx_packet_handle = NULL; // Initialize the devide state - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; rfm22b_dev->channel = 0; - rfm22b_dev->channel_index = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; - rfm22b_dev->last_contact = 0; + rfm22b_dev->channel_index = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_time = 0; + rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; + rfm22b_dev->last_contact = 0; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -1556,8 +1539,13 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // RX FIFO Almost Full Threshold (0 - 63) rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); - // Set the frequency calibration - rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); + // Set the xtal capacitor for frequency calibration + // Cint = 1.8 pF + 0.085 pF x xlc[6:0] + 3.7 pF x xlc[7] (xtalshift) + // cfg.RFXtalCap 0 to 171 range give Cint = 1.8pF to 16.295pF range + // Default is 127, equal to 12.595pF + rfm22_write(rfm22b_dev, + RFM22_xtal_osc_load_cap, + (rfm22b_dev->cfg.RFXtalCap < 128) ? rfm22b_dev->cfg.RFXtalCap : (rfm22b_dev->cfg.RFXtalCap + 84)); // Release the bus rfm22_releaseBus(rfm22b_dev); @@ -1566,7 +1554,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1))); // Initialize the frequency and datarate to te default. - rfm22_setNominalCarrierFrequency(rfm22b_dev, 0); + rfm22_setNominalCarrierFrequency(rfm22b_dev, 0, rfm22b_dev->base_freq); pios_rfm22_setDatarate(rfm22b_dev); return RADIO_EVENT_INITIALIZED; @@ -1656,10 +1644,8 @@ static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev) * @param[in] rfm33b_dev The device structure pointer. * @param[in] init_chan The initial channel to tune to. */ -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan) +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan, uint32_t frequency_hz) { - // Set the frequency channels to start at 430MHz - uint32_t frequency_hz = RFM22B_NOMINAL_CARRIER_FREQUENCY; // The step size is 10MHz / 250 = 40khz, and the step size is specified in 10khz increments. uint8_t freq_hop_step_size = 4; @@ -1682,11 +1668,11 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); - // Setthe frequency hopping step size. + // Set the frequency hopping step size. rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, freq_hop_step_size); - // frequency hopping channel (0-255) - rfm22b_dev->frequency_step_size = 156.25f * hbsel; + // frequency step + rfm22b_dev->frequency_step_size = 156.25f * (hbsel + 1); // frequency hopping channel (0-255) rfm22b_dev->channel = init_chan; @@ -1792,7 +1778,7 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_add_rx_status(rfm22b_dev, RADIO_FAILURE_RX_PACKET); rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->packet_start_time = 0; rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; } @@ -1824,6 +1810,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) } // Should we append PPM data to the packet? + bool ppm_valid = false; if (radio_dev->ppm_send_mode) { len = RFM22B_PPM_NUM_CHANNELS + (radio_dev->ppm_only_mode ? 2 : 1); @@ -1843,10 +1830,13 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) if ((val == PIOS_RCVR_INVALID) || (val == PIOS_RCVR_TIMEOUT)) { val = RFM22B_PPM_INVALID; } else if (val > RFM22B_PPM_MAX_US) { + ppm_valid = true; val = RFM22B_PPM_MAX; } else if (val < RFM22B_PPM_MIN_US) { + ppm_valid = true; val = RFM22B_PPM_MIN; } else { + ppm_valid = true; val = (val - RFM22B_PPM_MIN_US) / RFM22B_PPM_SCALE + RFM22B_PPM_MIN; } @@ -1870,10 +1860,30 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) } // Append data from the com interface if applicable. - if (!radio_dev->ppm_only_mode && radio_dev->tx_out_cb) { - // Try to get some data to send + bool packet_data = false; + if (!radio_dev->ppm_only_mode) { + uint8_t newlen = 0; bool need_yield = false; - len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield); + uint8_t i = 0; + // Try to get some data to send + while (newlen == 0 && i < 2) { + radio_dev->last_stream_sent = (radio_dev->last_stream_sent + 1) % 2; + if (!radio_dev->last_stream_sent) { + if (radio_dev->tx_out_cb) { + newlen = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len + 1, max_data_len - len - 1, NULL, &need_yield); + } + } else { + if (radio_dev->aux_tx_out_cb) { + newlen = (radio_dev->aux_tx_out_cb)(radio_dev->aux_tx_out_context, p + len + 1, max_data_len - len - 1, NULL, &need_yield); + } + } + i++; + } + if (newlen) { + packet_data = true; + *(p + len) = radio_dev->last_stream_sent; + len += newlen + 1; + } } // Always send a packet if this modem is a coordinator. @@ -1892,6 +1902,12 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) len += RS_ECC_NPARITY; } + // Only count the packet if it contains valid data. + if (ppm_valid || packet_data) { + TX_LED_ON; + radio_dev->stats.tx_byte_count += len; + } + // Transmit the packet. PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p, len); @@ -1911,14 +1927,12 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) // Is the transmition complete if (res == PIOS_RFM22B_TX_COMPLETE) { - radio_dev->tx_complete_ticks = xTaskGetTickCount(); - // Is this an ACK? ret_event = RADIO_EVENT_RX_MODE; - radio_dev->tx_packet_handle = 0; + radio_dev->tx_packet_handle = 0; radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0; // Start a new transaction - radio_dev->packet_start_ticks = 0; + radio_dev->packet_start_time = 0; #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D1_LED_OFF; @@ -1939,7 +1953,7 @@ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, rfm22b_dev->rx_packet)) { return RADIO_EVENT_NUM_EVENTS; } - rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->packet_start_time = 0; // No event generated return RADIO_EVENT_NUM_EVENTS; @@ -1955,9 +1969,10 @@ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) */ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, uint8_t *p, uint16_t rx_len) { - bool good_packet = true; + bool good_packet = true; bool corrected_packet = false; - uint8_t data_len = rx_len; + uint8_t stream_num = 0; + uint8_t data_len = rx_len; // We don't rsencode ppm only packets. if (!radio_dev->ppm_only_mode) { @@ -2015,7 +2030,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d // Call the PPM received callback if it's available. if (radio_dev->ppm_callback) { - radio_dev->ppm_callback(radio_dev->ppm); + radio_dev->ppm_callback(radio_dev->ppm_context, radio_dev->ppm); } } } @@ -2038,10 +2053,22 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d if (good_packet || corrected_packet) { // Send the data to the com port bool rx_need_yield; - if (radio_dev->rx_in_cb && (data_len > 0) && !radio_dev->ppm_only_mode) { - (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield); - } + + if ((data_len > 0) && !radio_dev->ppm_only_mode) { + stream_num = *p; + p++; + data_len--; + if (!stream_num) { + if (radio_dev->rx_in_cb) { + (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield); + } + } else { + if (radio_dev->aux_rx_in_cb) { + (radio_dev->aux_rx_in_cb)(radio_dev->aux_rx_in_context, p, data_len, NULL, &rx_need_yield); + } + } + } /* * If the packet is valid and destined for us we synchronize the clock. */ @@ -2049,9 +2076,10 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) { rfm22_synchronizeClock(radio_dev); } - radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; - radio_dev->last_contact = xTaskGetTickCount(); + radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + radio_dev->last_contact = pios_rfm22_time_ms(); radio_dev->stats.rssi = radio_dev->rssi_dBm; + radio_dev->stats.afc_correction = radio_dev->afc_correction_Hz; } else { ret_event = RADIO_EVENT_RX_COMPLETE; } @@ -2081,7 +2109,7 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) #endif // Start a new transaction - radio_dev->packet_start_ticks = 0; + radio_dev->packet_start_time = 0; break; case PIOS_RFM22B_INT_FAILURE: @@ -2101,50 +2129,6 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) * Link Statistics Functions *****************************************************************************/ -/** - * Update the modem pair status. - * - * @param[in] rfm22b_dev The device structure - */ -static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev) -{ - int8_t rssi = radio_dev->rssi_dBm; - int8_t afc = radio_dev->afc_correction_Hz; - uint32_t id = radio_dev->rx_destination_id; - - // Have we seen this device recently? - bool found = false; - uint8_t id_idx = 0; - - for (; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { - if (radio_dev->pair_stats[id_idx].pairID == id) { - found = true; - break; - } - } - - // If we have seen it, update the RSSI and reset the last contact counter - if (found) { - radio_dev->pair_stats[id_idx].rssi = rssi; - radio_dev->pair_stats[id_idx].afc_correction = afc; - radio_dev->pair_stats[id_idx].lastContact = 0; - } else { - // If we haven't seen it, find a slot to put it in. - uint8_t min_idx = 0; - int8_t min_rssi = radio_dev->pair_stats[0].rssi; - for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { - if (radio_dev->pair_stats[id_idx].rssi < min_rssi) { - min_rssi = radio_dev->pair_stats[id_idx].rssi; - min_idx = id_idx; - } - } - radio_dev->pair_stats[min_idx].pairID = id; - radio_dev->pair_stats[min_idx].rssi = rssi; - radio_dev->pair_stats[min_idx].afc_correction = afc; - radio_dev->pair_stats[min_idx].lastContact = 0; - } -} - /** * Calculate stats from the packet receipt, transmission statistics. * @@ -2205,7 +2189,7 @@ static void rfm22_updateStats(struct pios_rfm22b_dev *rfm22b_dev) */ static bool rfm22_checkTimeOut(struct pios_rfm22b_dev *rfm22b_dev) { - return pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, xTaskGetTickCount()) >= CONNECTED_TIMEOUT; + return pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, pios_rfm22_time_ms()) >= CONNECTED_TIMEOUT; } /** @@ -2268,7 +2252,7 @@ uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev) */ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev) { - portTickType start_time = rfm22b_dev->packet_start_ticks; + uint32_t start_time = rfm22b_dev->packet_start_time; // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started. uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * rfm22b_dev->num_channels; @@ -2282,17 +2266,19 @@ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev) } /** - * Return the extimated current clock ticks count on the coordinator modem. + * Return the estimated current time on the coordinator modem. * This is the master clock used for all synchronization. * * @param[in] rfm22b_dev The device structure */ -static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) +static uint32_t rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev) { + uint32_t time = pios_rfm22_time_ms(); + if (rfm22_isCoordinator(rfm22b_dev)) { - return ticks; + return time; } - return ticks + rfm22b_dev->time_delta; + return time + rfm22b_dev->time_delta; } /** @@ -2302,7 +2288,7 @@ static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, po */ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) { - portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + uint32_t time = rfm22_coordinatorTime(rfm22b_dev); bool is_coordinator = rfm22_isCoordinator(rfm22b_dev); // If this is a one-way link, only the coordinator can send. @@ -2357,10 +2343,10 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind */ static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev) { - portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + uint32_t time = rfm22_coordinatorTime(rfm22b_dev); // Divide time into 8ms blocks. Coordinator sends in first 2 ms, and remote send in 5th and 6th ms. // Channel changes occur in the last 2 ms. - uint8_t n = (time / rfm22b_dev->packet_time) % rfm22b_dev->num_channels; + uint8_t n = (time / rfm22b_dev->packet_time) % rfm22b_dev->num_channels; return rfm22_calcChannel(rfm22b_dev, n); } @@ -2394,7 +2380,7 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->stats.tx_failure++; - rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->packet_start_time = 0; rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; return RADIO_EVENT_TX_START; } @@ -2408,7 +2394,7 @@ static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->stats.timeouts++; - rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->packet_start_time = 0; // Release the Tx packet if it's set. if (rfm22b_dev->tx_packet_handle != 0) { rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; @@ -2478,20 +2464,28 @@ static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pi * Utility Functions *****************************************************************************/ +/** + * Get the current time in ms from the ticks counter. + */ +static uint32_t pios_rfm22_time_ms() +{ + return xTaskGetTickCount() * portTICK_RATE_MS; +} + /** * Calculate the time difference between the start time and end time. - * Times are in ticks. Also handles rollover. + * Times are in ms. Also handles rollover. * - * @param[in] start_time The start time in ticks. - * @param[in] end_time The end time in ticks. + * @param[in] start_time The start time in ms. + * @param[in] end_time The end time in ms. */ -static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time) +static uint32_t pios_rfm22_time_difference_ms(uint32_t start_time, uint32_t end_time) { if (end_time >= start_time) { - return (end_time - start_time) * portTICK_RATE_MS; + return end_time - start_time; } // Rollover - return ((portMAX_DELAY - start_time) + end_time) * portTICK_RATE_MS; + return (UINT32_MAX - start_time) + end_time; } /** @@ -2503,12 +2497,13 @@ static struct pios_rfm22b_dev *pios_rfm22_alloc(void) struct pios_rfm22b_dev *rfm22b_dev; rfm22b_dev = (struct pios_rfm22b_dev *)pios_malloc(sizeof(*rfm22b_dev)); - rfm22b_dev->spi_id = 0; if (!rfm22b_dev) { return NULL; } + memset(rfm22b_dev, 0, sizeof(*rfm22b_dev)); rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; + return rfm22b_dev; } #else @@ -2523,6 +2518,8 @@ static struct pios_rfm22b_dev *pios_rfm22_alloc(void) } rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++]; + + memset(rfm22b_dev, 0, sizeof(*rfm22b_dev)); rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; return rfm22b_dev; @@ -2684,9 +2681,16 @@ static void rfm22_hmac_sha1(const uint8_t *data, size_t len, static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint8_t min, uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen) { + // Define first and last channel to be used within min/max values + // according to the frequency deviation, without up/down overflow. + uint8_t chan_min_limit = min + channel_limits[rate]; + uint8_t chan_max_limit = max - channel_limits[rate]; + + // Define how many channels we can use according to the spacing. + uint8_t chan_count = ((chan_max_limit - chan_min_limit) / channel_spacing[rate]) + 1; + uint32_t data = 0; uint8_t cpos = 0; - uint8_t chan_range = (max / channel_spacing[rate] - min / channel_spacing[rate]) + 1; uint8_t key[SHA1_DIGEST_LENGTH] = { 0 }; uint8_t digest[SHA1_DIGEST_LENGTH]; uint8_t *all_channels; @@ -2695,12 +2699,16 @@ static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint memcpy(key, &coordid, sizeof(coordid)); - for (int i = 0; i < chan_range; i++) { - all_channels[i] = min / channel_spacing[rate] + i; + // Fill all_channels[] with usable channels + for (int i = 0; i < chan_count; i++) { + all_channels[i] = chan_min_limit + (i * channel_spacing[rate]); } + // DEBUG_PRINTF(3, "\r\nChannel Min: %d Max:%d - Spacing: %d Limits: %d\r\n", min, max, channel_spacing[rate], channel_limits[rate]); + // DEBUG_PRINTF(3, "Result: Channel count: %d - Usable channels from ch%d to ch%d\r\n", chan_count, all_channels[0], all_channels[chan_count - 1]); + int j = SHA1_DIGEST_LENGTH; - for (int i = 0; i < chan_range && i < MAX_CHANNELS; i++) { + for (int i = 0; i < chan_count && i < MAX_CHANNELS; i++) { uint8_t rnd; uint8_t r; uint8_t tmp; @@ -2712,14 +2720,16 @@ static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint } rnd = digest[j]; j++; - r = rnd % (chan_range - i) + i; + r = rnd % (chan_count - i) + i; tmp = all_channels[i]; all_channels[i] = all_channels[r]; all_channels[r] = tmp; } - for (int i = 0; i < chan_range && cpos < MAX_CHANNELS; i++, cpos++) { - channels[cpos] = all_channels[i] * channel_spacing[rate]; + // DEBUG_PRINTF(3, "Final channel list:"); + for (int i = 0; i < chan_count && cpos < MAX_CHANNELS; i++, cpos++) { + channels[cpos] = all_channels[i]; + // DEBUG_PRINTF(3, " %d ", all_channels[i]); } *clen = cpos & 0xfe; diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index 1669e10951..a8c4a4a229 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -40,6 +40,10 @@ static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_call static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail); static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail); + +static void PIOS_RFM22B_COM_RegisterAuxRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_RFM22B_COM_RegisterAuxTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context); + static uint32_t PIOS_RFM22B_COM_Available(uint32_t rfm22b_com_id); /* Local variables */ @@ -52,6 +56,15 @@ const struct pios_com_driver pios_rfm22b_com_driver = { .available = PIOS_RFM22B_COM_Available }; +/* Local variables */ +const struct pios_com_driver pios_rfm22b_aux_com_driver = { + .set_baud = PIOS_RFM22B_COM_ChangeBaud, + .tx_start = PIOS_RFM22B_COM_TxStart, + .rx_start = PIOS_RFM22B_COM_RxStart, + .bind_tx_cb = PIOS_RFM22B_COM_RegisterAuxTxCallback, + .bind_rx_cb = PIOS_RFM22B_COM_RegisterAuxRxCallback, + .available = PIOS_RFM22B_COM_Available +}; /** * Changes the baud rate of the RFM22B peripheral without re-initialising. * @@ -129,6 +142,51 @@ static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_call rfm22b_dev->tx_out_cb = tx_out_cb; } +/** + * Register the callback to pass received data to + * + * @param[in] rfm22b_dev The device ID. + * @param[in] rx_in_cb The Rx callback function. + * @param[in] context The callback context. + */ +static void PIOS_RFM22B_COM_RegisterAuxRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + rfm22b_dev->aux_rx_in_context = context; + rfm22b_dev->aux_rx_in_cb = rx_in_cb; +} + +/** + * Register the callback to get data from. + * + * @param[in] rfm22b_dev The device ID. + * @param[in] rx_in_cb The Tx callback function. + * @param[in] context The callback context. + */ +static void PIOS_RFM22B_COM_RegisterAuxTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + rfm22b_dev->aux_tx_out_context = context; + rfm22b_dev->aux_tx_out_cb = tx_out_cb; +} /** * See if the COM port is alive * diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index e354a5c5f7..44c3235f7a 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" @@ -77,9 +81,8 @@ struct pios_sbus_state { #define SBUS_FL_WEIGHTED_AVE 26 struct pios_sbus_dev { - enum pios_sbus_dev_magic magic; - const struct pios_sbus_cfg *cfg; - struct pios_sbus_state state; + enum pios_sbus_dev_magic magic; + struct pios_sbus_state state; }; /* Allocate S.Bus device descriptor */ @@ -125,6 +128,7 @@ static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state) { for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { state->channel_data[i] = PIOS_RCVR_TIMEOUT; + state->quality = 0.0f; } } @@ -148,7 +152,6 @@ int32_t PIOS_SBus_Init(uint32_t *sbus_id, uint32_t lower_id) { PIOS_DEBUG_Assert(sbus_id); - PIOS_DEBUG_Assert(cfg); PIOS_DEBUG_Assert(driver); struct pios_sbus_dev *sbus_dev; @@ -158,20 +161,26 @@ int32_t PIOS_SBus_Init(uint32_t *sbus_id, goto out_fail; } - /* Bind the configuration to the device instance */ - sbus_dev->cfg = cfg; - PIOS_SBus_ResetState(&(sbus_dev->state)); *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_Parity_Even, PIOS_COM_StopBits_2, PIOS_SBUS_BAUD_RATE); + } + + /* 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); @@ -302,6 +311,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) } #ifndef SBUS_GOOD_FRAME_COUNT /* Present quality as a weighted average of good frames */ + // TODO: Refactor quality computation, give 4% (quality_trend / SBUS_FL_WEIGHTED_AVE) at minimum state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) + quality_trend) / SBUS_FL_WEIGHTED_AVE; #endif /* SBUS_GOOD_FRAME_COUNT */ diff --git a/flight/pios/common/pios_servo.c b/flight/pios/common/pios_servo.c new file mode 100644 index 0000000000..1fd3bc5d86 --- /dev/null +++ b/flight/pios/common/pios_servo.c @@ -0,0 +1,614 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SERVO RC Servo Functions + * @brief Code to do set RC servo output + * @{ + * + * @file pios_servo.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief RC Servo routines (STM32 dependent) + * @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 + */ +/* + * DShot: Tribute belongs to dRonin, http://dRonin.org/ for sparking the idea of + * using gpio bitbang as more general solution over using timer dma. + */ + +#include "pios.h" + +#ifdef PIOS_INCLUDE_SERVO + +#include "pios_servo_priv.h" +#include "pios_tim_priv.h" + +/* Private Function Prototypes */ + +#define PIOS_SERVO_GPIO_BANKS 3 + +static const struct pios_servo_cfg *servo_cfg; + +static volatile uint32_t *pios_servo_bsrr[PIOS_SERVO_GPIO_BANKS]; // GPIO banks + +struct pios_servo_bank { + enum pios_servo_bank_mode mode; + uint16_t next_update; + uint16_t max_pulse; + TIM_TypeDef *timer; +}; + +struct pios_servo_pin { + struct pios_servo_bank *bank; + uint8_t bank_nr; + uint8_t gpio_bank; + uint16_t value; +}; + + +static struct pios_servo_bank pios_servo_banks[PIOS_SERVO_BANKS]; +static struct pios_servo_pin *pios_servo_pins; + + +// Dshot timing +static uint32_t pios_dshot_t0h_raw; +static uint32_t pios_dshot_t1h_raw; +static uint32_t pios_dshot_t_raw; + +static bool pios_servo_enabled = false; +static uint32_t pios_servo_active = 0; // No active outputs by default + +#define PIOS_SERVO_TIMER_CLOCK 1000000 +#define PIOS_SERVO_SAFE_MARGIN 50 + + +#define DSHOT_TIMING_ADJUST 8 +#define DSHOT_T0H_DIV 2666 +#define DSHOT_T1H_DIV 1333 +#define DSHOT_NUM_BITS 16 + +extern void PIOS_Servo_SetActive(uint32_t active) +{ + bool enabled = pios_servo_enabled; + + if (enabled) { + PIOS_Servo_Disable(); + } + + pios_servo_active = active; + + if (enabled) { + PIOS_Servo_Enable(); + } +} + +extern void PIOS_Servo_Disable() +{ + if (!servo_cfg) { + return; + } + pios_servo_enabled = false; + + /* NOTE: Following will stop pulses and force low level on output pins. + * this is ok with ESC and servos, but brushed motors could be in trouble + * if using inverted setup */ + + for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { + if (!(pios_servo_active & (1L << i))) { // This output is not active + continue; + } + + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; + + GPIO_InitTypeDef init = chan->pin.init; + +#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE) || defined(STM32F3) + init.GPIO_Mode = GPIO_Mode_OUT; +#elif defined(STM32F10X_MD) + init.GPIO_Mode = GPIO_Mode_Out_PP; +#else +#error Unsupported MCU +#endif + GPIO_ResetBits(chan->pin.gpio, chan->pin.init.GPIO_Pin); + + GPIO_Init(chan->pin.gpio, &init); + } +} + +static void PIOS_Servo_SetupBank(uint8_t bank_nr) +{ + struct pios_servo_bank *bank = &pios_servo_banks[bank_nr]; + + if (!bank->timer) { + return; + } + + // Setup the timer accordingly + switch (bank->mode) { + case PIOS_SERVO_BANK_MODE_PWM: + case PIOS_SERVO_BANK_MODE_SINGLE_PULSE: + TIM_ARRPreloadConfig(bank->timer, ENABLE); + TIM_CtrlPWMOutputs(bank->timer, ENABLE); + TIM_SelectOnePulseMode(bank->timer, TIM_OPMode_Repetitive); + TIM_Cmd(bank->timer, ENABLE); + break; + default:; + // do not manage timers otherwise + } + + // Setup GPIO/AF + for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; + + if (chan->timer != bank->timer) { // Not interested in this bank + continue; + } + + if (!(pios_servo_active & (1L << i))) { // This output is not active + continue; + } + + GPIO_InitTypeDef init = chan->pin.init; + + switch (bank->mode) { + case PIOS_SERVO_BANK_MODE_PWM: + case PIOS_SERVO_BANK_MODE_SINGLE_PULSE: +#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE) || defined(STM32F3) + GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap); +#elif defined(STM32F10X_MD) + init.GPIO_Mode = GPIO_Mode_AF_PP; + if (chan->remap) { + GPIO_PinRemapConfig(chan->remap, ENABLE); + } +#else +#error Unsupported MCU +#endif + GPIO_Init(chan->pin.gpio, &init); + + /* Set up for output compare function */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(chan->timer, (TIM_OCInitTypeDef *)&servo_cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(chan->timer, (TIM_OCInitTypeDef *)&servo_cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(chan->timer, (TIM_OCInitTypeDef *)&servo_cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(chan->timer, (TIM_OCInitTypeDef *)&servo_cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); + break; + } + break; + + case PIOS_SERVO_BANK_MODE_DSHOT: + { +#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE) || defined(STM32F3) + init.GPIO_Mode = GPIO_Mode_OUT; +#elif defined(STM32F10X_MD) + init.GPIO_Mode = GPIO_Mode_Out_PP; +#else +#error Unsupported MCU +#endif + GPIO_ResetBits(chan->pin.gpio, chan->pin.init.GPIO_Pin); + + GPIO_Init(chan->pin.gpio, &init); + } + break; + + default:; + } + } +} + +extern void PIOS_Servo_Enable() +{ + if (!servo_cfg) { + return; + } + + for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) { + PIOS_Servo_SetupBank(i); + } + + pios_servo_enabled = true; +} + +void PIOS_Servo_DSHot_Rate(uint32_t rate_in_khz) +{ + if (rate_in_khz < 150) { + rate_in_khz = 150; + } + + uint32_t raw_hz = PIOS_DELAY_GetRawHz(); + + uint32_t tmp = raw_hz / rate_in_khz; + + pios_dshot_t0h_raw = (tmp / DSHOT_T0H_DIV) - DSHOT_TIMING_ADJUST; + pios_dshot_t1h_raw = (tmp / DSHOT_T1H_DIV) - DSHOT_TIMING_ADJUST; + pios_dshot_t_raw = (tmp / 1000) - DSHOT_TIMING_ADJUST; +} + +/** + * Initialise Servos + */ +int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) +{ + /* Store away the requested configuration */ + servo_cfg = cfg; + + pios_servo_pins = pios_malloc(sizeof(*pios_servo_pins) * cfg->num_channels); + PIOS_Assert(pios_servo_pins); + + memset(pios_servo_pins, 0, sizeof(*pios_servo_pins) * cfg->num_channels); + + /* set default dshot timing */ + PIOS_Servo_DSHot_Rate(300); + + + uint8_t timer_bank = 0; + uint8_t gpio_bank = 0; + + for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; + bool new = true; + /* See if any previous channels use that same timer */ + for (uint8_t j = 0; (j < i) && new; j++) { + new &= chan->timer != servo_cfg->channels[j].timer; + } + + if (new) { + PIOS_Assert(timer_bank < PIOS_SERVO_BANKS); + struct pios_servo_bank *bank = &pios_servo_banks[timer_bank]; + + + for (uint8_t j = i; j < servo_cfg->num_channels; j++) { + if (servo_cfg->channels[j].timer == chan->timer) { + pios_servo_pins[j].bank = bank; + pios_servo_pins[j].bank_nr = timer_bank; + } + } + + bank->timer = chan->timer; + bank->mode = PIOS_SERVO_BANK_MODE_NONE; + + TIM_Cmd(chan->timer, DISABLE); + + timer_bank++; + } + + // now map gpio banks + new = true; + for (uint8_t j = 0; (j < i) && new; j++) { + new &= chan->pin.gpio != servo_cfg->channels[j].pin.gpio; + } + + if (new) { + PIOS_Assert(gpio_bank < PIOS_SERVO_GPIO_BANKS); + + for (uint8_t j = i; j < servo_cfg->num_channels; j++) { + if (servo_cfg->channels[j].pin.gpio == chan->pin.gpio) { + pios_servo_pins[j].gpio_bank = gpio_bank; + } + } +#if defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE) + pios_servo_bsrr[gpio_bank] = (uint32_t *)&chan->pin.gpio->BSRRL; +#else + pios_servo_bsrr[gpio_bank] = &chan->pin.gpio->BSRR; +#endif + + ++gpio_bank; + } + } + + static uint32_t dummy_bsrr; + + for (int i = gpio_bank; i < PIOS_SERVO_GPIO_BANKS; ++i) { + pios_servo_bsrr[gpio_bank] = &dummy_bsrr; + } + + PIOS_Servo_Enable(); + + return 0; +} + +void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode) +{ + PIOS_Assert(bank < PIOS_SERVO_BANKS); + pios_servo_banks[bank].mode = mode; + + if (!pios_servo_enabled) { + return; + } + + PIOS_Servo_SetupBank(bank); +} + +static void PIOS_Servo_DShot_Update() +{ + uint32_t next; + uint32_t data[PIOS_SERVO_GPIO_BANKS]; + uint16_t pins[PIOS_SERVO_GPIO_BANKS]; + uint16_t buffer[DSHOT_NUM_BITS][PIOS_SERVO_GPIO_BANKS]; + + for (uint8_t i = 0; i < PIOS_SERVO_GPIO_BANKS; ++i) { + pins[i] = 0; + } + + bool has_dshot = false; + + memset(buffer, 0, sizeof(buffer)); + + for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { + struct pios_servo_pin *pin = &pios_servo_pins[i]; + + if (pin->bank->mode != PIOS_SERVO_BANK_MODE_DSHOT) { + continue; + } + + if (!(pios_servo_active & (1L << i))) { // This output is not active + continue; + } + + has_dshot = true; + + uint16_t payload = pin->value; + if (payload > 2047) { + payload = 2047; + } + + payload <<= 5; + + payload |= ((payload >> 4) & 0xf) ^ + ((payload >> 8) & 0xf) ^ + ((payload >> 12) & 0xf); + + uint16_t gpio_pin = servo_cfg->channels[i].pin.init.GPIO_Pin; + + for (int j = 0; j < DSHOT_NUM_BITS; ++j) { + if (!(payload & 0x8000)) { + buffer[j][pin->gpio_bank] |= gpio_pin; + } + payload <<= 1; + } + + pins[pin->gpio_bank] |= gpio_pin; + + pin->value = 0; + } + + if (!has_dshot) { + return; + } + + PIOS_IRQ_Disable(); + + uint32_t start = PIOS_DELAY_GetRaw(); + + for (int i = 0; i < DSHOT_NUM_BITS; ++i) { + // single bit: + + COMPILER_BARRIER(); + // 1. write 3x BSRR to set gpio high + for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) { + *(pios_servo_bsrr[j]) = (uint32_t)pins[j]; + } + + // Prep data + for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) { + data[j] = buffer[i][j] << 16; + } + + // 2. wait until T0H, write 3x BSRR to clear whatever bits are set to 0 + next = start + pios_dshot_t0h_raw; + while ((next - PIOS_DELAY_GetRaw()) < pios_dshot_t0h_raw) { + ; + } + + COMPILER_BARRIER(); + for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) { + *(pios_servo_bsrr[j]) = data[j]; + } + + // Prep data + for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) { + data[j] = (uint32_t)pins[j] << 16; + } + + // 3. wait until T1H, then write 3x BSRR to set all to low + next = start + pios_dshot_t1h_raw; + while ((next - PIOS_DELAY_GetRaw()) < pios_dshot_t1h_raw) { + ; + } + + COMPILER_BARRIER(); + for (int j = 0; j < PIOS_SERVO_GPIO_BANKS; ++j) { + *(pios_servo_bsrr[j]) = data[j]; + } + + // 4. wait until Tend + start += pios_dshot_t_raw; + while ((start - PIOS_DELAY_GetRaw()) < pios_dshot_t_raw) { + ; + } + } + + PIOS_IRQ_Enable(); +} + + +void PIOS_Servo_Update() +{ + if (!pios_servo_enabled) { + return; + } + + for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) { + struct pios_servo_bank *bank = &pios_servo_banks[i]; + if (bank->timer && (bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE)) { + // a pulse to be generated is longer than cycle period. skip this update. + if (TIM_GetCounter((TIM_TypeDef *)bank->timer) > (uint32_t)(bank->next_update + PIOS_SERVO_SAFE_MARGIN)) { + TIM_GenerateEvent((TIM_TypeDef *)bank->timer, TIM_EventSource_Update); + bank->next_update = bank->max_pulse; + } + } + bank->max_pulse = 0; + } + + for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { + if ((pios_servo_active & (1L << i)) + && (pios_servo_pins[i].bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE)) { + /* Update the position */ + const struct pios_tim_channel *chan = &servo_cfg->channels[i]; + + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, 0); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, 0); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, 0); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, 0); + break; + } + } + } + + PIOS_Servo_DShot_Update(); +} +/** + * Set the servo update rate (Max 500Hz) + * \param[in] array of rates in Hz + * \param[in] array of timer clocks in Hz + * \param[in] maximum number of banks + */ +void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks) +{ + PIOS_Assert(banks <= PIOS_SERVO_BANKS); + if (!servo_cfg) { + return; + } + + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + + for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) { + const TIM_TypeDef *timer = pios_servo_banks[i].timer; + if (timer) { + uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK; + if (clock[i]) { + new_clock = clock[i]; + } + + uint32_t timer_clock; + + // Choose the correct prescaler value for the APB the timer is attached + +#if defined(STM32F10X_MD) || defined(STM32F3) + // F1 & F3 have both timer clock domains running at master clock speed + timer_clock = PIOS_MASTER_CLOCK; +#elif defined(STM32F40_41xxx) || defined(STM32F446xx) || defined(STM32F411xE) + if (timer == TIM1 || timer == TIM8 || timer == TIM9 || timer == TIM10 || timer == TIM11) { + timer_clock = PIOS_PERIPHERAL_APB2_CLOCK; + } else { + timer_clock = PIOS_PERIPHERAL_APB1_CLOCK; + } +#else +#error Unsupported MCU +#endif + TIM_TimeBaseStructure.TIM_Prescaler = (timer_clock / new_clock) - 1; + TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1); + TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure); + } + } +} + +/** + * Set servo position + * \param[in] Servo Servo number (0-7) + * \param[in] Position Servo position in microseconds + */ +void PIOS_Servo_Set(uint8_t servo, uint16_t position) +{ + /* Make sure servo exists */ + if (!pios_servo_enabled || !servo_cfg || servo >= servo_cfg->num_channels) { + return; + } + + + /* Update the position */ + + pios_servo_pins[servo].value = position; + + const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; + struct pios_servo_bank *bank = pios_servo_pins[servo].bank; + + if ((bank->mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) || (bank->mode == PIOS_SERVO_BANK_MODE_PWM)) { + uint16_t val = position; + uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps + if (val > (chan->timer->ARR - margin)) { + val = chan->timer->ARR - margin; + } + + if (bank->max_pulse < val) { + bank->max_pulse = val; + } + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, val); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, val); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, val); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, val); + break; + } + } +} + +uint8_t PIOS_Servo_GetPinBank(uint8_t pin) +{ + if (pin < servo_cfg->num_channels) { + return pios_servo_pins[pin].bank_nr; + } else { + return 0; + } +} + +const struct pios_servo_cfg *PIOS_Servo_GetConfig() +{ + return servo_cfg; +} + +#endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/common/pios_srxl.c b/flight/pios/common/pios_srxl.c index 711679829c..4dcd69d85c 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_Parity_No, PIOS_COM_StopBits_1, 115200); + + /* 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_ubx_dcc.c b/flight/pios/common/pios_ubx_dcc.c index fce4e0fff3..2b9a974717 100644 --- a/flight/pios/common/pios_ubx_dcc.c +++ b/flight/pios/common/pios_ubx_dcc.c @@ -27,7 +27,7 @@ #include #include #include -#define GPS_I2C_ADDRESS (0x42 << 1) +#define GPS_I2C_ADDRESS 0x42 #define GPS_I2C_STREAM_REG 0xFF #define GPS_I2C_STREAM_SIZE_HIGH_REG 0xFD #define GPS_I2C_STREAM_SIZE_LOW_REG 0xFE diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index e68e5ab680..fe2a606980 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -317,6 +317,27 @@ 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, +}; +#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) { 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 89cf9ebe41..ce45013882 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_bmp280.h b/flight/pios/inc/pios_bmp280.h new file mode 100644 index 0000000000..d559f138b8 --- /dev/null +++ b/flight/pios/inc/pios_bmp280.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BMP280 BMP280 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_bmp280.h + * @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2017. + * @brief BMP280 functions header. + * @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_BMP280_H +#define PIOS_BMP280_H +#include + +#define BMP280_OVERSAMP_SKIPPED (0x00) +#define BMP280_OVERSAMP_1X (0x01) +#define BMP280_OVERSAMP_2X (0x02) +#define BMP280_OVERSAMP_4X (0x03) +#define BMP280_OVERSAMP_8X (0x04) +#define BMP280_OVERSAMP_16X (0x05) + +#define BMP280_PRESSURE_OSR(x) (x << 2) +#define BMP280_TEMPERATURE_OSR(x) (x << 5) + +#define BMP280_STANDARD_RESOLUTION (BMP280_PRESSURE_OSR(BMP280_OVERSAMP_4X) | BMP280_TEMPERATURE_OSR(BMP280_OVERSAMP_1X)) +#define BMP280_HIGH_RESOLUTION (BMP280_PRESSURE_OSR(BMP280_OVERSAMP_8X) | BMP280_TEMPERATURE_OSR(BMP280_OVERSAMP_1X)) +#define BMP280_ULTRA_HIGH_RESOLUTION (BMP280_PRESSURE_OSR(BMP280_OVERSAMP_16X) | BMP280_TEMPERATURE_OSR(BMP280_OVERSAMP_2X)) + +struct pios_bmp280_cfg { + uint8_t oversampling; +}; + +/* Public Functions */ +extern void PIOS_BMP280_Init(const struct pios_bmp280_cfg *cfg, uint32_t i2c_device); + +#endif /* PIOS_BMP280_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_board_hw.h b/flight/pios/inc/pios_board_hw.h new file mode 100644 index 0000000000..591f54489a --- /dev/null +++ b/flight/pios/inc/pios_board_hw.h @@ -0,0 +1,74 @@ +/** + ****************************************************************************** + * + * @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_GetRfm22bCfg(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_MS56XX +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(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 +#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 +#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 +#ifdef PIOS_INCLUDE_BMP280 +const struct pios_bmp280_cfg *PIOS_BOARD_HW_DEFS_GetBMP280Cfg(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 0000000000..1f8500fd94 --- /dev/null +++ b/flight/pios/inc/pios_board_io.h @@ -0,0 +1,275 @@ +/** + ****************************************************************************** + * + * @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 + +#ifdef PIOS_INCLUDE_USART +#include +#endif + +#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 256 +#endif +#ifndef PIOS_COM_TELEM_RF_TX_BUF_LEN +# define PIOS_COM_TELEM_RF_TX_BUF_LEN 256 +#endif + +/* RFM22B telemetry */ +#ifdef PIOS_INCLUDE_RFM22B +extern uint32_t pios_rfm22b_id; /* RFM22B handle */ +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 256 +# endif +# ifndef PIOS_COM_PRI_RADIO_TX_BUF_LEN +# define PIOS_COM_PRI_RADIO_TX_BUF_LEN 256 +# endif +# ifndef PIOS_COM_AUX_RADIO_RX_BUF_LEN +# define PIOS_COM_AUX_RADIO_RX_BUF_LEN 256 +# endif +# ifndef PIOS_COM_AUX_RADIO_TX_BUF_LEN +# define PIOS_COM_AUX_RADIO_TX_BUF_LEN 256 +# endif +#endif + +#ifdef PIOS_INCLUDE_OPENLRS +extern uint32_t pios_openlrs_id; /* openlrs handle */ +#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 + +/* 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 + +/* Frsky Sensorhub */ +extern uint32_t pios_com_frsky_sensorhub_id; +#define PIOS_COM_FRSKY_SENSORHUB (pios_com_frsky_sensorhub_id) +#ifndef PIOS_COM_FRSKY_SENSORHUB_TX_BUF_LEN +# define PIOS_COM_FRSKY_SENSORHUB_TX_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 = 0, + 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, normal vs not-inverted from HwSettings.SBusMode */ + PIOS_BOARD_IO_UART_SBUS_NORMAL, /* helper only */ + PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED, /* helper only */ + 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_FRSKY_SPORT_TELEMETRY, /* com */ + PIOS_BOARD_IO_UART_HOTT_BRIDGE, /* com */ + PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, /* com */ +} PIOS_BOARD_IO_UART_Function; + + +#ifdef PIOS_INCLUDE_USB +# ifndef BOOTLOADER +# include "uavobjectmanager.h" +# include "hwsettings.h" + +void PIOS_BOARD_IO_Configure_USB(); + +typedef enum { + 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 = 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 // ifndef BOOTLOADER +#endif // ifdef PIOS_INCLUDE_USB +#ifdef PIOS_INCLUDE_PWM +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_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(); +void PIOS_BOARD_IO_Configure_RadioAuxStream(HwSettingsRadioAuxStreamOptions radioaux); /* not for OPLM */ +#endif + +#ifdef PIOS_INCLUDE_GCSRCVR +void PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR +void PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + +#endif /* PIOS_BOARD_IO_H */ diff --git a/flight/pios/inc/pios_board_sensors.h b/flight/pios/inc/pios_board_sensors.h new file mode 100644 index 0000000000..71a2920624 --- /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/inc/pios_com.h b/flight/pios/inc/pios_com.h index 53b2ed0794..3e33775da0 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -7,7 +7,8 @@ * @{ * * @file pios_com.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Thorsten Klose (tk@midibox.org) * @brief COM layer functions header * @see The GNU Public License (GPL) Version 3 @@ -38,10 +39,32 @@ typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken); typedef void (*pios_com_callback_ctrl_line)(uint32_t context, uint32_t mask, uint32_t state); typedef void (*pios_com_callback_baud_rate)(uint32_t context, uint32_t baud); +typedef void (*pios_com_callback_available)(uint32_t context, uint32_t available); + +enum PIOS_COM_Word_Length { + PIOS_COM_Word_length_Unchanged = 0, + PIOS_COM_Word_length_8b, + PIOS_COM_Word_length_9b, +}; + +enum PIOS_COM_StopBits { + PIOS_COM_StopBits_Unchanged = 0, + PIOS_COM_StopBits_0_5, + PIOS_COM_StopBits_1, + PIOS_COM_StopBits_1_5, + PIOS_COM_StopBits_2, +}; + +enum PIOS_COM_Parity { + PIOS_COM_Parity_Unchanged = 0, + PIOS_COM_Parity_No, + PIOS_COM_Parity_Even, + PIOS_COM_Parity_Odd, +}; struct pios_com_driver { - void (*init)(uint32_t id); void (*set_baud)(uint32_t id, uint32_t baud); + 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); @@ -50,6 +73,8 @@ struct pios_com_driver { void (*bind_ctrl_line_cb)(uint32_t id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); 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 */ @@ -59,6 +84,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_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); @@ -72,11 +98,48 @@ extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const ch extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms); extern uint32_t PIOS_COM_Available(uint32_t com_id); +extern int32_t PIOS_COM_RegisterAvailableCallback(uint32_t com_id, pios_com_callback_available, uint32_t context); + +/* Event driven asynchronous API */ +extern int32_t PIOS_COM_ASYNC_TxStart(uint32_t id, uint16_t tx_bytes_avail); +extern int32_t PIOS_COM_ASYNC_RxStart(uint32_t id, uint16_t rx_bytes_avail); +extern int32_t PIOS_COM_ASYNC_RegisterRxCallback(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); +extern int32_t PIOS_COM_ASYNC_RegisterTxCallback(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + +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) + +/* 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, +}; -#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_IOCTL_ENOSYS (-1) /* Function not implemented */ #endif /* PIOS_COM_H */ diff --git a/flight/pios/inc/pios_debug.h b/flight/pios/inc/pios_debug.h index 4d481dfbd6..da79b05f9e 100644 --- a/flight/pios/inc/pios_debug.h +++ b/flight/pios/inc/pios_debug.h @@ -32,17 +32,21 @@ #define PIOS_DEBUG_H #ifdef PIOS_INCLUDE_DEBUG_CONSOLE -#ifndef DEBUG_LEVEL -#define DEBUG_LEVEL 0 -#endif -#define DEBUG_PRINTF(level, ...) \ +# ifndef PIOS_COM_DEBUG +extern uint32_t pios_com_debug_id; +# define PIOS_COM_DEBUG (pios_com_debug_id) +# endif +# ifndef DEBUG_LEVEL +# define DEBUG_LEVEL 0 +# endif +# define DEBUG_PRINTF(level, ...) \ { \ if ((level <= DEBUG_LEVEL) && (PIOS_COM_DEBUG > 0)) { \ - PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, __VA_ARGS__); \ + PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, __VA_ARGS__); \ } \ } #else -#define DEBUG_PRINTF(level, ...) +# define DEBUG_PRINTF(level, ...) #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ extern const char *PIOS_DEBUG_AssertMsg; diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 63546d75d6..19d6b42600 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -32,13 +32,18 @@ #ifndef PIOS_DELAY_H #define PIOS_DELAY_H +#ifdef USE_SIM_POSIX +extern uint32_t PIOS_DELAY_GetRaw(); +#else +#include "pios_delay_raw.h" +#endif + /* Public Functions */ extern int32_t PIOS_DELAY_Init(void); extern int32_t PIOS_DELAY_WaituS(uint32_t uS); extern int32_t PIOS_DELAY_WaitmS(uint32_t mS); extern uint32_t PIOS_DELAY_GetuS(); extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t); -extern uint32_t PIOS_DELAY_GetRaw(); extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); extern uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later); diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index 40607e93ee..6e7ce1106b 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_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index eae55063d9..bc932a2098 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -72,6 +72,15 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, + { // 25q64 + .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, + .expect_memorytype = 0x40, + .expect_capacity = 0x17, + .sector_erase = 0x20, + .chip_erase = 0x60, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, + }, { // 25q512 .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, .expect_memorytype = 0xBA, diff --git a/flight/pios/inc/pios_helpers.h b/flight/pios/inc/pios_helpers.h index ba902ada4f..3f626f29f2 100644 --- a/flight/pios/inc/pios_helpers.h +++ b/flight/pios/inc/pios_helpers.h @@ -34,14 +34,31 @@ * @return number of elements in x. * */ -#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0])) +#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0])) +/** + * @brief preprocessor magic + * + */ + +#define _CONCAT5(a, b, c, d, e) a##b##c##d##e +#define _EVAL5(a, b, c, d, e) _CONCAT5(a, b, c, d, e) + +#define _CONCAT4(a, b, c, d) a##b##c##d +#define _EVAL4(a, b, c, d) _CONCAT4(a, b, c, d) + +#define _CONCAT3(a, b, c) a##b##c +#define _EVAL3(a, b, c) _CONCAT3(a, b, c) + +#define _CONCAT2(a, b) a##b +#define _EVAL2(a, b) _CONCAT4(a, b) + /** * @brief Compiler barrier: Disables compiler load/store reordering across the barrier * */ -#define COMPILER_BARRIER() asm volatile ("" ::: "memory") +#define COMPILER_BARRIER() asm volatile ("" ::: "memory") // Memory barriers: // Note that on single core Cortex M3 & M4, the is generally no need to use a processor memory barrier instruction such as DMB. diff --git a/flight/pios/inc/pios_i2c.h b/flight/pios/inc/pios_i2c.h index 0c4c714318..2f5a99e12a 100644 --- a/flight/pios/inc/pios_i2c.h +++ b/flight/pios/inc/pios_i2c.h @@ -73,9 +73,23 @@ enum pios_i2c_error_count { PIOS_I2C_ERROR_COUNT_NUMELEM, }; +enum pios_i2c_transfer_result { + PIOS_I2C_TRANSFER_OK = 0, + PIOS_I2C_TRANSFER_BUSY = -2, + PIOS_I2C_TRANSFER_BUS_ERROR = -1, + PIOS_I2C_TRANSFER_NACK = -3, + PIOS_I2C_TRANSFER_TIMEOUT = -4, + PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR = -5, + PIOS_I2C_TRANSFER_DEVICE_ERROR = -6, +}; + +typedef bool (*pios_i2c_callback)(enum pios_i2c_transfer_result result); + /* Public Functions */ extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns); -extern int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback); +extern int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback); +extern int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback, bool *woken); + extern void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id); extern void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id); extern void PIOS_I2C_IRQ_Handler(uint32_t i2c_id); diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h index 8086f50743..70e0840eb0 100644 --- a/flight/pios/inc/pios_i2c_priv.h +++ b/flight/pios/inc/pios_i2c_priv.h @@ -69,10 +69,12 @@ struct pios_i2c_adapter { const struct pios_i2c_txn *active_txn; const struct pios_i2c_txn *last_txn; - void (*callback)(); + pios_i2c_callback callback; uint8_t *active_byte; uint8_t *last_byte; + + enum pios_i2c_transfer_result *transfer_result; }; int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg); diff --git a/flight/pios/inc/pios_initcall.h b/flight/pios/inc/pios_initcall.h index f98b5b628e..3425b6ec07 100644 --- a/flight/pios/inc/pios_initcall.h +++ b/flight/pios/inc/pios_initcall.h @@ -52,6 +52,10 @@ typedef struct { /* Init module section */ extern initmodule_t __module_initcall_start[], __module_initcall_end[]; extern volatile int initTaskDone; + +/* Init settings section */ +extern initcall_t __settings_initcall_start[], __settings_initcall_end[]; + #ifdef USE_SIM_POSIX extern void InitModules(); @@ -60,6 +64,8 @@ extern int32_t SystemModInitialize(void); #define MODULE_INITCALL(ifn, sfn) +#define SETTINGS_INITCALL(fn) + #define MODULE_TASKCREATE_ALL \ { \ /* Start all module threads */ \ @@ -92,8 +98,15 @@ extern int32_t SystemModInitialize(void); static initmodule_t __initcall_##ifn __attribute__((__used__)) \ __attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn } +#define __define_settings_initcall(level, fn) \ + static initcall_t __initcall_##fn __attribute__((__used__)) \ + __attribute__((__section__(".initcall" level ".init"))) = fn + + #define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) +#define SETTINGS_INITCALL(fn) __define_settings_initcall("settings", fn) + #define MODULE_INITIALISE_ALL \ { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ if (fn->fn_minit) { \ @@ -111,6 +124,14 @@ extern int32_t SystemModInitialize(void); } \ } +#define SETTINGS_INITIALISE_ALL \ + { for (const initcall_t *fn = __settings_initcall_start; fn < __settings_initcall_end; fn++) { \ + if (*fn) { \ + (*fn)(); \ + } \ + } \ + } + #endif /* USE_SIM_POSIX */ #endif /* PIOS_INITCALL_H */ diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 8cc57cad53..8c89b13c5e 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -79,7 +79,7 @@ #define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define IS_REAL(f) (!isnan(f) && !isinf(f)) +#define IS_REAL(f) (isfinite(f)) // Bitfield access #define IS_SET(field, mask) (((field) & (mask)) == (mask)) diff --git a/flight/pios/inc/pios_mpu6000.h b/flight/pios/inc/pios_mpu6000.h index fc95380521..ac5fa2fae4 100644 --- a/flight/pios/inc/pios_mpu6000.h +++ b/flight/pios/inc/pios_mpu6000.h @@ -125,15 +125,27 @@ enum pios_mpu6000_accel_range { PIOS_MPU6000_ACCEL_16G = 0x18 }; -enum pios_mpu6000_orientation { // clockwise rotation from board forward - PIOS_MPU6000_TOP_0DEG = 0x00, - PIOS_MPU6000_TOP_90DEG = 0x01, - PIOS_MPU6000_TOP_180DEG = 0x02, - PIOS_MPU6000_TOP_270DEG = 0x03 + +#define PIOS_MPU6000_LOCATION_TOP 0x00 +#define PIOS_MPU6000_LOCATION_BOTTOM 0x10 + +#define PIOS_MPU6000_LOCATION_MASK 0xf0 + +enum pios_mpu6000_orientation { // clockwise rotation from board forward, when looking at sensor itself, which can be also on the bottom side + PIOS_MPU6000_TOP_0DEG = 0 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_TOP_90DEG = 1 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_TOP_180DEG = 2 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_TOP_270DEG = 3 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_BOTTOM_0DEG = 4 | PIOS_MPU6000_LOCATION_BOTTOM, + PIOS_MPU6000_BOTTOM_90DEG = 5 | PIOS_MPU6000_LOCATION_BOTTOM, + PIOS_MPU6000_BOTTOM_180DEG = 6 | PIOS_MPU6000_LOCATION_BOTTOM, + PIOS_MPU6000_BOTTOM_270DEG = 7 | PIOS_MPU6000_LOCATION_BOTTOM, }; + struct pios_mpu6000_cfg { const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ + uint8_t i2c_addr; uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */ @@ -148,13 +160,15 @@ struct pios_mpu6000_cfg { enum pios_mpu6000_range gyro_range; enum pios_mpu6000_filter filter; enum pios_mpu6000_orientation orientation; +#ifdef PIOS_INCLUDE_SPI SPIPrescalerTypeDef fast_prescaler; SPIPrescalerTypeDef std_prescaler; +#endif /* PIOS_INCLUDE_SPI */ uint8_t max_downsample; }; /* Public Functions */ -extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg); +extern int32_t PIOS_MPU6000_Init(uint32_t port_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg); extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting); extern int32_t PIOS_MPU6000_ReadID(); extern void PIOS_MPU6000_Register(); diff --git a/flight/pios/inc/pios_mpu6000_config.h b/flight/pios/inc/pios_mpu6000_config.h index 0c9e0ffb1d..96270847a5 100644 --- a/flight/pios/inc/pios_mpu6000_config.h +++ b/flight/pios/inc/pios_mpu6000_config.h @@ -61,8 +61,8 @@ */ int32_t PIOS_MPU6000_CONFIG_Configure() { - MPUGyroAccelSettingsInitialize(); MPUGyroAccelSettingsData mpuSettings; + MPUGyroAccelSettingsGet(&mpuSettings); return PIOS_MPU6000_ConfigureRanges( PIOS_MPU6000_CONFIG_MAP_GYROSCALE(mpuSettings.GyroScale), diff --git a/flight/pios/inc/pios_mpu9250.h b/flight/pios/inc/pios_mpu9250.h index 8215bc91ba..416842f63b 100644 --- a/flight/pios/inc/pios_mpu9250.h +++ b/flight/pios/inc/pios_mpu9250.h @@ -33,6 +33,8 @@ #define PIOS_MPU9250_H #include +#ifdef PIOS_INCLUDE_MPU9250 + /* MPU9250 Addresses */ #define PIOS_MPU9250_SMPLRT_DIV_REG 0X19 #define PIOS_MPU9250_DLPF_CFG_REG 0X1A @@ -153,6 +155,7 @@ #define PIOS_MPU9250_ASAZ 0X12 /* IDs */ +#define PIOS_MPU6500_GYRO_ACC_ID 0x70 #define PIOS_MPU9250_GYRO_ACC_ID 0x71 #define PIOS_MPU9250_MAG_ID 0x48 @@ -246,6 +249,8 @@ extern bool PIOS_MPU9250_IRQHandler(void); extern const PIOS_SENSORS_Driver PIOS_MPU9250_Main_Driver; extern const PIOS_SENSORS_Driver PIOS_MPU9250_Mag_Driver; +#endif /* PIOS_INCLUDE_MPU9250 */ + #endif /* PIOS_MPU9250_H */ /** diff --git a/flight/pios/inc/pios_mpu9250_config.h b/flight/pios/inc/pios_mpu9250_config.h index fff4868cfa..ef20edd586 100644 --- a/flight/pios/inc/pios_mpu9250_config.h +++ b/flight/pios/inc/pios_mpu9250_config.h @@ -61,8 +61,8 @@ */ int32_t PIOS_MPU9250_CONFIG_Configure() { - MPUGyroAccelSettingsInitialize(); MPUGyroAccelSettingsData mpuSettings; + MPUGyroAccelSettingsGet(&mpuSettings); return PIOS_MPU9250_ConfigureRanges( PIOS_MPU9250_CONFIG_MAP_GYROSCALE(mpuSettings.GyroScale), diff --git a/flight/pios/inc/pios_ms5611.h b/flight/pios/inc/pios_ms5611.h deleted file mode 100644 index 2536af8cc5..0000000000 --- a/flight/pios/inc/pios_ms5611.h +++ /dev/null @@ -1,66 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_MS5611 MS5611 Functions - * @brief Hardware functions to deal with the altitude pressure sensor - * @{ - * - * @file pios_ms5611.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief MS5611 functions header. - * @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_MS5611_H -#define PIOS_MS5611_H -#include -/* BMP085 Addresses */ -#define MS5611_I2C_ADDR 0x77 -#define MS5611_RESET 0x1E -#define MS5611_CALIB_ADDR 0xA2 /* First sample is factory stuff */ -#define MS5611_CALIB_LEN 16 -#define MS5611_ADC_READ 0x00 -#define MS5611_PRES_ADDR 0x40 -#define MS5611_TEMP_ADDR 0x50 -#define MS5611_ADC_MSB 0xF6 - -struct pios_ms5611_cfg { - uint32_t oversampling; -}; - -enum pios_ms5611_osr { - MS5611_OSR_256 = 0, - MS5611_OSR_512 = 2, - MS5611_OSR_1024 = 4, - MS5611_OSR_2048 = 6, - MS5611_OSR_4096 = 8, -}; - -/* Public Functions */ -extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device); -extern const PIOS_SENSORS_Driver PIOS_MS5611_Driver; -void PIOS_MS5611_Register(); - -#endif /* PIOS_MS5611_H */ - -/** - * @} - * @} - */ diff --git a/flight/pios/inc/pios_ms56xx.h b/flight/pios/inc/pios_ms56xx.h new file mode 100644 index 0000000000..37d7f2becd --- /dev/null +++ b/flight/pios/inc/pios_ms56xx.h @@ -0,0 +1,72 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MS5611 MS56XX Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_ms56xx.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief MS56XX functions header. + * @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_MS56XX_H +#define PIOS_MS56XX_H + +#ifdef PIOS_INCLUDE_MS56XX + +#include + +enum pios_ms56xx_version { + MS56XX_VERSION_5607 = 0, + MS56XX_VERSION_5611 = 1, + MS56XX_VERSION_5637 = 2, +}; + +enum pios_ms56xx_osr { + MS56XX_OSR_256 = 0, + MS56XX_OSR_512 = 2, + MS56XX_OSR_1024 = 4, + MS56XX_OSR_2048 = 6, + MS56XX_OSR_4096 = 8, + // Only supported by the ms5637 + MS56XX_OSR_8192 = 10 +}; + +struct pios_ms56xx_cfg { + uint8_t address; + enum pios_ms56xx_version version; + enum pios_ms56xx_osr oversampling; +}; + +/* Public Functions */ +extern void PIOS_MS56xx_Init(const struct pios_ms56xx_cfg *cfg, int32_t i2c_device); +extern void PIOS_MS56xx_Register(); +extern bool PIOS_MS56xx_Read(float *temperature, float *pressure); + +#endif /* PIOS_INCLUDE_MS56XX */ + +#endif /* PIOS_MS56XX_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_openlrs.h b/flight/pios/inc/pios_openlrs.h index 865e81ccf8..ac9e57e6fd 100644 --- a/flight/pios/inc/pios_openlrs.h +++ b/flight/pios/inc/pios_openlrs.h @@ -39,13 +39,12 @@ struct pios_openlrs_cfg { enum gpio_direction gpio_direction; /* Definition comes from pios_rfm22b.h */ }; -typedef void (*PIOS_OpenLRS_PPMReceivedCallback)(const int16_t *channels); +typedef void (*PIOS_OpenLRS_PPMReceivedCallback)(uint32_t context, const int16_t *channels); extern int32_t PIOS_OpenLRS_Init(uint32_t *openlrs_id, uint32_t spi_id, uint32_t slave_num, const struct pios_openlrs_cfg *cfg); -extern void PIOS_OpenLRS_RegisterRcvr(uint32_t openlrs_id, uint32_t rfm22b_rcvr_id); -extern void PIOS_OpenLRS_RegisterPPMCallback(uint32_t openlrs_id, PIOS_OpenLRS_PPMReceivedCallback callback); +extern void PIOS_OpenLRS_RegisterPPMCallback(uint32_t openlrs_id, PIOS_OpenLRS_PPMReceivedCallback callback, uint32_t context); extern uint8_t PIOS_OpenLRS_RSSI_Get(void); #endif /* PIOS_OPENLRS_H */ /** diff --git a/flight/pios/inc/pios_openlrs_priv.h b/flight/pios/inc/pios_openlrs_priv.h index c4ec33a268..46458e50fc 100644 --- a/flight/pios/inc/pios_openlrs_priv.h +++ b/flight/pios/inc/pios_openlrs_priv.h @@ -155,16 +155,15 @@ struct pios_openlrs_dev { struct pios_semaphore *sema_isr; // The PPM buffer - int16_t ppm[OPENLRS_PPM_NUM_CHANNELS]; - - // RFM22B RCVR interface - uintptr_t openlrs_rcvr_id; + int16_t ppm[OPENLRS_PPM_NUM_CHANNELS]; // PPM callback. PIOS_OpenLRS_PPMReceivedCallback ppm_callback; + // PPM context + uint32_t ppm_context; // Flag to indicate if link every acquired - bool link_acquired; + bool link_acquired; // Active bound information data struct bind_data bind_data; @@ -184,7 +183,8 @@ struct pios_openlrs_dev { uint8_t rx_buf[64]; uint8_t tx_buf[9]; - int8_t rssi; + uint8_t rssi; + uint16_t link_quality; // Variables from OpenLRS for radio control uint8_t hopcount; diff --git a/flight/pios/inc/pios_openlrs_rcvr_priv.h b/flight/pios/inc/pios_openlrs_rcvr_priv.h index 2997fb1a5f..c8b49c8f6b 100644 --- a/flight/pios/inc/pios_openlrs_rcvr_priv.h +++ b/flight/pios/inc/pios_openlrs_rcvr_priv.h @@ -37,7 +37,6 @@ extern const struct pios_rcvr_driver pios_openlrs_rcvr_driver; extern int32_t PIOS_OpenLRS_Rcvr_Init(uint32_t *openlrs_rcvr_id, uintptr_t openlrs_id); -extern int32_t PIOS_OpenLRS_Rcvr_UpdateChannels(uint32_t openlrs_rcvr_id, int16_t *channels); #endif /* PIOS_OPENLRS_RCVR_PRIV_H */ diff --git a/flight/pios/inc/pios_oplinkrcvr_priv.h b/flight/pios/inc/pios_oplinkrcvr_priv.h index 84c46b0bff..c8b948ae62 100644 --- a/flight/pios/inc/pios_oplinkrcvr_priv.h +++ b/flight/pios/inc/pios_oplinkrcvr_priv.h @@ -35,7 +35,11 @@ extern const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver; +#if defined(PIOS_INCLUDE_RFM22B) +extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id, uint32_t rfm22b_id); +#else extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id); +#endif #endif /* PIOS_OPLINKRCVR_PRIV_H */ diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index 16dd460b27..d2d43533ca 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -47,7 +47,7 @@ struct pios_rfm22b_cfg { uint8_t slave_num; enum gpio_direction gpio_direction; }; -typedef void (*PPMReceivedCallback)(const int16_t *channels); +typedef void (*PPMReceivedCallback)(uint32_t context, const int16_t *channels); enum rfm22b_tx_power { RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW @@ -96,27 +96,27 @@ struct rfm22b_stats { uint8_t timeouts; uint8_t link_quality; int8_t rssi; - int8_t afc_correction; + int32_t afc_correction; uint8_t link_state; }; /* Public Functions */ -extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); +extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg, OPLinkSettingsRFBandOptions band); extern void PIOS_RFM22B_Reinit(uint32_t rfb22b_id); extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr); extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, bool coordinator, bool ppm_mode, bool ppm_only); +extern void PIOS_RFM22B_SetXtalCap(uint32_t rfm22b_id, uint8_t xtal_cap); extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id); extern void PIOS_RFM22B_SetDeviceID(uint32_t rfm22b_id, uint32_t device_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); -extern uint8_t PIOS_RFM22B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id); extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id); extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p); extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id); -extern void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb); +extern void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb, uint32_t cb_context); extern void PIOS_RFM22B_PPMSet(uint32_t rfm22b_id, int16_t *channels, uint8_t nchan); extern void PIOS_RFM22B_PPMGet(uint32_t rfm22b_id, int16_t *channels, uint8_t nchan); diff --git a/flight/pios/inc/pios_rfm22b_com.h b/flight/pios/inc/pios_rfm22b_com.h index f461809a17..537206371f 100644 --- a/flight/pios/inc/pios_rfm22b_com.h +++ b/flight/pios/inc/pios_rfm22b_com.h @@ -28,12 +28,13 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef PIOS_RFM22B_H -#define PIOS_RFM22B_H +#ifndef PIOS_RFM22B_COM_H +#define PIOS_RFM22B_COM_H extern const struct pios_com_driver pios_rfm22b_com_driver; +extern const struct pios_com_driver pios_rfm22b_aux_com_driver; -#endif /* PIOS_RFM22B_H */ +#endif /* PIOS_RFM22B_COM_H */ /** * @} diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 953c90fc0b..52348c1122 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -200,18 +200,24 @@ struct pios_rfm22b_dev { // The task handle xTaskHandle taskHandle; - // The potential paired statistics - rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; - // ISR pending semaphore xSemaphoreHandle isrPending; - // The COM callback functions. + // The main COM callback functions. pios_com_callback rx_in_cb; uint32_t rx_in_context; pios_com_callback tx_out_cb; uint32_t tx_out_context; + // The Aux COM callback functions. + pios_com_callback aux_rx_in_cb; + uint32_t aux_rx_in_context; + pios_com_callback aux_tx_out_cb; + uint32_t aux_tx_out_context; + + // Send next packet on primary or aux channel? + bool last_stream_sent; + // the transmit power to use for data transmissions uint8_t tx_power; @@ -267,6 +273,8 @@ struct pios_rfm22b_dev { int16_t ppm[RFM22B_PPM_NUM_CHANNELS]; // The PPM packet received callback. PPMReceivedCallback ppm_callback; + // The PPM callback context + uint32_t ppm_context; // The id that the packet was received from uint32_t rx_destination_id; @@ -283,6 +291,9 @@ struct pios_rfm22b_dev { // Are we sending / receiving only PPM data? bool ppm_only_mode; + // The base freq in Hertz + uint32_t base_freq; + // The channel list uint8_t channels[RFM22B_NUM_CHANNELS]; // The number of frequency hopping channels. @@ -294,11 +305,10 @@ struct pios_rfm22b_dev { // current frequency hop channel index uint8_t channel_index; // afc correction reading (in Hz) - int8_t afc_correction_Hz; + int32_t afc_correction_Hz; // The packet timers. - portTickType packet_start_ticks; - portTickType tx_complete_ticks; + portTickType packet_start_time; portTickType time_delta; portTickType last_contact; }; diff --git a/flight/pios/inc/pios_sbus_priv.h b/flight/pios/inc/pios_sbus_priv.h index 791683831b..f260e6b31d 100644 --- a/flight/pios/inc/pios_sbus_priv.h +++ b/flight/pios/inc/pios_sbus_priv.h @@ -84,11 +84,7 @@ * 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; }; extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; diff --git a/flight/pios/inc/pios_servo.h b/flight/pios/inc/pios_servo.h index f3e9621ada..76c3e499da 100644 --- a/flight/pios/inc/pios_servo.h +++ b/flight/pios/inc/pios_servo.h @@ -6,7 +6,8 @@ * @{ * * @file pios_servo.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief RC Servo functions header. * @see The GNU Public License (GPL) Version 3 * @@ -32,16 +33,24 @@ /* Global types */ enum pios_servo_bank_mode { - PIOS_SERVO_BANK_MODE_PWM = 0, - PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 1 + PIOS_SERVO_BANK_MODE_NONE = 0, + PIOS_SERVO_BANK_MODE_PWM = 1, + PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 2, + PIOS_SERVO_BANK_MODE_DSHOT = 3, }; /* Public Functions */ extern void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); +extern void PIOS_Servo_SetActive(uint32_t Active); extern void PIOS_Servo_Update(); extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode); +extern void PIOS_Servo_DSHot_Rate(uint32_t rate_in_khz); extern uint8_t PIOS_Servo_GetPinBank(uint8_t pin); +/* ESC Bridge support */ +extern void PIOS_Servo_Disable(); +extern void PIOS_Servo_Enable(); + #endif /* PIOS_SERVO_H */ /** diff --git a/flight/pios/inc/pios_servo_priv.h b/flight/pios/inc/pios_servo_priv.h index be692cc269..61946fcc2d 100644 --- a/flight/pios/inc/pios_servo_priv.h +++ b/flight/pios/inc/pios_servo_priv.h @@ -7,7 +7,8 @@ * @{ * * @file pios_servo_priv.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Servo private structures. * @see The GNU Public License (GPL) Version 3 * @@ -45,6 +46,7 @@ struct pios_servo_cfg { }; extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg); +const struct pios_servo_cfg *PIOS_Servo_GetConfig(); #endif /* PIOS_SERVO_PRIV_H */ diff --git a/flight/pios/inc/pios_stm32.h b/flight/pios/inc/pios_stm32.h index 01aefe059c..03eaa26993 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 379223a6f5..ea64c44957 100644 --- a/flight/pios/inc/pios_usart.h +++ b/flight/pios/inc/pios_usart.h @@ -34,6 +34,29 @@ /* 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_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) + +/* 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) + +#define PIOS_IOCTL_USART_LOCK_CONFIG COM_IOCTL(COM_IOCTL_TYPE_USART, 8, bool) + #endif /* PIOS_USART_H */ /** diff --git a/flight/pios/inc/pios_usart_priv.h b/flight/pios/inc/pios_usart_priv.h index 68a44bc9df..d939e1f44c 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.h b/flight/pios/inc/pios_usb.h index a068a1705a..06ed1d48f1 100644 --- a/flight/pios/inc/pios_usb.h +++ b/flight/pios/inc/pios_usb.h @@ -38,6 +38,7 @@ extern int32_t PIOS_USB_ChangeConnectionState(bool connected); extern bool PIOS_USB_CableConnected(uint8_t id); extern bool PIOS_USB_CheckAvailable(uint32_t id); extern void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void)); +extern void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context); #endif /* PIOS_USB_H */ /** diff --git a/flight/pios/inc/pios_usb_defs.h b/flight/pios/inc/pios_usb_defs.h index cca131e859..81cf4217f8 100644 --- a/flight/pios/inc/pios_usb_defs.h +++ b/flight/pios/inc/pios_usb_defs.h @@ -362,8 +362,8 @@ enum usb_product_ids { USB_PRODUCT_ID_CC3D = 0x415D, USB_PRODUCT_ID_REVOLUTION = 0x415E, USB_PRODUCT_ID_SPARKY2 = 0x41D0, // was 0x415E during LP testing - USB_PRODUCT_ID_OSD = 0x4194, - USB_PRODUCT_ID_SPARE = 0x4195, + USB_PRODUCT_ID_OSD = 0x4194, + USB_PRODUCT_ID_LIBREPILOT = 0x4195, } __attribute__((packed)); enum usb_op_board_ids { @@ -372,8 +372,14 @@ enum usb_op_board_ids { USB_OP_BOARD_ID_OPLINK = 3, USB_OP_BOARD_ID_COPTERCONTROL = 4, USB_OP_BOARD_ID_REVOLUTION = 5, - USB_OP_BOARD_ID_SPARKY2 = 5, - USB_OP_BOARD_ID_OSD = 6, + USB_OP_BOARD_ID_SPARKY2 = 5, + USB_OP_BOARD_ID_OSD = 6, + USB_OP_BOARD_ID_CCF3D = 7, + USB_OP_BOARD_ID_SPRACINGF3 = 8, + USB_OP_BOARD_ID_SPRACINGF3EVO = 9, + USB_OP_BOARD_ID_NUCLEOF303RE = 10, + USB_OP_BOARD_ID_PIKOBLX = 11, + USB_OP_BOARD_ID_TINYFISH = 12, } __attribute__((packed)); enum usb_op_board_modes { 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 a828a1c6fd..1ff5c12985 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,20 @@ #define PIOS_USB_DESC_HID_CDC_PRIV_H #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/pios/inc/pios_usb_desc_hid_only_priv.h b/flight/pios/inc/pios_usb_desc_hid_only_priv.h index 6d98c3967f..b9c9dcda2c 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/inc/pios_usb_hid_istr.h b/flight/pios/inc/pios_usb_hid_istr.h similarity index 100% rename from flight/pios/stm32f10x/inc/pios_usb_hid_istr.h rename to flight/pios/inc/pios_usb_hid_istr.h diff --git a/flight/pios/stm32f10x/inc/pios_usb_hid_pwr.h b/flight/pios/inc/pios_usb_hid_pwr.h similarity index 100% rename from flight/pios/stm32f10x/inc/pios_usb_hid_pwr.h rename to flight/pios/inc/pios_usb_hid_pwr.h diff --git a/flight/pios/inc/pios_usbhook.h b/flight/pios/inc/pios_usbhook.h index 2d826bc2a7..0bf154a246 100644 --- a/flight/pios/inc/pios_usbhook.h +++ b/flight/pios/inc/pios_usbhook.h @@ -74,4 +74,7 @@ extern void PIOS_USBHOOK_EndpointRx(uint8_t epnum, uint8_t *buf, uint16_t len); extern void PIOS_USBHOOK_Activate(void); extern void PIOS_USBHOOK_Deactivate(void); +extern uint32_t PIOS_USBHOOK_EndpointGetStatus(uint8_t epnum); + + #endif /* PIOS_USBHOOK_H */ diff --git a/flight/pios/inc/pios_ws2811_cfg.h b/flight/pios/inc/pios_ws2811_cfg.h deleted file mode 100644 index a4a6b5739b..0000000000 --- a/flight/pios/inc/pios_ws2811_cfg.h +++ /dev/null @@ -1,136 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_ws2811_cfg.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. - * @brief A driver for ws2811 rgb led controller. - * this is a plain PiOS port of the very clever solution - * implemented by Omri Iluz in the chibios driver here: - * https://github.com/omriiluz/WS2812B-LED-Driver-ChibiOS - * @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_WS2811_CFG_H_ -#define PIOS_WS2811_CFG_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24)) -#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord -#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord -#define PIOS_WS2811_TIM_PERIOD 20 - -// Following times are keept on the lower side to accounts -// for bus contentions and irq response time -#define PIOS_WS2811_T0_HIGH_PERIOD 32 // .35us +/- 150nS -#define PIOS_WS2811_T1_HIGH_PERIOD 70 // .70us +/- 150nS - -#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \ - { \ - .DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \ - .DMA_Channel = channel, \ - .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ - .DMA_FIFOMode = DMA_FIFOMode_Enable, \ - .DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \ - .DMA_Memory0BaseAddr = 0, \ - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ - .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ - .DMA_MemoryInc = DMA_MemoryInc_Enable, \ - .DMA_Mode = DMA_Mode_Circular, \ - .DMA_PeripheralBaseAddr = 0, \ - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ - .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ - .DMA_Priority = DMA_Priority_VeryHigh, } - -#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \ - { \ - .DMA_BufferSize = 4, \ - .DMA_Channel = channel, \ - .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ - .DMA_FIFOMode = DMA_FIFOMode_Enable, \ - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ - .DMA_Memory0BaseAddr = 0, \ - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ - .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ - .DMA_MemoryInc = DMA_MemoryInc_Enable, \ - .DMA_Mode = DMA_Mode_Circular, \ - .DMA_PeripheralBaseAddr = 0, \ - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ - .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ - .DMA_Priority = DMA_Priority_VeryHigh, } - -#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \ - { \ - .DMA_BufferSize = 4, \ - .DMA_Channel = channel, \ - .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ - .DMA_FIFOMode = DMA_FIFOMode_Enable, \ - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ - .DMA_Memory0BaseAddr = 0, \ - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ - .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ - .DMA_MemoryInc = DMA_MemoryInc_Enable, \ - .DMA_Mode = DMA_Mode_Circular, \ - .DMA_PeripheralBaseAddr = 0, \ - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ - .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ - .DMA_Priority = DMA_Priority_High } - - -typedef uint16_t ledbuf_t; - -struct pios_ws2811_pin_cfg { - GPIO_TypeDef *gpio; - GPIO_InitTypeDef gpioInit; -}; -struct pios_ws2811_cfg { - TIM_TimeBaseInitTypeDef timerInit; - TIM_TypeDef *timer; - uint8_t timerCh1; - uint8_t timerCh2; - - DMA_InitTypeDef dmaInitCh1; - DMA_Stream_TypeDef *streamCh1; - uint32_t dmaItCh1; - - DMA_InitTypeDef dmaInitCh2; - DMA_Stream_TypeDef *streamCh2; - uint32_t dmaItCh2; - - DMA_InitTypeDef dmaInitUpdate; - DMA_Stream_TypeDef *streamUpdate; - uint32_t dmaItUpdate; - uint16_t dmaSource; - struct stm32_irq irq; -}; - -void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg); - -void PIOS_WS2811_DMA_irq_handler(); -#endif /* PIOS_WS2811_H_ */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 1baab4ac05..8d37b08528 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -64,6 +64,8 @@ extern "C" { #include #elif defined(STM32F0) #include +#elif defined(STM32F3) +#include #else #error "No Architecture defined" #endif diff --git a/flight/pios/stm32f0x/inc/pios_delay_raw.h b/flight/pios/stm32f0x/inc/pios_delay_raw.h new file mode 100644 index 0000000000..769ea2f5ea --- /dev/null +++ b/flight/pios/stm32f0x/inc/pios_delay_raw.h @@ -0,0 +1,36 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELAY Delay Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_delay_raw.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @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_DELAY_RAW_H +#define PIOS_DELAY_RAW_H + +extern uint32_t PIOS_DELAY_GetRaw(); + +#endif /* PIOS_DELAY_RAW_H */ diff --git a/flight/pios/stm32f0x/link_STM3205x_GPSP_sections.ld b/flight/pios/stm32f0x/link_STM3205x_GPSP_sections.ld index a526ca7854..5e217c838d 100644 --- a/flight/pios/stm32f0x/link_STM3205x_GPSP_sections.ld +++ b/flight/pios/stm32f0x/link_STM3205x_GPSP_sections.ld @@ -37,6 +37,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) diff --git a/flight/pios/stm32f0x/pios_adc.c b/flight/pios/stm32f0x/pios_adc.c index 3f57014491..1c33422a1e 100644 --- a/flight/pios/stm32f0x/pios_adc.c +++ b/flight/pios/stm32f0x/pios_adc.c @@ -244,9 +244,6 @@ void PIOS_ADC_Config(uint32_t oversampling) pios_adc_dev->fir_coeffs[i] = 1; } pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample] = pios_adc_dev->adc_oversample; - - /* Enable DMA1 clock */ - RCC_AHBPeriphClockCmd(pios_adc_dev->cfg->dma.ahb_clk, ENABLE); } /** diff --git a/flight/pios/stm32f0x/pios_bkp.c b/flight/pios/stm32f0x/pios_bkp.c index d15063ff89..a20a199bdc 100644 --- a/flight/pios/stm32f0x/pios_bkp.c +++ b/flight/pios/stm32f0x/pios_bkp.c @@ -57,12 +57,6 @@ const uint32_t pios_bkp_registers_map[] = { void PIOS_BKP_Init(void) { - /* Enable CRC clock */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); - - /* Enable PWR and BKP clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); - /* Clear Tamper pin Event(TE) pending flag */ RTC_ClearFlag(RTC_FLAG_TAMP1F | RTC_FLAG_TAMP2F); } diff --git a/flight/pios/stm32f0x/pios_bl_helper.c b/flight/pios/stm32f0x/pios_bl_helper.c index af96880e4f..7552681911 100644 --- a/flight/pios/stm32f0x/pios_bl_helper.c +++ b/flight/pios/stm32f0x/pios_bl_helper.c @@ -119,8 +119,6 @@ void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) } void PIOS_BL_HELPER_CRC_Ini() -{ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); -} +{} #endif /* PIOS_INCLUDE_BL_HELPER */ diff --git a/flight/pios/stm32f0x/pios_delay.c b/flight/pios/stm32f0x/pios_delay.c index 1a27f182da..90ae0975b4 100644 --- a/flight/pios/stm32f0x/pios_delay.c +++ b/flight/pios/stm32f0x/pios_delay.c @@ -47,8 +47,7 @@ int32_t PIOS_DELAY_Init(void) { // unfortunately F0 does not allow access to DWT and CoreDebug functionality from CPU side - // thus we are going to use timer3 for timing measurement - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + // thus we are going to use timer2 for timing measurement const TIM_TimeBaseInitTypeDef timerInit = { .TIM_Prescaler = (48000000 / 1000000), @@ -57,6 +56,7 @@ int32_t PIOS_DELAY_Init(void) .TIM_Period = 0xFFFFFFFF, .TIM_RepetitionCounter = 0x0000, }; + // Stop timer TIM_Cmd(TIM2, DISABLE); // Configure timebase and internal clock diff --git a/flight/pios/stm32f0x/pios_flash_internal.c b/flight/pios/stm32f0x/pios_flash_internal.c index 2c76a2eb04..1a2f78043a 100644 --- a/flight/pios/stm32f0x/pios_flash_internal.c +++ b/flight/pios/stm32f0x/pios_flash_internal.c @@ -28,7 +28,7 @@ #ifdef PIOS_INCLUDE_FLASH_INTERNAL -#include "stm32f10x_flash.h" +#include "stm32f0xx_flash.h" #include "pios_flash_internal_priv.h" #include "pios_flash.h" #include diff --git a/flight/pios/stm32f0x/pios_gpio.c b/flight/pios/stm32f0x/pios_gpio.c index 17830d2cdb..eb42c9711d 100644 --- a/flight/pios/stm32f0x/pios_gpio.c +++ b/flight/pios/stm32f0x/pios_gpio.c @@ -45,22 +45,6 @@ int32_t PIOS_GPIO_Init(uint32_t *gpios_dev_id, const struct pios_gpio_cfg *cfg) for (uint8_t i = 0; i < cfg->num_gpios; i++) { const struct pios_gpio *gpio = &(cfg->gpios[i]); - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)gpio->pin.gpio) { - case (uint32_t)GPIOA: - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); - break; - case (uint32_t)GPIOB: - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); - break; - case (uint32_t)GPIOC: - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } - if (gpio->remap) { GPIO_PinAFConfig(gpio->pin.gpio, gpio->pin.init.GPIO_Pin, gpio->remap); } diff --git a/flight/pios/stm32f0x/pios_i2c.c b/flight/pios/stm32f0x/pios_i2c.c index 385c5295e1..87ec65782f 100644 --- a/flight/pios/stm32f0x/pios_i2c.c +++ b/flight/pios/stm32f0x/pios_i2c.c @@ -274,12 +274,12 @@ void go_txn_setup(struct pios_i2c_adapter *i2c_adapter) I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { - I2C_TransferHandling(i2c_adapter->cfg->regs, i2c_adapter->active_txn->addr, i2c_adapter->active_txn->len, + I2C_TransferHandling(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr << 1), i2c_adapter->active_txn->len, /* Only last transaction generates Auto End */ i2c_adapter->active_txn == i2c_adapter->last_txn ? I2C_AutoEnd_Mode : I2C_SoftEnd_Mode, I2C_Generate_Start_Read); } else { - I2C_TransferHandling(i2c_adapter->cfg->regs, i2c_adapter->active_txn->addr, i2c_adapter->active_txn->len, + I2C_TransferHandling(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr << 1), i2c_adapter->active_txn->len, /* Only last transaction generates Auto End */ i2c_adapter->active_txn == i2c_adapter->last_txn ? I2C_AutoEnd_Mode : I2C_SoftEnd_Mode, I2C_Generate_Start_Write); @@ -541,13 +541,9 @@ int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) /* Enable the associated peripheral clock */ switch ((uint32_t)i2c_adapter->cfg->regs) { case (uint32_t)I2C1: - /* Enable I2C peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); RCC_I2CCLKConfig(RCC_I2C1CLK_HSI); break; case (uint32_t)I2C2: - /* Enable I2C peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); break; } diff --git a/flight/pios/stm32f0x/pios_rtc.c b/flight/pios/stm32f0x/pios_rtc.c index 8fe758145e..bfa5670216 100644 --- a/flight/pios/stm32f0x/pios_rtc.c +++ b/flight/pios/stm32f0x/pios_rtc.c @@ -51,7 +51,6 @@ void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg) { RCC_BackupResetCmd(ENABLE); RCC_BackupResetCmd(DISABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); PWR_BackupAccessCmd(ENABLE); // Divide external 8 MHz clock to 1 MHz RCC_RTCCLKConfig(cfg->clksrc); diff --git a/flight/pios/stm32f0x/pios_spi.c b/flight/pios/stm32f0x/pios_spi.c index 5758ce7429..f408ab7980 100644 --- a/flight/pios/stm32f0x/pios_spi.c +++ b/flight/pios/stm32f0x/pios_spi.c @@ -91,22 +91,8 @@ int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg) vSemaphoreCreateBinary(spi_dev->busy); xSemaphoreGive(spi_dev->busy); #else - spi_dev->busy = 0; + spi_dev->busy = 0; #endif - /* Enable the associated peripheral clock */ - switch ((uint32_t)spi_dev->cfg->regs) { - case (uint32_t)SPI1: - /* Enable SPI peripheral clock (APB2 == high speed) */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); - break; - case (uint32_t)SPI2: - /* Enable SPI peripheral clock (APB1 == slow speed) */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); - break; - } - /* Enable DMA clock */ - RCC_AHBPeriphClockCmd(spi_dev->cfg->dma.ahb_clk, ENABLE); - /* Disable callback function */ spi_dev->callback = NULL; diff --git a/flight/pios/stm32f0x/pios_sys.c b/flight/pios/stm32f0x/pios_sys.c index ab2aef92fc..c4a774e06c 100644 --- a/flight/pios/stm32f0x/pios_sys.c +++ b/flight/pios/stm32f0x/pios_sys.c @@ -61,9 +61,6 @@ void PIOS_SYS_Init(void) /* do this early to ensure that we take exceptions in the right place */ NVIC_Configuration(); - /* Init the delay system */ - PIOS_DELAY_Init(); - /* * Turn on all the peripheral clocks. * Micromanaging clocks makes no sense given the power situation in the system, so @@ -72,15 +69,35 @@ void PIOS_SYS_Init(void) RCC_AHBPeriphClockCmd( RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | + RCC_AHBPeriph_GPIOC | RCC_AHBPeriph_FLITF | RCC_AHBPeriph_SRAM | - RCC_AHBPeriph_DMA1 + RCC_AHBPeriph_DMA1 | + RCC_AHBPeriph_CRC + , ENABLE); + + RCC_APB1PeriphClockCmd( + RCC_APB1Periph_USART2 | + RCC_APB1Periph_USART3 | + RCC_APB1Periph_WWDG | + RCC_APB1Periph_PWR | + RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_I2C1 | + RCC_APB1Periph_I2C2 | + RCC_APB1Periph_SPI2 , ENABLE); RCC_APB2PeriphClockCmd( RCC_APB2Periph_SYSCFG | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_TIM1 | 0, ENABLE); + /* Init the delay system */ + PIOS_DELAY_Init(); + /* * Configure all pins as input / pullup to avoid issues with * uncommitted pins, excepting special-function pins that we need to @@ -124,12 +141,17 @@ int32_t PIOS_SYS_Reset(void) PIOS_IRQ_Disable(); // turn off all board LEDs -#if defined(PIOS_LED_HEARTBEAT) +#ifdef PIOS_INCLUDE_LED +# ifdef PIOS_LED_HEARTBEAT PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ -#if defined(PIOS_LED_ALARM) +# endif /* PIOS_LED_HEARTBEAT */ +# ifdef PIOS_LED_ALARM PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +# endif /* PIOS_LED_ALARM */ +# ifdef PIOS_BUZZER_ALARM + PIOS_LED_Off(PIOS_BUZZER_ALARM); +# endif /* PIOS_BUZZER_ALARM */ +#endif /* PIOS_INCLUDE_LED */ /* Reset STM32 */ NVIC_SystemReset(); diff --git a/flight/pios/stm32f0x/pios_tim.c b/flight/pios/stm32f0x/pios_tim.c index a519240ee0..7a9b649c84 100644 --- a/flight/pios/stm32f0x/pios_tim.c +++ b/flight/pios/stm32f0x/pios_tim.c @@ -61,36 +61,6 @@ int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) { PIOS_DEBUG_Assert(cfg); - /* Enable appropriate clock to timer module */ - switch ((uint32_t)cfg->timer) { - case (uint32_t)TIM1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (uint32_t)TIM2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (uint32_t)TIM3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (uint32_t)TIM4: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; -#ifdef STM32F10X_HD - case (uint32_t)TIM5: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (uint32_t)TIM6: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (uint32_t)TIM7: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (uint32_t)TIM8: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; -#endif - } - /* Configure the dividers for this timer */ TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); @@ -127,21 +97,6 @@ int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *c for (uint8_t i = 0; i < num_channels; i++) { const struct pios_tim_channel *chan = &(channels[i]); - /* Enable the peripheral clock for the GPIO */ - switch ((uint32_t)chan->pin.gpio) { - case (uint32_t)GPIOA: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - break; - case (uint32_t)GPIOB: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - break; - case (uint32_t)GPIOC: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); - break; - default: - PIOS_Assert(0); - break; - } GPIO_Init(chan->pin.gpio, &chan->pin.init); if (chan->remap) { diff --git a/flight/pios/stm32f0x/pios_usart.c b/flight/pios/stm32f0x/pios_usart.c index 4239f31d5b..6154341c39 100644 --- a/flight/pios/stm32f0x/pios_usart.c +++ b/flight/pios/stm32f0x/pios_usart.c @@ -7,7 +7,8 @@ * @{ * * @file pios_usart.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * @@ -37,6 +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_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); static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); @@ -44,6 +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, @@ -57,7 +64,7 @@ enum pios_usart_dev_magic { struct pios_usart_dev { enum pios_usart_dev_magic magic; const struct pios_usart_cfg *cfg; - + USART_InitTypeDef init; pios_com_callback rx_in_cb; uint32_t rx_in_context; pios_com_callback tx_out_cb; @@ -68,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) @@ -135,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 @@ -151,6 +173,29 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) PIOS_DEBUG_Assert(usart_id); PIOS_DEBUG_Assert(cfg); + uint32_t *local_id; + uint8_t irq_channel; + + /* Enable USART clock */ + switch ((uint32_t)cfg->regs) { + case (uint32_t)USART1: + local_id = &PIOS_USART_1_id; + irq_channel = USART1_IRQn; + break; + case (uint32_t)USART2: + local_id = &PIOS_USART_2_id; + irq_channel = USART2_IRQn; + break; +#if defined(STM32F072) + case (uint32_t)USART3: + local_id = &PIOS_USART_3_id; + irq_channel = USART3_4_IRQn; + break; +#endif + default: + goto out_fail; + } + struct pios_usart_dev *usart_dev; usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); @@ -160,46 +205,69 @@ 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->irq_channel = irq_channel; + + /* Initialize the comm parameter structure */ + USART_StructInit(&usart_dev->init); // 9600 8n1 + + /* We will set modes later, depending on installed callbacks */ + usart_dev->init.USART_Mode = 0; - /* 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); + *usart_id = (uint32_t)usart_dev; + *local_id = (uint32_t)usart_dev; + + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); + + /* Disable overrun detection */ + USART_OverrunDetectionConfig(usart_dev->cfg->regs, USART_OVRDetection_Disable); + + 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_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); } - /* 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); + /* 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); + } - /* Enable USART clock */ - switch ((uint32_t)cfg->regs) { - case (uint32_t)USART1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); - PIOS_USART_3_id = (uint32_t)usart_dev; - break; + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); } - /* Configure the USART */ - USART_Init(cfg->regs, (USART_InitTypeDef *)&cfg->init); - *usart_id = (uint32_t)usart_dev; + /* Write new configuration */ + { // fix parity stuff + USART_InitTypeDef init = usart_dev->init; - NVIC_Init((NVIC_InitTypeDef *)&cfg->irq.init); - 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); + if ((init.USART_Parity != USART_Parity_No) && (init.USART_WordLength == USART_WordLength_8b)) { + init.USART_WordLength = USART_WordLength_9b; + } - return 0; + USART_Init(usart_dev->cfg->regs, &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) @@ -222,21 +290,76 @@ static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16 */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) { - const struct pios_usart_dev *usart_dev = PIOS_USART_validate(usart_id); + struct pios_usart_dev *usart_dev = PIOS_USART_validate(usart_id); + + /* Use our working copy of the usart init structure */ + usart_dev->init.USART_BaudRate = baud; + + PIOS_USART_Setup(usart_dev); +} +#ifndef BOOTLOADER +/** + * Changes configuration of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] word_len Requested word length + * \param[in] stop_bits Requested stop bits + * \param[in] parity Requested parity + * + */ +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) +{ + struct pios_usart_dev *usart_dev = PIOS_USART_validate(usart_id); - USART_InitTypeDef USART_InitStructure; + switch (word_len) { + case PIOS_COM_Word_length_8b: + usart_dev->init.USART_WordLength = USART_WordLength_8b; + break; + case PIOS_COM_Word_length_9b: + usart_dev->init.USART_WordLength = USART_WordLength_9b; + break; + default: + break; + } - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; + switch (stop_bits) { + case PIOS_COM_StopBits_1: + usart_dev->init.USART_StopBits = USART_StopBits_1; + break; + case PIOS_COM_StopBits_1_5: + usart_dev->init.USART_StopBits = USART_StopBits_1_5; + break; + case PIOS_COM_StopBits_2: + usart_dev->init.USART_StopBits = USART_StopBits_2; + break; + default: + break; + } - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + switch (parity) { + case PIOS_COM_Parity_No: + usart_dev->init.USART_Parity = USART_Parity_No; + break; + case PIOS_COM_Parity_Even: + usart_dev->init.USART_Parity = USART_Parity_Even; + break; + case PIOS_COM_Parity_Odd: + usart_dev->init.USART_Parity = USART_Parity_Odd; + break; + default: + break; + } - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + if (baud_rate) { + usart_dev->init.USART_BaudRate = baud_rate; + } - USART_Cmd(usart_dev->cfg->regs, ENABLE); + PIOS_USART_Setup(usart_dev); } +#endif /* BOOTLOADER */ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { @@ -246,8 +369,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) @@ -258,8 +385,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) @@ -272,8 +403,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); @@ -286,7 +417,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); @@ -294,19 +425,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/pios/stm32f0x/pios_wdg.c b/flight/pios/stm32f0x/pios_wdg.c index f87af420db..ef51e28e83 100644 --- a/flight/pios/stm32f0x/pios_wdg.c +++ b/flight/pios/stm32f0x/pios_wdg.c @@ -70,7 +70,6 @@ uint16_t PIOS_WDG_Init() delay = 0x0fff; } #if defined(PIOS_INCLUDE_WDG) - RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE); DBGMCU_APB1PeriphConfig(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); IWDG_SetPrescaler(IWDG_Prescaler_32); diff --git a/flight/pios/stm32f10x/inc/pios_delay_raw.h b/flight/pios/stm32f10x/inc/pios_delay_raw.h new file mode 100644 index 0000000000..6a49415633 --- /dev/null +++ b/flight/pios/stm32f10x/inc/pios_delay_raw.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELAY Delay Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_delay_raw.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @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_DELAY_RAW_H +#define PIOS_DELAY_RAW_H + +/* these should be defined by CMSIS, but they aren't */ +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) + +#define PIOS_DELAY_GetRaw() (DWT_CYCCNT) + +extern uint32_t PIOS_DELAY_GetRawHz(); + +#endif /* PIOS_DELAY_RAW_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 0000000000..f8a03fcb75 --- /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/library.mk b/flight/pios/stm32f10x/library.mk index 592b95801a..318681ffad 100644 --- a/flight/pios/stm32f10x/library.mk +++ b/flight/pios/stm32f10x/library.mk @@ -9,7 +9,7 @@ PIOS_DEVLIB := $(dir $(lastword $(MAKEFILE_LIST))) LINKER_SCRIPTS_PATH = $(PIOS_DEVLIB) # Compiler options implied by the F10x -CDEFS += -DSTM32F10X -DSTM32F10X_$(MODEL) +CDEFS += -DSTM32F10X -DSTM32F10X_$(MODEL) -DSTM32F1 CDEFS += -DUSE_STDPERIPH_DRIVER CDEFS += -DARM_MATH_CM3 CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ) diff --git a/flight/pios/stm32f10x/link_STM32103CB_CC_Rev1_sections.ld b/flight/pios/stm32f10x/link_STM32103CB_CC_Rev1_sections.ld index f3c6b468f2..eb93b0e546 100644 --- a/flight/pios/stm32f10x/link_STM32103CB_CC_Rev1_sections.ld +++ b/flight/pios/stm32f10x/link_STM32103CB_CC_Rev1_sections.ld @@ -32,6 +32,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) diff --git a/flight/pios/stm32f10x/link_STM32103CB_OPLINKMINI_sections.ld b/flight/pios/stm32f10x/link_STM32103CB_OPLINKMINI_sections.ld index 7f87323a00..bddc314718 100644 --- a/flight/pios/stm32f10x/link_STM32103CB_OPLINKMINI_sections.ld +++ b/flight/pios/stm32f10x/link_STM32103CB_OPLINKMINI_sections.ld @@ -32,6 +32,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) diff --git a/flight/pios/stm32f10x/pios_delay.c b/flight/pios/stm32f10x/pios_delay.c index 9effa1ec7d..9907f2c175 100644 --- a/flight/pios/stm32f10x/pios_delay.c +++ b/flight/pios/stm32f10x/pios_delay.c @@ -34,14 +34,9 @@ #ifdef PIOS_INCLUDE_DELAY -/* these should be defined by CMSIS, but they aren't */ -#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) -#define CYCCNTENA (1 << 0) -#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) - - /* cycles per microsecond */ static uint32_t us_ticks; +static uint32_t raw_hz; /** * Initialises the Timer used by PIOS_DELAY functions. @@ -57,6 +52,7 @@ int32_t PIOS_DELAY_Init(void) RCC_GetClocksFreq(&clocks); us_ticks = clocks.SYSCLK_Frequency / 1000000; PIOS_DEBUG_Assert(us_ticks > 1); + raw_hz = clocks.SYSCLK_Frequency; /* turn on access to the DWT registers */ CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; @@ -84,7 +80,7 @@ int32_t PIOS_DELAY_WaituS(uint32_t uS) uint32_t last_count = DWT_CYCCNT; for (;;) { - uint32_t current_count = DWT_CYCCNT; + uint32_t current_count = PIOS_DELAY_GetRaw(); uint32_t elapsed_uS; /* measure the time elapsed since the last time we checked */ @@ -135,7 +131,7 @@ int32_t PIOS_DELAY_WaitmS(uint32_t mS) */ uint32_t PIOS_DELAY_GetuS(void) { - return DWT_CYCCNT / us_ticks; + return PIOS_DELAY_GetRaw() / us_ticks; } /** @@ -149,12 +145,12 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) } /** - * @brief Get the raw delay timer, useful for timing - * @return Unitless value (uint32 wrap around) + * @brief Get the raw delay timer frequency + * @return raw delay timer frequency in Hz */ -uint32_t PIOS_DELAY_GetRaw() +uint32_t PIOS_DELAY_GetRawHz() { - return DWT_CYCCNT; + return raw_hz; } /** @@ -163,7 +159,7 @@ uint32_t PIOS_DELAY_GetRaw() */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - uint32_t diff = DWT_CYCCNT - raw; + uint32_t diff = PIOS_DELAY_GetRaw() - raw; return diff / us_ticks; } diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c deleted file mode 100644 index 44fa484be1..0000000000 --- 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/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index e18edc2fe7..c411d7eac5 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -980,7 +980,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; if (!PIOS_I2C_validate(i2c_adapter)) { - return -1; + return PIOS_I2C_TRANSFER_DEVICE_ERROR; } PIOS_DEBUG_Assert(txn_list); @@ -993,9 +993,10 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else +#error PIOS_I2C_Transfer is broken for use without FreeRTOS uint32_t timeout = 0xfff; while (i2c_adapter->busy && --timeout) { ; @@ -1071,9 +1072,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], } #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - 0; + return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT : + i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR : + PIOS_I2C_TRANSFER_OK; } diff --git a/flight/pios/stm32f10x/pios_ppm_out.c b/flight/pios/stm32f10x/pios_ppm_out.c index d479341775..9f4a7adbbf 100644 --- a/flight/pios/stm32f10x/pios_ppm_out.c +++ b/flight/pios/stm32f10x/pios_ppm_out.c @@ -35,7 +35,7 @@ #include "pios_ppm_out_priv.h" #define PIOS_PPM_OUT_MAX_DEVS 1 -#define PIOS_PPM_OUT_MAX_CHANNELS 8 +#define PIOS_PPM_OUT_MAX_CHANNELS 8 + 1 // 8 RC channels + Rssi #define PIOS_PPM_OUT_FRAME_PERIOD_US 22500 // microseconds #define PIOS_PPM_OUT_HIGH_PULSE_US 400 // microseconds #define PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US 1000 // microseconds diff --git a/flight/pios/stm32f10x/pios_servo.c b/flight/pios/stm32f10x/pios_servo.c deleted file mode 100644 index 76da3d33a5..0000000000 --- a/flight/pios/stm32f10x/pios_servo.c +++ /dev/null @@ -1,283 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_SERVO RC Servo Functions - * @brief Code to do set RC servo output - * @{ - * - * @file pios_servo.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief RC Servo routines (STM32 dependent) - * @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_SERVO - -#include "pios_servo_priv.h" -#include "pios_tim_priv.h" - -/* Private Function Prototypes */ - -static const struct pios_servo_cfg *servo_cfg; - -// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode. -static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 }; -// used to skip updates when pulse length is higher than update cycle -static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 }; -static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 }; -// timer associated to each bank -static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 }; - -// index of bank used for each pin -static uint8_t *pios_servo_pin_bank; - -#define PIOS_SERVO_TIMER_CLOCK 1000000 -#define PIOS_SERVO_SAFE_MARGIN 50 -/** - * Initialise Servos - */ -int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) -{ - uint32_t tim_id; - - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { - return -1; - } - - /* Store away the requested configuration */ - servo_cfg = cfg; - pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels); - - uint8_t bank = 0; - /* Configure the channels to be in output compare mode */ - for (uint8_t i = 0; i < cfg->num_channels; i++) { - const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - bool new = true; - /* See if any previous channels use that same timer */ - for (uint8_t j = 0; (j < i) && new; j++) { - new &= chan->timer != servo_cfg->channels[j].timer; - } - - if (new) { - PIOS_Assert(bank < PIOS_SERVO_BANKS); - for (uint8_t j = i; j < servo_cfg->num_channels; j++) { - if (servo_cfg->channels[j].timer == chan->timer) { - pios_servo_pin_bank[j] = bank; - } - } - pios_servo_bank_timer[bank] = chan->timer; - - PIOS_Assert(bank < PIOS_SERVO_BANKS); - - for (uint8_t j = i; j < servo_cfg->num_channels; j++) { - if (servo_cfg->channels[j].timer == chan->timer) { - pios_servo_pin_bank[j] = bank; - } - } - pios_servo_bank_timer[bank] = chan->timer; - - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, ENABLE); - - bank++; - } - - /* Set up for output compare function */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } - } - - return 0; -} - -/** - * Set the servo update rate (Max 500Hz) - * \param[in] array of rates in Hz - * \param[in] array of timer clocks in Hz - * \param[in] maximum number of banks - */ -void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks) -{ - PIOS_Assert(banks <= PIOS_SERVO_BANKS); - if (!servo_cfg) { - return; - } - - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - - for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) { - const TIM_TypeDef *timer = pios_servo_bank_timer[i]; - if (timer) { - uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK; - if (clock[i]) { - new_clock = clock[i]; - } - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / new_clock) - 1; - TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1); - - TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure); - } - } -} - -/** - * Set servo position - * \param[in] Servo Servo number (0-7) - * \param[in] Position Servo position in microseconds - */ -void PIOS_Servo_Set(uint8_t servo, uint16_t position) -{ - /* Make sure servo exists */ - if (!servo_cfg || servo >= servo_cfg->num_channels) { - return; - } - - - /* Update the position */ - const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; - uint16_t val = position; - uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps - if (val > (chan->timer->ARR - margin)) { - val = chan->timer->ARR - margin; - } - - uint8_t bank = pios_servo_pin_bank[servo]; - if (pios_servo_bank_max_pulse[bank] < val) { - pios_servo_bank_max_pulse[bank] = val; - } - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, val); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, val); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, val); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, val); - break; - } -} - -void PIOS_Servo_Update() -{ - for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) { - const TIM_TypeDef *timer = pios_servo_bank_timer[i]; - if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) { - // a pulse to be generated is longer than cycle period. skip this update. - if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) { - TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update); - pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i]; - } - } - pios_servo_bank_max_pulse[i] = 0; - } - for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { - uint8_t bank = pios_servo_pin_bank[i]; - uint8_t mode = pios_servo_bank_mode[bank]; - if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) { - /* Update the position */ - const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, 0); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, 0); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, 0); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, 0); - break; - } - } - } -} - -void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode) -{ - PIOS_Assert(bank < PIOS_SERVO_BANKS); - pios_servo_bank_mode[bank] = mode; - - if (pios_servo_bank_timer[bank]) { - for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { - if (pios_servo_pin_bank[i] == bank) { - const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - /* Set up for output compare function */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - case TIM_Channel_2: - TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - case TIM_Channel_3: - TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - case TIM_Channel_4: - TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - } - } - } - - // Setup the timer accordingly - TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive); - TIM_Cmd(pios_servo_bank_timer[bank], ENABLE); - } -} - - -uint8_t PIOS_Servo_GetPinBank(uint8_t pin) -{ - if (pin < servo_cfg->num_channels) { - return pios_servo_pin_bank[pin]; - } else { - return 0; - } -} - -#endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 63791fa22e..288fc0775b 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -53,6 +53,11 @@ void PIOS_SYS_Init(void) /* Init the delay system */ PIOS_DELAY_Init(); + /* Enable DMA */ + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | + RCC_AHBPeriph_DMA2, + ENABLE); + /* 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); @@ -92,12 +97,17 @@ int32_t PIOS_SYS_Reset(void) PIOS_IRQ_Disable(); // turn off all board LEDs -#if defined(PIOS_LED_HEARTBEAT) +#ifdef PIOS_INCLUDE_LED +# ifdef PIOS_LED_HEARTBEAT PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ -#if defined(PIOS_LED_ALARM) +# endif /* PIOS_LED_HEARTBEAT */ +# ifdef PIOS_LED_ALARM PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +# endif /* PIOS_LED_ALARM */ +# ifdef PIOS_BUZZER_ALARM + PIOS_LED_Off(PIOS_BUZZER_ALARM); +# endif /* PIOS_BUZZER_ALARM */ +#endif /* PIOS_INCLUDE_LED */ RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); diff --git a/flight/pios/stm32f10x/pios_tim.c b/flight/pios/stm32f10x/pios_tim.c index 4df0932996..ae442eaa10 100644 --- a/flight/pios/stm32f10x/pios_tim.c +++ b/flight/pios/stm32f10x/pios_tim.c @@ -103,6 +103,13 @@ int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) /* Enable Interrupts */ NVIC_Init(&cfg->irq.init); + /* Advanced TIM1 has additional IRQ */ + if (cfg->timer == TIM1) { + NVIC_InitTypeDef init = cfg->irq.init; + init.NVIC_IRQChannel = TIM1_UP_IRQn; + NVIC_Init(&init); + } + return 0; } diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index ff3286cfa0..0f664429e3 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -7,7 +7,8 @@ * @{ * * @file pios_usart.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * @@ -33,20 +34,25 @@ #ifdef PIOS_INCLUDE_USART #include +#include /* Provide a COM driver */ +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); 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, + .set_config = PIOS_USART_ChangeConfig, .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 { @@ -56,13 +62,14 @@ enum pios_usart_dev_magic { struct pios_usart_dev { enum pios_usart_dev_magic magic; const struct pios_usart_cfg *cfg; - + USART_InitTypeDef init; pios_com_callback rx_in_cb; uint32_t rx_in_context; pios_com_callback tx_out_cb; uint32_t tx_out_context; - + bool config_locked; uint32_t rx_dropped; + uint8_t irq_channel; }; static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) @@ -70,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) { @@ -141,61 +172,74 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) PIOS_DEBUG_Assert(usart_id); PIOS_DEBUG_Assert(cfg); - struct pios_usart_dev *usart_dev; - - usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); - if (!usart_dev) { - goto out_fail; - } - - /* Bind the configuration to the device instance */ - usart_dev->cfg = cfg; - - /* Enable the USART Pins Software Remapping */ - if (usart_dev->cfg->remap) { - GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); - } - - /* 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); + uint32_t *local_id; + uint8_t irq_channel; - /* Enable USART clock */ - switch ((uint32_t)usart_dev->cfg->regs) { + switch ((uint32_t)cfg->regs) { case (uint32_t)USART1: + local_id = &PIOS_USART_1_id; + irq_channel = USART1_IRQn; RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); break; case (uint32_t)USART2: + local_id = &PIOS_USART_2_id; + irq_channel = USART2_IRQn; RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); break; case (uint32_t)USART3: + local_id = &PIOS_USART_3_id; + irq_channel = USART3_IRQn; RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); break; + default: + goto out_fail; } - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, &usart_dev->cfg->init); + if (*local_id) { + /* this port is already open */ + *usart_id = *local_id; + return 0; + } - *usart_id = (uint32_t)usart_dev; + struct pios_usart_dev *usart_dev; - /* Configure USART Interrupts */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - PIOS_USART_3_id = (uint32_t)usart_dev; - break; + usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); + if (!usart_dev) { + goto out_fail; } - NVIC_Init(&usart_dev->cfg->irq.init); - 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); + /* Bind the configuration to the device instance */ + usart_dev->cfg = cfg; + usart_dev->irq_channel = irq_channel; + + /* Initialize the comm parameter structure */ + USART_StructInit(&usart_dev->init); // 9600 8n1 + + /* We will set modes later, depending on installed callbacks */ + usart_dev->init.USART_Mode = 0; + + /* DTR handling? */ + +#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 + + *usart_id = (uint32_t)usart_dev; + *local_id = (uint32_t)usart_dev; + + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); return 0; @@ -203,6 +247,47 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) 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 */ + { // fix parity stuff + USART_InitTypeDef init = usart_dev->init; + + if ((init.USART_Parity != USART_Parity_No) && (init.USART_WordLength == USART_WordLength_8b)) { + init.USART_WordLength = USART_WordLength_9b; + } + + USART_Init(usart_dev->cfg->regs, &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; @@ -237,16 +322,89 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) PIOS_Assert(valid); - USART_InitTypeDef USART_InitStructure; + if (usart_dev->config_locked) { + return; + } + + /* Use our working copy of the usart init structure */ + usart_dev->init.USART_BaudRate = baud; + + PIOS_USART_Setup(usart_dev); +} + +/** + * Changes configuration of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] word_len Requested word length + * \param[in] stop_bits Requested stop bits + * \param[in] parity Requested parity + * \param[in] baud_rate Requested baud rate + * \param[in] mode Requested 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) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + if (usart_dev->config_locked) { + return; + } + + switch (word_len) { + case PIOS_COM_Word_length_8b: + usart_dev->init.USART_WordLength = USART_WordLength_8b; + break; + case PIOS_COM_Word_length_9b: + usart_dev->init.USART_WordLength = USART_WordLength_9b; + break; + default: + break; + } + + switch (stop_bits) { + case PIOS_COM_StopBits_0_5: + usart_dev->init.USART_StopBits = USART_StopBits_0_5; + break; + case PIOS_COM_StopBits_1: + usart_dev->init.USART_StopBits = USART_StopBits_1; + break; + case PIOS_COM_StopBits_1_5: + usart_dev->init.USART_StopBits = USART_StopBits_1_5; + break; + case PIOS_COM_StopBits_2: + usart_dev->init.USART_StopBits = USART_StopBits_2; + break; + default: + break; + } - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; + switch (parity) { + case PIOS_COM_Parity_No: + usart_dev->init.USART_Parity = USART_Parity_No; + break; + case PIOS_COM_Parity_Even: + usart_dev->init.USART_Parity = USART_Parity_Even; + break; + case PIOS_COM_Parity_Odd: + usart_dev->init.USART_Parity = USART_Parity_Odd; + break; + default: + break; + } - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + if (baud_rate) { + usart_dev->init.USART_BaudRate = baud_rate; + } - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) @@ -261,8 +419,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) @@ -277,8 +439,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) @@ -336,6 +502,62 @@ 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); + + /* 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; + 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; + case PIOS_IOCTL_USART_LOCK_CONFIG: + usart_dev->config_locked = *(bool *)param; + break; + default: + return COM_IOCTL_ENOSYS; /* unknown ioctl */ + } + + return 0; +} + #endif /* PIOS_INCLUDE_USART */ /** diff --git a/flight/pios/stm32f10x/pios_usb.c b/flight/pios/stm32f10x/pios_usb.c index f79b08c53d..12cd962470 100644 --- a/flight/pios/stm32f10x/pios_usb.c +++ b/flight/pios/stm32f10x/pios_usb.c @@ -38,11 +38,16 @@ #include "pios_usb_priv.h" -#ifdef PIOS_INCLUDE_USB_HID - /* Rx/Tx status */ static bool transfer_possible = false; +#ifdef PIOS_INCLUDE_FREERTOS +struct { + void (*callback)(bool connected, uint32_t context); + uint32_t context; +} connectionState_cb_list[3]; +#endif + enum pios_usb_dev_magic { PIOS_USB_DEV_MAGIC = 0x17365904, }; @@ -51,6 +56,9 @@ struct pios_usb_dev { enum pios_usb_dev_magic magic; const struct pios_usb_cfg *cfg; }; +#ifdef PIOS_INCLUDE_FREERTOS +static void raiseConnectionStateCallback(bool connected); +#endif /** * @brief Validate the usb device structure @@ -175,6 +183,10 @@ int32_t PIOS_USB_ChangeConnectionState(bool Connected) #endif } +#ifdef PIOS_INCLUDE_FREERTOS + raiseConnectionStateCallback(Connected); +#endif + return 0; } @@ -242,7 +254,36 @@ bool PIOS_USB_CheckAvailable(uint32_t id) return PIOS_USB_CableConnected(id) && transfer_possible; } -#endif /* PIOS_INCLUDE_USB_HID */ +#ifdef PIOS_INCLUDE_FREERTOS +void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context) +{ + PIOS_Assert(connectionStateCallback); + + for (uint32_t i = 0; i < NELEMENTS(connectionState_cb_list); i++) { + if (connectionState_cb_list[i].callback == NULL) { + connectionState_cb_list[i].callback = connectionStateCallback; + connectionState_cb_list[i].context = context; + return; + } + } + + PIOS_Assert(0); +} + +static void raiseConnectionStateCallback(bool connected) +{ + uint32_t i = 0; + + while (i < NELEMENTS(connectionState_cb_list) && connectionState_cb_list[i].callback != NULL) { + connectionState_cb_list[i].callback(connected, connectionState_cb_list[i].context); + i++; + } +} +#else /* PIOS_INCLUDE_FREERTOS */ +void PIOS_USB_RegisterConnectionStateCallback(__attribute__((unused)) void (*connectionStateCallback)(bool connected, uint32_t context), __attribute__((unused)) uint32_t context) +{} +#endif /* PIOS_INCLUDE_FREERTOS */ + #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f10x/pios_usb_cdc.c b/flight/pios/stm32f10x/pios_usb_cdc.c index 84255191f7..ebbaedbd1b 100644 --- a/flight/pios/stm32f10x/pios_usb_cdc.c +++ b/flight/pios/stm32f10x/pios_usb_cdc.c @@ -42,6 +42,8 @@ static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USB_CDC_RegisterBaudRateCallback(uint32_t usbcdc_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context); +static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available baud_rate_cb, uint32_t context); +static void PIOS_USB_CDC_ChangeConnectionState(bool connected, uint32_t usbcdc_id); static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id); @@ -53,6 +55,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = { .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, .bind_baud_rate_cb = PIOS_USB_CDC_RegisterBaudRateCallback, .available = PIOS_USB_CDC_Available, + .bind_available_cb = PIOS_USB_CDC_RegisterAvailableCallback, }; enum pios_usb_cdc_dev_magic { @@ -72,6 +75,8 @@ struct pios_usb_cdc_dev { pios_com_callback_baud_rate baud_rate_cb; uint32_t baud_rate_context; + pios_com_callback_available available_cb; + uint32_t available_context; uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; /* @@ -157,6 +162,8 @@ int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cf pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; + PIOS_USB_RegisterConnectionStateCallback(PIOS_USB_CDC_ChangeConnectionState, (uint32_t)usb_cdc_dev); + *usbcdc_id = (uint32_t)usb_cdc_dev; return 0; @@ -366,8 +373,7 @@ static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id) PIOS_Assert(valid); - return (PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) && - (control_line_state & USB_CDC_CONTROL_LINE_STATE_DTE_PRESENT)) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE; + return PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE; } static struct usb_cdc_line_coding line_coding = { @@ -487,5 +493,33 @@ void PIOS_USB_CDC_SetLineCoding_Completed() } } +static void PIOS_USB_CDC_ChangeConnectionState(__attribute__((unused)) bool connected, uint32_t usbcdc_id) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + if (usb_cdc_dev->available_cb) { + (usb_cdc_dev->available_cb)(usb_cdc_dev->available_context, PIOS_USB_CDC_Available(usbcdc_id)); + } +} + +static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available available_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->available_context = context; + usb_cdc_dev->available_cb = available_cb; +} #endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f10x/pios_usbhook.c b/flight/pios/stm32f10x/pios_usbhook.c index 55e0e40e58..210256f73f 100644 --- a/flight/pios/stm32f10x/pios_usbhook.c +++ b/flight/pios/stm32f10x/pios_usbhook.c @@ -86,6 +86,9 @@ void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t desc_size) Hid_Report_Descriptor.Descriptor_Size = desc_size; } +void PIOS_USBHOOK_Deactivate(void) +{} + #include "stm32f10x.h" /* __IO */ __IO uint8_t EXTI_Enable; diff --git a/flight/pios/stm32f30x/inc/pios_architecture.h b/flight/pios/stm32f30x/inc/pios_architecture.h new file mode 100644 index 0000000000..dddc4df639 --- /dev/null +++ b/flight/pios/stm32f30x/inc/pios_architecture.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * + * @file pios_architecture.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @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_ARCHITECTURE_H +#define PIOS_ARCHITECTURE_H + +// defines for adc +#define PIOS_ADC_VOLTAGE_SCALE 3.30f / 4096.0f + +// defines for Temp measurements +#define PIOS_ADC_STM32_TEMP_V25 0.76f /* V */ +#define PIOS_ADC_STM32_TEMP_AVG_SLOPE 2.5f /* mV/C */ +#define PIOS_CONVERT_VOLT_TO_CPU_TEMP(x) ((x - PIOS_ADC_STM32_TEMP_V25) * 1000.0f / PIOS_ADC_STM32_TEMP_AVG_SLOPE + 25.0f) + + +#endif /* PIOS_ARCHITECTURE_H */ diff --git a/flight/pios/stm32f30x/inc/pios_delay_raw.h b/flight/pios/stm32f30x/inc/pios_delay_raw.h new file mode 100644 index 0000000000..6a49415633 --- /dev/null +++ b/flight/pios/stm32f30x/inc/pios_delay_raw.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELAY Delay Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_delay_raw.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @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_DELAY_RAW_H +#define PIOS_DELAY_RAW_H + +/* these should be defined by CMSIS, but they aren't */ +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) + +#define PIOS_DELAY_GetRaw() (DWT_CYCCNT) + +extern uint32_t PIOS_DELAY_GetRawHz(); + +#endif /* PIOS_DELAY_RAW_H */ diff --git a/flight/pios/stm32f30x/inc/pios_servo_config.h b/flight/pios/stm32f30x/inc/pios_servo_config.h new file mode 100644 index 0000000000..3f8ca6da7d --- /dev/null +++ b/flight/pios/stm32f30x/inc/pios_servo_config.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * + * @file pios_servo_config.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @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 STM32F30X + */ +#define TIM_SERVO_CHANNEL_CONFIG(_timer, _channel, _gpio, _pin) \ + { \ + .timer = _timer, \ + .timer_chan = TIM_Channel_##_channel, \ + .pin = { \ + .gpio = GPIO##_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_pin, \ + .GPIO_Speed = GPIO_Speed_2MHz, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP \ + }, \ + .pin_source = GPIO_PinSource##_pin, \ + }, \ + .remap = GPIO_AF_P##_gpio##_pin##_##_timer, \ + } + + +#endif /* PIOS_SERVO_CONFIG_H_ */ diff --git a/flight/pios/stm32f30x/inc/pios_usart_config.h b/flight/pios/stm32f30x/inc/pios_usart_config.h new file mode 100644 index 0000000000..46e65bbf64 --- /dev/null +++ b/flight/pios/stm32f30x/inc/pios_usart_config.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file pios_usart_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_USART_CONFIG_H_ +#define PIOS_USART_CONFIG_H_ + +/** + * Generic USART configuration structure for an STM32F30x port. + */ +#define USART_CONFIG(_usart, _rx_gpio, _rx_pin, _tx_gpio, _tx_pin) \ + { \ + .regs = _usart, \ + .remap = GPIO_AF_##_usart, \ + .rx = { \ + .gpio = GPIO##_rx_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_rx_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP, \ + }, \ + .pin_source = GPIO_PinSource##_rx_pin, \ + }, \ + .tx = { \ + .gpio = GPIO##_tx_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_tx_pin, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_Speed = GPIO_Speed_50MHz, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP, \ + }, \ + .pin_source = GPIO_PinSource##_tx_pin, \ + }, \ + } + +#endif /* PIOS_USART_CONFIG_H_ */ diff --git a/flight/pios/stm32f30x/inc/pios_ws2811_cfg.h b/flight/pios/stm32f30x/inc/pios_ws2811_cfg.h new file mode 100644 index 0000000000..27aa8a4b64 --- /dev/null +++ b/flight/pios/stm32f30x/inc/pios_ws2811_cfg.h @@ -0,0 +1,78 @@ +/** + ****************************************************************************** + * + * @file pios_ws2811_cfg.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief A driver for ws2811 rgb led controller. + * this is a port of the CleanFlight/BetaFlight implementation. + * @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_WS2811_CFG_H_ +#define PIOS_WS2811_CFG_H_ + + +struct pios_ws2811_cfg { + TIM_TypeDef *timer; + uint8_t timer_chan; + + struct stm32_gpio pin; + uint32_t remap; + + + uint16_t timer_dma_source; + + DMA_Channel_TypeDef *dma_chan; + + uint32_t dma_tcif; + + uint8_t dma_irqn; +}; + + +#define PIOS_WS2811_CONFIG(_timer, _channel, _gpio, _pin) \ + { \ + .timer = _timer, \ + .timer_chan = TIM_Channel_##_channel, \ + .pin = { \ + .gpio = GPIO##_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_pin, \ + .GPIO_Speed = GPIO_Speed_2MHz, \ + .GPIO_Mode = GPIO_Mode_AF, \ + .GPIO_OType = GPIO_OType_PP, \ + .GPIO_PuPd = GPIO_PuPd_UP \ + }, \ + .pin_source = GPIO_PinSource##_pin, \ + }, \ + .remap = GPIO_AF_P##_gpio##_pin##_##_timer, \ + .timer_dma_source = TIM_DMA_CC##_channel, \ + .dma_chan = _EVAL4(DMA, _timer##_CH##_channel##_DMA_INSTANCE, _Channel, _timer##_CH##_channel##_DMA_CHANNEL), \ + .dma_tcif = _EVAL4(DMA, _timer##_CH##_channel##_DMA_INSTANCE, _IT_TC, _timer##_CH##_channel##_DMA_CHANNEL), \ + .dma_irqn = _EVAL5(DMA, _timer##_CH##_channel##_DMA_INSTANCE, _Channel, _timer##_CH##_channel##_DMA_CHANNEL, _IRQn), \ + } + +#define DMA0_Channel0 0 +#define DMA0_IT_TC0 0 +#define DMA0_Channel0_IRQn 0 + +void PIOS_WS2811_Init(uint32_t *dev_id, const struct pios_ws2811_cfg *ws2811_cfg); + +void PIOS_WS2811_DMA_irq_handler(); + +#endif /* PIOS_WS2811_CFG_H_ */ diff --git a/flight/pios/stm32f30x/inc/stm32f30x_conf.h b/flight/pios/stm32f30x/inc/stm32f30x_conf.h new file mode 100644 index 0000000000..781f7fa3fd --- /dev/null +++ b/flight/pios/stm32f30x/inc/stm32f30x_conf.h @@ -0,0 +1,82 @@ +/** + ****************************************************************************** + * @file stm32f30x_conf.h + * @author MCD Application Team + * @version V1.0.0 + * @date 04-September-2012 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30X_CONF_H +#define __STM32F30X_CONF_H + +/* Includes ------------------------------------------------------------------*/ +/* Comment the line below to disable peripheral header file inclusion */ +#include "stm32f30x_adc.h" +#include "stm32f30x_can.h" +#include "stm32f30x_crc.h" +#include "stm32f30x_comp.h" +#include "stm32f30x_dac.h" +#include "stm32f30x_dbgmcu.h" +#include "stm32f30x_dma.h" +#include "stm32f30x_exti.h" +#include "stm32f30x_flash.h" +#include "stm32f30x_gpio.h" +#include "stm32f30x_syscfg.h" +#include "stm32f30x_i2c.h" +#include "stm32f30x_iwdg.h" +#include "stm32f30x_opamp.h" +#include "stm32f30x_pwr.h" +#include "stm32f30x_rcc.h" +#include "stm32f30x_rtc.h" +#include "stm32f30x_spi.h" +#include "stm32f30x_tim.h" +#include "stm32f30x_usart.h" +#include "stm32f30x_wwdg.h" +#include "stm32f30x_misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Uncomment the line below to expanse the "assert_param" macro in the + Standard Peripheral Library drivers code */ +/* #define USE_FULL_ASSERT 1 */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT + +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function which reports + * the name of the source file and the source line number of the call + * that failed. If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ +void assert_failed(uint8_t *file, uint32_t line); +#else + #define assert_param(expr) ((void)0) +#endif /* USE_FULL_ASSERT */ + +#endif /* __STM32F30X_CONF_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/inc/usb_conf.h b/flight/pios/stm32f30x/inc/usb_conf.h new file mode 100644 index 0000000000..97ea79b836 --- /dev/null +++ b/flight/pios/stm32f30x/inc/usb_conf.h @@ -0,0 +1,197 @@ +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** +* File Name : usb_conf.h +* Author : MCD Application Team +* Version : V3.2.1 +* Date : 07/05/2010 +* Description : Custom HID demo configuration file +******************************************************************************** +* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS +* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. +* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, +* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE +* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING +* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +*******************************************************************************/ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_CONF_H +#define __USB_CONF_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* External variables --------------------------------------------------------*/ + +#ifndef STM32F10X_CL +/*-------------------------------------------------------------*/ +/* -------------- Buffer Description Table -----------------*/ +/*-------------------------------------------------------------*/ +/* buffer table base address */ +/* buffer table base address */ +#define BTABLE_ADDRESS (0x00) + +/* EP0 */ +/* rx/tx buffer base address */ +#define ENDP0_RXADDR (0x20) +#define ENDP0_TXADDR (0x40) + +/* EP1 */ +/* rx/tx buffer base address */ +#define ENDP1_TXADDR (0x60) +#define ENDP1_RXADDR (0x80) + +/* EP2 */ +/* rx/tx buffer base address */ +#define ENDP2_TXADDR (0x100) +#define ENDP2_RXADDR (0x140) + +/* EP3 */ +/* rx/tx buffer base address */ +#define ENDP3_TXADDR (0x180) +#define ENDP3_RXADDR (0x1C0) + +/*-------------------------------------------------------------*/ +/* ------------------- ISTR events -------------------------*/ +/*-------------------------------------------------------------*/ +/* IMR_MSK */ +/* mask defining which events has to be handled */ +/* by the device application software */ +#define IMR_MSK \ + (CNTR_CTRM | CNTR_WKUPM | CNTR_SUSPM | CNTR_ERRM | CNTR_SOFM \ + | CNTR_ESOFM | CNTR_RESETM) + +/* Provide a callback function for SOF so we can do early USB detection */ +#define SOF_CALLBACK + +/* Provide a callback function for SUSPend so we can notice a cable removal event */ +#define SUSP_CALLBACK + +#endif /* STM32F10X_CL */ + +#ifdef STM32F10X_CL + +/******************************************************************************* +* FIFO Size Configuration +* +* (i) Dedicated data FIFO SPRAM of 1.25 Kbytes = 1280 bytes = 320 32-bits words +* available for the endpoints IN and OUT. +* Device mode features: +* -1 bidirectional CTRL EP 0 +* -3 IN EPs to support any kind of Bulk, Interrupt or Isochronous transfer +* -3 OUT EPs to support any kind of Bulk, Interrupt or Isochronous transfer +* +* ii) Receive data FIFO size = RAM for setup packets + +* OUT endpoint control information + +* data OUT packets + miscellaneous +* Space = ONE 32-bits words +* --> RAM for setup packets = 4 * n + 6 space +* (n is the nbr of CTRL EPs the device core supports) +* --> OUT EP CTRL info = 1 space +* (one space for status information written to the FIFO along with each +* received packet) +* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces +* (MINIMUM to receive packets) +* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces +* (if high-bandwidth EP is enabled or multiple isochronous EPs) +* --> miscellaneous = 1 space per OUT EP +* (one space for transfer complete status information also pushed to the +* FIFO with each endpoint's last packet) +* +* (iii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for +* that particular IN EP. More space allocated in the IN EP Tx FIFO results +* in a better performance on the USB and can hide latencies on the AHB. +* +* (iv) TXn min size = 16 words. (n : Transmit FIFO index) +* (v) When a TxFIFO is not used, the Configuration should be as follows: +* case 1 : n > m and Txn is not used (n,m : Transmit FIFO indexes) +* --> Txm can use the space allocated for Txn. +* case2 : n < m and Txn is not used (n,m : Transmit FIFO indexes) +* --> Txn should be configured with the minimum space of 16 words +* (vi) The FIFO is used optimally when used TxFIFOs are allocated in the top +* of the FIFO.Ex: use EP1 and EP2 as IN instead of EP1 and EP3 as IN ones. +*******************************************************************************/ + +#define RX_FIFO_SIZE 128 +#define TX0_FIFO_SIZE 64 +#define TX1_FIFO_SIZE 64 +#define TX2_FIFO_SIZE 16 +#define TX3_FIFO_SIZE 16 + +/* OTGD-FS-DEVICE IP interrupts Enable definitions */ +/* Uncomment the define to enable the selected interrupt */ +// #define INTR_MODEMISMATCH +#define INTR_SOFINTR +#define INTR_RXSTSQLVL /* Mandatory */ +// #define INTR_NPTXFEMPTY +// #define INTR_GINNAKEFF +// #define INTR_GOUTNAKEFF +// #define INTR_ERLYSUSPEND +#define INTR_USBSUSPEND /* Mandatory */ +#define INTR_USBRESET /* Mandatory */ +#define INTR_ENUMDONE /* Mandatory */ +// #define INTR_ISOOUTDROP +// #define INTR_EOPFRAME +// #define INTR_EPMISMATCH +#define INTR_INEPINTR /* Mandatory */ +#define INTR_OUTEPINTR /* Mandatory */ +// #define INTR_INCOMPLISOIN +// #define INTR_INCOMPLISOOUT +#define INTR_WKUPINTR /* Mandatory */ + +/* OTGD-FS-DEVICE IP interrupts subroutines */ +/* Comment the define to enable the selected interrupt subroutine and replace it + by user code */ +#define INTR_MODEMISMATCH_Callback NOP_Process +#define INTR_SOFINTR_Callback NOP_Process +#define INTR_RXSTSQLVL_Callback NOP_Process +#define INTR_NPTXFEMPTY_Callback NOP_Process +#define INTR_NPTXFEMPTY_Callback NOP_Process +#define INTR_GINNAKEFF_Callback NOP_Process +#define INTR_GOUTNAKEFF_Callback NOP_Process +#define INTR_ERLYSUSPEND_Callback NOP_Process +#define INTR_USBSUSPEND_Callback NOP_Process +#define INTR_USBRESET_Callback NOP_Process +#define INTR_ENUMDONE_Callback NOP_Process +#define INTR_ISOOUTDROP_Callback NOP_Process +#define INTR_EOPFRAME_Callback NOP_Process +#define INTR_EPMISMATCH_Callback NOP_Process +#define INTR_INEPINTR_Callback NOP_Process +#define INTR_OUTEPINTR_Callback NOP_Process +#define INTR_INCOMPLISOIN_Callback NOP_Process +#define INTR_INCOMPLISOOUT_Callback NOP_Process +#define INTR_WKUPINTR_Callback NOP_Process + +/* Isochronous data update */ +#define INTR_RXSTSQLVL_ISODU_Callback NOP_Process + +/* Isochronous transfer parameters */ +/* Size of a single Isochronous buffer (size of a single transfer) */ +#define ISOC_BUFFER_SZE 1 +/* Number of sub-buffers (number of single buffers/transfers), should be even */ +#define NUM_SUB_BUFFERS 2 + +#endif /* STM32F10X_CL */ + +/* CTR service routines */ +/* associated to defined endpoints */ +#define EP1_IN_Callback NOP_Process +#define EP2_IN_Callback NOP_Process +#define EP3_IN_Callback NOP_Process +#define EP4_IN_Callback NOP_Process +#define EP5_IN_Callback NOP_Process +#define EP6_IN_Callback NOP_Process +#define EP7_IN_Callback NOP_Process + +#define EP1_OUT_Callback NOP_Process +#define EP2_OUT_Callback NOP_Process +#define EP3_OUT_Callback NOP_Process +#define EP4_OUT_Callback NOP_Process +#define EP5_OUT_Callback NOP_Process +#define EP6_OUT_Callback NOP_Process +#define EP7_OUT_Callback NOP_Process + +#endif /*__USB_CONF_H*/ + +/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/.no-auto-format b/flight/pios/stm32f30x/libraries/.no-auto-format new file mode 100644 index 0000000000..e69de29bb2 diff --git a/flight/pios/stm32f30x/libraries/CMSIS3/Device/ST/STM32F30x/Include/stm32f30x.h b/flight/pios/stm32f30x/libraries/CMSIS3/Device/ST/STM32F30x/Include/stm32f30x.h new file mode 100644 index 0000000000..7683e7376d --- /dev/null +++ b/flight/pios/stm32f30x/libraries/CMSIS3/Device/ST/STM32F30x/Include/stm32f30x.h @@ -0,0 +1,9086 @@ +/** + ****************************************************************************** + * @file stm32f30x.h + * @author MCD Application Team + * @version V1.2.2 + * @date 27-February-2015 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer Header File. + * This file contains all the peripheral registers definitions, bits + * definitions and memory mapping for STM32F30x devices. + * + * The file is the unique include file that the application programmer + * is using in the C source code, usually in main.c. This file contains: + * - Configuration section that allows to select: + * - The device used in the target application + * - To use or not the peripherals drivers in application code(i.e. + * code will be based on direct access to peripherals registers + * rather than drivers API), this option is controlled by + * "#define USE_STDPERIPH_DRIVER" + * - To change few application-specific parameters such as the HSE + * crystal frequency + * - Data structures and the address mapping for all peripherals + * - Peripheral registers declarations and bits definition + * - Macros to access peripheral registers hardware + * + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x + * @{ + */ + +#ifndef __STM32F30x_H +#define __STM32F30x_H + +#ifdef __cplusplus + extern "C" { +#endif /* __cplusplus */ + +/** @addtogroup Library_configuration_section + * @{ + */ + +/* Uncomment the line below according to the target STM32 device used in your + application + */ + +/* Old STM32F30X definition, maintained for legacy purpose */ +#if defined(STM32F30X) + #define STM32F303xC +#endif /* STM32F30X */ + +#if !defined (STM32F303xC) && !defined (STM32F334x8) && !defined (STM32F302x8) && !defined (STM32F303xE) +/* #define STM32F303xC */ /*!< STM32F303CB, STM32F303CC, STM32F303RB, STM32F303RC, STM32F303VB, STM32F303VC + STM32F302CB, STM32F302CC, STM32F302RC, STM32F302RB, STM32F302VC, STM32F302VB, + STM32F358CC, STM32F358RC and STM32F358VC Devices */ +/* #define STM32F334x8 */ /*!< STM32F334C4, STM32F334K4, STM32F334C6, STM32F334R6, STM32F334K6, STM32F334C8, STM32F334R8, STM32F334K8, + STM32F303K8, STM32F303K6, STM32F303C8, STM32F303C6, STM32F303R8, STM32F303R6 and STM32F328C8 Devices */ +/* #define STM32F302x8 */ /*!< STM32F302K6, STM32F302K8, STM32F302C6, STM32F302C8, STM32F302R6, STM32F302R8, + STM32F301K8, STM32F301C8, STM32F301R8, STM32F301K6, STM32F301C6, STM32F301R6, STM32F313K8 and STM32F318C8 Devices */ +/* #define STM32F303xE */ /*!< STM32F303CE, STM32F303CD, STM32F303RE, STM32F303RD, STM32F303VE, STM32F303VD, STM32F303ZE, + STM32F303ZD, STM32F302CE, STM32F302CD, STM32F302RE, STM32F302RD, STM32F302VE, STM32F302ZE, + STM32F302ZD and STM32F398VE Devices */ +#endif /* STM32F303xC || STM32F334x8 || STM32F302x8 || STM32F303xE */ + + +/* Tip: To avoid modifying this file each time you need to switch between these + devices, you can define the device in your toolchain compiler preprocessor. + */ + +#if !defined (STM32F303xC) && !defined (STM32F334x8) && !defined (STM32F302x8) && !defined (STM32F303xE) + #error "Please select first the target STM32F30X device used in your application (in stm32f30x.h file)" +#endif + +#if !defined (USE_STDPERIPH_DRIVER) +/** + * @brief Comment the line below if you will not use the peripherals drivers. + In this case, these drivers will not be included and the application code will + be based on direct access to peripherals registers + */ + /* #define USE_STDPERIPH_DRIVER */ +#endif /* USE_STDPERIPH_DRIVER */ + +/** + * @brief In the following line adjust the value of External High Speed oscillator (HSE) + used in your application + + Tip: To avoid modifying this file each time you need to use different HSE, you + can define the HSE value in your toolchain compiler preprocessor. + */ +#if !defined (HSE_VALUE) + #define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +/** + * @brief In the following line adjust the External High Speed oscillator (HSE) Startup + Timeout value + */ +#if !defined (HSE_STARTUP_TIMEOUT) + #define HSE_STARTUP_TIMEOUT ((uint16_t)0x5000) /*!< Time out for HSE start up */ +#endif /* HSE_STARTUP_TIMEOUT */ + +/** + * @brief In the following line adjust the Internal High Speed oscillator (HSI) Startup + Timeout value + */ +#if !defined (HSI_STARTUP_TIMEOUT) + #define HSI_STARTUP_TIMEOUT ((uint16_t)0x5000) /*!< Time out for HSI start up */ +#endif /* HSI_STARTUP_TIMEOUT */ + +#if !defined (HSI_VALUE) + #define HSI_VALUE ((uint32_t)8000000) +#endif /* HSI_VALUE */ /*!< Value of the Internal High Speed oscillator in Hz. + The real value may vary depending on the variations + in voltage and temperature. */ +#if !defined (LSI_VALUE) + #define LSI_VALUE ((uint32_t)40000) +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz + The real value may vary depending on the variations + in voltage and temperature. */ +#if !defined (LSE_VALUE) + #define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */ +#endif /* LSE_VALUE */ + + +/** + * @brief STM32F30x Standard Peripherals Library version number V1.2.2 + */ +#define __STM32F30X_STDPERIPH_VERSION_MAIN (0x01) /*!< [31:24] main version */ +#define __STM32F30X_STDPERIPH_VERSION_SUB1 (0x02) /*!< [23:16] sub1 version */ +#define __STM32F30X_STDPERIPH_VERSION_SUB2 (0x02) /*!< [15:8] sub2 version */ +#define __STM32F30X_STDPERIPH_VERSION_RC (0x00) /*!< [7:0] release candidate */ +#define __STM32F30X_STDPERIPH_VERSION ( (__STM32F30X_STDPERIPH_VERSION_MAIN << 24)\ + |(__STM32F30X_STDPERIPH_VERSION_SUB1 << 16)\ + |(__STM32F30X_STDPERIPH_VERSION_SUB2 << 8)\ + |(__STM32F30X_STDPERIPH_VERSION_RC)) + +/** + * @} + */ + +/** @addtogroup Configuration_section_for_CMSIS + * @{ + */ + +/** + * @brief Configuration of the Cortex-M4 Processor and Core Peripherals + */ +#define __CM4_REV 0x0001 /*!< Core revision r0p1 */ +#define __MPU_PRESENT 1 /*!< STM32F30X provide an MPU */ +#define __NVIC_PRIO_BITS 4 /*!< STM32F30X uses 4 Bits for the Priority Levels */ +#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */ +#define __FPU_PRESENT 1 /*!< STM32F30X provide an FPU */ + + +/** + * @brief STM32F30X Interrupt Number Definition, according to the selected device + * in @ref Library_configuration_section + */ +typedef enum IRQn +{ +/****** Cortex-M4 Processor Exceptions Numbers ****************************************************************/ + NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */ + MemoryManagement_IRQn = -12, /*!< 4 Cortex-M4 Memory Management Interrupt */ + BusFault_IRQn = -11, /*!< 5 Cortex-M4 Bus Fault Interrupt */ + UsageFault_IRQn = -10, /*!< 6 Cortex-M4 Usage Fault Interrupt */ + SVCall_IRQn = -5, /*!< 11 Cortex-M4 SV Call Interrupt */ + DebugMonitor_IRQn = -4, /*!< 12 Cortex-M4 Debug Monitor Interrupt */ + PendSV_IRQn = -2, /*!< 14 Cortex-M4 Pend SV Interrupt */ + SysTick_IRQn = -1, /*!< 15 Cortex-M4 System Tick Interrupt */ +/****** STM32 specific Interrupt Numbers **********************************************************************/ +#ifdef STM32F303xC + WWDG_IRQn = 0, /*!< Window WatchDog Interrupt */ + PVD_IRQn = 1, /*!< PVD through EXTI Line detection Interrupt */ + TAMPER_STAMP_IRQn = 2, /*!< Tamper and TimeStamp interrupts */ + RTC_WKUP_IRQn = 3, /*!< RTC Wakeup interrupt through the EXTI lines 17, 19 & 20 */ + FLASH_IRQn = 4, /*!< FLASH global Interrupt */ + RCC_IRQn = 5, /*!< RCC global Interrupt */ + EXTI0_IRQn = 6, /*!< EXTI Line0 Interrupt */ + EXTI1_IRQn = 7, /*!< EXTI Line1 Interrupt */ + EXTI2_TS_IRQn = 8, /*!< EXTI Line2 Interrupt and Touch Sense Interrupt */ + EXTI3_IRQn = 9, /*!< EXTI Line3 Interrupt */ + EXTI4_IRQn = 10, /*!< EXTI Line4 Interrupt */ + DMA1_Channel1_IRQn = 11, /*!< DMA1 Channel 1 Interrupt */ + DMA1_Channel2_IRQn = 12, /*!< DMA1 Channel 2 Interrupt */ + DMA1_Channel3_IRQn = 13, /*!< DMA1 Channel 3 Interrupt */ + DMA1_Channel4_IRQn = 14, /*!< DMA1 Channel 4 Interrupt */ + DMA1_Channel5_IRQn = 15, /*!< DMA1 Channel 5 Interrupt */ + DMA1_Channel6_IRQn = 16, /*!< DMA1 Channel 6 Interrupt */ + DMA1_Channel7_IRQn = 17, /*!< DMA1 Channel 7 Interrupt */ + ADC1_2_IRQn = 18, /*!< ADC1 & ADC2 Interrupts */ + USB_HP_CAN1_TX_IRQn = 19, /*!< USB Device High Priority or CAN1 TX Interrupts */ + USB_LP_CAN1_RX0_IRQn = 20, /*!< USB Device Low Priority or CAN1 RX0 Interrupts */ + CAN1_RX1_IRQn = 21, /*!< CAN1 RX1 Interrupt */ + CAN1_SCE_IRQn = 22, /*!< CAN1 SCE Interrupt */ + EXTI9_5_IRQn = 23, /*!< External Line[9:5] Interrupts */ + TIM1_BRK_TIM15_IRQn = 24, /*!< TIM1 Break and TIM15 Interrupts */ + TIM1_UP_TIM16_IRQn = 25, /*!< TIM1 Update and TIM16 Interrupts */ + TIM1_TRG_COM_TIM17_IRQn = 26, /*!< TIM1 Trigger and Commutation and TIM17 Interrupt */ + TIM1_CC_IRQn = 27, /*!< TIM1 Capture Compare Interrupt */ + TIM2_IRQn = 28, /*!< TIM2 global Interrupt */ + TIM3_IRQn = 29, /*!< TIM3 global Interrupt */ + TIM4_IRQn = 30, /*!< TIM4 global Interrupt */ + I2C1_EV_IRQn = 31, /*!< I2C1 Event Interrupt */ + I2C1_ER_IRQn = 32, /*!< I2C1 Error Interrupt */ + I2C2_EV_IRQn = 33, /*!< I2C2 Event Interrupt */ + I2C2_ER_IRQn = 34, /*!< I2C2 Error Interrupt */ + SPI1_IRQn = 35, /*!< SPI1 global Interrupt */ + SPI2_IRQn = 36, /*!< SPI2 global Interrupt */ + USART1_IRQn = 37, /*!< USART1 global Interrupt */ + USART2_IRQn = 38, /*!< USART2 global Interrupt */ + USART3_IRQn = 39, /*!< USART3 global Interrupt */ + EXTI15_10_IRQn = 40, /*!< External Line[15:10] Interrupts */ + RTC_Alarm_IRQn = 41, /*!< RTC Alarm (A and B) through EXTI Line Interrupt */ + USBWakeUp_IRQn = 42, /*!< USB Wakeup Interrupt */ + TIM8_BRK_IRQn = 43, /*!< TIM8 Break Interrupt */ + TIM8_UP_IRQn = 44, /*!< TIM8 Update Interrupt */ + TIM8_TRG_COM_IRQn = 45, /*!< TIM8 Trigger and Commutation Interrupt */ + TIM8_CC_IRQn = 46, /*!< TIM8 Capture Compare Interrupt */ + ADC3_IRQn = 47, /*!< ADC3 global Interrupt */ + SPI3_IRQn = 51, /*!< SPI3 global Interrupt */ + UART4_IRQn = 52, /*!< UART4 global Interrupt */ + UART5_IRQn = 53, /*!< UART5 global Interrupt */ + TIM6_DAC_IRQn = 54, /*!< TIM6 global and DAC1&2 underrun error interrupts */ + TIM7_IRQn = 55, /*!< TIM7 global Interrupt */ + DMA2_Channel1_IRQn = 56, /*!< DMA2 Channel 1 global Interrupt */ + DMA2_Channel2_IRQn = 57, /*!< DMA2 Channel 2 global Interrupt */ + DMA2_Channel3_IRQn = 58, /*!< DMA2 Channel 3 global Interrupt */ + DMA2_Channel4_IRQn = 59, /*!< DMA2 Channel 4 global Interrupt */ + DMA2_Channel5_IRQn = 60, /*!< DMA2 Channel 5 global Interrupt */ + ADC4_IRQn = 61, /*!< ADC4 global Interrupt */ + COMP1_2_3_IRQn = 64, /*!< COMP1, COMP2 and COMP3 global Interrupt */ + COMP4_5_6_IRQn = 65, /*!< COMP5, COMP6 and COMP4 global Interrupt */ + COMP7_IRQn = 66, /*!< COMP7 global Interrupt */ + USB_HP_IRQn = 74, /*!< USB High Priority global Interrupt remap */ + USB_LP_IRQn = 75, /*!< USB Low Priority global Interrupt remap */ + USBWakeUp_RMP_IRQn = 76, /*!< USB Wakeup Interrupt remap */ + FPU_IRQn = 81 /*!< Floating point Interrupt */ +#endif /* STM32F303xC */ +#ifdef STM32F334x8 + WWDG_IRQn = 0, /*!< Window WatchDog Interrupt */ + PVD_IRQn = 1, /*!< PVD through EXTI Line detection Interrupt */ + TAMPER_STAMP_IRQn = 2, /*!< Tamper and TimeStamp interrupts */ + RTC_WKUP_IRQn = 3, /*!< RTC Wakeup interrupt through the EXTI lines 17, 19 & 20 */ + FLASH_IRQn = 4, /*!< FLASH global Interrupt */ + RCC_IRQn = 5, /*!< RCC global Interrupt */ + EXTI0_IRQn = 6, /*!< EXTI Line0 Interrupt */ + EXTI1_IRQn = 7, /*!< EXTI Line1 Interrupt */ + EXTI2_TS_IRQn = 8, /*!< EXTI Line2 Interrupt and Touch Sense Interrupt */ + EXTI3_IRQn = 9, /*!< EXTI Line3 Interrupt */ + EXTI4_IRQn = 10, /*!< EXTI Line4 Interrupt */ + DMA1_Channel1_IRQn = 11, /*!< DMA1 Channel 1 Interrupt */ + DMA1_Channel2_IRQn = 12, /*!< DMA1 Channel 2 Interrupt */ + DMA1_Channel3_IRQn = 13, /*!< DMA1 Channel 3 Interrupt */ + DMA1_Channel4_IRQn = 14, /*!< DMA1 Channel 4 Interrupt */ + DMA1_Channel5_IRQn = 15, /*!< DMA1 Channel 5 Interrupt */ + DMA1_Channel6_IRQn = 16, /*!< DMA1 Channel 6 Interrupt */ + DMA1_Channel7_IRQn = 17, /*!< DMA1 Channel 7 Interrupt */ + ADC1_2_IRQn = 18, /*!< ADC1 & ADC2 Interrupts */ + CAN1_TX_IRQn = 19, /*!< CAN1 TX Interrupts */ + CAN1_RX0_IRQn = 20, /*!< CAN1 RX0 Interrupts */ + CAN1_RX1_IRQn = 21, /*!< CAN1 RX1 Interrupt */ + CAN1_SCE_IRQn = 22, /*!< CAN1 SCE Interrupt */ + EXTI9_5_IRQn = 23, /*!< External Line[9:5] Interrupts */ + TIM1_BRK_TIM15_IRQn = 24, /*!< TIM1 Break and TIM15 Interrupts */ + TIM1_UP_TIM16_IRQn = 25, /*!< TIM1 Update and TIM16 Interrupts */ + TIM1_TRG_COM_TIM17_IRQn = 26, /*!< TIM1 Trigger and Commutation and TIM17 Interrupt */ + TIM1_CC_IRQn = 27, /*!< TIM1 Capture Compare Interrupt */ + TIM2_IRQn = 28, /*!< TIM2 global Interrupt */ + TIM3_IRQn = 29, /*!< TIM3 global Interrupt */ + I2C1_EV_IRQn = 31, /*!< I2C1 Event Interrupt */ + I2C1_ER_IRQn = 32, /*!< I2C1 Error Interrupt */ + SPI1_IRQn = 35, /*!< SPI1 global Interrupt */ + USART1_IRQn = 37, /*!< USART1 global Interrupt */ + USART2_IRQn = 38, /*!< USART2 global Interrupt */ + USART3_IRQn = 39, /*!< USART3 global Interrupt */ + EXTI15_10_IRQn = 40, /*!< External Line[15:10] Interrupts */ + RTC_Alarm_IRQn = 41, /*!< RTC Alarm (A and B) through EXTI Line Interrupt */ + TIM6_DAC1_IRQn = 54, /*!< TIM6 global and DAC1 underrun error interrupts */ + TIM7_DAC2_IRQn = 55, /*!< TIM7 global and DAC2 underrun error Interrupt */ + COMP2_IRQn = 64, /*!< COMP2 global Interrupt */ + COMP4_6_IRQn = 65, /*!< COMP6 and COMP4 global Interrupt */ + HRTIM1_Master_IRQn = 67, /*!< HRTIM Master Timer global Interrupts */ + HRTIM1_TIMA_IRQn = 68, /*!< HRTIM Timer A global Interrupt */ + HRTIM1_TIMB_IRQn = 69, /*!< HRTIM Timer B global Interrupt */ + HRTIM1_TIMC_IRQn = 70, /*!< HRTIM Timer C global Interrupt */ + HRTIM1_TIMD_IRQn = 71, /*!< HRTIM Timer D global Interrupt */ + HRTIM1_TIME_IRQn = 72, /*!< HRTIM Timer E global Interrupt */ + HRTIM1_FLT_IRQn = 73, /*!< HRTIM Fault global Interrupt */ + FPU_IRQn = 81 /*!< Floating point Interrupt */ +#endif /* STM32F334x8 */ +#ifdef STM32F302x8 + WWDG_IRQn = 0, /*!< Window WatchDog Interrupt */ + PVD_IRQn = 1, /*!< PVD through EXTI Line detection Interrupt */ + TAMPER_STAMP_IRQn = 2, /*!< Tamper and TimeStamp interrupts */ + RTC_WKUP_IRQn = 3, /*!< RTC Wakeup interrupt through the EXTI lines 20 */ + FLASH_IRQn = 4, /*!< FLASH global Interrupt */ + RCC_IRQn = 5, /*!< RCC global Interrupt */ + EXTI0_IRQn = 6, /*!< EXTI Line0 Interrupt */ + EXTI1_IRQn = 7, /*!< EXTI Line1 Interrupt */ + EXTI2_TS_IRQn = 8, /*!< EXTI Line2 Interrupt and Touch Sense Interrupt */ + EXTI3_IRQn = 9, /*!< EXTI Line3 Interrupt */ + EXTI4_IRQn = 10, /*!< EXTI Line4 Interrupt */ + DMA1_Channel1_IRQn = 11, /*!< DMA1 Channel 1 Interrupt */ + DMA1_Channel2_IRQn = 12, /*!< DMA1 Channel 2 Interrupt */ + DMA1_Channel3_IRQn = 13, /*!< DMA1 Channel 3 Interrupt */ + DMA1_Channel4_IRQn = 14, /*!< DMA1 Channel 4 Interrupt */ + DMA1_Channel5_IRQn = 15, /*!< DMA1 Channel 5 Interrupt */ + DMA1_Channel6_IRQn = 16, /*!< DMA1 Channel 6 Interrupt */ + DMA1_Channel7_IRQn = 17, /*!< DMA1 Channel 7 Interrupt */ + ADC1_IRQn = 18, /*!< ADC1 Interrupts */ + USB_HP_CAN1_TX_IRQn = 19, /*!< USB Device High Priority or CAN1 TX Interrupts */ + USB_LP_CAN1_RX0_IRQn = 20, /*!< USB Device Low Priority or CAN1 RX0 Interrupts */ + CAN1_RX1_IRQn = 21, /*!< CAN1 RX1 Interrupt */ + CAN1_SCE_IRQn = 22, /*!< CAN1 SCE Interrupt */ + EXTI9_5_IRQn = 23, /*!< External Line[9:5] Interrupts */ + TIM1_BRK_TIM15_IRQn = 24, /*!< TIM1 Break and TIM15 Interrupts */ + TIM1_UP_TIM16_IRQn = 25, /*!< TIM1 Update and TIM16 Interrupts */ + TIM1_TRG_COM_TIM17_IRQn = 26, /*!< TIM1 Trigger and Commutation and TIM17 Interrupt */ + TIM1_CC_IRQn = 27, /*!< TIM1 Capture Compare Interrupt */ + TIM2_IRQn = 28, /*!< TIM2 global Interrupt */ + I2C1_EV_IRQn = 31, /*!< I2C1 Event Interrupt */ + I2C1_ER_IRQn = 32, /*!< I2C1 Error Interrupt */ + I2C2_EV_IRQn = 33, /*!< I2C2 Event Interrupt */ + I2C2_ER_IRQn = 34, /*!< I2C2 Error Interrupt */ + SPI2_IRQn = 36, /*!< SPI2 global Interrupt */ + USART1_IRQn = 37, /*!< USART1 global Interrupt */ + USART2_IRQn = 38, /*!< USART2 global Interrupt */ + USART3_IRQn = 39, /*!< USART3 global Interrupt */ + EXTI15_10_IRQn = 40, /*!< External Line[15:10] Interrupts */ + RTC_Alarm_IRQn = 41, /*!< RTC Alarm (A and B) through EXTI Line Interrupt */ + USBWakeUp_IRQn = 42, /*!< USB Wakeup Interrupt */ + SPI3_IRQn = 51, /*!< SPI3 global Interrupt */ + TIM6_DAC_IRQn = 54, /*!< TIM6 global and DAC1&2 underrun error interrupts */ + COMP2_IRQn = 64, /*!< COMP2 global Interrupt */ + COMP4_6_IRQn = 65, /*!< COMP5, COMP6 and COMP4 global Interrupt */ + COMP7_IRQn = 66, /*!< COMP7 global Interrupt */ + I2C3_EV_IRQn = 72, /*!< I2C3 Event Interrupt */ + I2C3_ER_IRQn = 73, /*!< I2C3 Error Interrupt */ + USB_HP_IRQn = 74, /*!< USB High Priority global Interrupt remap */ + USB_LP_IRQn = 75, /*!< USB Low Priority global Interrupt remap */ + USBWakeUp_RMP_IRQn = 76, /*!< USB Wakeup Interrupt remap */ + FPU_IRQn = 81 /*!< Floating point Interrupt */ +#endif /* STM32F302x8 */ +#ifdef STM32F303xE + WWDG_IRQn = 0, /*!< Window WatchDog Interrupt */ + PVD_IRQn = 1, /*!< PVD through EXTI Line detection Interrupt */ + TAMPER_STAMP_IRQn = 2, /*!< Tamper and TimeStamp interrupts */ + RTC_WKUP_IRQn = 3, /*!< RTC Wakeup interrupt through the EXTI lines 17, 19 & 20 */ + FLASH_IRQn = 4, /*!< FLASH global Interrupt */ + RCC_IRQn = 5, /*!< RCC global Interrupt */ + EXTI0_IRQn = 6, /*!< EXTI Line0 Interrupt */ + EXTI1_IRQn = 7, /*!< EXTI Line1 Interrupt */ + EXTI2_TS_IRQn = 8, /*!< EXTI Line2 Interrupt and Touch Sense Interrupt */ + EXTI3_IRQn = 9, /*!< EXTI Line3 Interrupt */ + EXTI4_IRQn = 10, /*!< EXTI Line4 Interrupt */ + DMA1_Channel1_IRQn = 11, /*!< DMA1 Channel 1 Interrupt */ + DMA1_Channel2_IRQn = 12, /*!< DMA1 Channel 2 Interrupt */ + DMA1_Channel3_IRQn = 13, /*!< DMA1 Channel 3 Interrupt */ + DMA1_Channel4_IRQn = 14, /*!< DMA1 Channel 4 Interrupt */ + DMA1_Channel5_IRQn = 15, /*!< DMA1 Channel 5 Interrupt */ + DMA1_Channel6_IRQn = 16, /*!< DMA1 Channel 6 Interrupt */ + DMA1_Channel7_IRQn = 17, /*!< DMA1 Channel 7 Interrupt */ + ADC1_2_IRQn = 18, /*!< ADC1 & ADC2 Interrupts */ + USB_HP_CAN1_TX_IRQn = 19, /*!< USB Device High Priority or CAN1 TX Interrupts */ + USB_LP_CAN1_RX0_IRQn = 20, /*!< USB Device Low Priority or CAN1 RX0 Interrupts */ + CAN1_RX1_IRQn = 21, /*!< CAN1 RX1 Interrupt */ + CAN1_SCE_IRQn = 22, /*!< CAN1 SCE Interrupt */ + EXTI9_5_IRQn = 23, /*!< External Line[9:5] Interrupts */ + TIM1_BRK_TIM15_IRQn = 24, /*!< TIM1 Break and TIM15 Interrupts */ + TIM1_UP_TIM16_IRQn = 25, /*!< TIM1 Update and TIM16 Interrupts */ + TIM1_TRG_COM_TIM17_IRQn = 26, /*!< TIM1 Trigger and Commutation and TIM17 Interrupt */ + TIM1_CC_IRQn = 27, /*!< TIM1 Capture Compare Interrupt */ + TIM2_IRQn = 28, /*!< TIM2 global Interrupt */ + TIM3_IRQn = 29, /*!< TIM3 global Interrupt */ + TIM4_IRQn = 30, /*!< TIM4 global Interrupt */ + I2C1_EV_IRQn = 31, /*!< I2C1 Event Interrupt */ + I2C1_ER_IRQn = 32, /*!< I2C1 Error Interrupt */ + I2C2_EV_IRQn = 33, /*!< I2C2 Event Interrupt */ + I2C2_ER_IRQn = 34, /*!< I2C2 Error Interrupt */ + SPI1_IRQn = 35, /*!< SPI1 global Interrupt */ + SPI2_IRQn = 36, /*!< SPI2 global Interrupt */ + USART1_IRQn = 37, /*!< USART1 global Interrupt */ + USART2_IRQn = 38, /*!< USART2 global Interrupt */ + USART3_IRQn = 39, /*!< USART3 global Interrupt */ + EXTI15_10_IRQn = 40, /*!< External Line[15:10] Interrupts */ + RTC_Alarm_IRQn = 41, /*!< RTC Alarm (A and B) through EXTI Line Interrupt */ + USBWakeUp_IRQn = 42, /*!< USB Wakeup Interrupt */ + TIM8_BRK_IRQn = 43, /*!< TIM8 Break Interrupt */ + TIM8_UP_IRQn = 44, /*!< TIM8 Update Interrupt */ + TIM8_TRG_COM_IRQn = 45, /*!< TIM8 Trigger and Commutation Interrupt */ + TIM8_CC_IRQn = 46, /*!< TIM8 Capture Compare Interrupt */ + ADC3_IRQn = 47, /*!< ADC3 global Interrupt */ + FMC_IRQn = 48, /*!< FMC global Interrupt */ + SPI3_IRQn = 51, /*!< SPI3 global Interrupt */ + UART4_IRQn = 52, /*!< UART4 global Interrupt */ + UART5_IRQn = 53, /*!< UART5 global Interrupt */ + TIM6_DAC_IRQn = 54, /*!< TIM6 global and DAC1&2 underrun error interrupts */ + TIM7_IRQn = 55, /*!< TIM7 global Interrupt */ + DMA2_Channel1_IRQn = 56, /*!< DMA2 Channel 1 global Interrupt */ + DMA2_Channel2_IRQn = 57, /*!< DMA2 Channel 2 global Interrupt */ + DMA2_Channel3_IRQn = 58, /*!< DMA2 Channel 3 global Interrupt */ + DMA2_Channel4_IRQn = 59, /*!< DMA2 Channel 4 global Interrupt */ + DMA2_Channel5_IRQn = 60, /*!< DMA2 Channel 5 global Interrupt */ + ADC4_IRQn = 61, /*!< ADC4 global Interrupt */ + COMP1_2_3_IRQn = 64, /*!< COMP1, COMP2 and COMP3 global Interrupt */ + COMP4_5_6_IRQn = 65, /*!< COMP5, COMP6 and COMP4 global Interrupt */ + COMP7_IRQn = 66, /*!< COMP7 global Interrupt */ + I2C3_EV_IRQn = 72, /*!< I2C3 event interrupt */ + I2C3_ER_IRQn = 73, /*!< I2C3 error interrupt */ + USB_HP_IRQn = 74, /*!< USB High Priority global Interrupt remap */ + USB_LP_IRQn = 75, /*!< USB Low Priority global Interrupt remap */ + USBWakeUp_RMP_IRQn = 76, /*!< USB Wakeup Interrupt remap */ + TIM20_BRK_IRQn = 77, /*!< TIM20 Break Interrupt */ + TIM20_UP_IRQn = 78, /*!< TIM20 Update Interrupt */ + TIM20_TRG_COM_IRQn = 79, /*!< TIM20 Trigger and Commutation Interrupt */ + TIM20_CC_IRQn = 80, /*!< TIM20 Capture Compare Interrupt */ + FPU_IRQn = 81, /*!< Floating point Interrupt */ + SPI4_IRQn = 84 /*!< SPI4 global Interrupt */ +#endif /* STM32F303xE */ +} IRQn_Type; + +/** + * @} + */ + +#include "core_cm4.h" /* Cortex-M4 processor and core peripherals */ +#include "system_stm32f30x.h" /* STM32F30x System Header */ +#include + +/** @addtogroup Exported_types + * @{ + */ +/*!< STM32F10x Standard Peripheral Library old types (maintained for legacy purpose) */ +typedef int32_t s32; +typedef int16_t s16; +typedef int8_t s8; + +typedef const int32_t sc32; /*!< Read Only */ +typedef const int16_t sc16; /*!< Read Only */ +typedef const int8_t sc8; /*!< Read Only */ + +typedef __IO int32_t vs32; +typedef __IO int16_t vs16; +typedef __IO int8_t vs8; + +typedef __I int32_t vsc32; /*!< Read Only */ +typedef __I int16_t vsc16; /*!< Read Only */ +typedef __I int8_t vsc8; /*!< Read Only */ + +typedef uint32_t u32; +typedef uint16_t u16; +typedef uint8_t u8; + +typedef const uint32_t uc32; /*!< Read Only */ +typedef const uint16_t uc16; /*!< Read Only */ +typedef const uint8_t uc8; /*!< Read Only */ + +typedef __IO uint32_t vu32; +typedef __IO uint16_t vu16; +typedef __IO uint8_t vu8; + +typedef __I uint32_t vuc32; /*!< Read Only */ +typedef __I uint16_t vuc16; /*!< Read Only */ +typedef __I uint8_t vuc8; /*!< Read Only */ + +typedef enum {RESET = 0, SET = !RESET} FlagStatus, ITStatus; + +typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; +#define IS_FUNCTIONAL_STATE(STATE) (((STATE) == DISABLE) || ((STATE) == ENABLE)) + +typedef enum {ERROR = 0, SUCCESS = !ERROR} ErrorStatus; + +/** + * @} + */ + +/** @addtogroup Peripheral_registers_structures + * @{ + */ + +/** + * @brief Analog to Digital Converter + */ + +typedef struct +{ + __IO uint32_t ISR; /*!< ADC Interrupt and Status Register, Address offset: 0x00 */ + __IO uint32_t IER; /*!< ADC Interrupt Enable Register, Address offset: 0x04 */ + __IO uint32_t CR; /*!< ADC control register, Address offset: 0x08 */ + __IO uint32_t CFGR; /*!< ADC Configuration register, Address offset: 0x0C */ + uint32_t RESERVED0; /*!< Reserved, 0x010 */ + __IO uint32_t SMPR1; /*!< ADC sample time register 1, Address offset: 0x14 */ + __IO uint32_t SMPR2; /*!< ADC sample time register 2, Address offset: 0x18 */ + uint32_t RESERVED1; /*!< Reserved, 0x01C */ + __IO uint32_t TR1; /*!< ADC watchdog threshold register 1, Address offset: 0x20 */ + __IO uint32_t TR2; /*!< ADC watchdog threshold register 2, Address offset: 0x24 */ + __IO uint32_t TR3; /*!< ADC watchdog threshold register 3, Address offset: 0x28 */ + uint32_t RESERVED2; /*!< Reserved, 0x02C */ + __IO uint32_t SQR1; /*!< ADC regular sequence register 1, Address offset: 0x30 */ + __IO uint32_t SQR2; /*!< ADC regular sequence register 2, Address offset: 0x34 */ + __IO uint32_t SQR3; /*!< ADC regular sequence register 3, Address offset: 0x38 */ + __IO uint32_t SQR4; /*!< ADC regular sequence register 4, Address offset: 0x3C */ + __IO uint32_t DR; /*!< ADC regular data register, Address offset: 0x40 */ + uint32_t RESERVED3; /*!< Reserved, 0x044 */ + uint32_t RESERVED4; /*!< Reserved, 0x048 */ + __IO uint32_t JSQR; /*!< ADC injected sequence register, Address offset: 0x4C */ + uint32_t RESERVED5[4]; /*!< Reserved, 0x050 - 0x05C */ + __IO uint32_t OFR1; /*!< ADC offset register 1, Address offset: 0x60 */ + __IO uint32_t OFR2; /*!< ADC offset register 2, Address offset: 0x64 */ + __IO uint32_t OFR3; /*!< ADC offset register 3, Address offset: 0x68 */ + __IO uint32_t OFR4; /*!< ADC offset register 4, Address offset: 0x6C */ + uint32_t RESERVED6[4]; /*!< Reserved, 0x070 - 0x07C */ + __IO uint32_t JDR1; /*!< ADC injected data register 1, Address offset: 0x80 */ + __IO uint32_t JDR2; /*!< ADC injected data register 2, Address offset: 0x84 */ + __IO uint32_t JDR3; /*!< ADC injected data register 3, Address offset: 0x88 */ + __IO uint32_t JDR4; /*!< ADC injected data register 4, Address offset: 0x8C */ + uint32_t RESERVED7[4]; /*!< Reserved, 0x090 - 0x09C */ + __IO uint32_t AWD2CR; /*!< ADC Analog Watchdog 2 Configuration Register, Address offset: 0xA0 */ + __IO uint32_t AWD3CR; /*!< ADC Analog Watchdog 3 Configuration Register, Address offset: 0xA4 */ + uint32_t RESERVED8; /*!< Reserved, 0x0A8 */ + uint32_t RESERVED9; /*!< Reserved, 0x0AC */ + __IO uint32_t DIFSEL; /*!< ADC Differential Mode Selection Register, Address offset: 0xB0 */ + __IO uint32_t CALFACT; /*!< ADC Calibration Factors, Address offset: 0xB4 */ + +} ADC_TypeDef; + +typedef struct +{ + __IO uint32_t CSR; /*!< ADC Common status register, Address offset: ADC1/3 base address + 0x300 */ + uint32_t RESERVED; /*!< Reserved, ADC1/3 base address + 0x304 */ + __IO uint32_t CCR; /*!< ADC common control register, Address offset: ADC1/3 base address + 0x308 */ + __IO uint32_t CDR; /*!< ADC common regular data register for dual + modes, Address offset: ADC1/3 base address + 0x30A */ +} ADC_Common_TypeDef; + + +/** + * @brief Controller Area Network TxMailBox + */ +typedef struct +{ + __IO uint32_t TIR; /*!< CAN TX mailbox identifier register */ + __IO uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */ + __IO uint32_t TDLR; /*!< CAN mailbox data low register */ + __IO uint32_t TDHR; /*!< CAN mailbox data high register */ +} CAN_TxMailBox_TypeDef; + +/** + * @brief Controller Area Network FIFOMailBox + */ +typedef struct +{ + __IO uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */ + __IO uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */ + __IO uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */ + __IO uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */ +} CAN_FIFOMailBox_TypeDef; + +/** + * @brief Controller Area Network FilterRegister + */ +typedef struct +{ + __IO uint32_t FR1; /*!< CAN Filter bank register 1 */ + __IO uint32_t FR2; /*!< CAN Filter bank register 1 */ +} CAN_FilterRegister_TypeDef; + +/** + * @brief Controller Area Network + */ +typedef struct +{ + __IO uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */ + __IO uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */ + __IO uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */ + __IO uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */ + __IO uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */ + __IO uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */ + __IO uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */ + __IO uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */ + uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */ + CAN_TxMailBox_TypeDef sTxMailBox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */ + CAN_FIFOMailBox_TypeDef sFIFOMailBox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */ + uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */ + __IO uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */ + __IO uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */ + uint32_t RESERVED2; /*!< Reserved, 0x208 */ + __IO uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */ + uint32_t RESERVED3; /*!< Reserved, 0x210 */ + __IO uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */ + uint32_t RESERVED4; /*!< Reserved, 0x218 */ + __IO uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */ + uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */ + CAN_FilterRegister_TypeDef sFilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */ +} CAN_TypeDef; + + +/** + * @brief Analog Comparators + */ + +typedef struct +{ + __IO uint32_t CSR; /*!< Comparator control Status register, Address offset: 0x00 */ +} COMP_TypeDef; + +/** + * @brief CRC calculation unit + */ + +typedef struct +{ + __IO uint32_t DR; /*!< CRC Data register, Address offset: 0x00 */ + __IO uint8_t IDR; /*!< CRC Independent data register, Address offset: 0x04 */ + uint8_t RESERVED0; /*!< Reserved, 0x05 */ + uint16_t RESERVED1; /*!< Reserved, 0x06 */ + __IO uint32_t CR; /*!< CRC Control register, Address offset: 0x08 */ + uint32_t RESERVED2; /*!< Reserved, 0x0C */ + __IO uint32_t INIT; /*!< Initial CRC value register, Address offset: 0x10 */ + __IO uint32_t POL; /*!< CRC polynomial register, Address offset: 0x14 */ +} CRC_TypeDef; + +/** + * @brief Digital to Analog Converter + */ + +typedef struct +{ + __IO uint32_t CR; /*!< DAC control register, Address offset: 0x00 */ + __IO uint32_t SWTRIGR; /*!< DAC software trigger register, Address offset: 0x04 */ + __IO uint32_t DHR12R1; /*!< DAC channel1 12-bit right-aligned data holding register, Address offset: 0x08 */ + __IO uint32_t DHR12L1; /*!< DAC channel1 12-bit left aligned data holding register, Address offset: 0x0C */ + __IO uint32_t DHR8R1; /*!< DAC channel1 8-bit right aligned data holding register, Address offset: 0x10 */ + __IO uint32_t DHR12R2; /*!< DAC channel2 12-bit right aligned data holding register, Address offset: 0x14 */ + __IO uint32_t DHR12L2; /*!< DAC channel2 12-bit left aligned data holding register, Address offset: 0x18 */ + __IO uint32_t DHR8R2; /*!< DAC channel2 8-bit right-aligned data holding register, Address offset: 0x1C */ + __IO uint32_t DHR12RD; /*!< Dual DAC 12-bit right-aligned data holding register, Address offset: 0x20 */ + __IO uint32_t DHR12LD; /*!< DUAL DAC 12-bit left aligned data holding register, Address offset: 0x24 */ + __IO uint32_t DHR8RD; /*!< DUAL DAC 8-bit right aligned data holding register, Address offset: 0x28 */ + __IO uint32_t DOR1; /*!< DAC channel1 data output register, Address offset: 0x2C */ + __IO uint32_t DOR2; /*!< DAC channel2 data output register, Address offset: 0x30 */ + __IO uint32_t SR; /*!< DAC status register, Address offset: 0x34 */ +} DAC_TypeDef; + +/** + * @brief Debug MCU + */ + +typedef struct +{ + __IO uint32_t IDCODE; /*!< MCU device ID code, Address offset: 0x00 */ + __IO uint32_t CR; /*!< Debug MCU configuration register, Address offset: 0x04 */ + __IO uint32_t APB1FZ; /*!< Debug MCU APB1 freeze register, Address offset: 0x08 */ + __IO uint32_t APB2FZ; /*!< Debug MCU APB2 freeze register, Address offset: 0x0C */ +}DBGMCU_TypeDef; + +/** + * @brief DMA Controller + */ + +typedef struct +{ + __IO uint32_t CCR; /*!< DMA channel x configuration register */ + __IO uint32_t CNDTR; /*!< DMA channel x number of data register */ + __IO uint32_t CPAR; /*!< DMA channel x peripheral address register */ + __IO uint32_t CMAR; /*!< DMA channel x memory address register */ +} DMA_Channel_TypeDef; + +typedef struct +{ + __IO uint32_t ISR; /*!< DMA interrupt status register, Address offset: 0x00 */ + __IO uint32_t IFCR; /*!< DMA interrupt clear flag register, Address offset: 0x04 */ +} DMA_TypeDef; + +/** + * @brief External Interrupt/Event Controller + */ + +typedef struct +{ + __IO uint32_t IMR; /*!< EXTI Interrupt mask register, Address offset: 0x00 */ + __IO uint32_t EMR; /*!< EXTI Event mask register, Address offset: 0x04 */ + __IO uint32_t RTSR; /*!< EXTI Rising trigger selection register, Address offset: 0x08 */ + __IO uint32_t FTSR; /*!< EXTI Falling trigger selection register, Address offset: 0x0C */ + __IO uint32_t SWIER; /*!< EXTI Software interrupt event register, Address offset: 0x10 */ + __IO uint32_t PR; /*!< EXTI Pending register, Address offset: 0x14 */ + uint32_t RESERVED1; /*!< Reserved, 0x18 */ + uint32_t RESERVED2; /*!< Reserved, 0x1C */ + __IO uint32_t IMR2; /*!< EXTI Interrupt mask register, Address offset: 0x20 */ + __IO uint32_t EMR2; /*!< EXTI Event mask register, Address offset: 0x24 */ + __IO uint32_t RTSR2; /*!< EXTI Rising trigger selection register, Address offset: 0x28 */ + __IO uint32_t FTSR2; /*!< EXTI Falling trigger selection register, Address offset: 0x2C */ + __IO uint32_t SWIER2; /*!< EXTI Software interrupt event register, Address offset: 0x30 */ + __IO uint32_t PR2; /*!< EXTI Pending register, Address offset: 0x34 */ +}EXTI_TypeDef; + +/** + * @brief FLASH Registers + */ + +typedef struct +{ + __IO uint32_t ACR; /*!< FLASH access control register, Address offset: 0x00 */ + __IO uint32_t KEYR; /*!< FLASH key register, Address offset: 0x04 */ + __IO uint32_t OPTKEYR; /*!< FLASH option key register, Address offset: 0x08 */ + __IO uint32_t SR; /*!< FLASH status register, Address offset: 0x0C */ + __IO uint32_t CR; /*!< FLASH control register, Address offset: 0x10 */ + __IO uint32_t AR; /*!< FLASH address register, Address offset: 0x14 */ + uint32_t RESERVED; /*!< Reserved, 0x18 */ + __IO uint32_t OBR; /*!< FLASH Option byte register, Address offset: 0x1C */ + __IO uint32_t WRPR; /*!< FLASH Write register, Address offset: 0x20 */ + +} FLASH_TypeDef; + +/** + * @brief Flexible Memory Controller + */ + +typedef struct +{ + __IO uint32_t BTCR[8]; /*!< NOR/PSRAM chip-select control register(BCR) and chip-select timing register(BTR), Address offset: 0x00-1C */ +} FMC_Bank1_TypeDef; + +/** + * @brief Flexible Memory Controller Bank1E + */ + +typedef struct +{ + __IO uint32_t BWTR[7]; /*!< NOR/PSRAM write timing registers, Address offset: 0x104-0x11C */ +} FMC_Bank1E_TypeDef; + +/** + * @brief Flexible Memory Controller Bank2 + */ + +typedef struct +{ + __IO uint32_t PCR2; /*!< NAND Flash control register 2, Address offset: 0x60 */ + __IO uint32_t SR2; /*!< NAND Flash FIFO status and interrupt register 2, Address offset: 0x64 */ + __IO uint32_t PMEM2; /*!< NAND Flash Common memory space timing register 2, Address offset: 0x68 */ + __IO uint32_t PATT2; /*!< NAND Flash Attribute memory space timing register 2, Address offset: 0x6C */ + uint32_t RESERVED0; /*!< Reserved, 0x70 */ + __IO uint32_t ECCR2; /*!< NAND Flash ECC result registers 2, Address offset: 0x74 */ +} FMC_Bank2_TypeDef; + +/** + * @brief Flexible Memory Controller Bank3 + */ + +typedef struct +{ + __IO uint32_t PCR3; /*!< NAND Flash control register 3, Address offset: 0x80 */ + __IO uint32_t SR3; /*!< NAND Flash FIFO status and interrupt register 3, Address offset: 0x84 */ + __IO uint32_t PMEM3; /*!< NAND Flash Common memory space timing register 3, Address offset: 0x88 */ + __IO uint32_t PATT3; /*!< NAND Flash Attribute memory space timing register 3, Address offset: 0x8C */ + uint32_t RESERVED0; /*!< Reserved, 0x90 */ + __IO uint32_t ECCR3; /*!< NAND Flash ECC result registers 3, Address offset: 0x94 */ +} FMC_Bank3_TypeDef; + +/** + * @brief Flexible Memory Controller Bank4 + */ + +typedef struct +{ + __IO uint32_t PCR4; /*!< PC Card control register 4, Address offset: 0xA0 */ + __IO uint32_t SR4; /*!< PC Card FIFO status and interrupt register 4, Address offset: 0xA4 */ + __IO uint32_t PMEM4; /*!< PC Card Common memory space timing register 4, Address offset: 0xA8 */ + __IO uint32_t PATT4; /*!< PC Card Attribute memory space timing register 4, Address offset: 0xAC */ + __IO uint32_t PIO4; /*!< PC Card I/O space timing register 4, Address offset: 0xB0 */ +} FMC_Bank4_TypeDef; + +/** + * @brief Option Bytes Registers + */ +typedef struct +{ + __IO uint16_t RDP; /*!
© COPYRIGHT 2015 STMicroelectronics
+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_adc.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_adc.h new file mode 100755 index 0000000000..00724e95a1 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_adc.h @@ -0,0 +1,820 @@ +/** + ****************************************************************************** + * @file stm32f30x_adc.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the ADC firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_ADC_H +#define __STM32F30x_ADC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup ADC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief ADC Init structure definition + */ +typedef struct +{ + + uint32_t ADC_ContinuousConvMode; /*!< Specifies whether the conversion is performed in + Continuous or Single mode. + This parameter can be set to ENABLE or DISABLE. */ + uint32_t ADC_Resolution; /*!< Configures the ADC resolution. + This parameter can be a value of @ref ADC_resolution */ + uint32_t ADC_ExternalTrigConvEvent; /*!< Defines the external trigger used to start the analog + to digital conversion of regular channels. This parameter + can be a value of @ref ADC_external_trigger_sources_for_regular_channels_conversion */ + uint32_t ADC_ExternalTrigEventEdge; /*!< Select the external trigger edge and enable the trigger of a regular group. + This parameter can be a value of + @ref ADC_external_trigger_edge_for_regular_channels_conversion */ + uint32_t ADC_DataAlign; /*!< Specifies whether the ADC data alignment is left or right. + This parameter can be a value of @ref ADC_data_align */ + uint32_t ADC_OverrunMode; /*!< Specifies the way data overrun are managed. + This parameter can be set to ENABLE or DISABLE. */ + uint32_t ADC_AutoInjMode; /*!< Enable/disable automatic injected group conversion after + regular group conversion. + This parameter can be set to ENABLE or DISABLE. */ + uint8_t ADC_NbrOfRegChannel; /*!< Specifies the number of ADC channels that will be converted + using the sequencer for regular channel group. + This parameter must range from 1 to 16. */ +}ADC_InitTypeDef; + +/** + * @} + */ +/** + * @brief ADC Init structure definition + */ +typedef struct +{ + + uint32_t ADC_ExternalTrigInjecConvEvent; /*!< Defines the external trigger used to start the analog + to digital conversion of injected channels. This parameter + can be a value of @ref ADC_external_trigger_sources_for_Injected_channels_conversion */ + uint32_t ADC_ExternalTrigInjecEventEdge; /*!< Select the external trigger edge and enable the trigger of an injected group. + This parameter can be a value of + @ref ADC_external_trigger_edge_for_Injected_channels_conversion */ + uint8_t ADC_NbrOfInjecChannel; /*!< Specifies the number of ADC channels that will be converted + using the sequencer for injected channel group. + This parameter must range from 1 to 4. */ + uint32_t ADC_InjecSequence1; + uint32_t ADC_InjecSequence2; + uint32_t ADC_InjecSequence3; + uint32_t ADC_InjecSequence4; +}ADC_InjectedInitTypeDef; + +/** + * @} + */ +typedef struct +{ + uint32_t ADC_Mode; /*!< Configures the ADC to operate in + independent or multi mode. + This parameter can be a value of @ref ADC_mode */ + uint32_t ADC_Clock; /*!< Select the clock of the ADC. The clock is common for both master + and slave ADCs. + This parameter can be a value of @ref ADC_Clock */ + uint32_t ADC_DMAAccessMode; /*!< Configures the Direct memory access mode for multi ADC mode. + This parameter can be a value of + @ref ADC_Direct_memory_access_mode_for_multi_mode */ + uint32_t ADC_DMAMode; /*!< Configures the DMA mode for ADC. + This parameter can be a value of @ref ADC_DMA_Mode_definition */ + uint8_t ADC_TwoSamplingDelay; /*!< Configures the Delay between 2 sampling phases. + This parameter can be a value between 0x0 and 0xF */ + +}ADC_CommonInitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup ADC_Exported_Constants + * @{ + */ + +#define IS_ADC_ALL_PERIPH(PERIPH) (((PERIPH) == ADC1) || \ + ((PERIPH) == ADC2) || \ + ((PERIPH) == ADC3) || \ + ((PERIPH) == ADC4)) + +#define IS_ADC_DMA_PERIPH(PERIPH) (((PERIPH) == ADC1) || \ + ((PERIPH) == ADC2) || \ + ((PERIPH) == ADC3) || \ + ((PERIPH) == ADC4)) + +/** @defgroup ADC_ContinuousConvMode + * @{ + */ +#define ADC_ContinuousConvMode_Enable ((uint32_t)0x00002000) /*!< ADC continuous conversion mode enable */ +#define ADC_ContinuousConvMode_Disable ((uint32_t)0x00000000) /*!< ADC continuous conversion mode disable */ +#define IS_ADC_CONVMODE(MODE) (((MODE) == ADC_ContinuousConvMode_Enable) || \ + ((MODE) == ADC_ContinuousConvMode_Disable)) +/** + * @} + */ +/** @defgroup ADC_OverunMode + * @{ + */ +#define ADC_OverrunMode_Enable ((uint32_t)0x00001000) /*!< ADC Overrun Mode enable */ +#define ADC_OverrunMode_Disable ((uint32_t)0x00000000) /*!< ADC Overrun Mode disable */ +#define IS_ADC_OVRUNMODE(MODE) (((MODE) == ADC_OverrunMode_Enable) || \ + ((MODE) == ADC_OverrunMode_Disable)) +/** + * @} + */ +/** @defgroup ADC_AutoInjecMode + * @{ + */ +#define ADC_AutoInjec_Enable ((uint32_t)0x02000000) /*!< ADC Auto injected Mode enable */ +#define ADC_AutoInjec_Disable ((uint32_t)0x00000000) /*!< ADC Auto injected Mode disable */ +#define IS_ADC_AUTOINJECMODE(MODE) (((MODE) == ADC_AutoInjec_Enable) || \ + ((MODE) == ADC_AutoInjec_Disable)) +/** + * @} + */ +/** @defgroup ADC_resolution + * @{ + */ +#define ADC_Resolution_12b ((uint32_t)0x00000000) /*!< ADC 12-bit resolution */ +#define ADC_Resolution_10b ((uint32_t)0x00000008) /*!< ADC 10-bit resolution */ +#define ADC_Resolution_8b ((uint32_t)0x00000010) /*!< ADC 8-bit resolution */ +#define ADC_Resolution_6b ((uint32_t)0x00000018) /*!< ADC 6-bit resolution */ +#define IS_ADC_RESOLUTION(RESOLUTION) (((RESOLUTION) == ADC_Resolution_12b) || \ + ((RESOLUTION) == ADC_Resolution_10b) || \ + ((RESOLUTION) == ADC_Resolution_8b) || \ + ((RESOLUTION) == ADC_Resolution_6b)) + +/** + * @} + */ + + +/** @defgroup ADC_external_trigger_edge_for_regular_channels_conversion + * @{ + */ +#define ADC_ExternalTrigEventEdge_None ((uint16_t)0x0000) /*!< ADC No external trigger for regular conversion */ +#define ADC_ExternalTrigEventEdge_RisingEdge ((uint16_t)0x0400) /*!< ADC external trigger rising edge for regular conversion */ +#define ADC_ExternalTrigEventEdge_FallingEdge ((uint16_t)0x0800) /*!< ADC ADC external trigger falling edge for regular conversion */ +#define ADC_ExternalTrigEventEdge_BothEdge ((uint16_t)0x0C00) /*!< ADC ADC external trigger both edges for regular conversion */ + +#define IS_EXTERNALTRIG_EDGE(EDGE) (((EDGE) == ADC_ExternalTrigEventEdge_None) || \ + ((EDGE) == ADC_ExternalTrigEventEdge_RisingEdge) || \ + ((EDGE) == ADC_ExternalTrigEventEdge_FallingEdge) || \ + ((EDGE) == ADC_ExternalTrigEventEdge_BothEdge)) + +/** + * @} + */ + +/** @defgroup ADC_external_trigger_edge_for_Injected_channels_conversion + * @{ + */ +#define ADC_ExternalTrigInjecEventEdge_None ((uint16_t)0x0000) /*!< ADC No external trigger for regular conversion */ +#define ADC_ExternalTrigInjecEventEdge_RisingEdge ((uint16_t)0x0040) /*!< ADC external trigger rising edge for injected conversion */ +#define ADC_ExternalTrigInjecEventEdge_FallingEdge ((uint16_t)0x0080) /*!< ADC external trigger falling edge for injected conversion */ +#define ADC_ExternalTrigInjecEventEdge_BothEdge ((uint16_t)0x00C0) /*!< ADC external trigger both edges for injected conversion */ + +#define IS_EXTERNALTRIGINJ_EDGE(EDGE) (((EDGE) == ADC_ExternalTrigInjecEventEdge_None) || \ + ((EDGE) == ADC_ExternalTrigInjecEventEdge_RisingEdge) || \ + ((EDGE) == ADC_ExternalTrigInjecEventEdge_FallingEdge) || \ + ((EDGE) == ADC_ExternalTrigInjecEventEdge_BothEdge)) + +/** @defgroup ADC_external_trigger_sources_for_regular_channels_conversion + * @{ + */ +#define ADC_ExternalTrigConvEvent_0 ((uint16_t)0x0000) /*!< ADC external trigger event 0 */ +#define ADC_ExternalTrigConvEvent_1 ((uint16_t)0x0040) /*!< ADC external trigger event 1 */ +#define ADC_ExternalTrigConvEvent_2 ((uint16_t)0x0080) /*!< ADC external trigger event 2 */ +#define ADC_ExternalTrigConvEvent_3 ((uint16_t)0x00C0) /*!< ADC external trigger event 3 */ +#define ADC_ExternalTrigConvEvent_4 ((uint16_t)0x0100) /*!< ADC external trigger event 4 */ +#define ADC_ExternalTrigConvEvent_5 ((uint16_t)0x0140) /*!< ADC external trigger event 5 */ +#define ADC_ExternalTrigConvEvent_6 ((uint16_t)0x0180) /*!< ADC external trigger event 6 */ +#define ADC_ExternalTrigConvEvent_7 ((uint16_t)0x01C0) /*!< ADC external trigger event 7 */ +#define ADC_ExternalTrigConvEvent_8 ((uint16_t)0x0200) /*!< ADC external trigger event 8 */ +#define ADC_ExternalTrigConvEvent_9 ((uint16_t)0x0240) /*!< ADC external trigger event 9 */ +#define ADC_ExternalTrigConvEvent_10 ((uint16_t)0x0280) /*!< ADC external trigger event 10 */ +#define ADC_ExternalTrigConvEvent_11 ((uint16_t)0x02C0) /*!< ADC external trigger event 11 */ +#define ADC_ExternalTrigConvEvent_12 ((uint16_t)0x0300) /*!< ADC external trigger event 12 */ +#define ADC_ExternalTrigConvEvent_13 ((uint16_t)0x0340) /*!< ADC external trigger event 13 */ +#define ADC_ExternalTrigConvEvent_14 ((uint16_t)0x0380) /*!< ADC external trigger event 14 */ +#define ADC_ExternalTrigConvEvent_15 ((uint16_t)0x03C0) /*!< ADC external trigger event 15 */ + +#define IS_ADC_EXT_TRIG(REGTRIG) (((REGTRIG) == ADC_ExternalTrigConvEvent_0) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_1) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_2) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_3) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_4) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_5) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_6) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_7) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_8) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_9) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_10) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_11) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_12) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_13) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_14) || \ + ((REGTRIG) == ADC_ExternalTrigConvEvent_15)) + +/** + * @} + */ + +/** @defgroup ADC_external_trigger_sources_for_Injected_channels_conversion + * @{ + */ + +#define ADC_ExternalTrigInjecConvEvent_0 ((uint16_t)0x0000) /*!< ADC external trigger for injected conversion event 0 */ +#define ADC_ExternalTrigInjecConvEvent_1 ((uint16_t)0x0004) /*!< ADC external trigger for injected conversion event 1 */ +#define ADC_ExternalTrigInjecConvEvent_2 ((uint16_t)0x0008) /*!< ADC external trigger for injected conversion event 2 */ +#define ADC_ExternalTrigInjecConvEvent_3 ((uint16_t)0x000C) /*!< ADC external trigger for injected conversion event 3 */ +#define ADC_ExternalTrigInjecConvEvent_4 ((uint16_t)0x0010) /*!< ADC external trigger for injected conversion event 4 */ +#define ADC_ExternalTrigInjecConvEvent_5 ((uint16_t)0x0014) /*!< ADC external trigger for injected conversion event 5 */ +#define ADC_ExternalTrigInjecConvEvent_6 ((uint16_t)0x0018) /*!< ADC external trigger for injected conversion event 6 */ +#define ADC_ExternalTrigInjecConvEvent_7 ((uint16_t)0x001C) /*!< ADC external trigger for injected conversion event 7 */ +#define ADC_ExternalTrigInjecConvEvent_8 ((uint16_t)0x0020) /*!< ADC external trigger for injected conversion event 8 */ +#define ADC_ExternalTrigInjecConvEvent_9 ((uint16_t)0x0024) /*!< ADC external trigger for injected conversion event 9 */ +#define ADC_ExternalTrigInjecConvEvent_10 ((uint16_t)0x0028) /*!< ADC external trigger for injected conversion event 10 */ +#define ADC_ExternalTrigInjecConvEvent_11 ((uint16_t)0x002C) /*!< ADC external trigger for injected conversion event 11 */ +#define ADC_ExternalTrigInjecConvEvent_12 ((uint16_t)0x0030) /*!< ADC external trigger for injected conversion event 12 */ +#define ADC_ExternalTrigInjecConvEvent_13 ((uint16_t)0x0034) /*!< ADC external trigger for injected conversion event 13 */ +#define ADC_ExternalTrigInjecConvEvent_14 ((uint16_t)0x0038) /*!< ADC external trigger for injected conversion event 14 */ +#define ADC_ExternalTrigInjecConvEvent_15 ((uint16_t)0x003C) /*!< ADC external trigger for injected conversion event 15 */ + +#define IS_ADC_EXT_INJEC_TRIG(INJTRIG) (((INJTRIG) == ADC_ExternalTrigInjecConvEvent_0) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_1) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_2) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_3) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_4) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_5) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_6) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_7) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_8) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_9) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_10) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_11) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_12) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_13) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_14) || \ + ((INJTRIG) == ADC_ExternalTrigInjecConvEvent_15)) +/** + * @} + */ +/** @defgroup ADC_data_align + * @{ + */ + +#define ADC_DataAlign_Right ((uint32_t)0x00000000) /*!< ADC Data alignment right */ +#define ADC_DataAlign_Left ((uint32_t)0x00000020) /*!< ADC Data alignment left */ +#define IS_ADC_DATA_ALIGN(ALIGN) (((ALIGN) == ADC_DataAlign_Right) || \ + ((ALIGN) == ADC_DataAlign_Left)) +/** + * @} + */ + +/** @defgroup ADC_channels + * @{ + */ + +#define ADC_Channel_1 ((uint8_t)0x01) /*!< ADC Channel 1 */ +#define ADC_Channel_2 ((uint8_t)0x02) /*!< ADC Channel 2 */ +#define ADC_Channel_3 ((uint8_t)0x03) /*!< ADC Channel 3 */ +#define ADC_Channel_4 ((uint8_t)0x04) /*!< ADC Channel 4 */ +#define ADC_Channel_5 ((uint8_t)0x05) /*!< ADC Channel 5 */ +#define ADC_Channel_6 ((uint8_t)0x06) /*!< ADC Channel 6 */ +#define ADC_Channel_7 ((uint8_t)0x07) /*!< ADC Channel 7 */ +#define ADC_Channel_8 ((uint8_t)0x08) /*!< ADC Channel 8 */ +#define ADC_Channel_9 ((uint8_t)0x09) /*!< ADC Channel 9 */ +#define ADC_Channel_10 ((uint8_t)0x0A) /*!< ADC Channel 10 */ +#define ADC_Channel_11 ((uint8_t)0x0B) /*!< ADC Channel 11 */ +#define ADC_Channel_12 ((uint8_t)0x0C) /*!< ADC Channel 12 */ +#define ADC_Channel_13 ((uint8_t)0x0D) /*!< ADC Channel 13 */ +#define ADC_Channel_14 ((uint8_t)0x0E) /*!< ADC Channel 14 */ +#define ADC_Channel_15 ((uint8_t)0x0F) /*!< ADC Channel 15 */ +#define ADC_Channel_16 ((uint8_t)0x10) /*!< ADC Channel 16 */ +#define ADC_Channel_17 ((uint8_t)0x11) /*!< ADC Channel 17 */ +#define ADC_Channel_18 ((uint8_t)0x12) /*!< ADC Channel 18 */ + +#define ADC_Channel_TempSensor ((uint8_t)ADC_Channel_16) +#define ADC_Channel_Vrefint ((uint8_t)ADC_Channel_18) +#define ADC_Channel_Vbat ((uint8_t)ADC_Channel_17) + +#define IS_ADC_CHANNEL(CHANNEL) (((CHANNEL) == ADC_Channel_1) || \ + ((CHANNEL) == ADC_Channel_2) || \ + ((CHANNEL) == ADC_Channel_3) || \ + ((CHANNEL) == ADC_Channel_4) || \ + ((CHANNEL) == ADC_Channel_5) || \ + ((CHANNEL) == ADC_Channel_6) || \ + ((CHANNEL) == ADC_Channel_7) || \ + ((CHANNEL) == ADC_Channel_8) || \ + ((CHANNEL) == ADC_Channel_9) || \ + ((CHANNEL) == ADC_Channel_10) || \ + ((CHANNEL) == ADC_Channel_11) || \ + ((CHANNEL) == ADC_Channel_12) || \ + ((CHANNEL) == ADC_Channel_13) || \ + ((CHANNEL) == ADC_Channel_14) || \ + ((CHANNEL) == ADC_Channel_15) || \ + ((CHANNEL) == ADC_Channel_16) || \ + ((CHANNEL) == ADC_Channel_17) || \ + ((CHANNEL) == ADC_Channel_18)) +#define IS_ADC_DIFFCHANNEL(CHANNEL) (((CHANNEL) == ADC_Channel_1) || \ + ((CHANNEL) == ADC_Channel_2) || \ + ((CHANNEL) == ADC_Channel_3) || \ + ((CHANNEL) == ADC_Channel_4) || \ + ((CHANNEL) == ADC_Channel_5) || \ + ((CHANNEL) == ADC_Channel_6) || \ + ((CHANNEL) == ADC_Channel_7) || \ + ((CHANNEL) == ADC_Channel_8) || \ + ((CHANNEL) == ADC_Channel_9) || \ + ((CHANNEL) == ADC_Channel_10) || \ + ((CHANNEL) == ADC_Channel_11) || \ + ((CHANNEL) == ADC_Channel_12) || \ + ((CHANNEL) == ADC_Channel_13) || \ + ((CHANNEL) == ADC_Channel_14)) +/** + * @} + */ + +/** @defgroup ADC_mode + * @{ + */ +#define ADC_Mode_Independent ((uint32_t)0x00000000) /*!< ADC independent mode */ +#define ADC_Mode_CombRegSimulInjSimul ((uint32_t)0x00000001) /*!< ADC multi ADC mode: Combined Regular simultaneous injected simultaneous mode */ +#define ADC_Mode_CombRegSimulAltTrig ((uint32_t)0x00000002) /*!< ADC multi ADC mode: Combined Regular simultaneous Alternate trigger mode */ +#define ADC_Mode_InjSimul ((uint32_t)0x00000005) /*!< ADC multi ADC mode: Injected simultaneous mode */ +#define ADC_Mode_RegSimul ((uint32_t)0x00000006) /*!< ADC multi ADC mode: Regular simultaneous mode */ +#define ADC_Mode_Interleave ((uint32_t)0x00000007) /*!< ADC multi ADC mode: Interleave mode */ +#define ADC_Mode_AltTrig ((uint32_t)0x00000009) /*!< ADC multi ADC mode: Alternate Trigger mode */ + +#define IS_ADC_MODE(MODE) (((MODE) == ADC_Mode_Independent) || \ + ((MODE) == ADC_Mode_CombRegSimulInjSimul) || \ + ((MODE) == ADC_Mode_CombRegSimulAltTrig) || \ + ((MODE) == ADC_Mode_InjSimul) || \ + ((MODE) == ADC_Mode_RegSimul) || \ + ((MODE) == ADC_Mode_Interleave) || \ + ((MODE) == ADC_Mode_AltTrig)) + +/** + * @} + */ + +/** @defgroup ADC_Clock + * @{ + */ +#define ADC_Clock_AsynClkMode ((uint32_t)0x00000000) /*!< ADC Asynchronous clock mode */ +#define ADC_Clock_SynClkModeDiv1 ((uint32_t)0x00010000) /*!< Synchronous clock mode divided by 1 */ +#define ADC_Clock_SynClkModeDiv2 ((uint32_t)0x00020000) /*!< Synchronous clock mode divided by 2 */ +#define ADC_Clock_SynClkModeDiv4 ((uint32_t)0x00030000) /*!< Synchronous clock mode divided by 4 */ +#define IS_ADC_CLOCKMODE(CLOCK) (((CLOCK) == ADC_Clock_AsynClkMode) ||\ + ((CLOCK) == ADC_Clock_SynClkModeDiv1) ||\ + ((CLOCK) == ADC_Clock_SynClkModeDiv2)||\ + ((CLOCK) == ADC_Clock_SynClkModeDiv4)) +/** + * @} + */ +/** @defgroup ADC_Direct_memory_access_mode_for_multi_mode + * @{ + */ +#define ADC_DMAAccessMode_Disabled ((uint32_t)0x00000000) /*!< DMA mode disabled */ +#define ADC_DMAAccessMode_1 ((uint32_t)0x00008000) /*!< DMA mode enabled for 12 and 10-bit resolution (6 bit) */ +#define ADC_DMAAccessMode_2 ((uint32_t)0x0000C000) /*!< DMA mode enabled for 8 and 6-bit resolution (8bit) */ +#define IS_ADC_DMA_ACCESS_MODE(MODE) (((MODE) == ADC_DMAAccessMode_Disabled) || \ + ((MODE) == ADC_DMAAccessMode_1) || \ + ((MODE) == ADC_DMAAccessMode_2)) + +/** + * @} + */ +/** @defgroup ADC_sampling_time + * @{ + */ + +#define ADC_SampleTime_1Cycles5 ((uint8_t)0x00) /*!< ADC sampling time 1.5 cycle */ +#define ADC_SampleTime_2Cycles5 ((uint8_t)0x01) /*!< ADC sampling time 2.5 cycles */ +#define ADC_SampleTime_4Cycles5 ((uint8_t)0x02) /*!< ADC sampling time 4.5 cycles */ +#define ADC_SampleTime_7Cycles5 ((uint8_t)0x03) /*!< ADC sampling time 7.5 cycles */ +#define ADC_SampleTime_19Cycles5 ((uint8_t)0x04) /*!< ADC sampling time 19.5 cycles */ +#define ADC_SampleTime_61Cycles5 ((uint8_t)0x05) /*!< ADC sampling time 61.5 cycles */ +#define ADC_SampleTime_181Cycles5 ((uint8_t)0x06) /*!< ADC sampling time 181.5 cycles */ +#define ADC_SampleTime_601Cycles5 ((uint8_t)0x07) /*!< ADC sampling time 601.5 cycles */ +#define IS_ADC_SAMPLE_TIME(TIME) (((TIME) == ADC_SampleTime_1Cycles5) || \ + ((TIME) == ADC_SampleTime_2Cycles5) || \ + ((TIME) == ADC_SampleTime_4Cycles5) || \ + ((TIME) == ADC_SampleTime_7Cycles5) || \ + ((TIME) == ADC_SampleTime_19Cycles5) || \ + ((TIME) == ADC_SampleTime_61Cycles5) || \ + ((TIME) == ADC_SampleTime_181Cycles5) || \ + ((TIME) == ADC_SampleTime_601Cycles5)) +/** + * @} + */ + +/** @defgroup ADC_injected_Channel_selection + * @{ + */ + +#define ADC_InjectedChannel_1 ADC_Channel_1 /*!< ADC Injected channel 1 */ +#define ADC_InjectedChannel_2 ADC_Channel_2 /*!< ADC Injected channel 2 */ +#define ADC_InjectedChannel_3 ADC_Channel_3 /*!< ADC Injected channel 3 */ +#define ADC_InjectedChannel_4 ADC_Channel_4 /*!< ADC Injected channel 4 */ +#define ADC_InjectedChannel_5 ADC_Channel_5 /*!< ADC Injected channel 5 */ +#define ADC_InjectedChannel_6 ADC_Channel_6 /*!< ADC Injected channel 6 */ +#define ADC_InjectedChannel_7 ADC_Channel_7 /*!< ADC Injected channel 7 */ +#define ADC_InjectedChannel_8 ADC_Channel_8 /*!< ADC Injected channel 8 */ +#define ADC_InjectedChannel_9 ADC_Channel_9 /*!< ADC Injected channel 9 */ +#define ADC_InjectedChannel_10 ADC_Channel_10 /*!< ADC Injected channel 10 */ +#define ADC_InjectedChannel_11 ADC_Channel_11 /*!< ADC Injected channel 11 */ +#define ADC_InjectedChannel_12 ADC_Channel_12 /*!< ADC Injected channel 12 */ +#define ADC_InjectedChannel_13 ADC_Channel_13 /*!< ADC Injected channel 13 */ +#define ADC_InjectedChannel_14 ADC_Channel_14 /*!< ADC Injected channel 14 */ +#define ADC_InjectedChannel_15 ADC_Channel_15 /*!< ADC Injected channel 15 */ +#define ADC_InjectedChannel_16 ADC_Channel_16 /*!< ADC Injected channel 16 */ +#define ADC_InjectedChannel_17 ADC_Channel_17 /*!< ADC Injected channel 17 */ +#define ADC_InjectedChannel_18 ADC_Channel_18 /*!< ADC Injected channel 18 */ + +#define IS_ADC_INJECTED_CHANNEL(CHANNEL) (((CHANNEL) == ADC_InjectedChannel_1) || \ + ((CHANNEL) == ADC_InjectedChannel_2) || \ + ((CHANNEL) == ADC_InjectedChannel_3) || \ + ((CHANNEL) == ADC_InjectedChannel_4) ||\ + ((CHANNEL) == ADC_InjectedChannel_5) ||\ + ((CHANNEL) == ADC_InjectedChannel_6) ||\ + ((CHANNEL) == ADC_InjectedChannel_7) ||\ + ((CHANNEL) == ADC_InjectedChannel_8) ||\ + ((CHANNEL) == ADC_InjectedChannel_9) ||\ + ((CHANNEL) == ADC_InjectedChannel_10) ||\ + ((CHANNEL) == ADC_InjectedChannel_11) ||\ + ((CHANNEL) == ADC_InjectedChannel_12) ||\ + ((CHANNEL) == ADC_InjectedChannel_13) ||\ + ((CHANNEL) == ADC_InjectedChannel_14) ||\ + ((CHANNEL) == ADC_InjectedChannel_15) ||\ + ((CHANNEL) == ADC_InjectedChannel_16) ||\ + ((CHANNEL) == ADC_InjectedChannel_17) ||\ + ((CHANNEL) == ADC_InjectedChannel_18)) +/** + * @} + */ + +/** @defgroup ADC_injected_Sequence_selection + * @{ + */ + +#define ADC_InjectedSequence_1 ADC_Channel_1 /*!< ADC Injected sequence 1 */ +#define ADC_InjectedSequence_2 ADC_Channel_2 /*!< ADC Injected sequence 2 */ +#define ADC_InjectedSequence_3 ADC_Channel_3 /*!< ADC Injected sequence 3 */ +#define ADC_InjectedSequence_4 ADC_Channel_4 /*!< ADC Injected sequence 4 */ +#define IS_ADC_INJECTED_SEQUENCE(SEQUENCE) (((SEQUENCE) == ADC_InjectedSequence_1) || \ + ((SEQUENCE) == ADC_InjectedSequence_2) || \ + ((SEQUENCE) == ADC_InjectedSequence_3) || \ + ((SEQUENCE) == ADC_InjectedSequence_4)) +/** + * @} + */ + +/** @defgroup ADC_analog_watchdog_selection + * @{ + */ + +#define ADC_AnalogWatchdog_SingleRegEnable ((uint32_t)0x00C00000) /*!< ADC Analog watchdog single regular mode */ +#define ADC_AnalogWatchdog_SingleInjecEnable ((uint32_t)0x01400000) /*!< ADC Analog watchdog single injected mode */ +#define ADC_AnalogWatchdog_SingleRegOrInjecEnable ((uint32_t)0x01C00000) /*!< ADC Analog watchdog single regular or injected mode */ +#define ADC_AnalogWatchdog_AllRegEnable ((uint32_t)0x00800000) /*!< ADC Analog watchdog all regular mode */ +#define ADC_AnalogWatchdog_AllInjecEnable ((uint32_t)0x01000000) /*!< ADC Analog watchdog all injected mode */ +#define ADC_AnalogWatchdog_AllRegAllInjecEnable ((uint32_t)0x01800000) /*!< ADC Analog watchdog all regular and all injected mode */ +#define ADC_AnalogWatchdog_None ((uint32_t)0x00000000) /*!< ADC Analog watchdog off */ + +#define IS_ADC_ANALOG_WATCHDOG(WATCHDOG) (((WATCHDOG) == ADC_AnalogWatchdog_SingleRegEnable) || \ + ((WATCHDOG) == ADC_AnalogWatchdog_SingleInjecEnable) || \ + ((WATCHDOG) == ADC_AnalogWatchdog_SingleRegOrInjecEnable) || \ + ((WATCHDOG) == ADC_AnalogWatchdog_AllRegEnable) || \ + ((WATCHDOG) == ADC_AnalogWatchdog_AllInjecEnable) || \ + ((WATCHDOG) == ADC_AnalogWatchdog_AllRegAllInjecEnable) || \ + ((WATCHDOG) == ADC_AnalogWatchdog_None)) +/** + * @} + */ + +/** @defgroup ADC_Calibration_Mode_definition + * @{ + */ +#define ADC_CalibrationMode_Single ((uint32_t)0x00000000) /*!< ADC Calibration for single ended channel */ +#define ADC_CalibrationMode_Differential ((uint32_t)0x40000000) /*!< ADC Calibration for differential channel */ + +#define IS_ADC_CALIBRATION_MODE(MODE) (((MODE) == ADC_CalibrationMode_Single) ||((MODE) == ADC_CalibrationMode_Differential)) + +/** + * @} + */ + +/** @defgroup ADC_DMA_Mode_definition + * @{ + */ +#define ADC_DMAMode_OneShot ((uint32_t)0x00000000) /*!< ADC DMA Oneshot mode */ +#define ADC_DMAMode_Circular ((uint32_t)0x00000002) /*!< ADC DMA circular mode */ + +#define IS_ADC_DMA_MODE(MODE) (((MODE) == ADC_DMAMode_OneShot) || ((MODE) == ADC_DMAMode_Circular)) +/** + * @} + */ + +/** @defgroup ADC_interrupts_definition + * @{ + */ + +#define ADC_IT_RDY ((uint16_t)0x0001) /*!< ADC Ready (ADRDY) interrupt source */ +#define ADC_IT_EOSMP ((uint16_t)0x0002) /*!< ADC End of Sampling interrupt source */ +#define ADC_IT_EOC ((uint16_t)0x0004) /*!< ADC End of Regular Conversion interrupt source */ +#define ADC_IT_EOS ((uint16_t)0x0008) /*!< ADC End of Regular sequence of Conversions interrupt source */ +#define ADC_IT_OVR ((uint16_t)0x0010) /*!< ADC overrun interrupt source */ +#define ADC_IT_JEOC ((uint16_t)0x0020) /*!< ADC End of Injected Conversion interrupt source */ +#define ADC_IT_JEOS ((uint16_t)0x0040) /*!< ADC End of Injected sequence of Conversions interrupt source */ +#define ADC_IT_AWD1 ((uint16_t)0x0080) /*!< ADC Analog watchdog 1 interrupt source */ +#define ADC_IT_AWD2 ((uint16_t)0x0100) /*!< ADC Analog watchdog 2 interrupt source */ +#define ADC_IT_AWD3 ((uint16_t)0x0200) /*!< ADC Analog watchdog 3 interrupt source */ +#define ADC_IT_JQOVF ((uint16_t)0x0400) /*!< ADC Injected Context Queue Overflow interrupt source */ + + +#define IS_ADC_IT(IT) ((((IT) & (uint16_t)0xF800) == 0x0000) && ((IT) != 0x0000)) + +#define IS_ADC_GET_IT(IT) (((IT) == ADC_IT_RDY) || ((IT) == ADC_IT_EOSMP) || \ + ((IT) == ADC_IT_EOC) || ((IT) == ADC_IT_EOS) || \ + ((IT) == ADC_IT_OVR) || ((IT) == ADC_IT_EOS) || \ + ((IT) == ADC_IT_JEOS) || ((IT) == ADC_IT_AWD1) || \ + ((IT) == ADC_IT_AWD2) || ((IT) == ADC_IT_AWD3) || \ + ((IT) == ADC_IT_JQOVF)) +/** + * @} + */ + +/** @defgroup ADC_flags_definition + * @{ + */ + +#define ADC_FLAG_RDY ((uint16_t)0x0001) /*!< ADC Ready (ADRDY) flag */ +#define ADC_FLAG_EOSMP ((uint16_t)0x0002) /*!< ADC End of Sampling flag */ +#define ADC_FLAG_EOC ((uint16_t)0x0004) /*!< ADC End of Regular Conversion flag */ +#define ADC_FLAG_EOS ((uint16_t)0x0008) /*!< ADC End of Regular sequence of Conversions flag */ +#define ADC_FLAG_OVR ((uint16_t)0x0010) /*!< ADC overrun flag */ +#define ADC_FLAG_JEOC ((uint16_t)0x0020) /*!< ADC End of Injected Conversion flag */ +#define ADC_FLAG_JEOS ((uint16_t)0x0040) /*!< ADC End of Injected sequence of Conversions flag */ +#define ADC_FLAG_AWD1 ((uint16_t)0x0080) /*!< ADC Analog watchdog 1 flag */ +#define ADC_FLAG_AWD2 ((uint16_t)0x0100) /*!< ADC Analog watchdog 2 flag */ +#define ADC_FLAG_AWD3 ((uint16_t)0x0200) /*!< ADC Analog watchdog 3 flag */ +#define ADC_FLAG_JQOVF ((uint16_t)0x0400) /*!< ADC Injected Context Queue Overflow flag */ + +#define IS_ADC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint16_t)0xF800) == 0x0000) && ((FLAG) != 0x0000)) +#define IS_ADC_GET_FLAG(FLAG) (((FLAG) == ADC_FLAG_RDY) || ((FLAG) == ADC_FLAG_EOSMP) || \ + ((FLAG) == ADC_FLAG_EOC) || ((FLAG) == ADC_FLAG_EOS) || \ + ((FLAG) == ADC_FLAG_OVR) || ((FLAG) == ADC_FLAG_JEOC) || \ + ((FLAG) == ADC_FLAG_JEOS) || ((FLAG) == ADC_FLAG_AWD1) || \ + ((FLAG) == ADC_FLAG_AWD2) || ((FLAG) == ADC_FLAG_AWD3) || \ + ((FLAG) == ADC_FLAG_JQOVF)) +/** + * @} + */ + +/** @defgroup ADC_Common_flags_definition + * @{ + */ + +#define ADC_FLAG_MSTRDY ((uint32_t)0x00000001) /*!< ADC Master Ready (ADRDY) flag */ +#define ADC_FLAG_MSTEOSMP ((uint32_t)0x00000002) /*!< ADC Master End of Sampling flag */ +#define ADC_FLAG_MSTEOC ((uint32_t)0x00000004) /*!< ADC Master End of Regular Conversion flag */ +#define ADC_FLAG_MSTEOS ((uint32_t)0x00000008) /*!< ADC Master End of Regular sequence of Conversions flag */ +#define ADC_FLAG_MSTOVR ((uint32_t)0x00000010) /*!< ADC Master overrun flag */ +#define ADC_FLAG_MSTJEOC ((uint32_t)0x00000020) /*!< ADC Master End of Injected Conversion flag */ +#define ADC_FLAG_MSTJEOS ((uint32_t)0x00000040) /*!< ADC Master End of Injected sequence of Conversions flag */ +#define ADC_FLAG_MSTAWD1 ((uint32_t)0x00000080) /*!< ADC Master Analog watchdog 1 flag */ +#define ADC_FLAG_MSTAWD2 ((uint32_t)0x00000100) /*!< ADC Master Analog watchdog 2 flag */ +#define ADC_FLAG_MSTAWD3 ((uint32_t)0x00000200) /*!< ADC Master Analog watchdog 3 flag */ +#define ADC_FLAG_MSTJQOVF ((uint32_t)0x00000400) /*!< ADC Master Injected Context Queue Overflow flag */ + +#define ADC_FLAG_SLVRDY ((uint32_t)0x00010000) /*!< ADC Slave Ready (ADRDY) flag */ +#define ADC_FLAG_SLVEOSMP ((uint32_t)0x00020000) /*!< ADC Slave End of Sampling flag */ +#define ADC_FLAG_SLVEOC ((uint32_t)0x00040000) /*!< ADC Slave End of Regular Conversion flag */ +#define ADC_FLAG_SLVEOS ((uint32_t)0x00080000) /*!< ADC Slave End of Regular sequence of Conversions flag */ +#define ADC_FLAG_SLVOVR ((uint32_t)0x00100000) /*!< ADC Slave overrun flag */ +#define ADC_FLAG_SLVJEOC ((uint32_t)0x00200000) /*!< ADC Slave End of Injected Conversion flag */ +#define ADC_FLAG_SLVJEOS ((uint32_t)0x00400000) /*!< ADC Slave End of Injected sequence of Conversions flag */ +#define ADC_FLAG_SLVAWD1 ((uint32_t)0x00800000) /*!< ADC Slave Analog watchdog 1 flag */ +#define ADC_FLAG_SLVAWD2 ((uint32_t)0x01000000) /*!< ADC Slave Analog watchdog 2 flag */ +#define ADC_FLAG_SLVAWD3 ((uint32_t)0x02000000) /*!< ADC Slave Analog watchdog 3 flag */ +#define ADC_FLAG_SLVJQOVF ((uint32_t)0x04000000) /*!< ADC Slave Injected Context Queue Overflow flag */ + +#define IS_ADC_CLEAR_COMMONFLAG(FLAG) ((((FLAG) & (uint32_t)0xF800F800) == 0x0000) && ((FLAG) != 0x00000000)) +#define IS_ADC_GET_COMMONFLAG(FLAG) (((FLAG) == ADC_FLAG_MSTRDY) || ((FLAG) == ADC_FLAG_MSTEOSMP) || \ + ((FLAG) == ADC_FLAG_MSTEOC) || ((FLAG) == ADC_FLAG_MSTEOS) || \ + ((FLAG) == ADC_FLAG_MSTOVR) || ((FLAG) == ADC_FLAG_MSTEOS) || \ + ((FLAG) == ADC_FLAG_MSTJEOS) || ((FLAG) == ADC_FLAG_MSTAWD1) || \ + ((FLAG) == ADC_FLAG_MSTAWD2) || ((FLAG) == ADC_FLAG_MSTAWD3) || \ + ((FLAG) == ADC_FLAG_MSTJQOVF) || \ + ((FLAG) == ADC_FLAG_SLVRDY) || ((FLAG) == ADC_FLAG_SLVEOSMP) || \ + ((FLAG) == ADC_FLAG_SLVEOC) || ((FLAG) == ADC_FLAG_SLVEOS) || \ + ((FLAG) == ADC_FLAG_SLVOVR) || ((FLAG) == ADC_FLAG_SLVEOS) || \ + ((FLAG) == ADC_FLAG_SLVJEOS) || ((FLAG) == ADC_FLAG_SLVAWD1) || \ + ((FLAG) == ADC_FLAG_SLVAWD2) || ((FLAG) == ADC_FLAG_SLVAWD3) || \ + ((FLAG) == ADC_FLAG_SLVJQOVF)) +/** + * @} + */ + +/** @defgroup ADC_thresholds + * @{ + */ + +#define IS_ADC_THRESHOLD(THRESHOLD) ((THRESHOLD) <= 0xFFF) + +/** + * @} + */ + +/** @defgroup ADC_injected_offset + * @{ + */ + +#define IS_ADC_OFFSET(OFFSET) ((OFFSET) <= 0xFFF) + +/** + * @} + */ + +/** @defgroup ADC_injected_length + * @{ + */ + +#define IS_ADC_INJECTED_LENGTH(LENGTH) (((LENGTH) >= 0x1) && ((LENGTH) <= 0x4)) + +/** + * @} + */ + + +/** @defgroup ADC_regular_length + * @{ + */ + +#define IS_ADC_REGULAR_LENGTH(LENGTH) (((LENGTH) >= 0x1) && ((LENGTH) <= 0x10)) +/** + * @} + */ + +/** @defgroup ADC_regular_discontinuous_mode_number + * @{ + */ + +#define IS_ADC_REGULAR_DISC_NUMBER(NUMBER) (((NUMBER) >= 0x1) && ((NUMBER) <= 0x8)) + +/** + * @} + */ + +/** @defgroup ADC_two_sampling_delay_number + * @{ + */ +#define IS_ADC_TWOSAMPLING_DELAY(DELAY) (((DELAY) <= 0xF)) + +/** + * @} + */ +/** + * @} + */ + + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the ADC configuration to the default reset state *****/ +void ADC_DeInit(ADC_TypeDef* ADCx); + +/* Initialization and Configuration functions *********************************/ +void ADC_Init(ADC_TypeDef* ADCx, ADC_InitTypeDef* ADC_InitStruct); +void ADC_StructInit(ADC_InitTypeDef* ADC_InitStruct); +void ADC_InjectedInit(ADC_TypeDef* ADCx, ADC_InjectedInitTypeDef* ADC_InjectedInitStruct); +void ADC_InjectedStructInit(ADC_InjectedInitTypeDef* ADC_InjectedInitStruct); +void ADC_CommonInit(ADC_TypeDef* ADCx, ADC_CommonInitTypeDef* ADC_CommonInitStruct); +void ADC_CommonStructInit(ADC_CommonInitTypeDef* ADC_CommonInitStruct); + +void ADC_Cmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_StartCalibration(ADC_TypeDef* ADCx); +uint32_t ADC_GetCalibrationValue(ADC_TypeDef* ADCx); +void ADC_SetCalibrationValue(ADC_TypeDef* ADCx, uint32_t ADC_Calibration); +void ADC_SelectCalibrationMode(ADC_TypeDef* ADCx, uint32_t ADC_CalibrationMode); +FlagStatus ADC_GetCalibrationStatus(ADC_TypeDef* ADCx); +void ADC_DisableCmd(ADC_TypeDef* ADCx); +FlagStatus ADC_GetDisableCmdStatus(ADC_TypeDef* ADCx); +void ADC_VoltageRegulatorCmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_SelectDifferentialMode(ADC_TypeDef* ADCx, uint8_t ADC_Channel, FunctionalState NewState); +void ADC_SelectQueueOfContextMode(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_AutoDelayCmd(ADC_TypeDef* ADCx, FunctionalState NewState); + +/* Analog Watchdog configuration functions ************************************/ +void ADC_AnalogWatchdogCmd(ADC_TypeDef* ADCx, uint32_t ADC_AnalogWatchdog); +void ADC_AnalogWatchdog1ThresholdsConfig(ADC_TypeDef* ADCx, uint16_t HighThreshold, uint16_t LowThreshold); +void ADC_AnalogWatchdog2ThresholdsConfig(ADC_TypeDef* ADCx, uint8_t HighThreshold, uint8_t LowThreshold); +void ADC_AnalogWatchdog3ThresholdsConfig(ADC_TypeDef* ADCx, uint8_t HighThreshold, uint8_t LowThreshold); +void ADC_AnalogWatchdog1SingleChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel); +void ADC_AnalogWatchdog2SingleChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel); +void ADC_AnalogWatchdog3SingleChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel); + +/* Temperature Sensor, Vrefint and Vbat management function */ +void ADC_TempSensorCmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_VrefintCmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_VbatCmd(ADC_TypeDef* ADCx, FunctionalState NewState); + +/* Channels Configuration functions ***********************************/ +void ADC_RegularChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint8_t Rank, uint8_t ADC_SampleTime); +void ADC_RegularChannelSequencerLengthConfig(ADC_TypeDef* ADCx, uint8_t SequencerLength); +void ADC_ExternalTriggerConfig(ADC_TypeDef* ADCx, uint16_t ADC_ExternalTrigConvEvent, uint16_t ADC_ExternalTrigEventEdge); + +void ADC_StartConversion(ADC_TypeDef* ADCx); +FlagStatus ADC_GetStartConversionStatus(ADC_TypeDef* ADCx); +void ADC_StopConversion(ADC_TypeDef* ADCx); +void ADC_DiscModeChannelCountConfig(ADC_TypeDef* ADCx, uint8_t Number); +void ADC_DiscModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState); +uint16_t ADC_GetConversionValue(ADC_TypeDef* ADCx); +uint32_t ADC_GetDualModeConversionValue(ADC_TypeDef* ADCx); + +void ADC_SetChannelOffset1(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset); +void ADC_SetChannelOffset2(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset); +void ADC_SetChannelOffset3(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset); +void ADC_SetChannelOffset4(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset); + +void ADC_ChannelOffset1Cmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_ChannelOffset2Cmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_ChannelOffset3Cmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_ChannelOffset4Cmd(ADC_TypeDef* ADCx, FunctionalState NewState); + +/* Regular Channels DMA Configuration functions *******************************/ +void ADC_DMACmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_DMAConfig(ADC_TypeDef* ADCx, uint32_t ADC_DMAMode); + +/* Injected channels Configuration functions **********************************/ +void ADC_InjectedChannelSampleTimeConfig(ADC_TypeDef* ADCx, uint8_t ADC_InjectedChannel, uint8_t ADC_SampleTime); +void ADC_StartInjectedConversion(ADC_TypeDef* ADCx); +FlagStatus ADC_GetStartInjectedConversionStatus(ADC_TypeDef* ADCx); +void ADC_StopInjectedConversion(ADC_TypeDef* ADCx); +void ADC_AutoInjectedConvCmd(ADC_TypeDef* ADCx, FunctionalState NewState); +void ADC_InjectedDiscModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState); +uint16_t ADC_GetInjectedConversionValue(ADC_TypeDef* ADCx, uint8_t ADC_InjectedChannel); + +/* ADC Dual Modes Configuration functions *************************************/ +FlagStatus ADC_GetCommonFlagStatus(ADC_TypeDef* ADCx, uint32_t ADC_FLAG); +void ADC_ClearCommonFlag(ADC_TypeDef* ADCx, uint32_t ADC_FLAG); + +/* Interrupts and flags management functions **********************************/ +void ADC_ITConfig(ADC_TypeDef* ADCx, uint32_t ADC_IT, FunctionalState NewState); +FlagStatus ADC_GetFlagStatus(ADC_TypeDef* ADCx, uint32_t ADC_FLAG); +void ADC_ClearFlag(ADC_TypeDef* ADCx, uint32_t ADC_FLAG); +ITStatus ADC_GetITStatus(ADC_TypeDef* ADCx, uint32_t ADC_IT); +void ADC_ClearITPendingBit(ADC_TypeDef* ADCx, uint32_t ADC_IT); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_ADC_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_can.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_can.h new file mode 100755 index 0000000000..dbac025334 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_can.h @@ -0,0 +1,643 @@ +/** + ****************************************************************************** + * @file stm32f30x_can.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the CAN firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_CAN_H +#define __STM32F30x_CAN_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup CAN + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +#define IS_CAN_ALL_PERIPH(PERIPH) (((PERIPH) == CAN1)) + +/** + * @brief CAN init structure definition + */ +typedef struct +{ + uint16_t CAN_Prescaler; /*!< Specifies the length of a time quantum. + It ranges from 1 to 1024. */ + + uint8_t CAN_Mode; /*!< Specifies the CAN operating mode. + This parameter can be a value of @ref CAN_operating_mode */ + + uint8_t CAN_SJW; /*!< Specifies the maximum number of time quanta + the CAN hardware is allowed to lengthen or + shorten a bit to perform resynchronization. + This parameter can be a value of @ref CAN_synchronisation_jump_width */ + + uint8_t CAN_BS1; /*!< Specifies the number of time quanta in Bit + Segment 1. This parameter can be a value of + @ref CAN_time_quantum_in_bit_segment_1 */ + + uint8_t CAN_BS2; /*!< Specifies the number of time quanta in Bit Segment 2. + This parameter can be a value of @ref CAN_time_quantum_in_bit_segment_2 */ + + FunctionalState CAN_TTCM; /*!< Enable or disable the time triggered communication mode. + This parameter can be set either to ENABLE or DISABLE. */ + + FunctionalState CAN_ABOM; /*!< Enable or disable the automatic bus-off management. + This parameter can be set either to ENABLE or DISABLE. */ + + FunctionalState CAN_AWUM; /*!< Enable or disable the automatic wake-up mode. + This parameter can be set either to ENABLE or DISABLE. */ + + FunctionalState CAN_NART; /*!< Enable or disable the non-automatic retransmission mode. + This parameter can be set either to ENABLE or DISABLE. */ + + FunctionalState CAN_RFLM; /*!< Enable or disable the Receive FIFO Locked mode. + This parameter can be set either to ENABLE or DISABLE. */ + + FunctionalState CAN_TXFP; /*!< Enable or disable the transmit FIFO priority. + This parameter can be set either to ENABLE or DISABLE. */ +} CAN_InitTypeDef; + +/** + * @brief CAN filter init structure definition + */ +typedef struct +{ + uint16_t CAN_FilterIdHigh; /*!< Specifies the filter identification number (MSBs for a 32-bit + configuration, first one for a 16-bit configuration). + This parameter can be a value between 0x0000 and 0xFFFF */ + + uint16_t CAN_FilterIdLow; /*!< Specifies the filter identification number (LSBs for a 32-bit + configuration, second one for a 16-bit configuration). + This parameter can be a value between 0x0000 and 0xFFFF */ + + uint16_t CAN_FilterMaskIdHigh; /*!< Specifies the filter mask number or identification number, + according to the mode (MSBs for a 32-bit configuration, + first one for a 16-bit configuration). + This parameter can be a value between 0x0000 and 0xFFFF */ + + uint16_t CAN_FilterMaskIdLow; /*!< Specifies the filter mask number or identification number, + according to the mode (LSBs for a 32-bit configuration, + second one for a 16-bit configuration). + This parameter can be a value between 0x0000 and 0xFFFF */ + + uint16_t CAN_FilterFIFOAssignment; /*!< Specifies the FIFO (0 or 1) which will be assigned to the filter. + This parameter can be a value of @ref CAN_filter_FIFO */ + + uint8_t CAN_FilterNumber; /*!< Specifies the filter which will be initialized. It ranges from 0 to 13. */ + + uint8_t CAN_FilterMode; /*!< Specifies the filter mode to be initialized. + This parameter can be a value of @ref CAN_filter_mode */ + + uint8_t CAN_FilterScale; /*!< Specifies the filter scale. + This parameter can be a value of @ref CAN_filter_scale */ + + FunctionalState CAN_FilterActivation; /*!< Enable or disable the filter. + This parameter can be set either to ENABLE or DISABLE. */ +} CAN_FilterInitTypeDef; + +/** + * @brief CAN Tx message structure definition + */ +typedef struct +{ + uint32_t StdId; /*!< Specifies the standard identifier. + This parameter can be a value between 0 to 0x7FF. */ + + uint32_t ExtId; /*!< Specifies the extended identifier. + This parameter can be a value between 0 to 0x1FFFFFFF. */ + + uint8_t IDE; /*!< Specifies the type of identifier for the message that + will be transmitted. This parameter can be a value + of @ref CAN_identifier_type */ + + uint8_t RTR; /*!< Specifies the type of frame for the message that will + be transmitted. This parameter can be a value of + @ref CAN_remote_transmission_request */ + + uint8_t DLC; /*!< Specifies the length of the frame that will be + transmitted. This parameter can be a value between + 0 to 8 */ + + uint8_t Data[8]; /*!< Contains the data to be transmitted. It ranges from 0 + to 0xFF. */ +} CanTxMsg; + +/** + * @brief CAN Rx message structure definition + */ +typedef struct +{ + uint32_t StdId; /*!< Specifies the standard identifier. + This parameter can be a value between 0 to 0x7FF. */ + + uint32_t ExtId; /*!< Specifies the extended identifier. + This parameter can be a value between 0 to 0x1FFFFFFF. */ + + uint8_t IDE; /*!< Specifies the type of identifier for the message that + will be received. This parameter can be a value of + @ref CAN_identifier_type */ + + uint8_t RTR; /*!< Specifies the type of frame for the received message. + This parameter can be a value of + @ref CAN_remote_transmission_request */ + + uint8_t DLC; /*!< Specifies the length of the frame that will be received. + This parameter can be a value between 0 to 8 */ + + uint8_t Data[8]; /*!< Contains the data to be received. It ranges from 0 to + 0xFF. */ + + uint8_t FMI; /*!< Specifies the index of the filter the message stored in + the mailbox passes through. This parameter can be a + value between 0 to 0xFF */ +} CanRxMsg; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup CAN_Exported_Constants + * @{ + */ + +/** @defgroup CAN_InitStatus + * @{ + */ + +#define CAN_InitStatus_Failed ((uint8_t)0x00) /*!< CAN initialization failed */ +#define CAN_InitStatus_Success ((uint8_t)0x01) /*!< CAN initialization OK */ + + +/* Legacy defines */ +#define CANINITFAILED CAN_InitStatus_Failed +#define CANINITOK CAN_InitStatus_Success +/** + * @} + */ + +/** @defgroup CAN_operating_mode + * @{ + */ + +#define CAN_Mode_Normal ((uint8_t)0x00) /*!< normal mode */ +#define CAN_Mode_LoopBack ((uint8_t)0x01) /*!< loopback mode */ +#define CAN_Mode_Silent ((uint8_t)0x02) /*!< silent mode */ +#define CAN_Mode_Silent_LoopBack ((uint8_t)0x03) /*!< loopback combined with silent mode */ + +#define IS_CAN_MODE(MODE) (((MODE) == CAN_Mode_Normal) || \ + ((MODE) == CAN_Mode_LoopBack)|| \ + ((MODE) == CAN_Mode_Silent) || \ + ((MODE) == CAN_Mode_Silent_LoopBack)) +/** + * @} + */ + + + /** + * @defgroup CAN_operating_mode + * @{ + */ +#define CAN_OperatingMode_Initialization ((uint8_t)0x00) /*!< Initialization mode */ +#define CAN_OperatingMode_Normal ((uint8_t)0x01) /*!< Normal mode */ +#define CAN_OperatingMode_Sleep ((uint8_t)0x02) /*!< sleep mode */ + + +#define IS_CAN_OPERATING_MODE(MODE) (((MODE) == CAN_OperatingMode_Initialization) ||\ + ((MODE) == CAN_OperatingMode_Normal)|| \ + ((MODE) == CAN_OperatingMode_Sleep)) +/** + * @} + */ + +/** + * @defgroup CAN_operating_mode_status + * @{ + */ + +#define CAN_ModeStatus_Failed ((uint8_t)0x00) /*!< CAN entering the specific mode failed */ +#define CAN_ModeStatus_Success ((uint8_t)!CAN_ModeStatus_Failed) /*!< CAN entering the specific mode Succeed */ +/** + * @} + */ + +/** @defgroup CAN_synchronisation_jump_width + * @{ + */ +#define CAN_SJW_1tq ((uint8_t)0x00) /*!< 1 time quantum */ +#define CAN_SJW_2tq ((uint8_t)0x01) /*!< 2 time quantum */ +#define CAN_SJW_3tq ((uint8_t)0x02) /*!< 3 time quantum */ +#define CAN_SJW_4tq ((uint8_t)0x03) /*!< 4 time quantum */ + +#define IS_CAN_SJW(SJW) (((SJW) == CAN_SJW_1tq) || ((SJW) == CAN_SJW_2tq)|| \ + ((SJW) == CAN_SJW_3tq) || ((SJW) == CAN_SJW_4tq)) +/** + * @} + */ + +/** @defgroup CAN_time_quantum_in_bit_segment_1 + * @{ + */ +#define CAN_BS1_1tq ((uint8_t)0x00) /*!< 1 time quantum */ +#define CAN_BS1_2tq ((uint8_t)0x01) /*!< 2 time quantum */ +#define CAN_BS1_3tq ((uint8_t)0x02) /*!< 3 time quantum */ +#define CAN_BS1_4tq ((uint8_t)0x03) /*!< 4 time quantum */ +#define CAN_BS1_5tq ((uint8_t)0x04) /*!< 5 time quantum */ +#define CAN_BS1_6tq ((uint8_t)0x05) /*!< 6 time quantum */ +#define CAN_BS1_7tq ((uint8_t)0x06) /*!< 7 time quantum */ +#define CAN_BS1_8tq ((uint8_t)0x07) /*!< 8 time quantum */ +#define CAN_BS1_9tq ((uint8_t)0x08) /*!< 9 time quantum */ +#define CAN_BS1_10tq ((uint8_t)0x09) /*!< 10 time quantum */ +#define CAN_BS1_11tq ((uint8_t)0x0A) /*!< 11 time quantum */ +#define CAN_BS1_12tq ((uint8_t)0x0B) /*!< 12 time quantum */ +#define CAN_BS1_13tq ((uint8_t)0x0C) /*!< 13 time quantum */ +#define CAN_BS1_14tq ((uint8_t)0x0D) /*!< 14 time quantum */ +#define CAN_BS1_15tq ((uint8_t)0x0E) /*!< 15 time quantum */ +#define CAN_BS1_16tq ((uint8_t)0x0F) /*!< 16 time quantum */ + +#define IS_CAN_BS1(BS1) ((BS1) <= CAN_BS1_16tq) +/** + * @} + */ + +/** @defgroup CAN_time_quantum_in_bit_segment_2 + * @{ + */ +#define CAN_BS2_1tq ((uint8_t)0x00) /*!< 1 time quantum */ +#define CAN_BS2_2tq ((uint8_t)0x01) /*!< 2 time quantum */ +#define CAN_BS2_3tq ((uint8_t)0x02) /*!< 3 time quantum */ +#define CAN_BS2_4tq ((uint8_t)0x03) /*!< 4 time quantum */ +#define CAN_BS2_5tq ((uint8_t)0x04) /*!< 5 time quantum */ +#define CAN_BS2_6tq ((uint8_t)0x05) /*!< 6 time quantum */ +#define CAN_BS2_7tq ((uint8_t)0x06) /*!< 7 time quantum */ +#define CAN_BS2_8tq ((uint8_t)0x07) /*!< 8 time quantum */ + +#define IS_CAN_BS2(BS2) ((BS2) <= CAN_BS2_8tq) +/** + * @} + */ + +/** @defgroup CAN_clock_prescaler + * @{ + */ +#define IS_CAN_PRESCALER(PRESCALER) (((PRESCALER) >= 1) && ((PRESCALER) <= 1024)) +/** + * @} + */ + +/** @defgroup CAN_filter_number + * @{ + */ +#define IS_CAN_FILTER_NUMBER(NUMBER) ((NUMBER) <= 27) +/** + * @} + */ + +/** @defgroup CAN_filter_mode + * @{ + */ +#define CAN_FilterMode_IdMask ((uint8_t)0x00) /*!< identifier/mask mode */ +#define CAN_FilterMode_IdList ((uint8_t)0x01) /*!< identifier list mode */ + +#define IS_CAN_FILTER_MODE(MODE) (((MODE) == CAN_FilterMode_IdMask) || \ + ((MODE) == CAN_FilterMode_IdList)) +/** + * @} + */ + +/** @defgroup CAN_filter_scale + * @{ + */ +#define CAN_FilterScale_16bit ((uint8_t)0x00) /*!< Two 16-bit filters */ +#define CAN_FilterScale_32bit ((uint8_t)0x01) /*!< One 32-bit filter */ + +#define IS_CAN_FILTER_SCALE(SCALE) (((SCALE) == CAN_FilterScale_16bit) || \ + ((SCALE) == CAN_FilterScale_32bit)) +/** + * @} + */ + +/** @defgroup CAN_filter_FIFO + * @{ + */ +#define CAN_Filter_FIFO0 ((uint8_t)0x00) /*!< Filter FIFO 0 assignment for filter x */ +#define CAN_Filter_FIFO1 ((uint8_t)0x01) /*!< Filter FIFO 1 assignment for filter x */ +#define IS_CAN_FILTER_FIFO(FIFO) (((FIFO) == CAN_FilterFIFO0) || \ + ((FIFO) == CAN_FilterFIFO1)) + +/* Legacy defines */ +#define CAN_FilterFIFO0 CAN_Filter_FIFO0 +#define CAN_FilterFIFO1 CAN_Filter_FIFO1 +/** + * @} + */ + +/** @defgroup CAN_Start_bank_filter_for_slave_CAN + * @{ + */ +#define IS_CAN_BANKNUMBER(BANKNUMBER) (((BANKNUMBER) >= 1) && ((BANKNUMBER) <= 27)) +/** + * @} + */ + +/** @defgroup CAN_Tx + * @{ + */ +#define IS_CAN_TRANSMITMAILBOX(TRANSMITMAILBOX) ((TRANSMITMAILBOX) <= ((uint8_t)0x02)) +#define IS_CAN_STDID(STDID) ((STDID) <= ((uint32_t)0x7FF)) +#define IS_CAN_EXTID(EXTID) ((EXTID) <= ((uint32_t)0x1FFFFFFF)) +#define IS_CAN_DLC(DLC) ((DLC) <= ((uint8_t)0x08)) +/** + * @} + */ + +/** @defgroup CAN_identifier_type + * @{ + */ +#define CAN_Id_Standard ((uint32_t)0x00000000) /*!< Standard Id */ +#define CAN_Id_Extended ((uint32_t)0x00000004) /*!< Extended Id */ +#define IS_CAN_IDTYPE(IDTYPE) (((IDTYPE) == CAN_Id_Standard) || \ + ((IDTYPE) == CAN_Id_Extended)) + +/* Legacy defines */ +#define CAN_ID_STD CAN_Id_Standard +#define CAN_ID_EXT CAN_Id_Extended +/** + * @} + */ + +/** @defgroup CAN_remote_transmission_request + * @{ + */ +#define CAN_RTR_Data ((uint32_t)0x00000000) /*!< Data frame */ +#define CAN_RTR_Remote ((uint32_t)0x00000002) /*!< Remote frame */ +#define IS_CAN_RTR(RTR) (((RTR) == CAN_RTR_Data) || ((RTR) == CAN_RTR_Remote)) + +/* Legacy defines */ +#define CAN_RTR_DATA CAN_RTR_Data +#define CAN_RTR_REMOTE CAN_RTR_Remote +/** + * @} + */ + +/** @defgroup CAN_transmit_constants + * @{ + */ +#define CAN_TxStatus_Failed ((uint8_t)0x00)/*!< CAN transmission failed */ +#define CAN_TxStatus_Ok ((uint8_t)0x01) /*!< CAN transmission succeeded */ +#define CAN_TxStatus_Pending ((uint8_t)0x02) /*!< CAN transmission pending */ +#define CAN_TxStatus_NoMailBox ((uint8_t)0x04) /*!< CAN cell did not provide + an empty mailbox */ +/* Legacy defines */ +#define CANTXFAILED CAN_TxStatus_Failed +#define CANTXOK CAN_TxStatus_Ok +#define CANTXPENDING CAN_TxStatus_Pending +#define CAN_NO_MB CAN_TxStatus_NoMailBox +/** + * @} + */ + +/** @defgroup CAN_receive_FIFO_number_constants + * @{ + */ +#define CAN_FIFO0 ((uint8_t)0x00) /*!< CAN FIFO 0 used to receive */ +#define CAN_FIFO1 ((uint8_t)0x01) /*!< CAN FIFO 1 used to receive */ + +#define IS_CAN_FIFO(FIFO) (((FIFO) == CAN_FIFO0) || ((FIFO) == CAN_FIFO1)) +/** + * @} + */ + +/** @defgroup CAN_sleep_constants + * @{ + */ +#define CAN_Sleep_Failed ((uint8_t)0x00) /*!< CAN did not enter the sleep mode */ +#define CAN_Sleep_Ok ((uint8_t)0x01) /*!< CAN entered the sleep mode */ + +/* Legacy defines */ +#define CANSLEEPFAILED CAN_Sleep_Failed +#define CANSLEEPOK CAN_Sleep_Ok +/** + * @} + */ + +/** @defgroup CAN_wake_up_constants + * @{ + */ +#define CAN_WakeUp_Failed ((uint8_t)0x00) /*!< CAN did not leave the sleep mode */ +#define CAN_WakeUp_Ok ((uint8_t)0x01) /*!< CAN leaved the sleep mode */ + +/* Legacy defines */ +#define CANWAKEUPFAILED CAN_WakeUp_Failed +#define CANWAKEUPOK CAN_WakeUp_Ok +/** + * @} + */ + +/** + * @defgroup CAN_Error_Code_constants + * @{ + */ +#define CAN_ErrorCode_NoErr ((uint8_t)0x00) /*!< No Error */ +#define CAN_ErrorCode_StuffErr ((uint8_t)0x10) /*!< Stuff Error */ +#define CAN_ErrorCode_FormErr ((uint8_t)0x20) /*!< Form Error */ +#define CAN_ErrorCode_ACKErr ((uint8_t)0x30) /*!< Acknowledgment Error */ +#define CAN_ErrorCode_BitRecessiveErr ((uint8_t)0x40) /*!< Bit Recessive Error */ +#define CAN_ErrorCode_BitDominantErr ((uint8_t)0x50) /*!< Bit Dominant Error */ +#define CAN_ErrorCode_CRCErr ((uint8_t)0x60) /*!< CRC Error */ +#define CAN_ErrorCode_SoftwareSetErr ((uint8_t)0x70) /*!< Software Set Error */ +/** + * @} + */ + +/** @defgroup CAN_flags + * @{ + */ +/* If the flag is 0x3XXXXXXX, it means that it can be used with CAN_GetFlagStatus() + and CAN_ClearFlag() functions. */ +/* If the flag is 0x1XXXXXXX, it means that it can only be used with + CAN_GetFlagStatus() function. */ + +/* Transmit Flags */ +#define CAN_FLAG_RQCP0 ((uint32_t)0x38000001) /*!< Request MailBox0 Flag */ +#define CAN_FLAG_RQCP1 ((uint32_t)0x38000100) /*!< Request MailBox1 Flag */ +#define CAN_FLAG_RQCP2 ((uint32_t)0x38010000) /*!< Request MailBox2 Flag */ + +/* Receive Flags */ +#define CAN_FLAG_FMP0 ((uint32_t)0x12000003) /*!< FIFO 0 Message Pending Flag */ +#define CAN_FLAG_FF0 ((uint32_t)0x32000008) /*!< FIFO 0 Full Flag */ +#define CAN_FLAG_FOV0 ((uint32_t)0x32000010) /*!< FIFO 0 Overrun Flag */ +#define CAN_FLAG_FMP1 ((uint32_t)0x14000003) /*!< FIFO 1 Message Pending Flag */ +#define CAN_FLAG_FF1 ((uint32_t)0x34000008) /*!< FIFO 1 Full Flag */ +#define CAN_FLAG_FOV1 ((uint32_t)0x34000010) /*!< FIFO 1 Overrun Flag */ + +/* Operating Mode Flags */ +#define CAN_FLAG_WKU ((uint32_t)0x31000008) /*!< Wake up Flag */ +#define CAN_FLAG_SLAK ((uint32_t)0x31000012) /*!< Sleep acknowledge Flag */ +/* @note When SLAK interrupt is disabled (SLKIE=0), no polling on SLAKI is possible. + In this case the SLAK bit can be polled.*/ + +/* Error Flags */ +#define CAN_FLAG_EWG ((uint32_t)0x10F00001) /*!< Error Warning Flag */ +#define CAN_FLAG_EPV ((uint32_t)0x10F00002) /*!< Error Passive Flag */ +#define CAN_FLAG_BOF ((uint32_t)0x10F00004) /*!< Bus-Off Flag */ +#define CAN_FLAG_LEC ((uint32_t)0x30F00070) /*!< Last error code Flag */ + +#define IS_CAN_GET_FLAG(FLAG) (((FLAG) == CAN_FLAG_LEC) || ((FLAG) == CAN_FLAG_BOF) || \ + ((FLAG) == CAN_FLAG_EPV) || ((FLAG) == CAN_FLAG_EWG) || \ + ((FLAG) == CAN_FLAG_WKU) || ((FLAG) == CAN_FLAG_FOV0) || \ + ((FLAG) == CAN_FLAG_FF0) || ((FLAG) == CAN_FLAG_FMP0) || \ + ((FLAG) == CAN_FLAG_FOV1) || ((FLAG) == CAN_FLAG_FF1) || \ + ((FLAG) == CAN_FLAG_FMP1) || ((FLAG) == CAN_FLAG_RQCP2) || \ + ((FLAG) == CAN_FLAG_RQCP1)|| ((FLAG) == CAN_FLAG_RQCP0) || \ + ((FLAG) == CAN_FLAG_SLAK )) + +#define IS_CAN_CLEAR_FLAG(FLAG)(((FLAG) == CAN_FLAG_LEC) || ((FLAG) == CAN_FLAG_RQCP2) || \ + ((FLAG) == CAN_FLAG_RQCP1) || ((FLAG) == CAN_FLAG_RQCP0) || \ + ((FLAG) == CAN_FLAG_FF0) || ((FLAG) == CAN_FLAG_FOV0) ||\ + ((FLAG) == CAN_FLAG_FF1) || ((FLAG) == CAN_FLAG_FOV1) || \ + ((FLAG) == CAN_FLAG_WKU) || ((FLAG) == CAN_FLAG_SLAK)) +/** + * @} + */ + + +/** @defgroup CAN_interrupts + * @{ + */ +#define CAN_IT_TME ((uint32_t)0x00000001) /*!< Transmit mailbox empty Interrupt*/ + +/* Receive Interrupts */ +#define CAN_IT_FMP0 ((uint32_t)0x00000002) /*!< FIFO 0 message pending Interrupt*/ +#define CAN_IT_FF0 ((uint32_t)0x00000004) /*!< FIFO 0 full Interrupt*/ +#define CAN_IT_FOV0 ((uint32_t)0x00000008) /*!< FIFO 0 overrun Interrupt*/ +#define CAN_IT_FMP1 ((uint32_t)0x00000010) /*!< FIFO 1 message pending Interrupt*/ +#define CAN_IT_FF1 ((uint32_t)0x00000020) /*!< FIFO 1 full Interrupt*/ +#define CAN_IT_FOV1 ((uint32_t)0x00000040) /*!< FIFO 1 overrun Interrupt*/ + +/* Operating Mode Interrupts */ +#define CAN_IT_WKU ((uint32_t)0x00010000) /*!< Wake-up Interrupt*/ +#define CAN_IT_SLK ((uint32_t)0x00020000) /*!< Sleep acknowledge Interrupt*/ + +/* Error Interrupts */ +#define CAN_IT_EWG ((uint32_t)0x00000100) /*!< Error warning Interrupt*/ +#define CAN_IT_EPV ((uint32_t)0x00000200) /*!< Error passive Interrupt*/ +#define CAN_IT_BOF ((uint32_t)0x00000400) /*!< Bus-off Interrupt*/ +#define CAN_IT_LEC ((uint32_t)0x00000800) /*!< Last error code Interrupt*/ +#define CAN_IT_ERR ((uint32_t)0x00008000) /*!< Error Interrupt*/ + +/* Flags named as Interrupts : kept only for FW compatibility */ +#define CAN_IT_RQCP0 CAN_IT_TME +#define CAN_IT_RQCP1 CAN_IT_TME +#define CAN_IT_RQCP2 CAN_IT_TME + + +#define IS_CAN_IT(IT) (((IT) == CAN_IT_TME) || ((IT) == CAN_IT_FMP0) ||\ + ((IT) == CAN_IT_FF0) || ((IT) == CAN_IT_FOV0) ||\ + ((IT) == CAN_IT_FMP1) || ((IT) == CAN_IT_FF1) ||\ + ((IT) == CAN_IT_FOV1) || ((IT) == CAN_IT_EWG) ||\ + ((IT) == CAN_IT_EPV) || ((IT) == CAN_IT_BOF) ||\ + ((IT) == CAN_IT_LEC) || ((IT) == CAN_IT_ERR) ||\ + ((IT) == CAN_IT_WKU) || ((IT) == CAN_IT_SLK)) + +#define IS_CAN_CLEAR_IT(IT) (((IT) == CAN_IT_TME) || ((IT) == CAN_IT_FF0) ||\ + ((IT) == CAN_IT_FOV0)|| ((IT) == CAN_IT_FF1) ||\ + ((IT) == CAN_IT_FOV1)|| ((IT) == CAN_IT_EWG) ||\ + ((IT) == CAN_IT_EPV) || ((IT) == CAN_IT_BOF) ||\ + ((IT) == CAN_IT_LEC) || ((IT) == CAN_IT_ERR) ||\ + ((IT) == CAN_IT_WKU) || ((IT) == CAN_IT_SLK)) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +/* Function used to set the CAN configuration to the default reset state *****/ +void CAN_DeInit(CAN_TypeDef* CANx); + +/* Initialization and Configuration functions *********************************/ +uint8_t CAN_Init(CAN_TypeDef* CANx, CAN_InitTypeDef* CAN_InitStruct); +void CAN_FilterInit(CAN_FilterInitTypeDef* CAN_FilterInitStruct); +void CAN_StructInit(CAN_InitTypeDef* CAN_InitStruct); +void CAN_SlaveStartBank(uint8_t CAN_BankNumber); +void CAN_DBGFreeze(CAN_TypeDef* CANx, FunctionalState NewState); +void CAN_TTComModeCmd(CAN_TypeDef* CANx, FunctionalState NewState); + +/* CAN Frames Transmission functions ******************************************/ +uint8_t CAN_Transmit(CAN_TypeDef* CANx, CanTxMsg* TxMessage); +uint8_t CAN_TransmitStatus(CAN_TypeDef* CANx, uint8_t TransmitMailbox); +void CAN_CancelTransmit(CAN_TypeDef* CANx, uint8_t Mailbox); + +/* CAN Frames Reception functions *********************************************/ +void CAN_Receive(CAN_TypeDef* CANx, uint8_t FIFONumber, CanRxMsg* RxMessage); +void CAN_FIFORelease(CAN_TypeDef* CANx, uint8_t FIFONumber); +uint8_t CAN_MessagePending(CAN_TypeDef* CANx, uint8_t FIFONumber); + +/* Operation modes functions **************************************************/ +uint8_t CAN_OperatingModeRequest(CAN_TypeDef* CANx, uint8_t CAN_OperatingMode); +uint8_t CAN_Sleep(CAN_TypeDef* CANx); +uint8_t CAN_WakeUp(CAN_TypeDef* CANx); + +/* CAN Bus Error management functions *****************************************/ +uint8_t CAN_GetLastErrorCode(CAN_TypeDef* CANx); +uint8_t CAN_GetReceiveErrorCounter(CAN_TypeDef* CANx); +uint8_t CAN_GetLSBTransmitErrorCounter(CAN_TypeDef* CANx); + +/* Interrupts and flags management functions **********************************/ +void CAN_ITConfig(CAN_TypeDef* CANx, uint32_t CAN_IT, FunctionalState NewState); +FlagStatus CAN_GetFlagStatus(CAN_TypeDef* CANx, uint32_t CAN_FLAG); +void CAN_ClearFlag(CAN_TypeDef* CANx, uint32_t CAN_FLAG); +ITStatus CAN_GetITStatus(CAN_TypeDef* CANx, uint32_t CAN_IT); +void CAN_ClearITPendingBit(CAN_TypeDef* CANx, uint32_t CAN_IT); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_CAN_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_comp.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_comp.h new file mode 100755 index 0000000000..b3ebab997d --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_comp.h @@ -0,0 +1,435 @@ +/** + ****************************************************************************** + * @file stm32f30x_comp.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the COMP firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_COMP_H +#define __STM32F30x_COMP_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup COMP + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief COMP Init structure definition + */ + +typedef struct +{ + + uint32_t COMP_InvertingInput; /*!< Selects the inverting input of the comparator. + This parameter can be a value of @ref COMP_InvertingInput */ + + uint32_t COMP_NonInvertingInput; /*!< Selects the non inverting input of the comparator. + This parameter can be a value of @ref COMP_NonInvertingInput */ + + uint32_t COMP_Output; /*!< Selects the output redirection of the comparator. + This parameter can be a value of @ref COMP_Output */ + + uint32_t COMP_BlankingSrce; /*!< Selects the output blanking source of the comparator. + This parameter can be a value of @ref COMP_BlankingSrce */ + + uint32_t COMP_OutputPol; /*!< Selects the output polarity of the comparator. + This parameter can be a value of @ref COMP_OutputPoloarity */ + + uint32_t COMP_Hysteresis; /*!< Selects the hysteresis voltage of the comparator. + This parameter can be a value of @ref COMP_Hysteresis */ + + uint32_t COMP_Mode; /*!< Selects the operating mode of the comparator + and allows to adjust the speed/consumption. + This parameter can be a value of @ref COMP_Mode */ +}COMP_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup COMP_Exported_Constants + * @{ + */ + +/** @defgroup COMP_Selection + * @{ + */ + +#define COMP_Selection_COMP1 ((uint32_t)0x00000000) /*!< COMP1 Selection */ +#define COMP_Selection_COMP2 ((uint32_t)0x00000004) /*!< COMP2 Selection */ +#define COMP_Selection_COMP3 ((uint32_t)0x00000008) /*!< COMP3 Selection */ +#define COMP_Selection_COMP4 ((uint32_t)0x0000000C) /*!< COMP4 Selection */ +#define COMP_Selection_COMP5 ((uint32_t)0x00000010) /*!< COMP5 Selection */ +#define COMP_Selection_COMP6 ((uint32_t)0x00000014) /*!< COMP6 Selection */ +#define COMP_Selection_COMP7 ((uint32_t)0x00000018) /*!< COMP7 Selection */ + +#define IS_COMP_ALL_PERIPH(PERIPH) (((PERIPH) == COMP_Selection_COMP1) || \ + ((PERIPH) == COMP_Selection_COMP2) || \ + ((PERIPH) == COMP_Selection_COMP3) || \ + ((PERIPH) == COMP_Selection_COMP4) || \ + ((PERIPH) == COMP_Selection_COMP5) || \ + ((PERIPH) == COMP_Selection_COMP6) || \ + ((PERIPH) == COMP_Selection_COMP7)) + +/** + * @} + */ + +/** @defgroup COMP_InvertingInput + * @{ + */ + +#define COMP_InvertingInput_1_4VREFINT ((uint32_t)0x00000000) /*!< 1/4 VREFINT connected to comparator inverting input */ +#define COMP_InvertingInput_1_2VREFINT COMP_CSR_COMPxINSEL_0 /*!< 1/2 VREFINT connected to comparator inverting input */ +#define COMP_InvertingInput_3_4VREFINT COMP_CSR_COMPxINSEL_1 /*!< 3/4 VREFINT connected to comparator inverting input */ +#define COMP_InvertingInput_VREFINT ((uint32_t)0x00000030) /*!< VREFINT connected to comparator inverting input */ +#define COMP_InvertingInput_DAC1OUT1 COMP_CSR_COMPxINSEL_2 /*!< DAC1_OUT1 (PA4) connected to comparator inverting input */ +#define COMP_InvertingInput_DAC1OUT2 ((uint32_t)0x00000050) /*!< DAC1_OUT2 (PA5) connected to comparator inverting input */ + +#define COMP_InvertingInput_IO1 ((uint32_t)0x00000060) /*!< I/O1 (PA0 for COMP1, PA2 for COMP2, PD15 for COMP3, + PE8 for COMP4, PD13 for COMP5, PD10 for COMP6, + PC0 for COMP7) connected to comparator inverting input */ + +#define COMP_InvertingInput_IO2 COMP_CSR_COMPxINSEL /*!< I/O2 (PB12 for COMP3, PB2 for COMP4, PB10 for COMP5, + PB15 for COMP6) connected to comparator inverting input. + It is valid only for STM32F303xC devices */ + +#define COMP_InvertingInput_DAC2OUT1 COMP_CSR_COMPxINSEL_3 /*!< DAC2_OUT1 (PA6) connected to comparator inverting input */ + +#define IS_COMP_INVERTING_INPUT(INPUT) (((INPUT) == COMP_InvertingInput_1_4VREFINT) || \ + ((INPUT) == COMP_InvertingInput_1_2VREFINT) || \ + ((INPUT) == COMP_InvertingInput_3_4VREFINT) || \ + ((INPUT) == COMP_InvertingInput_VREFINT) || \ + ((INPUT) == COMP_InvertingInput_DAC1OUT1) || \ + ((INPUT) == COMP_InvertingInput_DAC1OUT2) || \ + ((INPUT) == COMP_InvertingInput_IO1) || \ + ((INPUT) == COMP_InvertingInput_IO2) || \ + ((INPUT) == COMP_InvertingInput_DAC2OUT1)) +/** + * @} + */ + +/** @defgroup COMP_NonInvertingInput + * @{ + */ + +#define COMP_NonInvertingInput_IO1 ((uint32_t)0x00000000) /*!< I/O1 (PA1 for COMP1, PA7 for COMP2, PB14 for COMP3, + PB0 for COMP4, PD12 for COMP5, PD11 for COMP6, + PA0 for COMP7) connected to comparator non inverting input */ + +#define COMP_NonInvertingInput_IO2 COMP_CSR_COMPxNONINSEL /*!< I/O2 (PA3 for COMP2, PD14 for COMP3, PE7 for COMP4, PB13 for COMP5, + PB11 for COMP6, PC1 for COMP7) connected to comparator non inverting input */ + +#define IS_COMP_NONINVERTING_INPUT(INPUT) (((INPUT) == COMP_NonInvertingInput_IO1) || \ + ((INPUT) == COMP_NonInvertingInput_IO2)) +/** + * @} + */ + +/** @defgroup COMP_Output + * @{ + */ + +#define COMP_Output_None ((uint32_t)0x00000000) /*!< COMP output isn't connected to other peripherals */ + +/* Output Redirection common for all comparators COMP1...COMP7 */ +#define COMP_Output_TIM1BKIN COMP_CSR_COMPxOUTSEL_0 /*!< COMP output connected to TIM1 Break Input (BKIN) */ +#define COMP_Output_TIM1BKIN2 ((uint32_t)0x00000800) /*!< COMP output connected to TIM1 Break Input 2 (BKIN2) */ +#define COMP_Output_TIM8BKIN ((uint32_t)0x00000C00) /*!< COMP output connected to TIM8 Break Input (BKIN) */ +#define COMP_Output_TIM8BKIN2 ((uint32_t)0x00001000) /*!< COMP output connected to TIM8 Break Input 2 (BKIN2) */ +#define COMP_Output_TIM1BKIN2_TIM8BKIN2 ((uint32_t)0x00001400) /*!< COMP output connected to TIM1 Break Input 2 and TIM8 Break Input 2 */ +#define COMP_Output_TIM20BKIN ((uint32_t)0x00003000) /*!< COMP output connected to TIM20 Break Input (BKIN) */ +#define COMP_Output_TIM20BKIN2 ((uint32_t)0x00003400) /*!< COMP output connected to TIM20 Break Input 2 (BKIN2) */ +#define COMP_Output_TIM1BKIN2_TIM8BKIN2_TIM20BKIN2 ((uint32_t)0x00001400) /*!< COMP output connected to TIM1 Break Input 2, TIM8 Break Input 2 and TIM20 Break Input2 */ + +/* Output Redirection common for COMP1 and COMP2 */ +#define COMP_Output_TIM1OCREFCLR ((uint32_t)0x00001800) /*!< COMP output connected to TIM1 OCREF Clear */ +#define COMP_Output_TIM1IC1 ((uint32_t)0x00001C00) /*!< COMP output connected to TIM1 Input Capture 1 */ +#define COMP_Output_TIM2IC4 ((uint32_t)0x00002000) /*!< COMP output connected to TIM2 Input Capture 4 */ +#define COMP_Output_TIM2OCREFCLR ((uint32_t)0x00002400) /*!< COMP output connected to TIM2 OCREF Clear */ +#define COMP_Output_TIM3IC1 ((uint32_t)0x00002800) /*!< COMP output connected to TIM3 Input Capture 1 */ +#define COMP_Output_TIM3OCREFCLR ((uint32_t)0x00002C00) /*!< COMP output connected to TIM3 OCREF Clear */ + +/* Output Redirection specific to COMP2 */ +#define COMP_Output_HRTIM1_FLT6 ((uint32_t)0x00003000) /*!< COMP output connected to HRTIM1 FLT6 */ +#define COMP_Output_HRTIM1_EE1_2 ((uint32_t)0x00003400) /*!< COMP output connected to HRTIM1 EE1_2*/ +#define COMP_Output_HRTIM1_EE6_2 ((uint32_t)0x00003800) /*!< COMP output connected to HRTIM1 EE6_2 */ +#define COMP_Output_TIM20OCREFCLR ((uint32_t)0x00003C00) /*!< COMP output connected to TIM20 OCREF Clear */ + +/* Output Redirection specific to COMP3 */ +#define COMP_Output_TIM4IC1 ((uint32_t)0x00001C00) /*!< COMP output connected to TIM4 Input Capture 1 */ +#define COMP_Output_TIM3IC2 ((uint32_t)0x00002000) /*!< COMP output connected to TIM3 Input Capture 2 */ +#define COMP_Output_TIM15IC1 ((uint32_t)0x00002800) /*!< COMP output connected to TIM15 Input Capture 1 */ +#define COMP_Output_TIM15BKIN ((uint32_t)0x00002C00) /*!< COMP output connected to TIM15 Break Input (BKIN) */ + +/* Output Redirection specific to COMP4 */ +#define COMP_Output_TIM3IC3 ((uint32_t)0x00001800) /*!< COMP output connected to TIM3 Input Capture 3 */ +#define COMP_Output_TIM8OCREFCLR ((uint32_t)0x00001C00) /*!< COMP output connected to TIM8 OCREF Clear */ +#define COMP_Output_TIM15IC2 ((uint32_t)0x00002000) /*!< COMP output connected to TIM15 Input Capture 2 */ +#define COMP_Output_TIM4IC2 ((uint32_t)0x00002400) /*!< COMP output connected to TIM4 Input Capture 2 */ +#define COMP_Output_TIM15OCREFCLR ((uint32_t)0x00002800) /*!< COMP output connected to TIM15 OCREF Clear */ + +#define COMP_Output_HRTIM1_FLT7 ((uint32_t)0x00003000) /*!< COMP output connected to HRTIM1 FLT7 */ +#define COMP_Output_HRTIM1_EE2_2 ((uint32_t)0x00003400) /*!< COMP output connected to HRTIM1 EE2_2*/ +#define COMP_Output_HRTIM1_EE7_2 ((uint32_t)0x00003800) /*!< COMP output connected to HRTIM1 EE7_2 */ + +/* Output Redirection specific to COMP5 */ +#define COMP_Output_TIM2IC1 ((uint32_t)0x00001800) /*!< COMP output connected to TIM2 Input Capture 1 */ +#define COMP_Output_TIM17IC1 ((uint32_t)0x00002000) /*!< COMP output connected to TIM17 Input Capture 1 */ +#define COMP_Output_TIM4IC3 ((uint32_t)0x00002400) /*!< COMP output connected to TIM4 Input Capture 3 */ +#define COMP_Output_TIM16BKIN ((uint32_t)0x00002800) /*!< COMP output connected to TIM16 Break Input (BKIN) */ + +/* Output Redirection specific to COMP6 */ +#define COMP_Output_TIM2IC2 ((uint32_t)0x00001800) /*!< COMP output connected to TIM2 Input Capture 2 */ +#define COMP_Output_COMP6TIM2OCREFCLR ((uint32_t)0x00002000) /*!< COMP output connected to TIM2 OCREF Clear */ +#define COMP_Output_TIM16OCREFCLR ((uint32_t)0x00002400) /*!< COMP output connected to TIM16 OCREF Clear */ +#define COMP_Output_TIM16IC1 ((uint32_t)0x00002800) /*!< COMP output connected to TIM16 Input Capture 1 */ +#define COMP_Output_TIM4IC4 ((uint32_t)0x00002C00) /*!< COMP output connected to TIM4 Input Capture 4 */ + +#define COMP_Output_HRTIM1_FLT8 ((uint32_t)0x00003000) /*!< COMP output connected to HRTIM1 FLT8 */ +#define COMP_Output_HRTIM1_EE3_2 ((uint32_t)0x00003400) /*!< COMP output connected to HRTIM1 EE3_2*/ +#define COMP_Output_HRTIM1_EE8_2 ((uint32_t)0x00003800) /*!< COMP output connected to HRTIM1 EE8_2 */ + +/* Output Redirection specific to COMP7 */ +#define COMP_Output_TIM2IC3 ((uint32_t)0x00002000) /*!< COMP output connected to TIM2 Input Capture 3 */ +#define COMP_Output_TIM1IC2 ((uint32_t)0x00002400) /*!< COMP output connected to TIM1 Input Capture 2 */ +#define COMP_Output_TIM17OCREFCLR ((uint32_t)0x00002800) /*!< COMP output connected to TIM16 OCREF Clear */ +#define COMP_Output_TIM17BKIN ((uint32_t)0x00002C00) /*!< COMP output connected to TIM16 Break Input (BKIN) */ + +#define IS_COMP_OUTPUT(OUTPUT) (((OUTPUT) == COMP_Output_None) || \ + ((OUTPUT) == COMP_Output_TIM1BKIN) || \ + ((OUTPUT) == COMP_Output_TIM1IC1) || \ + ((OUTPUT) == COMP_Output_TIM1OCREFCLR) || \ + ((OUTPUT) == COMP_Output_TIM2IC4) || \ + ((OUTPUT) == COMP_Output_TIM2OCREFCLR) || \ + ((OUTPUT) == COMP_Output_COMP6TIM2OCREFCLR) || \ + ((OUTPUT) == COMP_Output_TIM3IC1) || \ + ((OUTPUT) == COMP_Output_TIM3OCREFCLR) || \ + ((OUTPUT) == COMP_Output_TIM8BKIN) || \ + ((OUTPUT) == COMP_Output_TIM1BKIN2) || \ + ((OUTPUT) == COMP_Output_TIM8BKIN2) || \ + ((OUTPUT) == COMP_Output_TIM2OCREFCLR) || \ + ((OUTPUT) == COMP_Output_TIM1BKIN2_TIM8BKIN2) || \ + ((OUTPUT) == COMP_Output_TIM3IC2) || \ + ((OUTPUT) == COMP_Output_TIM4IC1) || \ + ((OUTPUT) == COMP_Output_TIM15IC1) || \ + ((OUTPUT) == COMP_Output_TIM15BKIN) || \ + ((OUTPUT) == COMP_Output_TIM8OCREFCLR) || \ + ((OUTPUT) == COMP_Output_TIM3IC3) || \ + ((OUTPUT) == COMP_Output_TIM4IC1) || \ + ((OUTPUT) == COMP_Output_TIM15IC1) || \ + ((OUTPUT) == COMP_Output_TIM2IC1) || \ + ((OUTPUT) == COMP_Output_TIM4IC3) || \ + ((OUTPUT) == COMP_Output_TIM16BKIN) || \ + ((OUTPUT) == COMP_Output_TIM17IC1) || \ + ((OUTPUT) == COMP_Output_TIM2IC2) || \ + ((OUTPUT) == COMP_Output_TIM16IC1) || \ + ((OUTPUT) == COMP_Output_TIM4IC4) || \ + ((OUTPUT) == COMP_Output_TIM16OCREFCLR) || \ + ((OUTPUT) == COMP_Output_TIM2IC3) || \ + ((OUTPUT) == COMP_Output_TIM1IC2) || \ + ((OUTPUT) == COMP_Output_TIM17BKIN) || \ + ((OUTPUT) == COMP_Output_TIM17OCREFCLR) || \ + ((OUTPUT) == COMP_Output_HRTIM1_FLT6) || \ + ((OUTPUT) == COMP_Output_HRTIM1_EE1_2) || \ + ((OUTPUT) == COMP_Output_HRTIM1_EE6_2) || \ + ((OUTPUT) == COMP_Output_HRTIM1_FLT7) || \ + ((OUTPUT) == COMP_Output_HRTIM1_EE2_2) || \ + ((OUTPUT) == COMP_Output_HRTIM1_EE7_2) || \ + ((OUTPUT) == COMP_Output_HRTIM1_FLT8) || \ + ((OUTPUT) == COMP_Output_HRTIM1_EE3_2) || \ + ((OUTPUT) == COMP_Output_HRTIM1_EE8_2) || \ + ((OUTPUT) == COMP_Output_TIM20BKIN) || \ + ((OUTPUT) == COMP_Output_TIM20BKIN2) || \ + ((OUTPUT) == COMP_Output_TIM1BKIN2_TIM8BKIN2_TIM20BKIN2)|| \ + ((OUTPUT) == COMP_Output_TIM20OCREFCLR)) + +/** + * @} + */ + +/** @defgroup COMP_BlankingSrce + * @{ + */ + +/* No blanking source can be selected for all comparators */ +#define COMP_BlankingSrce_None ((uint32_t)0x00000000) /*!< No blanking source */ + +/* Blanking source common for COMP1, COMP2, COMP3 and COMP7 */ +#define COMP_BlankingSrce_TIM1OC5 COMP_CSR_COMPxBLANKING_0 /*!< TIM1 OC5 selected as blanking source for compartor */ + +/* Blanking source common for COMP1 and COMP2 */ +#define COMP_BlankingSrce_TIM2OC3 COMP_CSR_COMPxBLANKING_1 /*!< TIM2 OC5 selected as blanking source for compartor */ + +/* Blanking source common for COMP1, COMP2 and COMP5 */ +#define COMP_BlankingSrce_TIM3OC3 ((uint32_t)0x000C0000) /*!< TIM2 OC3 selected as blanking source for compartor */ + +/* Blanking source common for COMP3 and COMP6 */ +#define COMP_BlankingSrce_TIM2OC4 ((uint32_t)0x000C0000) /*!< TIM2 OC4 selected as blanking source for compartor */ + +/* Blanking source common for COMP4, COMP5, COMP6 and COMP7 */ +#define COMP_BlankingSrce_TIM8OC5 COMP_CSR_COMPxBLANKING_1 /*!< TIM8 OC5 selected as blanking source for compartor */ + +/* Blanking source for COMP4 */ +#define COMP_BlankingSrce_TIM3OC4 COMP_CSR_COMPxBLANKING_0 /*!< TIM3 OC4 selected as blanking source for compartor */ +#define COMP_BlankingSrce_TIM15OC1 ((uint32_t)0x000C0000) /*!< TIM15 OC1 selected as blanking source for compartor */ + +/* Blanking source common for COMP6 and COMP7 */ +#define COMP_BlankingSrce_TIM15OC2 COMP_CSR_COMPxBLANKING_2 /*!< TIM15 OC2 selected as blanking source for compartor */ + +#define IS_COMP_BLANKING_SOURCE(SOURCE) (((SOURCE) == COMP_BlankingSrce_None) || \ + ((SOURCE) == COMP_BlankingSrce_TIM1OC5) || \ + ((SOURCE) == COMP_BlankingSrce_TIM2OC3) || \ + ((SOURCE) == COMP_BlankingSrce_TIM3OC3) || \ + ((SOURCE) == COMP_BlankingSrce_TIM2OC4) || \ + ((SOURCE) == COMP_BlankingSrce_TIM8OC5) || \ + ((SOURCE) == COMP_BlankingSrce_TIM3OC4) || \ + ((SOURCE) == COMP_BlankingSrce_TIM15OC1) || \ + ((SOURCE) == COMP_BlankingSrce_TIM15OC2)) +/** + * @} + */ + +/** @defgroup COMP_OutputPoloarity + * @{ + */ +#define COMP_OutputPol_NonInverted ((uint32_t)0x00000000) /*!< COMP output on GPIO isn't inverted */ +#define COMP_OutputPol_Inverted COMP_CSR_COMPxPOL /*!< COMP output on GPIO is inverted */ + +#define IS_COMP_OUTPUT_POL(POL) (((POL) == COMP_OutputPol_NonInverted) || \ + ((POL) == COMP_OutputPol_Inverted)) + +/** + * @} + */ + +/** @defgroup COMP_Hysteresis + * @{ + */ +/* Please refer to the electrical characteristics in the device datasheet for + the hysteresis level */ +#define COMP_Hysteresis_No 0x00000000 /*!< No hysteresis */ +#define COMP_Hysteresis_Low COMP_CSR_COMPxHYST_0 /*!< Hysteresis level low */ +#define COMP_Hysteresis_Medium COMP_CSR_COMPxHYST_1 /*!< Hysteresis level medium */ +#define COMP_Hysteresis_High COMP_CSR_COMPxHYST /*!< Hysteresis level high */ + +#define IS_COMP_HYSTERESIS(HYSTERESIS) (((HYSTERESIS) == COMP_Hysteresis_No) || \ + ((HYSTERESIS) == COMP_Hysteresis_Low) || \ + ((HYSTERESIS) == COMP_Hysteresis_Medium) || \ + ((HYSTERESIS) == COMP_Hysteresis_High)) +/** + * @} + */ + +/** @defgroup COMP_Mode + * @{ + */ +/* Please refer to the electrical characteristics in the device datasheet for + the power consumption values */ +#define COMP_Mode_HighSpeed 0x00000000 /*!< High Speed */ +#define COMP_Mode_MediumSpeed COMP_CSR_COMPxMODE_0 /*!< Medium Speed */ +#define COMP_Mode_LowPower COMP_CSR_COMPxMODE_1 /*!< Low power mode */ +#define COMP_Mode_UltraLowPower COMP_CSR_COMPxMODE /*!< Ultra-low power mode */ + +#define IS_COMP_MODE(MODE) (((MODE) == COMP_Mode_UltraLowPower) || \ + ((MODE) == COMP_Mode_LowPower) || \ + ((MODE) == COMP_Mode_MediumSpeed) || \ + ((MODE) == COMP_Mode_HighSpeed)) +/** + * @} + */ + +/** @defgroup COMP_OutputLevel + * @{ + */ +/* When output polarity is not inverted, comparator output is high when + the non-inverting input is at a higher voltage than the inverting input */ +#define COMP_OutputLevel_High COMP_CSR_COMPxOUT +/* When output polarity is not inverted, comparator output is low when + the non-inverting input is at a lower voltage than the inverting input*/ +#define COMP_OutputLevel_Low ((uint32_t)0x00000000) + +/** + * @} + */ + +/** @defgroup COMP_WindowMode + * @{ + */ +#define IS_COMP_WINDOW(WINDOW) (((WINDOW) == COMP_Selection_COMP2) || \ + ((WINDOW) == COMP_Selection_COMP4) || \ + ((WINDOW) == COMP_Selection_COMP6)) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the COMP configuration to the default reset state ****/ +void COMP_DeInit(uint32_t COMP_Selection); + +/* Initialization and Configuration functions *********************************/ +void COMP_Init(uint32_t COMP_Selection, COMP_InitTypeDef* COMP_InitStruct); +void COMP_StructInit(COMP_InitTypeDef* COMP_InitStruct); +void COMP_Cmd(uint32_t COMP_Selection, FunctionalState NewState); +void COMP_SwitchCmd(uint32_t COMP_Selection, FunctionalState NewState); +uint32_t COMP_GetOutputLevel(uint32_t COMP_Selection); + +/* Window mode control function ***********************************************/ +void COMP_WindowCmd(uint32_t COMP_Selection, FunctionalState NewState); + +/* COMP configuration locking function ****************************************/ +void COMP_LockConfig(uint32_t COMP_Selection); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_COMP_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_crc.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_crc.h new file mode 100755 index 0000000000..50db40dafc --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_crc.h @@ -0,0 +1,121 @@ +/** + ****************************************************************************** + * @file stm32f30x_crc.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the CRC firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_CRC_H +#define __STM32F30x_CRC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/*!< Includes ----------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup CRC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup CRC_ReverseInputData + * @{ + */ +#define CRC_ReverseInputData_No ((uint32_t)0x00000000) /*!< No reverse operation of Input Data */ +#define CRC_ReverseInputData_8bits CRC_CR_REV_IN_0 /*!< Reverse operation of Input Data on 8 bits */ +#define CRC_ReverseInputData_16bits CRC_CR_REV_IN_1 /*!< Reverse operation of Input Data on 16 bits */ +#define CRC_ReverseInputData_32bits CRC_CR_REV_IN /*!< Reverse operation of Input Data on 32 bits */ + +#define IS_CRC_REVERSE_INPUT_DATA(DATA) (((DATA) == CRC_ReverseInputData_No) || \ + ((DATA) == CRC_ReverseInputData_8bits) || \ + ((DATA) == CRC_ReverseInputData_16bits) || \ + ((DATA) == CRC_ReverseInputData_32bits)) + +/** + * @} + */ + +/** @defgroup CRC_PolynomialSize + * @{ + */ +#define CRC_PolSize_7 CRC_CR_POLSIZE /*!< 7-bit polynomial for CRC calculation */ +#define CRC_PolSize_8 CRC_CR_POLSIZE_1 /*!< 8-bit polynomial for CRC calculation */ +#define CRC_PolSize_16 CRC_CR_POLSIZE_0 /*!< 16-bit polynomial for CRC calculation */ +#define CRC_PolSize_32 ((uint32_t)0x00000000)/*!< 32-bit polynomial for CRC calculation */ + +#define IS_CRC_POL_SIZE(SIZE) (((SIZE) == CRC_PolSize_7) || \ + ((SIZE) == CRC_PolSize_8) || \ + ((SIZE) == CRC_PolSize_16) || \ + ((SIZE) == CRC_PolSize_32)) + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* Configuration of the CRC computation unit **********************************/ +void CRC_DeInit(void); +void CRC_ResetDR(void); +void CRC_PolynomialSizeSelect(uint32_t CRC_PolSize); +void CRC_ReverseInputDataSelect(uint32_t CRC_ReverseInputData); +void CRC_ReverseOutputDataCmd(FunctionalState NewState); +void CRC_SetInitRegister(uint32_t CRC_InitValue); +void CRC_SetPolynomial(uint32_t CRC_Pol); + +/* CRC computation ************************************************************/ +uint32_t CRC_CalcCRC(uint32_t CRC_Data); +uint32_t CRC_CalcCRC16bits(uint16_t CRC_Data); +uint32_t CRC_CalcCRC8bits(uint8_t CRC_Data); +uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); +uint32_t CRC_GetCRC(void); + +/* Independent register (IDR) access (write/read) *****************************/ +void CRC_SetIDRegister(uint8_t CRC_IDValue); +uint8_t CRC_GetIDRegister(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_CRC_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dac.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dac.h new file mode 100755 index 0000000000..9481708450 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dac.h @@ -0,0 +1,322 @@ +/** + ****************************************************************************** + * @file stm32f30x_dac.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the DAC firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_DAC_H +#define __STM32F30x_DAC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup DAC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +#define DAC_CR_DMAUDRIE ((uint32_t)0x00002000) /*!< DAC channel DMA underrun interrupt enable */ + +/** + * @brief DAC Init structure definition + */ + +typedef struct +{ + uint32_t DAC_Trigger; /*!< Specifies the external trigger for the selected DAC channel. + This parameter can be a value of @ref DAC_trigger_selection */ + + uint32_t DAC_WaveGeneration; /*!< Specifies whether DAC channel noise waves or triangle waves + are generated, or whether no wave is generated. + This parameter can be a value of @ref DAC_wave_generation */ + + uint32_t DAC_LFSRUnmask_TriangleAmplitude; /*!< Specifies the LFSR mask for noise wave generation or + the maximum amplitude triangle generation for the DAC channel. + This parameter can be a value of @ref DAC_lfsrunmask_triangleamplitude */ + + uint32_t DAC_Buffer_Switch; /*!< Specifies whether the DAC channel output buffer is enabled or disabled or + the DAC channel output switch is enabled or disabled. + This parameter can be a value of @ref DAC_buffer_switch */ +}DAC_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup DAC_Exported_Constants + * @{ + */ + +#define IS_DAC_ALL_PERIPH(PERIPH) (((PERIPH) == DAC1) || \ + ((PERIPH) == DAC2)) + +#define IS_DAC_LIST1_PERIPH(PERIPH) (((PERIPH) == DAC1)) + +/** @defgroup DAC_trigger_selection + * @{ + */ + +#define DAC_Trigger_None ((uint32_t)0x00000000) /*!< Conversion is automatic once the DAC1_DHRxxxx register + has been loaded, and not by external trigger */ +#define DAC_Trigger_T6_TRGO ((uint32_t)0x00000004) /*!< TIM6 TRGO selected as external conversion trigger for DAC1/2 channel1/2 */ +#define DAC_Trigger_T3_TRGO ((uint32_t)0x0000000C) /*!< TIM3 TRGO selected as external conversion trigger for DAC1/2 channel1/2 */ +#define DAC_Trigger_T8_TRGO ((uint32_t)0x0000000C) /*!< TIM8 TRGO selected as external conversion trigger for DAC1 channel1/2 */ +#define DAC_Trigger_T7_TRGO ((uint32_t)0x00000014) /*!< TIM7 TRGO selected as external conversion trigger for DAC1/2 channel1/2 */ +#define DAC_Trigger_T15_TRGO ((uint32_t)0x0000001C) /*!< TIM15 TRGO selected as external conversion trigger for DAC1/2 channel1/2 */ +#define DAC_Trigger_HRTIM1_DACTRG1 ((uint32_t)0x0000001C) /*!< HRTIM1 DACTRG1 selected as external conversion trigger for DAC1 channel1/2 */ +#define DAC_Trigger_T2_TRGO ((uint32_t)0x00000024) /*!< TIM2 TRGO selected as external conversion trigger for DAC1/2 channel1/2 */ +#define DAC_Trigger_T4_TRGO ((uint32_t)0x0000002C) /*!< TIM4 TRGO selected as external conversion trigger for DAC channel */ +#define DAC_Trigger_HRTIM1_DACTRG2 ((uint32_t)0x0000002C) /*!< HRTIM1 DACTRG2 selected as external conversion trigger for DAC1 channel1/2 */ +#define DAC_Trigger_HRTIM1_DACTRG3 ((uint32_t)0x0000002C) /*!< HRTIM1 DACTRG3 selected as external conversion trigger for DAC2 channel1 */ +#define DAC_Trigger_Ext_IT9 ((uint32_t)0x00000034) /*!< EXTI Line9 event selected as external conversion trigger for DAC1/2 channel1/2 */ +#define DAC_Trigger_Software ((uint32_t)0x0000003C) /*!< Conversion started by software trigger for DAC1/2 channel1/2 */ + +#define IS_DAC_TRIGGER(TRIGGER) (((TRIGGER) == DAC_Trigger_None) || \ + ((TRIGGER) == DAC_Trigger_T6_TRGO) || \ + ((TRIGGER) == DAC_Trigger_T3_TRGO) || \ + ((TRIGGER) == DAC_Trigger_T8_TRGO) || \ + ((TRIGGER) == DAC_Trigger_T7_TRGO) || \ + ((TRIGGER) == DAC_Trigger_T15_TRGO) || \ + ((TRIGGER) == DAC_Trigger_HRTIM1_DACTRG1)|| \ + ((TRIGGER) == DAC_Trigger_T2_TRGO) || \ + ((TRIGGER) == DAC_Trigger_T4_TRGO) || \ + ((TRIGGER) == DAC_Trigger_HRTIM1_DACTRG2)|| \ + ((TRIGGER) == DAC_Trigger_HRTIM1_DACTRG3)|| \ + ((TRIGGER) == DAC_Trigger_Ext_IT9) || \ + ((TRIGGER) == DAC_Trigger_Software)) + +/** + * @} + */ + +/** @defgroup DAC_wave_generation + * @{ + */ + +#define DAC_WaveGeneration_None ((uint32_t)0x00000000) +#define DAC_WaveGeneration_Noise ((uint32_t)0x00000040) +#define DAC_WaveGeneration_Triangle ((uint32_t)0x00000080) + +#define IS_DAC_GENERATE_WAVE(WAVE) (((WAVE) == DAC_WaveGeneration_None) || \ + ((WAVE) == DAC_WaveGeneration_Noise) || \ + ((WAVE) == DAC_WaveGeneration_Triangle)) +/** + * @} + */ + +/** @defgroup DAC_lfsrunmask_triangleamplitude + * @{ + */ + +#define DAC_LFSRUnmask_Bit0 ((uint32_t)0x00000000) /*!< Unmask DAC channel LFSR bit0 for noise wave generation */ +#define DAC_LFSRUnmask_Bits1_0 ((uint32_t)0x00000100) /*!< Unmask DAC channel LFSR bit[1:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits2_0 ((uint32_t)0x00000200) /*!< Unmask DAC channel LFSR bit[2:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits3_0 ((uint32_t)0x00000300) /*!< Unmask DAC channel LFSR bit[3:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits4_0 ((uint32_t)0x00000400) /*!< Unmask DAC channel LFSR bit[4:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits5_0 ((uint32_t)0x00000500) /*!< Unmask DAC channel LFSR bit[5:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits6_0 ((uint32_t)0x00000600) /*!< Unmask DAC channel LFSR bit[6:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits7_0 ((uint32_t)0x00000700) /*!< Unmask DAC channel LFSR bit[7:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits8_0 ((uint32_t)0x00000800) /*!< Unmask DAC channel LFSR bit[8:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits9_0 ((uint32_t)0x00000900) /*!< Unmask DAC channel LFSR bit[9:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits10_0 ((uint32_t)0x00000A00) /*!< Unmask DAC channel LFSR bit[10:0] for noise wave generation */ +#define DAC_LFSRUnmask_Bits11_0 ((uint32_t)0x00000B00) /*!< Unmask DAC channel LFSR bit[11:0] for noise wave generation */ +#define DAC_TriangleAmplitude_1 ((uint32_t)0x00000000) /*!< Select max triangle amplitude of 1 */ +#define DAC_TriangleAmplitude_3 ((uint32_t)0x00000100) /*!< Select max triangle amplitude of 3 */ +#define DAC_TriangleAmplitude_7 ((uint32_t)0x00000200) /*!< Select max triangle amplitude of 7 */ +#define DAC_TriangleAmplitude_15 ((uint32_t)0x00000300) /*!< Select max triangle amplitude of 15 */ +#define DAC_TriangleAmplitude_31 ((uint32_t)0x00000400) /*!< Select max triangle amplitude of 31 */ +#define DAC_TriangleAmplitude_63 ((uint32_t)0x00000500) /*!< Select max triangle amplitude of 63 */ +#define DAC_TriangleAmplitude_127 ((uint32_t)0x00000600) /*!< Select max triangle amplitude of 127 */ +#define DAC_TriangleAmplitude_255 ((uint32_t)0x00000700) /*!< Select max triangle amplitude of 255 */ +#define DAC_TriangleAmplitude_511 ((uint32_t)0x00000800) /*!< Select max triangle amplitude of 511 */ +#define DAC_TriangleAmplitude_1023 ((uint32_t)0x00000900) /*!< Select max triangle amplitude of 1023 */ +#define DAC_TriangleAmplitude_2047 ((uint32_t)0x00000A00) /*!< Select max triangle amplitude of 2047 */ +#define DAC_TriangleAmplitude_4095 ((uint32_t)0x00000B00) /*!< Select max triangle amplitude of 4095 */ + +#define IS_DAC_LFSR_UNMASK_TRIANGLE_AMPLITUDE(VALUE) (((VALUE) == DAC_LFSRUnmask_Bit0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits1_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits2_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits3_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits4_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits5_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits6_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits7_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits8_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits9_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits10_0) || \ + ((VALUE) == DAC_LFSRUnmask_Bits11_0) || \ + ((VALUE) == DAC_TriangleAmplitude_1) || \ + ((VALUE) == DAC_TriangleAmplitude_3) || \ + ((VALUE) == DAC_TriangleAmplitude_7) || \ + ((VALUE) == DAC_TriangleAmplitude_15) || \ + ((VALUE) == DAC_TriangleAmplitude_31) || \ + ((VALUE) == DAC_TriangleAmplitude_63) || \ + ((VALUE) == DAC_TriangleAmplitude_127) || \ + ((VALUE) == DAC_TriangleAmplitude_255) || \ + ((VALUE) == DAC_TriangleAmplitude_511) || \ + ((VALUE) == DAC_TriangleAmplitude_1023) || \ + ((VALUE) == DAC_TriangleAmplitude_2047) || \ + ((VALUE) == DAC_TriangleAmplitude_4095)) +/** + * @} + */ + +/** @defgroup DAC_buffer_switch + * @{ + */ + +#define DAC_BufferSwitch_Disable ((uint32_t)0x00000000) +#define DAC_BufferSwitch_Enable ((uint32_t)0x00000002) + +#define IS_DAC_BUFFER_SWITCH_STATE(STATE) (((STATE) == DAC_BufferSwitch_Enable) || \ + ((STATE) == DAC_BufferSwitch_Disable)) +/** + * @} + */ + +/** @defgroup DAC_Channel_selection + * @{ + */ +#define DAC_Channel_1 ((uint32_t)0x00000000) +#define DAC_Channel_2 ((uint32_t)0x00000010) + +#define IS_DAC_CHANNEL(CHANNEL) (((CHANNEL) == DAC_Channel_1) || \ + ((CHANNEL) == DAC_Channel_2)) +/** + * @} + */ + +/** @defgroup DAC_data_alignement + * @{ + */ + +#define DAC_Align_12b_R ((uint32_t)0x00000000) +#define DAC_Align_12b_L ((uint32_t)0x00000004) +#define DAC_Align_8b_R ((uint32_t)0x00000008) + +#define IS_DAC_ALIGN(ALIGN) (((ALIGN) == DAC_Align_12b_R) || \ + ((ALIGN) == DAC_Align_12b_L) || \ + ((ALIGN) == DAC_Align_8b_R)) +/** + * @} + */ + +/** @defgroup DAC_wave_generation + * @{ + */ + +#define DAC_Wave_Noise ((uint32_t)0x00000040) +#define DAC_Wave_Triangle ((uint32_t)0x00000080) + +#define IS_DAC_WAVE(WAVE) (((WAVE) == DAC_Wave_Noise) || \ + ((WAVE) == DAC_Wave_Triangle)) +/** + * @} + */ + +/** @defgroup DAC_data + * @{ + */ + +#define IS_DAC_DATA(DATA) ((DATA) <= 0xFFF0) +/** + * @} + */ + +/** @defgroup DAC_interrupts_definition + * @{ + */ +#define DAC_IT_DMAUDR ((uint32_t)0x00002000) +#define IS_DAC_IT(IT) (((IT) == DAC_IT_DMAUDR)) + +/** + * @} + */ + +/** @defgroup DAC_flags_definition + * @{ + */ + +#define DAC_FLAG_DMAUDR ((uint32_t)0x00002000) +#define IS_DAC_FLAG(FLAG) (((FLAG) == DAC_FLAG_DMAUDR)) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +/* Function used to set the DAC configuration to the default reset state *****/ +void DAC_DeInit(DAC_TypeDef* DACx); + +/* DAC channels configuration: trigger, output buffer, data format functions */ +void DAC_Init(DAC_TypeDef* DACx, uint32_t DAC_Channel, DAC_InitTypeDef* DAC_InitStruct); +void DAC_StructInit(DAC_InitTypeDef* DAC_InitStruct); +void DAC_Cmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, FunctionalState NewState); +void DAC_SoftwareTriggerCmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, FunctionalState NewState); +void DAC_DualSoftwareTriggerCmd(DAC_TypeDef* DACx, FunctionalState NewState); +void DAC_WaveGenerationCmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_Wave, FunctionalState NewState); +void DAC_SetChannel1Data(DAC_TypeDef* DACx, uint32_t DAC_Align, uint16_t Data); +void DAC_SetChannel2Data(DAC_TypeDef* DACx, uint32_t DAC_Align, uint16_t Data); +void DAC_SetDualChannelData(DAC_TypeDef* DACx, uint32_t DAC_Align, uint16_t Data2, uint16_t Data1); +uint16_t DAC_GetDataOutputValue(DAC_TypeDef* DACx, uint32_t DAC_Channel); + +/* DMA management functions ***************************************************/ +void DAC_DMACmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, FunctionalState NewState); + +/* Interrupts and flags management functions **********************************/ +void DAC_ITConfig(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_IT, FunctionalState NewState); +FlagStatus DAC_GetFlagStatus(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_FLAG); +void DAC_ClearFlag(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_FLAG); +ITStatus DAC_GetITStatus(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_IT); +void DAC_ClearITPendingBit(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_IT); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_DAC_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dbgmcu.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dbgmcu.h new file mode 100755 index 0000000000..0968bb60e4 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dbgmcu.h @@ -0,0 +1,110 @@ +/** + ****************************************************************************** + * @file stm32f30x_dbgmcu.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the DBGMCU firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_DBGMCU_H +#define __STM32F30x_DBGMCU_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup DBGMCU + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup DBGMCU_Exported_Constants + * @{ + */ +#define DBGMCU_SLEEP ((uint32_t)0x00000001) +#define DBGMCU_STOP ((uint32_t)0x00000002) +#define DBGMCU_STANDBY ((uint32_t)0x00000004) +#define IS_DBGMCU_PERIPH(PERIPH) ((((PERIPH) & 0xFFFFFFF8) == 0x00) && ((PERIPH) != 0x00)) + +#define DBGMCU_TIM2_STOP ((uint32_t)0x00000001) +#define DBGMCU_TIM3_STOP ((uint32_t)0x00000002) +#define DBGMCU_TIM4_STOP ((uint32_t)0x00000004) +#define DBGMCU_TIM6_STOP ((uint32_t)0x00000010) +#define DBGMCU_TIM7_STOP ((uint32_t)0x00000020) +#define DBGMCU_RTC_STOP ((uint32_t)0x00000400) +#define DBGMCU_WWDG_STOP ((uint32_t)0x00000800) +#define DBGMCU_IWDG_STOP ((uint32_t)0x00001000) +#define DBGMCU_I2C1_SMBUS_TIMEOUT ((uint32_t)0x00200000) +#define DBGMCU_I2C2_SMBUS_TIMEOUT ((uint32_t)0x00400000) +#define DBGMCU_CAN1_STOP ((uint32_t)0x02000000) +#define DBGMCU_I2C3_SMBUS_TIMEOUT ((uint32_t)0x40000000) + +#define IS_DBGMCU_APB1PERIPH(PERIPH) ((((PERIPH) & 0xBD9FE3C8) == 0x00) && ((PERIPH) != 0x00)) + +#define DBGMCU_TIM1_STOP ((uint32_t)0x00000001) +#define DBGMCU_TIM8_STOP ((uint32_t)0x00000002) +#define DBGMCU_TIM15_STOP ((uint32_t)0x00000004) +#define DBGMCU_TIM16_STOP ((uint32_t)0x00000008) +#define DBGMCU_TIM17_STOP ((uint32_t)0x00000010) +#define DBGMCU_TIM20_STOP ((uint32_t)0x00000020) +#define IS_DBGMCU_APB2PERIPH(PERIPH) ((((PERIPH) & 0xFFFFFFC0) == 0x00) && ((PERIPH) != 0x00)) + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ +/* Device and Revision ID management functions ********************************/ +uint32_t DBGMCU_GetREVID(void); +uint32_t DBGMCU_GetDEVID(void); + +/* Peripherals Configuration functions ****************************************/ +void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState); +void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState); +void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_DBGMCU_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dma.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dma.h new file mode 100755 index 0000000000..e05d27149c --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_dma.h @@ -0,0 +1,436 @@ +/** + ****************************************************************************** + * @file stm32f30x_dma.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the DMA firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_DMA_H +#define __STM32F30x_DMA_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup DMA + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief DMA Init structures definition + */ +typedef struct +{ + uint32_t DMA_PeripheralBaseAddr; /*!< Specifies the peripheral base address for DMAy Channelx. */ + + uint32_t DMA_MemoryBaseAddr; /*!< Specifies the memory base address for DMAy Channelx. */ + + uint32_t DMA_DIR; /*!< Specifies if the peripheral is the source or destination. + This parameter can be a value of @ref DMA_data_transfer_direction */ + + uint16_t DMA_BufferSize; /*!< Specifies the buffer size, in data unit, of the specified Channel. + The data unit is equal to the configuration set in DMA_PeripheralDataSize + or DMA_MemoryDataSize members depending in the transfer direction. */ + + uint32_t DMA_PeripheralInc; /*!< Specifies whether the Peripheral address register is incremented or not. + This parameter can be a value of @ref DMA_peripheral_incremented_mode */ + + uint32_t DMA_MemoryInc; /*!< Specifies whether the memory address register is incremented or not. + This parameter can be a value of @ref DMA_memory_incremented_mode */ + + uint32_t DMA_PeripheralDataSize; /*!< Specifies the Peripheral data width. + This parameter can be a value of @ref DMA_peripheral_data_size */ + + uint32_t DMA_MemoryDataSize; /*!< Specifies the Memory data width. + This parameter can be a value of @ref DMA_memory_data_size */ + + uint32_t DMA_Mode; /*!< Specifies the operation mode of the DMAy Channelx. + This parameter can be a value of @ref DMA_circular_normal_mode + @note: The circular buffer mode cannot be used if the memory-to-memory + data transfer is configured on the selected Channel */ + + uint32_t DMA_Priority; /*!< Specifies the software priority for the DMAy Channelx. + This parameter can be a value of @ref DMA_priority_level */ + + uint32_t DMA_M2M; /*!< Specifies if the DMAy Channelx will be used in memory-to-memory transfer. + This parameter can be a value of @ref DMA_memory_to_memory */ +}DMA_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup DMA_Exported_Constants + * @{ + */ + +#define IS_DMA_ALL_PERIPH(PERIPH) (((PERIPH) == DMA1_Channel1) || \ + ((PERIPH) == DMA1_Channel2) || \ + ((PERIPH) == DMA1_Channel3) || \ + ((PERIPH) == DMA1_Channel4) || \ + ((PERIPH) == DMA1_Channel5) || \ + ((PERIPH) == DMA1_Channel6) || \ + ((PERIPH) == DMA1_Channel7) || \ + ((PERIPH) == DMA2_Channel1) || \ + ((PERIPH) == DMA2_Channel2) || \ + ((PERIPH) == DMA2_Channel3) || \ + ((PERIPH) == DMA2_Channel4) || \ + ((PERIPH) == DMA2_Channel5)) + +/** @defgroup DMA_data_transfer_direction + * @{ + */ + +#define DMA_DIR_PeripheralSRC ((uint32_t)0x00000000) +#define DMA_DIR_PeripheralDST DMA_CCR_DIR + +#define IS_DMA_DIR(DIR) (((DIR) == DMA_DIR_PeripheralSRC) || \ + ((DIR) == DMA_DIR_PeripheralDST)) +/** + * @} + */ + + +/** @defgroup DMA_peripheral_incremented_mode + * @{ + */ + +#define DMA_PeripheralInc_Disable ((uint32_t)0x00000000) +#define DMA_PeripheralInc_Enable DMA_CCR_PINC + +#define IS_DMA_PERIPHERAL_INC_STATE(STATE) (((STATE) == DMA_PeripheralInc_Disable) || \ + ((STATE) == DMA_PeripheralInc_Enable)) +/** + * @} + */ + +/** @defgroup DMA_memory_incremented_mode + * @{ + */ + +#define DMA_MemoryInc_Disable ((uint32_t)0x00000000) +#define DMA_MemoryInc_Enable DMA_CCR_MINC + +#define IS_DMA_MEMORY_INC_STATE(STATE) (((STATE) == DMA_MemoryInc_Disable) || \ + ((STATE) == DMA_MemoryInc_Enable)) +/** + * @} + */ + +/** @defgroup DMA_peripheral_data_size + * @{ + */ + +#define DMA_PeripheralDataSize_Byte ((uint32_t)0x00000000) +#define DMA_PeripheralDataSize_HalfWord DMA_CCR_PSIZE_0 +#define DMA_PeripheralDataSize_Word DMA_CCR_PSIZE_1 + +#define IS_DMA_PERIPHERAL_DATA_SIZE(SIZE) (((SIZE) == DMA_PeripheralDataSize_Byte) || \ + ((SIZE) == DMA_PeripheralDataSize_HalfWord) || \ + ((SIZE) == DMA_PeripheralDataSize_Word)) +/** + * @} + */ + +/** @defgroup DMA_memory_data_size + * @{ + */ + +#define DMA_MemoryDataSize_Byte ((uint32_t)0x00000000) +#define DMA_MemoryDataSize_HalfWord DMA_CCR_MSIZE_0 +#define DMA_MemoryDataSize_Word DMA_CCR_MSIZE_1 + +#define IS_DMA_MEMORY_DATA_SIZE(SIZE) (((SIZE) == DMA_MemoryDataSize_Byte) || \ + ((SIZE) == DMA_MemoryDataSize_HalfWord) || \ + ((SIZE) == DMA_MemoryDataSize_Word)) +/** + * @} + */ + +/** @defgroup DMA_circular_normal_mode + * @{ + */ + +#define DMA_Mode_Normal ((uint32_t)0x00000000) +#define DMA_Mode_Circular DMA_CCR_CIRC + +#define IS_DMA_MODE(MODE) (((MODE) == DMA_Mode_Normal) || ((MODE) == DMA_Mode_Circular)) +/** + * @} + */ + +/** @defgroup DMA_priority_level + * @{ + */ + +#define DMA_Priority_VeryHigh DMA_CCR_PL +#define DMA_Priority_High DMA_CCR_PL_1 +#define DMA_Priority_Medium DMA_CCR_PL_0 +#define DMA_Priority_Low ((uint32_t)0x00000000) + +#define IS_DMA_PRIORITY(PRIORITY) (((PRIORITY) == DMA_Priority_VeryHigh) || \ + ((PRIORITY) == DMA_Priority_High) || \ + ((PRIORITY) == DMA_Priority_Medium) || \ + ((PRIORITY) == DMA_Priority_Low)) +/** + * @} + */ + +/** @defgroup DMA_memory_to_memory + * @{ + */ + +#define DMA_M2M_Disable ((uint32_t)0x00000000) +#define DMA_M2M_Enable DMA_CCR_MEM2MEM + +#define IS_DMA_M2M_STATE(STATE) (((STATE) == DMA_M2M_Disable) || ((STATE) == DMA_M2M_Enable)) + +/** + * @} + */ + +/** @defgroup DMA_interrupts_definition + * @{ + */ + +#define DMA_IT_TC ((uint32_t)0x00000002) +#define DMA_IT_HT ((uint32_t)0x00000004) +#define DMA_IT_TE ((uint32_t)0x00000008) +#define IS_DMA_CONFIG_IT(IT) ((((IT) & 0xFFFFFFF1) == 0x00) && ((IT) != 0x00)) + +#define DMA1_IT_GL1 ((uint32_t)0x00000001) +#define DMA1_IT_TC1 ((uint32_t)0x00000002) +#define DMA1_IT_HT1 ((uint32_t)0x00000004) +#define DMA1_IT_TE1 ((uint32_t)0x00000008) +#define DMA1_IT_GL2 ((uint32_t)0x00000010) +#define DMA1_IT_TC2 ((uint32_t)0x00000020) +#define DMA1_IT_HT2 ((uint32_t)0x00000040) +#define DMA1_IT_TE2 ((uint32_t)0x00000080) +#define DMA1_IT_GL3 ((uint32_t)0x00000100) +#define DMA1_IT_TC3 ((uint32_t)0x00000200) +#define DMA1_IT_HT3 ((uint32_t)0x00000400) +#define DMA1_IT_TE3 ((uint32_t)0x00000800) +#define DMA1_IT_GL4 ((uint32_t)0x00001000) +#define DMA1_IT_TC4 ((uint32_t)0x00002000) +#define DMA1_IT_HT4 ((uint32_t)0x00004000) +#define DMA1_IT_TE4 ((uint32_t)0x00008000) +#define DMA1_IT_GL5 ((uint32_t)0x00010000) +#define DMA1_IT_TC5 ((uint32_t)0x00020000) +#define DMA1_IT_HT5 ((uint32_t)0x00040000) +#define DMA1_IT_TE5 ((uint32_t)0x00080000) +#define DMA1_IT_GL6 ((uint32_t)0x00100000) +#define DMA1_IT_TC6 ((uint32_t)0x00200000) +#define DMA1_IT_HT6 ((uint32_t)0x00400000) +#define DMA1_IT_TE6 ((uint32_t)0x00800000) +#define DMA1_IT_GL7 ((uint32_t)0x01000000) +#define DMA1_IT_TC7 ((uint32_t)0x02000000) +#define DMA1_IT_HT7 ((uint32_t)0x04000000) +#define DMA1_IT_TE7 ((uint32_t)0x08000000) + +#define DMA2_IT_GL1 ((uint32_t)0x10000001) +#define DMA2_IT_TC1 ((uint32_t)0x10000002) +#define DMA2_IT_HT1 ((uint32_t)0x10000004) +#define DMA2_IT_TE1 ((uint32_t)0x10000008) +#define DMA2_IT_GL2 ((uint32_t)0x10000010) +#define DMA2_IT_TC2 ((uint32_t)0x10000020) +#define DMA2_IT_HT2 ((uint32_t)0x10000040) +#define DMA2_IT_TE2 ((uint32_t)0x10000080) +#define DMA2_IT_GL3 ((uint32_t)0x10000100) +#define DMA2_IT_TC3 ((uint32_t)0x10000200) +#define DMA2_IT_HT3 ((uint32_t)0x10000400) +#define DMA2_IT_TE3 ((uint32_t)0x10000800) +#define DMA2_IT_GL4 ((uint32_t)0x10001000) +#define DMA2_IT_TC4 ((uint32_t)0x10002000) +#define DMA2_IT_HT4 ((uint32_t)0x10004000) +#define DMA2_IT_TE4 ((uint32_t)0x10008000) +#define DMA2_IT_GL5 ((uint32_t)0x10010000) +#define DMA2_IT_TC5 ((uint32_t)0x10020000) +#define DMA2_IT_HT5 ((uint32_t)0x10040000) +#define DMA2_IT_TE5 ((uint32_t)0x10080000) + +#define IS_DMA_CLEAR_IT(IT) (((((IT) & 0xF0000000) == 0x00) || (((IT) & 0xEFF00000) == 0x00)) && ((IT) != 0x00)) + +#define IS_DMA_GET_IT(IT) (((IT) == DMA1_IT_GL1) || ((IT) == DMA1_IT_TC1) || \ + ((IT) == DMA1_IT_HT1) || ((IT) == DMA1_IT_TE1) || \ + ((IT) == DMA1_IT_GL2) || ((IT) == DMA1_IT_TC2) || \ + ((IT) == DMA1_IT_HT2) || ((IT) == DMA1_IT_TE2) || \ + ((IT) == DMA1_IT_GL3) || ((IT) == DMA1_IT_TC3) || \ + ((IT) == DMA1_IT_HT3) || ((IT) == DMA1_IT_TE3) || \ + ((IT) == DMA1_IT_GL4) || ((IT) == DMA1_IT_TC4) || \ + ((IT) == DMA1_IT_HT4) || ((IT) == DMA1_IT_TE4) || \ + ((IT) == DMA1_IT_GL5) || ((IT) == DMA1_IT_TC5) || \ + ((IT) == DMA1_IT_HT5) || ((IT) == DMA1_IT_TE5) || \ + ((IT) == DMA1_IT_GL6) || ((IT) == DMA1_IT_TC6) || \ + ((IT) == DMA1_IT_HT6) || ((IT) == DMA1_IT_TE6) || \ + ((IT) == DMA1_IT_GL7) || ((IT) == DMA1_IT_TC7) || \ + ((IT) == DMA1_IT_HT7) || ((IT) == DMA1_IT_TE7) || \ + ((IT) == DMA2_IT_GL1) || ((IT) == DMA2_IT_TC1) || \ + ((IT) == DMA2_IT_HT1) || ((IT) == DMA2_IT_TE1) || \ + ((IT) == DMA2_IT_GL2) || ((IT) == DMA2_IT_TC2) || \ + ((IT) == DMA2_IT_HT2) || ((IT) == DMA2_IT_TE2) || \ + ((IT) == DMA2_IT_GL3) || ((IT) == DMA2_IT_TC3) || \ + ((IT) == DMA2_IT_HT3) || ((IT) == DMA2_IT_TE3) || \ + ((IT) == DMA2_IT_GL4) || ((IT) == DMA2_IT_TC4) || \ + ((IT) == DMA2_IT_HT4) || ((IT) == DMA2_IT_TE4) || \ + ((IT) == DMA2_IT_GL5) || ((IT) == DMA2_IT_TC5) || \ + ((IT) == DMA2_IT_HT5) || ((IT) == DMA2_IT_TE5)) + +/** + * @} + */ + +/** @defgroup DMA_flags_definition + * @{ + */ + +#define DMA1_FLAG_GL1 ((uint32_t)0x00000001) +#define DMA1_FLAG_TC1 ((uint32_t)0x00000002) +#define DMA1_FLAG_HT1 ((uint32_t)0x00000004) +#define DMA1_FLAG_TE1 ((uint32_t)0x00000008) +#define DMA1_FLAG_GL2 ((uint32_t)0x00000010) +#define DMA1_FLAG_TC2 ((uint32_t)0x00000020) +#define DMA1_FLAG_HT2 ((uint32_t)0x00000040) +#define DMA1_FLAG_TE2 ((uint32_t)0x00000080) +#define DMA1_FLAG_GL3 ((uint32_t)0x00000100) +#define DMA1_FLAG_TC3 ((uint32_t)0x00000200) +#define DMA1_FLAG_HT3 ((uint32_t)0x00000400) +#define DMA1_FLAG_TE3 ((uint32_t)0x00000800) +#define DMA1_FLAG_GL4 ((uint32_t)0x00001000) +#define DMA1_FLAG_TC4 ((uint32_t)0x00002000) +#define DMA1_FLAG_HT4 ((uint32_t)0x00004000) +#define DMA1_FLAG_TE4 ((uint32_t)0x00008000) +#define DMA1_FLAG_GL5 ((uint32_t)0x00010000) +#define DMA1_FLAG_TC5 ((uint32_t)0x00020000) +#define DMA1_FLAG_HT5 ((uint32_t)0x00040000) +#define DMA1_FLAG_TE5 ((uint32_t)0x00080000) +#define DMA1_FLAG_GL6 ((uint32_t)0x00100000) +#define DMA1_FLAG_TC6 ((uint32_t)0x00200000) +#define DMA1_FLAG_HT6 ((uint32_t)0x00400000) +#define DMA1_FLAG_TE6 ((uint32_t)0x00800000) +#define DMA1_FLAG_GL7 ((uint32_t)0x01000000) +#define DMA1_FLAG_TC7 ((uint32_t)0x02000000) +#define DMA1_FLAG_HT7 ((uint32_t)0x04000000) +#define DMA1_FLAG_TE7 ((uint32_t)0x08000000) + +#define DMA2_FLAG_GL1 ((uint32_t)0x10000001) +#define DMA2_FLAG_TC1 ((uint32_t)0x10000002) +#define DMA2_FLAG_HT1 ((uint32_t)0x10000004) +#define DMA2_FLAG_TE1 ((uint32_t)0x10000008) +#define DMA2_FLAG_GL2 ((uint32_t)0x10000010) +#define DMA2_FLAG_TC2 ((uint32_t)0x10000020) +#define DMA2_FLAG_HT2 ((uint32_t)0x10000040) +#define DMA2_FLAG_TE2 ((uint32_t)0x10000080) +#define DMA2_FLAG_GL3 ((uint32_t)0x10000100) +#define DMA2_FLAG_TC3 ((uint32_t)0x10000200) +#define DMA2_FLAG_HT3 ((uint32_t)0x10000400) +#define DMA2_FLAG_TE3 ((uint32_t)0x10000800) +#define DMA2_FLAG_GL4 ((uint32_t)0x10001000) +#define DMA2_FLAG_TC4 ((uint32_t)0x10002000) +#define DMA2_FLAG_HT4 ((uint32_t)0x10004000) +#define DMA2_FLAG_TE4 ((uint32_t)0x10008000) +#define DMA2_FLAG_GL5 ((uint32_t)0x10010000) +#define DMA2_FLAG_TC5 ((uint32_t)0x10020000) +#define DMA2_FLAG_HT5 ((uint32_t)0x10040000) +#define DMA2_FLAG_TE5 ((uint32_t)0x10080000) + +#define IS_DMA_CLEAR_FLAG(FLAG) (((((FLAG) & 0xF0000000) == 0x00) || (((FLAG) & 0xEFF00000) == 0x00)) && ((FLAG) != 0x00)) + +#define IS_DMA_GET_FLAG(FLAG) (((FLAG) == DMA1_FLAG_GL1) || ((FLAG) == DMA1_FLAG_TC1) || \ + ((FLAG) == DMA1_FLAG_HT1) || ((FLAG) == DMA1_FLAG_TE1) || \ + ((FLAG) == DMA1_FLAG_GL2) || ((FLAG) == DMA1_FLAG_TC2) || \ + ((FLAG) == DMA1_FLAG_HT2) || ((FLAG) == DMA1_FLAG_TE2) || \ + ((FLAG) == DMA1_FLAG_GL3) || ((FLAG) == DMA1_FLAG_TC3) || \ + ((FLAG) == DMA1_FLAG_HT3) || ((FLAG) == DMA1_FLAG_TE3) || \ + ((FLAG) == DMA1_FLAG_GL4) || ((FLAG) == DMA1_FLAG_TC4) || \ + ((FLAG) == DMA1_FLAG_HT4) || ((FLAG) == DMA1_FLAG_TE4) || \ + ((FLAG) == DMA1_FLAG_GL5) || ((FLAG) == DMA1_FLAG_TC5) || \ + ((FLAG) == DMA1_FLAG_HT5) || ((FLAG) == DMA1_FLAG_TE5) || \ + ((FLAG) == DMA1_FLAG_GL6) || ((FLAG) == DMA1_FLAG_TC6) || \ + ((FLAG) == DMA1_FLAG_HT6) || ((FLAG) == DMA1_FLAG_TE6) || \ + ((FLAG) == DMA1_FLAG_GL7) || ((FLAG) == DMA1_FLAG_TC7) || \ + ((FLAG) == DMA1_FLAG_HT7) || ((FLAG) == DMA1_FLAG_TE7) || \ + ((FLAG) == DMA2_FLAG_GL1) || ((FLAG) == DMA2_FLAG_TC1) || \ + ((FLAG) == DMA2_FLAG_HT1) || ((FLAG) == DMA2_FLAG_TE1) || \ + ((FLAG) == DMA2_FLAG_GL2) || ((FLAG) == DMA2_FLAG_TC2) || \ + ((FLAG) == DMA2_FLAG_HT2) || ((FLAG) == DMA2_FLAG_TE2) || \ + ((FLAG) == DMA2_FLAG_GL3) || ((FLAG) == DMA2_FLAG_TC3) || \ + ((FLAG) == DMA2_FLAG_HT3) || ((FLAG) == DMA2_FLAG_TE3) || \ + ((FLAG) == DMA2_FLAG_GL4) || ((FLAG) == DMA2_FLAG_TC4) || \ + ((FLAG) == DMA2_FLAG_HT4) || ((FLAG) == DMA2_FLAG_TE4) || \ + ((FLAG) == DMA2_FLAG_GL5) || ((FLAG) == DMA2_FLAG_TC5) || \ + ((FLAG) == DMA2_FLAG_HT5) || ((FLAG) == DMA2_FLAG_TE5)) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the DMA configuration to the default reset state ******/ +void DMA_DeInit(DMA_Channel_TypeDef* DMAy_Channelx); + +/* Initialization and Configuration functions *********************************/ +void DMA_Init(DMA_Channel_TypeDef* DMAy_Channelx, DMA_InitTypeDef* DMA_InitStruct); +void DMA_StructInit(DMA_InitTypeDef* DMA_InitStruct); +void DMA_Cmd(DMA_Channel_TypeDef* DMAy_Channelx, FunctionalState NewState); + +/* Data Counter functions******************************************************/ +void DMA_SetCurrDataCounter(DMA_Channel_TypeDef* DMAy_Channelx, uint16_t DataNumber); +uint16_t DMA_GetCurrDataCounter(DMA_Channel_TypeDef* DMAy_Channelx); + +/* Interrupts and flags management functions **********************************/ +void DMA_ITConfig(DMA_Channel_TypeDef* DMAy_Channelx, uint32_t DMA_IT, FunctionalState NewState); +FlagStatus DMA_GetFlagStatus(uint32_t DMAy_FLAG); +void DMA_ClearFlag(uint32_t DMAy_FLAG); +ITStatus DMA_GetITStatus(uint32_t DMAy_IT); +void DMA_ClearITPendingBit(uint32_t DMAy_IT); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_DMA_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_exti.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_exti.h new file mode 100755 index 0000000000..120141044c --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_exti.h @@ -0,0 +1,234 @@ +/** + ****************************************************************************** + * @file stm32f30x_exti.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the EXTI + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_EXTI_H +#define __STM32F30x_EXTI_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup EXTI + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief EXTI mode enumeration + */ + +typedef enum +{ + EXTI_Mode_Interrupt = 0x00, + EXTI_Mode_Event = 0x04 +}EXTIMode_TypeDef; + +#define IS_EXTI_MODE(MODE) (((MODE) == EXTI_Mode_Interrupt) || ((MODE) == EXTI_Mode_Event)) + +/** + * @brief EXTI Trigger enumeration + */ + +typedef enum +{ + EXTI_Trigger_Rising = 0x08, + EXTI_Trigger_Falling = 0x0C, + EXTI_Trigger_Rising_Falling = 0x10 +}EXTITrigger_TypeDef; + +#define IS_EXTI_TRIGGER(TRIGGER) (((TRIGGER) == EXTI_Trigger_Rising) || \ + ((TRIGGER) == EXTI_Trigger_Falling) || \ + ((TRIGGER) == EXTI_Trigger_Rising_Falling)) +/** + * @brief EXTI Init Structure definition + */ + +typedef struct +{ + uint32_t EXTI_Line; /*!< Specifies the EXTI lines to be enabled or disabled. + This parameter can be any combination of @ref EXTI_Lines */ + + EXTIMode_TypeDef EXTI_Mode; /*!< Specifies the mode for the EXTI lines. + This parameter can be a value of @ref EXTIMode_TypeDef */ + + EXTITrigger_TypeDef EXTI_Trigger; /*!< Specifies the trigger signal active edge for the EXTI lines. + This parameter can be a value of @ref EXTITrigger_TypeDef */ + + FunctionalState EXTI_LineCmd; /*!< Specifies the new state of the selected EXTI lines. + This parameter can be set either to ENABLE or DISABLE */ +}EXTI_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup EXTI_Exported_Constants + * @{ + */ +/** @defgroup EXTI_Lines + * @{ + */ + +#define EXTI_Line0 ((uint32_t)0x00) /*!< External interrupt line 0 */ +#define EXTI_Line1 ((uint32_t)0x01) /*!< External interrupt line 1 */ +#define EXTI_Line2 ((uint32_t)0x02) /*!< External interrupt line 2 */ +#define EXTI_Line3 ((uint32_t)0x03) /*!< External interrupt line 3 */ +#define EXTI_Line4 ((uint32_t)0x04) /*!< External interrupt line 4 */ +#define EXTI_Line5 ((uint32_t)0x05) /*!< External interrupt line 5 */ +#define EXTI_Line6 ((uint32_t)0x06) /*!< External interrupt line 6 */ +#define EXTI_Line7 ((uint32_t)0x07) /*!< External interrupt line 7 */ +#define EXTI_Line8 ((uint32_t)0x08) /*!< External interrupt line 8 */ +#define EXTI_Line9 ((uint32_t)0x09) /*!< External interrupt line 9 */ +#define EXTI_Line10 ((uint32_t)0x0A) /*!< External interrupt line 10 */ +#define EXTI_Line11 ((uint32_t)0x0B) /*!< External interrupt line 11 */ +#define EXTI_Line12 ((uint32_t)0x0C) /*!< External interrupt line 12 */ +#define EXTI_Line13 ((uint32_t)0x0D) /*!< External interrupt line 13 */ +#define EXTI_Line14 ((uint32_t)0x0E) /*!< External interrupt line 14 */ +#define EXTI_Line15 ((uint32_t)0x0F) /*!< External interrupt line 15 */ +#define EXTI_Line16 ((uint32_t)0x10) /*!< External interrupt line 16 + Connected to the PVD Output */ +#define EXTI_Line17 ((uint32_t)0x11) /*!< Internal interrupt line 17 + Connected to the RTC Alarm + event */ +#define EXTI_Line18 ((uint32_t)0x12) /*!< Internal interrupt line 18 + Connected to the USB Device + Wakeup from suspend event */ +#define EXTI_Line19 ((uint32_t)0x13) /*!< Internal interrupt line 19 + Connected to the RTC Tamper + and Time Stamp events */ +#define EXTI_Line20 ((uint32_t)0x14) /*!< Internal interrupt line 20 + Connected to the RTC wakeup + event */ +#define EXTI_Line21 ((uint32_t)0x15) /*!< Internal interrupt line 21 + Connected to the Comparator 1 + event */ +#define EXTI_Line22 ((uint32_t)0x16) /*!< Internal interrupt line 22 + Connected to the Comparator 2 + event */ +#define EXTI_Line23 ((uint32_t)0x17) /*!< Internal interrupt line 23 + Connected to the I2C1 wakeup + event */ +#define EXTI_Line24 ((uint32_t)0x18) /*!< Internal interrupt line 24 + Connected to the I2C2 wakeup + event */ +#define EXTI_Line25 ((uint32_t)0x19) /*!< Internal interrupt line 25 + Connected to the USART1 wakeup + event */ +#define EXTI_Line26 ((uint32_t)0x1A) /*!< Internal interrupt line 26 + Connected to the USART2 wakeup + event */ +#define EXTI_Line27 ((uint32_t)0x1B) /*!< Internal interrupt line 27 + reserved */ +#define EXTI_Line28 ((uint32_t)0x1C) /*!< Internal interrupt line 28 + Connected to the USART3 wakeup + event */ +#define EXTI_Line29 ((uint32_t)0x1D) /*!< Internal interrupt line 29 + Connected to the Comparator 3 + event */ +#define EXTI_Line30 ((uint32_t)0x1E) /*!< Internal interrupt line 30 + Connected to the Comparator 4 + event */ +#define EXTI_Line31 ((uint32_t)0x1F) /*!< Internal interrupt line 31 + Connected to the Comparator 5 + event */ +#define EXTI_Line32 ((uint32_t)0x20) /*!< Internal interrupt line 32 + Connected to the Comparator 6 + event */ +#define EXTI_Line33 ((uint32_t)0x21) /*!< Internal interrupt line 33 + Connected to the Comparator 7 + event */ +#define EXTI_Line34 ((uint32_t)0x22) /*!< Internal interrupt line 34 + Connected to the USART4 wakeup + event */ +#define EXTI_Line35 ((uint32_t)0x23) /*!< Internal interrupt line 35 + Connected to the USART5 wakeup + event */ + +#define IS_EXTI_LINE_ALL(LINE) ((LINE) <= 0x23) +#define IS_EXTI_LINE_EXT(LINE) (((LINE) <= 0x16) || (((LINE) == EXTI_Line29) || ((LINE) == EXTI_Line30) || \ + ((LINE) == EXTI_Line31) || ((LINE) == EXTI_Line32) || ((LINE) == EXTI_Line33))) + +#define IS_GET_EXTI_LINE(LINE) (((LINE) == EXTI_Line0) || ((LINE) == EXTI_Line1) || \ + ((LINE) == EXTI_Line2) || ((LINE) == EXTI_Line3) || \ + ((LINE) == EXTI_Line4) || ((LINE) == EXTI_Line5) || \ + ((LINE) == EXTI_Line6) || ((LINE) == EXTI_Line7) || \ + ((LINE) == EXTI_Line8) || ((LINE) == EXTI_Line9) || \ + ((LINE) == EXTI_Line10) || ((LINE) == EXTI_Line11) || \ + ((LINE) == EXTI_Line12) || ((LINE) == EXTI_Line13) || \ + ((LINE) == EXTI_Line14) || ((LINE) == EXTI_Line15) || \ + ((LINE) == EXTI_Line16) || ((LINE) == EXTI_Line17) || \ + ((LINE) == EXTI_Line18) || ((LINE) == EXTI_Line19) || \ + ((LINE) == EXTI_Line20) || ((LINE) == EXTI_Line21) || \ + ((LINE) == EXTI_Line22) || ((LINE) == EXTI_Line29) || \ + ((LINE) == EXTI_Line30) || ((LINE) == EXTI_Line31) || \ + ((LINE) == EXTI_Line32) || ((LINE) == EXTI_Line33)) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* Function used to set the EXTI configuration to the default reset state *****/ +void EXTI_DeInit(void); + +/* Initialization and Configuration functions *********************************/ +void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct); +void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct); +void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line); + +/* Interrupts and flags management functions **********************************/ +FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line); +void EXTI_ClearFlag(uint32_t EXTI_Line); +ITStatus EXTI_GetITStatus(uint32_t EXTI_Line); +void EXTI_ClearITPendingBit(uint32_t EXTI_Line); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_EXTI_H */ +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_flash.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_flash.h new file mode 100755 index 0000000000..4d0d1d4e85 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_flash.h @@ -0,0 +1,334 @@ +/** + ****************************************************************************** + * @file stm32f30x_flash.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the FLASH + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_FLASH_H +#define __STM32F30x_FLASH_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup FLASH + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/** + * @brief FLASH Status + */ +typedef enum +{ + FLASH_BUSY = 1, + FLASH_ERROR_WRP, + FLASH_ERROR_PROGRAM, + FLASH_COMPLETE, + FLASH_TIMEOUT +}FLASH_Status; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup FLASH_Exported_Constants + * @{ + */ + +/** @defgroup Flash_Latency + * @{ + */ +#define FLASH_Latency_0 ((uint8_t)0x0000) /*!< FLASH Zero Latency cycle */ +#define FLASH_Latency_1 FLASH_ACR_LATENCY_0 /*!< FLASH One Latency cycle */ +#define FLASH_Latency_2 FLASH_ACR_LATENCY_1 /*!< FLASH Two Latency cycles */ + +#define IS_FLASH_LATENCY(LATENCY) (((LATENCY) == FLASH_Latency_0) || \ + ((LATENCY) == FLASH_Latency_1) || \ + ((LATENCY) == FLASH_Latency_2)) +/** + * @} + */ + +/** @defgroup FLASH_Interrupts + * @{ + */ + +#define FLASH_IT_EOP FLASH_CR_EOPIE /*!< End of programming interrupt source */ +#define FLASH_IT_ERR FLASH_CR_ERRIE /*!< Error interrupt source */ +#define IS_FLASH_IT(IT) ((((IT) & (uint32_t)0xFFFFEBFF) == 0x00000000) && (((IT) != 0x00000000))) +/** + * @} + */ +/** @defgroup FLASH_Address + * @{ + */ + +#define IS_FLASH_PROGRAM_ADDRESS(ADDRESS) (((ADDRESS) >= 0x08000000) && ((ADDRESS) <= 0x0803FFFF)) + +/** + * @} + */ + +/** @defgroup FLASH_OB_DATA_ADDRESS + * @{ + */ +#define IS_OB_DATA_ADDRESS(ADDRESS) (((ADDRESS) == 0x1FFFF804) || ((ADDRESS) == 0x1FFFF806)) + +/** + * @} + */ + +/** @defgroup Option_Bytes_Write_Protection + * @{ + */ + +#define OB_WRP_Pages0to1 ((uint32_t)0x00000001) /* Write protection of page 0 to 1 */ +#define OB_WRP_Pages2to3 ((uint32_t)0x00000002) /* Write protection of page 2 to 3 */ +#define OB_WRP_Pages4to5 ((uint32_t)0x00000004) /* Write protection of page 4 to 5 */ +#define OB_WRP_Pages6to7 ((uint32_t)0x00000008) /* Write protection of page 6 to 7 */ +#define OB_WRP_Pages8to9 ((uint32_t)0x00000010) /* Write protection of page 8 to 9 */ +#define OB_WRP_Pages10to11 ((uint32_t)0x00000020) /* Write protection of page 10 to 11 */ +#define OB_WRP_Pages12to13 ((uint32_t)0x00000040) /* Write protection of page 12 to 13 */ +#define OB_WRP_Pages14to15 ((uint32_t)0x00000080) /* Write protection of page 14 to 15 */ +#define OB_WRP_Pages16to17 ((uint32_t)0x00000100) /* Write protection of page 16 to 17 */ +#define OB_WRP_Pages18to19 ((uint32_t)0x00000200) /* Write protection of page 18 to 19 */ +#define OB_WRP_Pages20to21 ((uint32_t)0x00000400) /* Write protection of page 20 to 21 */ +#define OB_WRP_Pages22to23 ((uint32_t)0x00000800) /* Write protection of page 22 to 23 */ +#define OB_WRP_Pages24to25 ((uint32_t)0x00001000) /* Write protection of page 24 to 25 */ +#define OB_WRP_Pages26to27 ((uint32_t)0x00002000) /* Write protection of page 26 to 27 */ +#define OB_WRP_Pages28to29 ((uint32_t)0x00004000) /* Write protection of page 28 to 29 */ +#define OB_WRP_Pages30to31 ((uint32_t)0x00008000) /* Write protection of page 30 to 31 */ +#define OB_WRP_Pages32to33 ((uint32_t)0x00010000) /* Write protection of page 32 to 33 */ +#define OB_WRP_Pages34to35 ((uint32_t)0x00020000) /* Write protection of page 34 to 35 */ +#define OB_WRP_Pages36to37 ((uint32_t)0x00040000) /* Write protection of page 36 to 37 */ +#define OB_WRP_Pages38to39 ((uint32_t)0x00080000) /* Write protection of page 38 to 39 */ +#define OB_WRP_Pages40to41 ((uint32_t)0x00100000) /* Write protection of page 40 to 41 */ +#define OB_WRP_Pages42to43 ((uint32_t)0x00200000) /* Write protection of page 42 to 43 */ +#define OB_WRP_Pages44to45 ((uint32_t)0x00400000) /* Write protection of page 44 to 45 */ +#define OB_WRP_Pages46to47 ((uint32_t)0x00800000) /* Write protection of page 46 to 47 */ +#define OB_WRP_Pages48to49 ((uint32_t)0x01000000) /* Write protection of page 48 to 49 */ +#define OB_WRP_Pages50to51 ((uint32_t)0x02000000) /* Write protection of page 50 to 51 */ +#define OB_WRP_Pages52to53 ((uint32_t)0x04000000) /* Write protection of page 52 to 53 */ +#define OB_WRP_Pages54to55 ((uint32_t)0x08000000) /* Write protection of page 54 to 55 */ +#define OB_WRP_Pages56to57 ((uint32_t)0x10000000) /* Write protection of page 56 to 57 */ +#define OB_WRP_Pages58to59 ((uint32_t)0x20000000) /* Write protection of page 58 to 59 */ +#define OB_WRP_Pages60to61 ((uint32_t)0x40000000) /* Write protection of page 60 to 61 */ + +#ifdef STM32F303xE +#define OB_WRP_Pages62to263 ((uint32_t)0x80000000) /* Write protection of page 62 to 263 */ +#else +#define OB_WRP_Pages62to127 ((uint32_t)0x80000000) /* Write protection of page 62 to 127 */ +#endif /* STM32F303xE */ + +#define OB_WRP_AllPages ((uint32_t)0xFFFFFFFF) /*!< Write protection of all Sectors */ + +#define IS_OB_WRP(PAGE) (((PAGE) != 0x0000000)) + +/** + * @} + */ + +/** @defgroup Option_Bytes_Read_Protection + * @{ + */ + +/** + * @brief Read Protection Level + */ +#define OB_RDP_Level_0 ((uint8_t)0xAA) +#define OB_RDP_Level_1 ((uint8_t)0xBB) +/*#define OB_RDP_Level_2 ((uint8_t)0xCC)*/ /* Warning: When enabling read protection level 2 + it's no more possible to go back to level 1 or 0 */ + +#define IS_OB_RDP(LEVEL) (((LEVEL) == OB_RDP_Level_0)||\ + ((LEVEL) == OB_RDP_Level_1))/*||\ + ((LEVEL) == OB_RDP_Level_2))*/ +/** + * @} + */ + +/** @defgroup Option_Bytes_IWatchdog + * @{ + */ + +#define OB_IWDG_SW ((uint8_t)0x01) /*!< Software IWDG selected */ +#define OB_IWDG_HW ((uint8_t)0x00) /*!< Hardware IWDG selected */ +#define IS_OB_IWDG_SOURCE(SOURCE) (((SOURCE) == OB_IWDG_SW) || ((SOURCE) == OB_IWDG_HW)) + +/** + * @} + */ + +/** @defgroup Option_Bytes_nRST_STOP + * @{ + */ + +#define OB_STOP_NoRST ((uint8_t)0x02) /*!< No reset generated when entering in STOP */ +#define OB_STOP_RST ((uint8_t)0x00) /*!< Reset generated when entering in STOP */ +#define IS_OB_STOP_SOURCE(SOURCE) (((SOURCE) == OB_STOP_NoRST) || ((SOURCE) == OB_STOP_RST)) + +/** + * @} + */ + +/** @defgroup Option_Bytes_nRST_STDBY + * @{ + */ + +#define OB_STDBY_NoRST ((uint8_t)0x04) /*!< No reset generated when entering in STANDBY */ +#define OB_STDBY_RST ((uint8_t)0x00) /*!< Reset generated when entering in STANDBY */ +#define IS_OB_STDBY_SOURCE(SOURCE) (((SOURCE) == OB_STDBY_NoRST) || ((SOURCE) == OB_STDBY_RST)) + +/** + * @} + */ +/** @defgroup Option_Bytes_BOOT1 + * @{ + */ + +#define OB_BOOT1_RESET ((uint8_t)0x00) /*!< BOOT1 Reset */ +#define OB_BOOT1_SET ((uint8_t)0x10) /*!< BOOT1 Set */ +#define IS_OB_BOOT1(BOOT1) (((BOOT1) == OB_BOOT1_RESET) || ((BOOT1) == OB_BOOT1_SET)) + +/** + * @} + */ +/** @defgroup Option_Bytes_VDDA_Analog_Monitoring + * @{ + */ + +#define OB_VDDA_ANALOG_ON ((uint8_t)0x20) /*!< Analog monitoring on VDDA Power source ON */ +#define OB_VDDA_ANALOG_OFF ((uint8_t)0x00) /*!< Analog monitoring on VDDA Power source OFF */ + +#define IS_OB_VDDA_ANALOG(ANALOG) (((ANALOG) == OB_VDDA_ANALOG_ON) || ((ANALOG) == OB_VDDA_ANALOG_OFF)) + +/** + * @} + */ + +/** @defgroup FLASH_Option_Bytes_SRAM_Parity_Enable + * @{ + */ + +#define OB_SRAM_PARITY_SET ((uint8_t)0x00) /*!< SRAM parity enable Set */ +#define OB_SRAM_PARITY_RESET ((uint8_t)0x40) /*!< SRAM parity enable reset */ + +#define IS_OB_SRAM_PARITY(PARITY) (((PARITY) == OB_SRAM_PARITY_SET) || ((PARITY) == OB_SRAM_PARITY_RESET)) + +/** + * @} + */ + +/** @defgroup FLASH_Flags + * @{ + */ + +#define FLASH_FLAG_BSY FLASH_SR_BSY /*!< FLASH Busy flag */ +#define FLASH_FLAG_PGERR FLASH_SR_PGERR /*!< FLASH Programming error flag */ +#define FLASH_FLAG_WRPERR FLASH_SR_WRPERR /*!< FLASH Write protected error flag */ +#define FLASH_FLAG_EOP FLASH_SR_EOP /*!< FLASH End of Programming flag */ + +#define IS_FLASH_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFFFFCB) == 0x00000000) && ((FLAG) != 0x00000000)) + +#define IS_FLASH_GET_FLAG(FLAG) (((FLAG) == FLASH_FLAG_BSY) || ((FLAG) == FLASH_FLAG_PGERR) || \ + ((FLAG) == FLASH_FLAG_WRPERR) || ((FLAG) == FLASH_FLAG_EOP)) +/** + * @} + */ +/** @defgroup Timeout_definition + * @{ + */ +#define FLASH_ER_PRG_TIMEOUT ((uint32_t)0x000B0000) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +/* FLASH Interface configuration functions ************************************/ +void FLASH_SetLatency(uint32_t FLASH_Latency); +void FLASH_HalfCycleAccessCmd(FunctionalState NewState); +void FLASH_PrefetchBufferCmd(FunctionalState NewState); + +/* FLASH Memory Programming functions *****************************************/ +void FLASH_Unlock(void); +void FLASH_Lock(void); +FLASH_Status FLASH_ErasePage(uint32_t Page_Address); +FLASH_Status FLASH_EraseAllPages(void); +FLASH_Status FLASH_ProgramWord(uint32_t Address, uint32_t Data); +FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data); + +/* Option Bytes Programming functions *****************************************/ +void FLASH_OB_Unlock(void); +void FLASH_OB_Lock(void); +void FLASH_OB_Launch(void); +FLASH_Status FLASH_OB_Erase(void); +FLASH_Status FLASH_OB_EnableWRP(uint32_t OB_WRP); +FLASH_Status FLASH_OB_RDPConfig(uint8_t OB_RDP); +FLASH_Status FLASH_OB_UserConfig(uint8_t OB_IWDG, uint8_t OB_STOP, uint8_t OB_STDBY); +FLASH_Status FLASH_OB_BOOTConfig(uint8_t OB_BOOT1); +FLASH_Status FLASH_OB_VDDAConfig(uint8_t OB_VDDA_ANALOG); +FLASH_Status FLASH_OB_SRAMParityConfig(uint8_t OB_SRAM_Parity); +FLASH_Status FLASH_OB_WriteUser(uint8_t OB_USER); +FLASH_Status FLASH_ProgramOptionByteData(uint32_t Address, uint8_t Data); +uint8_t FLASH_OB_GetUser(void); +uint32_t FLASH_OB_GetWRP(void); +FlagStatus FLASH_OB_GetRDP(void); + +/* Interrupts and flags management functions **********************************/ +void FLASH_ITConfig(uint32_t FLASH_IT, FunctionalState NewState); +FlagStatus FLASH_GetFlagStatus(uint32_t FLASH_FLAG); +void FLASH_ClearFlag(uint32_t FLASH_FLAG); +FLASH_Status FLASH_GetStatus(void); +FLASH_Status FLASH_WaitForLastOperation(uint32_t Timeout); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_FLASH_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_fmc.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_fmc.h new file mode 100755 index 0000000000..eb8c434e8f --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_fmc.h @@ -0,0 +1,722 @@ +/** + ****************************************************************************** + * @file stm32f30x_fmc.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the FMC firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_FMC_H +#define __STM32F30x_FMC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup FMC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief Timing parameters For NOR/SRAM Banks + */ +typedef struct +{ + uint32_t FMC_AddressSetupTime; /*!< Defines the number of HCLK cycles to configure + the duration of the address setup time. + This parameter can be a value between 0 and 15. + @note This parameter is not used with synchronous NOR Flash memories. */ + + uint32_t FMC_AddressHoldTime; /*!< Defines the number of HCLK cycles to configure + the duration of the address hold time. + This parameter can be a value between 1 and 15. + @note This parameter is not used with synchronous NOR Flash memories.*/ + + uint32_t FMC_DataSetupTime; /*!< Defines the number of HCLK cycles to configure + the duration of the data setup time. + This parameter can be a value between 1 and 255. + @note This parameter is used for SRAMs, ROMs and asynchronous multiplexed NOR Flash memories. */ + + uint32_t FMC_BusTurnAroundDuration; /*!< Defines the number of HCLK cycles to configure + the duration of the bus turnaround. + This parameter can be a value between 0 and 15. + @note This parameter is only used for multiplexed NOR Flash memories. */ + + uint32_t FMC_CLKDivision; /*!< Defines the period of CLK clock output signal, expressed in number of HCLK cycles. + This parameter can be a value between 2 and 16. + @note This parameter is not used for asynchronous NOR Flash, SRAM or ROM accesses. */ + + uint32_t FMC_DataLatency; /*!< Defines the number of memory clock cycles to issue + to the memory before getting the first data. + The parameter value depends on the memory type as shown below: + - It must be set to 0 in case of a CRAM + - It is don't care in asynchronous NOR, SRAM or ROM accesses + - It may assume a value between 2 and 17 in NOR Flash memories + with synchronous burst mode enable */ + + uint32_t FMC_AccessMode; /*!< Specifies the asynchronous access mode. + This parameter can be a value of @ref FMC_Access_Mode */ +}FMC_NORSRAMTimingInitTypeDef; + +/** + * @brief FMC NOR/SRAM Init structure definition + */ +typedef struct +{ + uint32_t FMC_Bank; /*!< Specifies the NOR/SRAM memory bank that will be used. + This parameter can be a value of @ref FMC_NORSRAM_Bank */ + + uint32_t FMC_DataAddressMux; /*!< Specifies whether the address and data values are + multiplexed on the databus or not. + This parameter can be a value of @ref FMC_Data_Address_Bus_Multiplexing */ + + uint32_t FMC_MemoryType; /*!< Specifies the type of external memory attached to + the corresponding memory bank. + This parameter can be a value of @ref FMC_Memory_Type */ + + uint32_t FMC_MemoryDataWidth; /*!< Specifies the external memory device width. + This parameter can be a value of @ref FMC_NORSRAM_Data_Width */ + + uint32_t FMC_BurstAccessMode; /*!< Enables or disables the burst access mode for Flash memory, + valid only with synchronous burst Flash memories. + This parameter can be a value of @ref FMC_Burst_Access_Mode */ + + uint32_t FMC_WaitSignalPolarity; /*!< Specifies the wait signal polarity, valid only when accessing + the Flash memory in burst mode. + This parameter can be a value of @ref FMC_Wait_Signal_Polarity */ + + uint32_t FMC_WrapMode; /*!< Enables or disables the Wrapped burst access mode for Flash + memory, valid only when accessing Flash memories in burst mode. + This parameter can be a value of @ref FMC_Wrap_Mode */ + + uint32_t FMC_WaitSignalActive; /*!< Specifies if the wait signal is asserted by the memory one + clock cycle before the wait state or during the wait state, + valid only when accessing memories in burst mode. + This parameter can be a value of @ref FMC_Wait_Timing */ + + uint32_t FMC_WriteOperation; /*!< Enables or disables the write operation in the selected bank by the FMC. + This parameter can be a value of @ref FMC_Write_Operation */ + + uint32_t FMC_WaitSignal; /*!< Enables or disables the wait state insertion via wait + signal, valid for Flash memory access in burst mode. + This parameter can be a value of @ref FMC_Wait_Signal */ + + uint32_t FMC_ExtendedMode; /*!< Enables or disables the extended mode. + This parameter can be a value of @ref FMC_Extended_Mode */ + + uint32_t FMC_AsynchronousWait; /*!< Enables or disables wait signal during asynchronous transfers, + valid only with asynchronous Flash memories. + This parameter can be a value of @ref FMC_AsynchronousWait */ + + uint32_t FMC_WriteBurst; /*!< Enables or disables the write burst operation. + This parameter can be a value of @ref FMC_Write_Burst */ + + + FMC_NORSRAMTimingInitTypeDef* FMC_ReadWriteTimingStruct; /*!< Timing Parameters for write and read access if the Extended Mode is not used*/ + + FMC_NORSRAMTimingInitTypeDef* FMC_WriteTimingStruct; /*!< Timing Parameters for write access if the Extended Mode is used*/ +}FMC_NORSRAMInitTypeDef; + +/** + * @brief Timing parameters For FMC NAND and PCCARD Banks + */ +typedef struct +{ + uint32_t FMC_SetupTime; /*!< Defines the number of HCLK cycles to setup address before + the command assertion for NAND-Flash read or write access + to common/Attribute or I/O memory space (depending on + the memory space timing to be configured). + This parameter can be a value between 0 and 255.*/ + + uint32_t FMC_WaitSetupTime; /*!< Defines the minimum number of HCLK cycles to assert the + command for NAND-Flash read or write access to + common/Attribute or I/O memory space (depending on the + memory space timing to be configured). + This parameter can be a number between 0 and 255 */ + + uint32_t FMC_HoldSetupTime; /*!< Defines the number of HCLK clock cycles to hold address + (and data for write access) after the command de-assertion + for NAND-Flash read or write access to common/Attribute + or I/O memory space (depending on the memory space timing + to be configured). + This parameter can be a number between 0 and 255 */ + + uint32_t FMC_HiZSetupTime; /*!< Defines the number of HCLK clock cycles during which the + databus is kept in HiZ after the start of a NAND-Flash + write access to common/Attribute or I/O memory space (depending + on the memory space timing to be configured). + This parameter can be a number between 0 and 255 */ +}FMC_NAND_PCCARDTimingInitTypeDef; + +/** + * @brief FMC NAND Init structure definition + */ +typedef struct +{ + uint32_t FMC_Bank; /*!< Specifies the NAND memory bank that will be used. + This parameter can be a value of @ref FMC_NAND_Bank */ + + uint32_t FMC_Waitfeature; /*!< Enables or disables the Wait feature for the NAND Memory Bank. + This parameter can be any value of @ref FMC_Wait_feature */ + + uint32_t FMC_MemoryDataWidth; /*!< Specifies the external memory device width. + This parameter can be any value of @ref FMC_NAND_Data_Width */ + + uint32_t FMC_ECC; /*!< Enables or disables the ECC computation. + This parameter can be any value of @ref FMC_ECC */ + + uint32_t FMC_ECCPageSize; /*!< Defines the page size for the extended ECC. + This parameter can be any value of @ref FMC_ECC_Page_Size */ + + uint32_t FMC_TCLRSetupTime; /*!< Defines the number of HCLK cycles to configure the + delay between CLE low and RE low. + This parameter can be a value between 0 and 255. */ + + uint32_t FMC_TARSetupTime; /*!< Defines the number of HCLK cycles to configure the + delay between ALE low and RE low. + This parameter can be a number between 0 and 255 */ + + FMC_NAND_PCCARDTimingInitTypeDef* FMC_CommonSpaceTimingStruct; /*!< FMC Common Space Timing */ + + FMC_NAND_PCCARDTimingInitTypeDef* FMC_AttributeSpaceTimingStruct; /*!< FMC Attribute Space Timing */ +}FMC_NANDInitTypeDef; + +/** + * @brief FMC PCCARD Init structure definition + */ + +typedef struct +{ + uint32_t FMC_Waitfeature; /*!< Enables or disables the Wait feature for the Memory Bank. + This parameter can be any value of @ref FMC_Wait_feature */ + + uint32_t FMC_TCLRSetupTime; /*!< Defines the number of HCLK cycles to configure the + delay between CLE low and RE low. + This parameter can be a value between 0 and 255. */ + + uint32_t FMC_TARSetupTime; /*!< Defines the number of HCLK cycles to configure the + delay between ALE low and RE low. + This parameter can be a number between 0 and 255 */ + + + FMC_NAND_PCCARDTimingInitTypeDef* FMC_CommonSpaceTimingStruct; /*!< FMC Common Space Timing */ + + FMC_NAND_PCCARDTimingInitTypeDef* FMC_AttributeSpaceTimingStruct; /*!< FMC Attribute Space Timing */ + + FMC_NAND_PCCARDTimingInitTypeDef* FMC_IOSpaceTimingStruct; /*!< FMC IO Space Timing */ +}FMC_PCCARDInitTypeDef; + + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup FMC_Exported_Constants + * @{ + */ + +/** @defgroup FMC_NORSRAM_Bank + * @{ + */ +#define FMC_Bank1_NORSRAM1 ((uint32_t)0x00000000) +#define FMC_Bank1_NORSRAM2 ((uint32_t)0x00000002) +#define FMC_Bank1_NORSRAM3 ((uint32_t)0x00000004) +#define FMC_Bank1_NORSRAM4 ((uint32_t)0x00000006) + +#define IS_FMC_NORSRAM_BANK(BANK) (((BANK) == FMC_Bank1_NORSRAM1) || \ + ((BANK) == FMC_Bank1_NORSRAM2) || \ + ((BANK) == FMC_Bank1_NORSRAM3) || \ + ((BANK) == FMC_Bank1_NORSRAM4)) +/** + * @} + */ + +/** @defgroup FMC_NAND_Bank + * @{ + */ +#define FMC_Bank2_NAND ((uint32_t)0x00000010) +#define FMC_Bank3_NAND ((uint32_t)0x00000100) + +#define IS_FMC_NAND_BANK(BANK) (((BANK) == FMC_Bank2_NAND) || \ + ((BANK) == FMC_Bank3_NAND)) +/** + * @} + */ + +/** @defgroup FMC_PCCARD_Bank + * @{ + */ +#define FMC_Bank4_PCCARD ((uint32_t)0x00001000) +/** + * @} + */ + + +/** @defgroup FMC_NOR_SRAM_Controller + * @{ + */ + +/** @defgroup FMC_Data_Address_Bus_Multiplexing + * @{ + */ + +#define FMC_DataAddressMux_Disable ((uint32_t)0x00000000) +#define FMC_DataAddressMux_Enable ((uint32_t)0x00000002) + +#define IS_FMC_MUX(MUX) (((MUX) == FMC_DataAddressMux_Disable) || \ + ((MUX) == FMC_DataAddressMux_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Memory_Type + * @{ + */ + +#define FMC_MemoryType_SRAM ((uint32_t)0x00000000) +#define FMC_MemoryType_PSRAM ((uint32_t)0x00000004) +#define FMC_MemoryType_NOR ((uint32_t)0x00000008) + +#define IS_FMC_MEMORY(MEMORY) (((MEMORY) == FMC_MemoryType_SRAM) || \ + ((MEMORY) == FMC_MemoryType_PSRAM)|| \ + ((MEMORY) == FMC_MemoryType_NOR)) +/** + * @} + */ + +/** @defgroup FMC_NORSRAM_Data_Width + * @{ + */ + +#define FMC_NORSRAM_MemoryDataWidth_8b ((uint32_t)0x00000000) +#define FMC_NORSRAM_MemoryDataWidth_16b ((uint32_t)0x00000010) + +#define IS_FMC_NORSRAM_MEMORY_WIDTH(WIDTH) (((WIDTH) == FMC_NORSRAM_MemoryDataWidth_8b) || \ + ((WIDTH) == FMC_NORSRAM_MemoryDataWidth_16b)) +/** + * @} + */ + +/** @defgroup FMC_Burst_Access_Mode + * @{ + */ + +#define FMC_BurstAccessMode_Disable ((uint32_t)0x00000000) +#define FMC_BurstAccessMode_Enable ((uint32_t)0x00000100) + +#define IS_FMC_BURSTMODE(STATE) (((STATE) == FMC_BurstAccessMode_Disable) || \ + ((STATE) == FMC_BurstAccessMode_Enable)) +/** + * @} + */ + +/** @defgroup FMC_AsynchronousWait + * @{ + */ +#define FMC_AsynchronousWait_Disable ((uint32_t)0x00000000) +#define FMC_AsynchronousWait_Enable ((uint32_t)0x00008000) + +#define IS_FMC_ASYNWAIT(STATE) (((STATE) == FMC_AsynchronousWait_Disable) || \ + ((STATE) == FMC_AsynchronousWait_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Wait_Signal_Polarity + * @{ + */ +#define FMC_WaitSignalPolarity_Low ((uint32_t)0x00000000) +#define FMC_WaitSignalPolarity_High ((uint32_t)0x00000200) + +#define IS_FMC_WAIT_POLARITY(POLARITY) (((POLARITY) == FMC_WaitSignalPolarity_Low) || \ + ((POLARITY) == FMC_WaitSignalPolarity_High)) +/** + * @} + */ + +/** @defgroup FMC_Wrap_Mode + * @{ + */ +#define FMC_WrapMode_Disable ((uint32_t)0x00000000) +#define FMC_WrapMode_Enable ((uint32_t)0x00000400) + +#define IS_FMC_WRAP_MODE(MODE) (((MODE) == FMC_WrapMode_Disable) || \ + ((MODE) == FMC_WrapMode_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Wait_Timing + * @{ + */ +#define FMC_WaitSignalActive_BeforeWaitState ((uint32_t)0x00000000) +#define FMC_WaitSignalActive_DuringWaitState ((uint32_t)0x00000800) + +#define IS_FMC_WAIT_SIGNAL_ACTIVE(ACTIVE) (((ACTIVE) == FMC_WaitSignalActive_BeforeWaitState) || \ + ((ACTIVE) == FMC_WaitSignalActive_DuringWaitState)) +/** + * @} + */ + +/** @defgroup FMC_Write_Operation + * @{ + */ +#define FMC_WriteOperation_Disable ((uint32_t)0x00000000) +#define FMC_WriteOperation_Enable ((uint32_t)0x00001000) + +#define IS_FMC_WRITE_OPERATION(OPERATION) (((OPERATION) == FMC_WriteOperation_Disable) || \ + ((OPERATION) == FMC_WriteOperation_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Wait_Signal + * @{ + */ +#define FMC_WaitSignal_Disable ((uint32_t)0x00000000) +#define FMC_WaitSignal_Enable ((uint32_t)0x00002000) + +#define IS_FMC_WAITE_SIGNAL(SIGNAL) (((SIGNAL) == FMC_WaitSignal_Disable) || \ + ((SIGNAL) == FMC_WaitSignal_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Extended_Mode + * @{ + */ +#define FMC_ExtendedMode_Disable ((uint32_t)0x00000000) +#define FMC_ExtendedMode_Enable ((uint32_t)0x00004000) + +#define IS_FMC_EXTENDED_MODE(MODE) (((MODE) == FMC_ExtendedMode_Disable) || \ + ((MODE) == FMC_ExtendedMode_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Write_Burst + * @{ + */ + +#define FMC_WriteBurst_Disable ((uint32_t)0x00000000) +#define FMC_WriteBurst_Enable ((uint32_t)0x00080000) + +#define IS_FMC_WRITE_BURST(BURST) (((BURST) == FMC_WriteBurst_Disable) || \ + ((BURST) == FMC_WriteBurst_Enable)) +/** + * @} + */ + +/** @defgroup FMC_Continous_Clock + * @{ + */ + +#define FMC_CClock_SyncOnly ((uint32_t)0x00000000) +#define FMC_CClock_SyncAsync ((uint32_t)0x00100000) + +#define IS_FMC_CONTINOUS_CLOCK(CCLOCK) (((CCLOCK) == FMC_CClock_SyncOnly) || \ + ((CCLOCK) == FMC_CClock_SyncAsync)) +/** + * @} + */ + +/** @defgroup FMC_Address_Setup_Time + * @{ + */ +#define IS_FMC_ADDRESS_SETUP_TIME(TIME) ((TIME) <= 15) +/** + * @} + */ + +/** @defgroup FMC_Address_Hold_Time + * @{ + */ +#define IS_FMC_ADDRESS_HOLD_TIME(TIME) (((TIME) > 0) && ((TIME) <= 15)) +/** + * @} + */ + +/** @defgroup FMC_Data_Setup_Time + * @{ + */ +#define IS_FMC_DATASETUP_TIME(TIME) (((TIME) > 0) && ((TIME) <= 255)) +/** + * @} + */ + +/** @defgroup FMC_Bus_Turn_around_Duration + * @{ + */ +#define IS_FMC_TURNAROUND_TIME(TIME) ((TIME) <= 15) +/** + * @} + */ + +/** @defgroup FMC_CLK_Division + * @{ + */ +#define IS_FMC_CLK_DIV(DIV) (((DIV) > 1) && ((DIV) <= 16)) +/** + * @} + */ + +/** @defgroup FMC_Data_Latency + * @{ + */ +#define IS_FMC_DATA_LATENCY(LATENCY) (((LATENCY) > 1) && ((LATENCY) <= 17)) +/** + * @} + */ + +/** @defgroup FMC_Access_Mode + * @{ + */ +#define FMC_AccessMode_A ((uint32_t)0x00000000) +#define FMC_AccessMode_B ((uint32_t)0x10000000) +#define FMC_AccessMode_C ((uint32_t)0x20000000) +#define FMC_AccessMode_D ((uint32_t)0x30000000) + +#define IS_FMC_ACCESS_MODE(MODE) (((MODE) == FMC_AccessMode_A) || \ + ((MODE) == FMC_AccessMode_B) || \ + ((MODE) == FMC_AccessMode_C) || \ + ((MODE) == FMC_AccessMode_D)) +/** + * @} + */ + +/** + * @} + */ + +/** @defgroup FMC_NAND_PCCARD_Controller + * @{ + */ + +/** @defgroup FMC_Wait_feature + * @{ + */ +#define FMC_Waitfeature_Disable ((uint32_t)0x00000000) +#define FMC_Waitfeature_Enable ((uint32_t)0x00000002) + +#define IS_FMC_WAIT_FEATURE(FEATURE) (((FEATURE) == FMC_Waitfeature_Disable) || \ + ((FEATURE) == FMC_Waitfeature_Enable)) +/** + * @} + */ + +/** @defgroup FMC_NAND_Data_Width + * @{ + */ +#define FMC_NAND_MemoryDataWidth_8b ((uint32_t)0x00000000) +#define FMC_NAND_MemoryDataWidth_16b ((uint32_t)0x00000010) + +#define IS_FMC_NAND_MEMORY_WIDTH(WIDTH) (((WIDTH) == FMC_NAND_MemoryDataWidth_8b) || \ + ((WIDTH) == FMC_NAND_MemoryDataWidth_16b)) +/** + * @} + */ + +/** @defgroup FMC_ECC + * @{ + */ +#define FMC_ECC_Disable ((uint32_t)0x00000000) +#define FMC_ECC_Enable ((uint32_t)0x00000040) + +#define IS_FMC_ECC_STATE(STATE) (((STATE) == FMC_ECC_Disable) || \ + ((STATE) == FMC_ECC_Enable)) +/** + * @} + */ + +/** @defgroup FMC_ECC_Page_Size + * @{ + */ +#define FMC_ECCPageSize_256Bytes ((uint32_t)0x00000000) +#define FMC_ECCPageSize_512Bytes ((uint32_t)0x00020000) +#define FMC_ECCPageSize_1024Bytes ((uint32_t)0x00040000) +#define FMC_ECCPageSize_2048Bytes ((uint32_t)0x00060000) +#define FMC_ECCPageSize_4096Bytes ((uint32_t)0x00080000) +#define FMC_ECCPageSize_8192Bytes ((uint32_t)0x000A0000) + +#define IS_FMC_ECCPAGE_SIZE(SIZE) (((SIZE) == FMC_ECCPageSize_256Bytes) || \ + ((SIZE) == FMC_ECCPageSize_512Bytes) || \ + ((SIZE) == FMC_ECCPageSize_1024Bytes) || \ + ((SIZE) == FMC_ECCPageSize_2048Bytes) || \ + ((SIZE) == FMC_ECCPageSize_4096Bytes) || \ + ((SIZE) == FMC_ECCPageSize_8192Bytes)) +/** + * @} + */ + +/** @defgroup FMC_TCLR_Setup_Time + * @{ + */ +#define IS_FMC_TCLR_TIME(TIME) ((TIME) <= 255) +/** + * @} + */ + +/** @defgroup FMC_TAR_Setup_Time + * @{ + */ +#define IS_FMC_TAR_TIME(TIME) ((TIME) <= 255) +/** + * @} + */ + +/** @defgroup FMC_Setup_Time + * @{ + */ +#define IS_FMC_SETUP_TIME(TIME) ((TIME) <= 255) +/** + * @} + */ + +/** @defgroup FMC_Wait_Setup_Time + * @{ + */ +#define IS_FMC_WAIT_TIME(TIME) ((TIME) <= 255) +/** + * @} + */ + +/** @defgroup FMC_Hold_Setup_Time + * @{ + */ +#define IS_FMC_HOLD_TIME(TIME) ((TIME) <= 255) +/** + * @} + */ + +/** @defgroup FMC_HiZ_Setup_Time + * @{ + */ +#define IS_FMC_HIZ_TIME(TIME) ((TIME) <= 255) +/** + * @} + */ + +/** + * @} + */ + +/** @defgroup FMC_Interrupt_sources + * @{ + */ +#define FMC_IT_RisingEdge ((uint32_t)0x00000008) +#define FMC_IT_Level ((uint32_t)0x00000010) +#define FMC_IT_FallingEdge ((uint32_t)0x00000020) + +#define IS_FMC_IT(IT) ((((IT) & (uint32_t)0xFFFFBFC7) == 0x00000000) && ((IT) != 0x00000000)) +#define IS_FMC_GET_IT(IT) (((IT) == FMC_IT_RisingEdge) || \ + ((IT) == FMC_IT_Level) || \ + ((IT) == FMC_IT_FallingEdge)) + +#define IS_FMC_IT_BANK(BANK) (((BANK) == FMC_Bank2_NAND) || \ + ((BANK) == FMC_Bank3_NAND) || \ + ((BANK) == FMC_Bank4_PCCARD)) +/** + * @} + */ + +/** @defgroup FMC_Flags + * @{ + */ +#define FMC_FLAG_RisingEdge ((uint32_t)0x00000001) +#define FMC_FLAG_Level ((uint32_t)0x00000002) +#define FMC_FLAG_FallingEdge ((uint32_t)0x00000004) +#define FMC_FLAG_FEMPT ((uint32_t)0x00000040) + +#define IS_FMC_GET_FLAG(FLAG) (((FLAG) == FMC_FLAG_RisingEdge) || \ + ((FLAG) == FMC_FLAG_Level) || \ + ((FLAG) == FMC_FLAG_FallingEdge) || \ + ((FLAG) == FMC_FLAG_FEMPT)) + +#define IS_FMC_GETFLAG_BANK(BANK) (((BANK) == FMC_Bank2_NAND) || \ + ((BANK) == FMC_Bank3_NAND) || \ + ((BANK) == FMC_Bank4_PCCARD)) + +#define IS_FMC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFFFFF8) == 0x00000000) && ((FLAG) != 0x00000000)) + + +/** + * @} + */ + +/** + * @} + */ + + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +/* NOR/SRAM Controller functions **********************************************/ +void FMC_NORSRAMDeInit(uint32_t FMC_Bank); +void FMC_NORSRAMInit(FMC_NORSRAMInitTypeDef* FMC_NORSRAMInitStruct); +void FMC_NORSRAMStructInit(FMC_NORSRAMInitTypeDef* FMC_NORSRAMInitStruct); +void FMC_NORSRAMCmd(uint32_t FMC_Bank, FunctionalState NewState); + +/* NAND Controller functions **************************************************/ +void FMC_NANDDeInit(uint32_t FMC_Bank); +void FMC_NANDInit(FMC_NANDInitTypeDef* FMC_NANDInitStruct); +void FMC_NANDStructInit(FMC_NANDInitTypeDef* FMC_NANDInitStruct); +void FMC_NANDCmd(uint32_t FMC_Bank, FunctionalState NewState); +void FMC_NANDECCCmd(uint32_t FMC_Bank, FunctionalState NewState); +uint32_t FMC_GetECC(uint32_t FMC_Bank); + +/* PCCARD Controller functions ************************************************/ +void FMC_PCCARDDeInit(void); +void FMC_PCCARDInit(FMC_PCCARDInitTypeDef* FMC_PCCARDInitStruct); +void FMC_PCCARDStructInit(FMC_PCCARDInitTypeDef* FMC_PCCARDInitStruct); +void FMC_PCCARDCmd(FunctionalState NewState); + +/* Interrupts and flags management functions **********************************/ +void FMC_ITConfig(uint32_t FMC_Bank, uint32_t FMC_IT, FunctionalState NewState); +FlagStatus FMC_GetFlagStatus(uint32_t FMC_Bank, uint32_t FMC_FLAG); +void FMC_ClearFlag(uint32_t FMC_Bank, uint32_t FMC_FLAG); +ITStatus FMC_GetITStatus(uint32_t FMC_Bank, uint32_t FMC_IT); +void FMC_ClearITPendingBit(uint32_t FMC_Bank, uint32_t FMC_IT); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_FMC_H */ +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_gpio.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_gpio.h new file mode 100755 index 0000000000..3fd578771a --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_gpio.h @@ -0,0 +1,404 @@ +/** + ****************************************************************************** + * @file stm32f30x_gpio.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the GPIO + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_GPIO_H +#define __STM32F30x_GPIO_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup GPIO + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +#define IS_GPIO_ALL_PERIPH(PERIPH) (((PERIPH) == GPIOA) || \ + ((PERIPH) == GPIOB) || \ + ((PERIPH) == GPIOC) || \ + ((PERIPH) == GPIOD) || \ + ((PERIPH) == GPIOE) || \ + ((PERIPH) == GPIOF) || \ + ((PERIPH) == GPIOG) || \ + ((PERIPH) == GPIOH)) + +#define IS_GPIO_LIST_PERIPH(PERIPH) (((PERIPH) == GPIOA) || \ + ((PERIPH) == GPIOB) || \ + ((PERIPH) == GPIOC)|| \ + ((PERIPH) == GPIOD) || \ + ((PERIPH) == GPIOF)) +/** @defgroup Configuration_Mode_enumeration + * @{ + */ +typedef enum +{ + GPIO_Mode_IN = 0x00, /*!< GPIO Input Mode */ + GPIO_Mode_OUT = 0x01, /*!< GPIO Output Mode */ + GPIO_Mode_AF = 0x02, /*!< GPIO Alternate function Mode */ + GPIO_Mode_AN = 0x03 /*!< GPIO Analog In/Out Mode */ +}GPIOMode_TypeDef; + +#define IS_GPIO_MODE(MODE) (((MODE) == GPIO_Mode_IN)|| ((MODE) == GPIO_Mode_OUT) || \ + ((MODE) == GPIO_Mode_AF)|| ((MODE) == GPIO_Mode_AN)) +/** + * @} + */ + +/** @defgroup Output_type_enumeration + * @{ + */ +typedef enum +{ + GPIO_OType_PP = 0x00, + GPIO_OType_OD = 0x01 +}GPIOOType_TypeDef; + +#define IS_GPIO_OTYPE(OTYPE) (((OTYPE) == GPIO_OType_PP) || ((OTYPE) == GPIO_OType_OD)) + +/** + * @} + */ + +/** @defgroup Output_Maximum_frequency_enumeration + * @{ + */ +typedef enum +{ + GPIO_Speed_Level_1 = 0x01, /*!< Fast Speed */ + GPIO_Speed_Level_2 = 0x02, /*!< Meduim Speed */ + GPIO_Speed_Level_3 = 0x03 /*!< High Speed */ +}GPIOSpeed_TypeDef; + +#define IS_GPIO_SPEED(SPEED) (((SPEED) == GPIO_Speed_Level_1) || ((SPEED) == GPIO_Speed_Level_2) || \ + ((SPEED) == GPIO_Speed_Level_3)) +/** + * @} + */ + +/** @defgroup Configuration_Pull-Up_Pull-Down_enumeration + * @{ + */ +typedef enum +{ + GPIO_PuPd_NOPULL = 0x00, + GPIO_PuPd_UP = 0x01, + GPIO_PuPd_DOWN = 0x02 +}GPIOPuPd_TypeDef; + +#define IS_GPIO_PUPD(PUPD) (((PUPD) == GPIO_PuPd_NOPULL) || ((PUPD) == GPIO_PuPd_UP) || \ + ((PUPD) == GPIO_PuPd_DOWN)) +/** + * @} + */ + +/** @defgroup Bit_SET_and_Bit_RESET_enumeration + * @{ + */ +typedef enum +{ + Bit_RESET = 0, + Bit_SET +}BitAction; + +#define IS_GPIO_BIT_ACTION(ACTION) (((ACTION) == Bit_RESET) || ((ACTION) == Bit_SET)) +/** + * @} + */ + +/** + * @brief GPIO Init structure definition + */ +typedef struct +{ + uint32_t GPIO_Pin; /*!< Specifies the GPIO pins to be configured. + This parameter can be any value of @ref GPIO_pins_define */ + + GPIOMode_TypeDef GPIO_Mode; /*!< Specifies the operating mode for the selected pins. + This parameter can be a value of @ref GPIOMode_TypeDef */ + + GPIOSpeed_TypeDef GPIO_Speed; /*!< Specifies the speed for the selected pins. + This parameter can be a value of @ref GPIOSpeed_TypeDef */ + + GPIOOType_TypeDef GPIO_OType; /*!< Specifies the operating output type for the selected pins. + This parameter can be a value of @ref GPIOOType_TypeDef */ + + GPIOPuPd_TypeDef GPIO_PuPd; /*!< Specifies the operating Pull-up/Pull down for the selected pins. + This parameter can be a value of @ref GPIOPuPd_TypeDef */ +}GPIO_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup GPIO_Exported_Constants + * @{ + */ + +/** @defgroup GPIO_pins_define + * @{ + */ +#define GPIO_Pin_0 ((uint16_t)0x0001) /*!< Pin 0 selected */ +#define GPIO_Pin_1 ((uint16_t)0x0002) /*!< Pin 1 selected */ +#define GPIO_Pin_2 ((uint16_t)0x0004) /*!< Pin 2 selected */ +#define GPIO_Pin_3 ((uint16_t)0x0008) /*!< Pin 3 selected */ +#define GPIO_Pin_4 ((uint16_t)0x0010) /*!< Pin 4 selected */ +#define GPIO_Pin_5 ((uint16_t)0x0020) /*!< Pin 5 selected */ +#define GPIO_Pin_6 ((uint16_t)0x0040) /*!< Pin 6 selected */ +#define GPIO_Pin_7 ((uint16_t)0x0080) /*!< Pin 7 selected */ +#define GPIO_Pin_8 ((uint16_t)0x0100) /*!< Pin 8 selected */ +#define GPIO_Pin_9 ((uint16_t)0x0200) /*!< Pin 9 selected */ +#define GPIO_Pin_10 ((uint16_t)0x0400) /*!< Pin 10 selected */ +#define GPIO_Pin_11 ((uint16_t)0x0800) /*!< Pin 11 selected */ +#define GPIO_Pin_12 ((uint16_t)0x1000) /*!< Pin 12 selected */ +#define GPIO_Pin_13 ((uint16_t)0x2000) /*!< Pin 13 selected */ +#define GPIO_Pin_14 ((uint16_t)0x4000) /*!< Pin 14 selected */ +#define GPIO_Pin_15 ((uint16_t)0x8000) /*!< Pin 15 selected */ +#define GPIO_Pin_All ((uint16_t)0xFFFF) /*!< All pins selected */ + +#define IS_GPIO_PIN(PIN) ((PIN) != (uint16_t)0x00) + +#define IS_GET_GPIO_PIN(PIN) (((PIN) == GPIO_Pin_0) || \ + ((PIN) == GPIO_Pin_1) || \ + ((PIN) == GPIO_Pin_2) || \ + ((PIN) == GPIO_Pin_3) || \ + ((PIN) == GPIO_Pin_4) || \ + ((PIN) == GPIO_Pin_5) || \ + ((PIN) == GPIO_Pin_6) || \ + ((PIN) == GPIO_Pin_7) || \ + ((PIN) == GPIO_Pin_8) || \ + ((PIN) == GPIO_Pin_9) || \ + ((PIN) == GPIO_Pin_10) || \ + ((PIN) == GPIO_Pin_11) || \ + ((PIN) == GPIO_Pin_12) || \ + ((PIN) == GPIO_Pin_13) || \ + ((PIN) == GPIO_Pin_14) || \ + ((PIN) == GPIO_Pin_15)) + +/** + * @} + */ + +/** @defgroup GPIO_Pin_sources + * @{ + */ +#define GPIO_PinSource0 ((uint8_t)0x00) +#define GPIO_PinSource1 ((uint8_t)0x01) +#define GPIO_PinSource2 ((uint8_t)0x02) +#define GPIO_PinSource3 ((uint8_t)0x03) +#define GPIO_PinSource4 ((uint8_t)0x04) +#define GPIO_PinSource5 ((uint8_t)0x05) +#define GPIO_PinSource6 ((uint8_t)0x06) +#define GPIO_PinSource7 ((uint8_t)0x07) +#define GPIO_PinSource8 ((uint8_t)0x08) +#define GPIO_PinSource9 ((uint8_t)0x09) +#define GPIO_PinSource10 ((uint8_t)0x0A) +#define GPIO_PinSource11 ((uint8_t)0x0B) +#define GPIO_PinSource12 ((uint8_t)0x0C) +#define GPIO_PinSource13 ((uint8_t)0x0D) +#define GPIO_PinSource14 ((uint8_t)0x0E) +#define GPIO_PinSource15 ((uint8_t)0x0F) + +#define IS_GPIO_PIN_SOURCE(PINSOURCE) (((PINSOURCE) == GPIO_PinSource0) || \ + ((PINSOURCE) == GPIO_PinSource1) || \ + ((PINSOURCE) == GPIO_PinSource2) || \ + ((PINSOURCE) == GPIO_PinSource3) || \ + ((PINSOURCE) == GPIO_PinSource4) || \ + ((PINSOURCE) == GPIO_PinSource5) || \ + ((PINSOURCE) == GPIO_PinSource6) || \ + ((PINSOURCE) == GPIO_PinSource7) || \ + ((PINSOURCE) == GPIO_PinSource8) || \ + ((PINSOURCE) == GPIO_PinSource9) || \ + ((PINSOURCE) == GPIO_PinSource10) || \ + ((PINSOURCE) == GPIO_PinSource11) || \ + ((PINSOURCE) == GPIO_PinSource12) || \ + ((PINSOURCE) == GPIO_PinSource13) || \ + ((PINSOURCE) == GPIO_PinSource14) || \ + ((PINSOURCE) == GPIO_PinSource15)) +/** + * @} + */ + +/** @defgroup GPIO_Alternate_function_selection_define + * @{ + */ + +/** + * @brief AF 0 selection + */ +#define GPIO_AF_0 ((uint8_t)0x00) /* JTCK-SWCLK, JTDI, JTDO/TRACESW0, JTMS-SWDAT, + MCO, NJTRST, TRACED, TRACECK */ +/** + * @brief AF 1 selection + */ +#define GPIO_AF_1 ((uint8_t)0x01) /* OUT, TIM2, TIM15, TIM16, TIM17 */ + +/** + * @brief AF 2 selection + */ +#define GPIO_AF_2 ((uint8_t)0x02) /* COMP1_OUT, TIM1, TIM2, TIM3, TIM4, TIM8, TIM15, TIM16 */ + +/** + * @brief AF 3 selection + */ +#define GPIO_AF_3 ((uint8_t)0x03) /* COMP7_OUT, TIM8, TIM15, Touch, HRTIM1 */ + +/** + * @brief AF 4 selection + */ +#define GPIO_AF_4 ((uint8_t)0x04) /* I2C1, I2C2, TIM1, TIM8, TIM16, TIM17 */ + +/** + * @brief AF 5 selection + */ +#define GPIO_AF_5 ((uint8_t)0x05) /* IR_OUT, I2S2, I2S3, SPI1, SPI2, TIM8, USART4, USART5 */ + +/** + * @brief AF 6 selection + */ +#define GPIO_AF_6 ((uint8_t)0x06) /* IR_OUT, I2S2, I2S3, SPI2, SPI3, TIM1, TIM8 */ + +/** + * @brief AF 7 selection + */ +#define GPIO_AF_7 ((uint8_t)0x07) /* AOP2_OUT, CAN, COMP3_OUT, COMP5_OUT, COMP6_OUT, + USART1, USART2, USART3 */ + +/** + * @brief AF 8 selection + */ +#define GPIO_AF_8 ((uint8_t)0x08) /* COMP1_OUT, COMP2_OUT, COMP3_OUT, COMP4_OUT, + COMP5_OUT, COMP6_OUT */ + +/** + * @brief AF 9 selection + */ +#define GPIO_AF_9 ((uint8_t)0x09) /* AOP4_OUT, CAN, TIM1, TIM8, TIM15 */ + +/** + * @brief AF 10 selection + */ +#define GPIO_AF_10 ((uint8_t)0x0A) /* AOP1_OUT, AOP3_OUT, TIM2, TIM3, TIM4, TIM8, TIM17 */ + +/** + * @brief AF 11 selection + */ +#define GPIO_AF_11 ((uint8_t)0x0B) /* TIM1, TIM8 */ + +/** + * @brief AF 12 selection + */ +#define GPIO_AF_12 ((uint8_t)0x0C) /* TIM1, HRTIM1 */ + +/** + * @brief AF 13 selection + */ +#define GPIO_AF_13 ((uint8_t)0x0D) /* HRTIM1, AOP2_OUT */ + +/** + * @brief AF 14 selection + */ +#define GPIO_AF_14 ((uint8_t)0x0E) /* USBDM, USBDP */ + +/** + * @brief AF 15 selection + */ +#define GPIO_AF_15 ((uint8_t)0x0F) /* OUT */ + +#define IS_GPIO_AF(AF) (((AF) == GPIO_AF_0)||((AF) == GPIO_AF_1)||\ + ((AF) == GPIO_AF_2)||((AF) == GPIO_AF_3)||\ + ((AF) == GPIO_AF_4)||((AF) == GPIO_AF_5)||\ + ((AF) == GPIO_AF_6)||((AF) == GPIO_AF_7)||\ + ((AF) == GPIO_AF_8)||((AF) == GPIO_AF_9)||\ + ((AF) == GPIO_AF_10)||((AF) == GPIO_AF_11)||\ + ((AF) == GPIO_AF_12)||((AF) == GPIO_AF_13)||\ + ((AF) == GPIO_AF_14)||((AF) == GPIO_AF_15)) + +/** + * @} + */ + +/** @defgroup GPIO_Speed_Legacy + * @{ + */ + +#define GPIO_Speed_10MHz GPIO_Speed_Level_1 /*!< Fast Speed:10MHz */ +#define GPIO_Speed_2MHz GPIO_Speed_Level_2 /*!< Medium Speed:2MHz */ +#define GPIO_Speed_50MHz GPIO_Speed_Level_3 /*!< High Speed:50MHz */ + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* Function used to set the GPIO configuration to the default reset state *****/ +void GPIO_DeInit(GPIO_TypeDef* GPIOx); + +/* Initialization and Configuration functions *********************************/ +void GPIO_Init(GPIO_TypeDef* GPIOx, GPIO_InitTypeDef* GPIO_InitStruct); +void GPIO_StructInit(GPIO_InitTypeDef* GPIO_InitStruct); +void GPIO_PinLockConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); + +/* GPIO Read and Write functions **********************************************/ +uint8_t GPIO_ReadInputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); +uint16_t GPIO_ReadInputData(GPIO_TypeDef* GPIOx); +uint8_t GPIO_ReadOutputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); +uint16_t GPIO_ReadOutputData(GPIO_TypeDef* GPIOx); +void GPIO_SetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); +void GPIO_ResetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin); +void GPIO_WriteBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, BitAction BitVal); +void GPIO_Write(GPIO_TypeDef* GPIOx, uint16_t PortVal); + +/* GPIO Alternate functions configuration functions ***************************/ +void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_GPIO_H */ +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_hrtim.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_hrtim.h new file mode 100755 index 0000000000..56a6eed72b --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_hrtim.h @@ -0,0 +1,2741 @@ +/** + ****************************************************************************** + * @file stm32f30x_hrtim.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the HRTIM firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_HRTIM_H +#define __STM32F30x_HRTIM_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup ADC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief HRTIM Configuration Structure definition - Time base related parameters + */ +typedef struct +{ + uint32_t Period; /*!< Specifies the timer period + The period value must be above 3 periods of the fHRTIM clock. + Maximum value is = 0xFFDF */ + uint32_t RepetitionCounter; /*!< Specifies the timer repetition period + This parameter must be a number between Min_Data = 0x00 and Max_Data = 0xFF. */ + uint32_t PrescalerRatio; /*!< Specifies the timer clock prescaler ratio. + This parameter can be any value of @ref HRTIM_PrescalerRatio */ + uint32_t Mode; /*!< Specifies the counter operating mode + This parameter can be any value of @ref HRTIM_Mode */ +} HRTIM_BaseInitTypeDef; +/** + * @brief Waveform mode initialization parameters definition + */ +typedef struct { + uint32_t HalfModeEnable; /*!< Specifies whether or not half mode is enabled + This parameter can be a combination of @ref HRTIM_HalfModeEnable */ + uint32_t StartOnSync; /*!< Specifies whether or not timer is reset by a rising edge on the synchronization input (when enabled) + This parameter can be a combination of @ref HRTIM_StartOnSyncInputEvent */ + uint32_t ResetOnSync; /*!< Specifies whether or not timer is reset by a rising edge on the synchronization input (when enabled) + This parameter can be a combination of @ref HRTIM_ResetOnSyncInputEvent */ + uint32_t DACSynchro; /*!< Indicates whether or not the a DAC synchronization event is generated + This parameter can be any value of @ref HRTIM_DACSynchronization */ + uint32_t PreloadEnable; /*!< Specifies whether or not register preload is enabled + This parameter can be a combination of @ref HRTIM_RegisterPreloadEnable */ + uint32_t UpdateGating; /*!< Specifies how the update occurs with respect to a burst DMA transaction or + update enable inputs (Slave timers only) + This parameter can be any value of @ref HRTIM_UpdateGating */ + uint32_t BurstMode; /*!< Specifies how the timer behaves during a burst mode operation + This parameter can be a combination of @ref HRTIM_TimerBurstMode */ + uint32_t RepetitionUpdate; /*!< Specifies whether or not registers update is triggered by the repetition event + This parameter can be a combination of @ref HRTIM_TimerRepetitionUpdate */ +} HRTIM_TimerInitTypeDef; + +/** + * @brief Basic output compare mode configuration definition + */ +typedef struct { + uint32_t Mode; /*!< Specifies the output compare mode (toggle, active, inactive) + This parameter can be a combination of @ref HRTIM_BasicOCMode */ + uint32_t Pulse; /*!< Specifies the compare value to be loaded into the Compare Register. + The compare value must be above or equal to 3 periods of the fHRTIM clock */ + uint32_t Polarity; /*!< Specifies the output polarity + This parameter can be any value of @ref HRTIM_Output_Polarity */ + uint32_t IdleState; /*!< Specifies whether the output level is active or inactive when in IDLE state + This parameter can be any value of @ref HRTIM_OutputIDLEState */ +} HRTIM_BasicOCChannelCfgTypeDef; + +/** + * @brief Basic PWM output mode configuration definition + */ +typedef struct { + uint32_t Pulse; /*!< Specifies the compare value to be loaded into the Compare Register. + The compare value must be above or equal to 3 periods of the fHRTIM clock */ + uint32_t Polarity; /*!< Specifies the output polarity + This parameter can be any value of @ref HRTIM_OutputPolarity */ + uint32_t IdleState; /*!< Specifies whether the output level is active or inactive when in IDLE state + This parameter can be any value of @ref HRTIM_OutputIDLEState */ +} HRTIM_BasicPWMChannelCfgTypeDef; + +/** + * @brief Basic capture mode configuration definition + */ +typedef struct { + uint32_t CaptureUnit; /*!< Specifies the external event Channel + This parameter can be any 'EEVx' value of @ref HRTIM_CaptureUnit */ + uint32_t Event; /*!< Specifies the external event triggering the capture + This parameter can be any 'EEVx' value of @ref HRTIM_ExternalEventChannels */ + uint32_t EventPolarity; /*!< Specifies the polarity of the external event (in case of level sensitivity) + This parameter can be a value of @ref HRTIM_ExternalEventPolarity */ + uint32_t EventSensitivity; /*!< Specifies the sensitivity of the external event + This parameter can be a value of @ref HRTIM_ExternalEventSensitivity */ + uint32_t EventFilter; /*!< Defines the frequency used to sample the External Event and the length of the digital filter + This parameter can be a value of @ref HRTIM_ExternalEventFilter */ +} HRTIM_BasicCaptureChannelCfgTypeDef; + +/** + * @brief Basic One Pulse mode configuration definition + */ +typedef struct { + uint32_t Pulse; /*!< Specifies the compare value to be loaded into the Compare Register. + The compare value must be above or equal to 3 periods of the fHRTIM clock */ + uint32_t OutputPolarity; /*!< Specifies the output polarity + This parameter can be any value of @ref HRTIM_Output_Polarity */ + uint32_t OutputIdleState; /*!< Specifies whether the output level is active or inactive when in IDLE state + This parameter can be any value of @ref HRTIM_Output_IDLE_State */ + uint32_t Event; /*!< Specifies the external event triggering the pulse generation + This parameter can be any 'EEVx' value of @ref HRTIM_Capture_Unit_Trigger */ + uint32_t EventPolarity; /*!< Specifies the polarity of the external event (in case of level sensitivity) + This parameter can be a value of @ref HRTIM_ExternalEventPolarity */ + uint32_t EventSensitivity; /*!< Specifies the sensitivity of the external event + This parameter can be a value of @ref HRTIM_ExternalEventSensitivity */ + uint32_t EventFilter; /*!< Defines the frequency used to sample the External Event and the length of the digital filter + This parameter can be a value of @ref HRTIM_ExternalEventFilter */ +} HRTIM_BasicOnePulseChannelCfgTypeDef; + +/** + * @brief Timer configuration definition + */ +typedef struct { + uint32_t PushPull; /*!< Specifies whether or not the push-pull mode is enabled + This parameter can be a value of @ref HRTIM_TimerPushPullMode */ + uint32_t FaultEnable; /*!< Specifies which fault channels are enabled for the timer + This parameter can be a combination of @ref HRTIM_TimerFaultEnabling */ + uint32_t FaultLock; /*!< Specifies whether or not fault enabling status is write protected + This parameter can be a value of @ref HRTIM_TimerFaultLock */ + uint32_t DeadTimeInsertion; /*!< Specifies whether or not dead time insertion is enabled for the timer + This parameter can be a value of @ref HRTIM_TimerDeadtimeInsertion */ + uint32_t DelayedProtectionMode; /*!< Specifies the delayed protection mode + This parameter can be a value of @ref HRTIM_TimerDelayedProtectionMode */ + uint32_t UpdateTrigger; /*!< Specifies source(s) triggering the timer registers update + This parameter can be a combination of @ref HRTIM_TimerUpdateTrigger */ + uint32_t ResetTrigger; /*!< Specifies source(s) triggering the timer counter reset + This parameter can be a combination of @ref HRTIM_TimerResetTrigger */ + uint32_t ResetUpdate; /*!< Specifies whether or not registers update is triggered when the timer counter is reset + This parameter can be a combination of @ref HRTIM_TimerResetUpdate */ +} HRTIM_TimerCfgTypeDef; + +/** + * @brief Compare unit configuration definition + */ +typedef struct { + uint32_t CompareValue; /*!< Specifies the compare value of the timer compare unit + the minimum value must be greater than or equal to 3 periods of the fHRTIM clock + the maximum value must be less than or equal to 0xFFFF - 1 periods of the fHRTIM clock */ + uint32_t AutoDelayedMode; /*!< Specifies the auto delayed mode for compare unit 2 or 4 + This parameter can be a value of @ref HRTIM_CompareUnitAutoDelayedMode */ + uint32_t AutoDelayedTimeout; /*!< Specifies compare value for timing unit 1 or 3 when auto delayed mode with time out is selected + CompareValue + AutoDelayedTimeout must be less than 0xFFFF */ +} HRTIM_CompareCfgTypeDef; + +/** + * @brief Capture unit configuration definition + */ +typedef struct { + uint32_t Trigger; /*!< Specifies source(s) triggering the capture + This parameter can be a combination of @ref HRTIM_CaptureUnitTrigger */ +} HRTIM_CaptureCfgTypeDef; + +/** + * @brief Output configuration definition + */ +typedef struct { + uint32_t Polarity; /*!< Specifies the output polarity + This parameter can be any value of @ref HRTIM_Output_Polarity */ + uint32_t SetSource; /*!< Specifies the event(s) transitioning the output from its inactive level to its active level + This parameter can be any value of @ref HRTIM_OutputSetSource */ + uint32_t ResetSource; /*!< Specifies the event(s) transitioning the output from its active level to its inactive level + This parameter can be any value of @ref HRTIM_OutputResetSource */ + uint32_t IdleMode; /*!< Specifies whether or not the output is affected by a burst mode operation + This parameter can be any value of @ref HRTIM_OutputIdleMode */ + uint32_t IdleState; /*!< Specifies whether the output level is active or inactive when in IDLE state + This parameter can be any value of @ref HRTIM_OutputIDLEState */ + uint32_t FaultState; /*!< Specifies whether the output level is active or inactive when in FAULT state + This parameter can be any value of @ref HRTIM_OutputFAULTState */ + uint32_t ChopperModeEnable; /*!< Indicates whether or not the chopper mode is enabled + This parameter can be any value of @ref HRTIM_OutputChopperModeEnable */ + uint32_t BurstModeEntryDelayed; /* !HRTIM_MASTER.MCR |= (__TIMERS__)) + +/* The counter of a timing unit is disabled only if all the timer outputs */ +/* are disabled and no capture is configured */ +#define HRTIM_TAOEN_MASK (HRTIM_OENR_TA2OEN | HRTIM_OENR_TA1OEN) +#define HRTIM_TBOEN_MASK (HRTIM_OENR_TB2OEN | HRTIM_OENR_TB1OEN) +#define HRTIM_TCOEN_MASK (HRTIM_OENR_TC2OEN | HRTIM_OENR_TC1OEN) +#define HRTIM_TDOEN_MASK (HRTIM_OENR_TD2OEN | HRTIM_OENR_TD1OEN) +#define HRTIM_TEOEN_MASK (HRTIM_OENR_TE2OEN | HRTIM_OENR_TE1OEN) +#define __HRTIM_DISABLE(__HANDLE__, __TIMERS__)\ + do {\ + if (((__TIMERS__) & HRTIM_TIMERID_MASTER) == HRTIM_TIMERID_MASTER)\ + {\ + ((__HANDLE__)->HRTIM_MASTER.MCR &= ~HRTIM_TIMERID_MASTER);\ + }\ + if (((__TIMERS__) & HRTIM_TIMERID_TIMER_A) == HRTIM_TIMERID_TIMER_A)\ + {\ + if (((__HANDLE__)->HRTIM_COMMON.OENR & HRTIM_TAOEN_MASK) == RESET)\ + {\ + ((__HANDLE__)->HRTIM_MASTER.MCR &= ~HRTIM_TIMERID_TIMER_A);\ + }\ + }\ + if (((__TIMERS__) & HRTIM_TIMERID_TIMER_B) == HRTIM_TIMERID_TIMER_B)\ + {\ + if (((__HANDLE__)->HRTIM_COMMON.OENR & HRTIM_TBOEN_MASK) == RESET)\ + {\ + ((__HANDLE__)->HRTIM_MASTER.MCR &= ~HRTIM_TIMERID_TIMER_B);\ + }\ + }\ + if (((__TIMERS__) & HRTIM_TIMERID_TIMER_C) == HRTIM_TIMERID_TIMER_C)\ + {\ + if (((__HANDLE__)->HRTIM_COMMON.OENR & HRTIM_TCOEN_MASK) == RESET)\ + {\ + ((__HANDLE__)->HRTIM_MASTER.MCR &= ~HRTIM_TIMERID_TIMER_C);\ + }\ + }\ + if (((__TIMERS__) & HRTIM_TIMERID_TIMER_D) == HRTIM_TIMERID_TIMER_D)\ + {\ + if (((__HANDLE__)->HRTIM_COMMON.OENR & HRTIM_TDOEN_MASK) == RESET)\ + {\ + ((__HANDLE__)->HRTIM_MASTER.MCR &= ~HRTIM_TIMERID_TIMER_D);\ + }\ + }\ + if (((__TIMERS__) & HRTIM_TIMERID_TIMER_E) == HRTIM_TIMERID_TIMER_E)\ + {\ + if (((__HANDLE__)->HRTIM_COMMON.OENR & HRTIM_TEOEN_MASK) == RESET)\ + {\ + ((__HANDLE__)->HRTIM_MASTER.MCR &= ~HRTIM_TIMERID_TIMER_E);\ + }\ + }\ + } while(0) + +/* Exported functions --------------------------------------------------------*/ + +/* Simple time base related functions *****************************************/ +void HRTIM_SimpleBase_Init(HRTIM_TypeDef* HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct); + +void HRTIM_DeInit(HRTIM_TypeDef* HRTIMx); + +void HRTIM_SimpleBaseStart(HRTIM_TypeDef *HRTIMx, uint32_t TimerIdx); +void HRTIM_SimpleBaseStop(HRTIM_TypeDef *HRTIMx, uint32_t TimerIdx); + +/* Simple output compare related functions ************************************/ +void HRTIM_SimpleOC_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct); + +void HRTIM_SimpleOCChannelConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t OCChannel, + HRTIM_BasicOCChannelCfgTypeDef* pBasicOCChannelCfg); + +void HRTIM_SimpleOCStart(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t OCChannel); +void HRTIM_SimpleOCStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OCChannel); +/* Simple PWM output related functions ****************************************/ +void HRTIM_SimplePWM_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct); + +void HRTIM_SimplePWMChannelConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t PWMChannel, + HRTIM_BasicPWMChannelCfgTypeDef* pBasicPWMChannelCfg); + +void HRTIM_SimplePWMStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t PWMChannel); +void HRTIM_SimplePWMStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t PWMChannel); +/* Simple capture related functions *******************************************/ +void HRTIM_SimpleCapture_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct); + +void HRTIM_SimpleCaptureChannelConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureChannel, + HRTIM_BasicCaptureChannelCfgTypeDef* pBasicCaptureChannelCfg); + +void HRTIM_SimpleCaptureStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureChannel); +void HRTIM_SimpleCaptureStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureChannel); +/* SImple one pulse related functions *****************************************/ +void HRTIM_SimpleOnePulse_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct); + +void HRTIM_SimpleOnePulseChannelConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t OnePulseChannel, + HRTIM_BasicOnePulseChannelCfgTypeDef* pBasicOnePulseChannelCfg); + +void HRTIM_SimpleOnePulseStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OnePulseChannel); +void HRTIM_SimpleOnePulseStop(HRTIM_TypeDef * HRTIM_, + uint32_t TimerIdx, + uint32_t OnePulseChannel); +/* Waveform related functions *************************************************/ +void HRTIM_Waveform_Init(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct, + HRTIM_TimerInitTypeDef* HRTIM_TimerInitStruct); + +void HRTIM_WaveformTimerConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + HRTIM_TimerCfgTypeDef * HRTIM_TimerCfgStruct); + +void HRTIM_WaveformCompareConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t CompareUnit, + HRTIM_CompareCfgTypeDef* pCompareCfg); + +void HRTIM_SlaveSetCompare(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CompareUnit, + uint32_t Compare); + +void HRTIM_MasterSetCompare(HRTIM_TypeDef * HRTIMx, + uint32_t CompareUnit, + uint32_t Compare); + +void HRTIM_WaveformCaptureConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit, + HRTIM_CaptureCfgTypeDef* pCaptureCfg); + +void HRTIM_TimerEventFilteringConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t Event, + HRTIM_TimerEventFilteringCfgTypeDef * pTimerEventFilteringCfg); + +void HRTIM_DeadTimeConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + HRTIM_DeadTimeCfgTypeDef* pDeadTimeCfg); + +void HRTIM_ChopperModeConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + HRTIM_ChopperModeCfgTypeDef* pChopperModeCfg); + +void HRTIM_BurstDMAConfig(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t RegistersToUpdate); + +void HRTIM_SynchronizationConfig(HRTIM_TypeDef *HRTIMx, + HRTIM_SynchroCfgTypeDef * pSynchroCfg); + +void HRTIM_BurstModeConfig(HRTIM_TypeDef *HRTIMx, + HRTIM_BurstModeCfgTypeDef* pBurstModeCfg); + +void HRTIM_EventConfig(HRTIM_TypeDef *HRTIMx, + uint32_t Event, + HRTIM_EventCfgTypeDef* pEventCfg); + +void HRTIM_EventPrescalerConfig(HRTIM_TypeDef *HRTIMx, + uint32_t Prescaler); + +void HRTIM_FaultConfig(HRTIM_TypeDef *hrtim, + HRTIM_FaultCfgTypeDef* pFaultCfg, + uint32_t Fault); + +void HRTIM_FaultPrescalerConfig(HRTIM_TypeDef *HRTIMx, + uint32_t Prescaler); +void HRTIM_FaultModeCtl(HRTIM_TypeDef * HRTIMx, uint32_t Fault, uint32_t Enable); + +void HRTIM_ADCTriggerConfig(HRTIM_TypeDef *HRTIMx, + uint32_t ADCTrigger, + HRTIM_ADCTriggerCfgTypeDef* pADCTriggerCfg); + +void HRTIM_WaveformCounterStart(HRTIM_TypeDef *HRTIMx, + uint32_t TimersToStart); + +void HRTIM_WaveformCounterStop(HRTIM_TypeDef *HRTIMx, + uint32_t TimersToStop); + +void HRTIM_WaveformOutputStart(HRTIM_TypeDef *HRTIMx, + uint32_t OutputsToStart); +void HRTIM_WaveformOutputStop(HRTIM_TypeDef * HRTIM_, + uint32_t OutputsToStop); + +void HRTIM_DLLCalibrationStart(HRTIM_TypeDef *HRTIMx, + uint32_t CalibrationRate); + +/* Interrupt/flags and DMA management */ +void HRTIM_ITConfig(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_TIM_IT, FunctionalState NewState); +void HRTIM_ITCommonConfig(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonIT, FunctionalState NewState); + +void HRTIM_ClearFlag(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_FLAG); +void HRTIM_ClearCommonFlag(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonFLAG); + +void HRTIM_ClearITPendingBit(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_IT); +void HRTIM_ClearCommonITPendingBit(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonIT); + +FlagStatus HRTIM_GetFlagStatus(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_FLAG); +FlagStatus HRTIM_GetCommonFlagStatus(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonFLAG); + +ITStatus HRTIM_GetITStatus(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_IT); +ITStatus HRTIM_GetCommonITStatus(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonIT); + + +void HRTIM_DMACmd(HRTIM_TypeDef* HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_DMA, FunctionalState NewState); + +void HRTIM_BurstModeCtl(HRTIM_TypeDef *HRTIMx, + uint32_t Enable); + +void HRTIM_SoftwareCapture(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit); + +void HRTIM_SoftwareUpdate(HRTIM_TypeDef *HRTIMx, + uint32_t TimersToUpdate); + +void HRTIM_SoftwareReset(HRTIM_TypeDef *HRTIMx, + uint32_t TimersToReset); + + +uint32_t HRTIM_GetCapturedValue(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit); + +void HRTIM_WaveformOutputConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output, + HRTIM_OutputCfgTypeDef * pOutputCfg); + +void HRTIM_WaveformSetOutputLevel(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t Output, + uint32_t OutputLevel); + +uint32_t HRTIM_WaveformGetOutputLevel(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t Output); + +uint32_t HRTIM_WaveformGetOutputState(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output); + +uint32_t HRTIM_GetDelayedProtectionStatus(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx, + uint32_t Output); + +uint32_t HRTIM_GetBurstStatus(HRTIM_TypeDef *HRTIMx); + +uint32_t HRTIM_GetCurrentPushPullStatus(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx); + +uint32_t HRTIM_GetIdlePushPullStatus(HRTIM_TypeDef *HRTIMx, + uint32_t TimerIdx); +/** + * @} + */ + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_HRTIM_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_i2c.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_i2c.h new file mode 100755 index 0000000000..1def7513a4 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_i2c.h @@ -0,0 +1,477 @@ +/** + ****************************************************************************** + * @file stm32f30x_i2c.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the I2C firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_I2C_H +#define __STM32F30x_I2C_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup I2C + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief I2C Init structure definition + */ + +typedef struct +{ + uint32_t I2C_Timing; /*!< Specifies the I2C_TIMINGR_register value. + This parameter calculated by referring to I2C initialization + section in Reference manual*/ + + uint32_t I2C_AnalogFilter; /*!< Enables or disables analog noise filter. + This parameter can be a value of @ref I2C_Analog_Filter */ + + uint32_t I2C_DigitalFilter; /*!< Configures the digital noise filter. + This parameter can be a number between 0x00 and 0x0F */ + + uint32_t I2C_Mode; /*!< Specifies the I2C mode. + This parameter can be a value of @ref I2C_mode */ + + uint32_t I2C_OwnAddress1; /*!< Specifies the device own address 1. + This parameter can be a 7-bit or 10-bit address */ + + uint32_t I2C_Ack; /*!< Enables or disables the acknowledgement. + This parameter can be a value of @ref I2C_acknowledgement */ + + uint32_t I2C_AcknowledgedAddress; /*!< Specifies if 7-bit or 10-bit address is acknowledged. + This parameter can be a value of @ref I2C_acknowledged_address */ +}I2C_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + + +/** @defgroup I2C_Exported_Constants + * @{ + */ + +#define IS_I2C_ALL_PERIPH(PERIPH) (((PERIPH) == I2C1) || \ + ((PERIPH) == I2C2)) + +/** @defgroup I2C_Analog_Filter + * @{ + */ + +#define I2C_AnalogFilter_Enable ((uint32_t)0x00000000) +#define I2C_AnalogFilter_Disable I2C_CR1_ANFOFF + +#define IS_I2C_ANALOG_FILTER(FILTER) (((FILTER) == I2C_AnalogFilter_Enable) || \ + ((FILTER) == I2C_AnalogFilter_Disable)) +/** + * @} + */ + +/** @defgroup I2C_Digital_Filter + * @{ + */ + +#define IS_I2C_DIGITAL_FILTER(FILTER) ((FILTER) <= 0x0000000F) +/** + * @} + */ + +/** @defgroup I2C_mode + * @{ + */ + +#define I2C_Mode_I2C ((uint32_t)0x00000000) +#define I2C_Mode_SMBusDevice I2C_CR1_SMBDEN +#define I2C_Mode_SMBusHost I2C_CR1_SMBHEN + +#define IS_I2C_MODE(MODE) (((MODE) == I2C_Mode_I2C) || \ + ((MODE) == I2C_Mode_SMBusDevice) || \ + ((MODE) == I2C_Mode_SMBusHost)) +/** + * @} + */ + +/** @defgroup I2C_acknowledgement + * @{ + */ + +#define I2C_Ack_Enable ((uint32_t)0x00000000) +#define I2C_Ack_Disable I2C_CR2_NACK + +#define IS_I2C_ACK(ACK) (((ACK) == I2C_Ack_Enable) || \ + ((ACK) == I2C_Ack_Disable)) +/** + * @} + */ + +/** @defgroup I2C_acknowledged_address + * @{ + */ + +#define I2C_AcknowledgedAddress_7bit ((uint32_t)0x00000000) +#define I2C_AcknowledgedAddress_10bit I2C_OAR1_OA1MODE + +#define IS_I2C_ACKNOWLEDGE_ADDRESS(ADDRESS) (((ADDRESS) == I2C_AcknowledgedAddress_7bit) || \ + ((ADDRESS) == I2C_AcknowledgedAddress_10bit)) +/** + * @} + */ + +/** @defgroup I2C_own_address1 + * @{ + */ + +#define IS_I2C_OWN_ADDRESS1(ADDRESS1) ((ADDRESS1) <= (uint32_t)0x000003FF) +/** + * @} + */ + +/** @defgroup I2C_transfer_direction + * @{ + */ + +#define I2C_Direction_Transmitter ((uint16_t)0x0000) +#define I2C_Direction_Receiver ((uint16_t)0x0400) + +#define IS_I2C_DIRECTION(DIRECTION) (((DIRECTION) == I2C_Direction_Transmitter) || \ + ((DIRECTION) == I2C_Direction_Receiver)) +/** + * @} + */ + +/** @defgroup I2C_DMA_transfer_requests + * @{ + */ + +#define I2C_DMAReq_Tx I2C_CR1_TXDMAEN +#define I2C_DMAReq_Rx I2C_CR1_RXDMAEN + +#define IS_I2C_DMA_REQ(REQ) ((((REQ) & (uint32_t)0xFFFF3FFF) == 0x00) && ((REQ) != 0x00)) +/** + * @} + */ + +/** @defgroup I2C_slave_address + * @{ + */ + +#define IS_I2C_SLAVE_ADDRESS(ADDRESS) ((ADDRESS) <= (uint16_t)0x03FF) +/** + * @} + */ + + +/** @defgroup I2C_own_address2 + * @{ + */ + +#define IS_I2C_OWN_ADDRESS2(ADDRESS2) ((ADDRESS2) <= (uint16_t)0x00FF) + +/** + * @} + */ + +/** @defgroup I2C_own_address2_mask + * @{ + */ + +#define I2C_OA2_NoMask ((uint8_t)0x00) +#define I2C_OA2_Mask01 ((uint8_t)0x01) +#define I2C_OA2_Mask02 ((uint8_t)0x02) +#define I2C_OA2_Mask03 ((uint8_t)0x03) +#define I2C_OA2_Mask04 ((uint8_t)0x04) +#define I2C_OA2_Mask05 ((uint8_t)0x05) +#define I2C_OA2_Mask06 ((uint8_t)0x06) +#define I2C_OA2_Mask07 ((uint8_t)0x07) + +#define IS_I2C_OWN_ADDRESS2_MASK(MASK) (((MASK) == I2C_OA2_NoMask) || \ + ((MASK) == I2C_OA2_Mask01) || \ + ((MASK) == I2C_OA2_Mask02) || \ + ((MASK) == I2C_OA2_Mask03) || \ + ((MASK) == I2C_OA2_Mask04) || \ + ((MASK) == I2C_OA2_Mask05) || \ + ((MASK) == I2C_OA2_Mask06) || \ + ((MASK) == I2C_OA2_Mask07)) + +/** + * @} + */ + +/** @defgroup I2C_timeout + * @{ + */ + +#define IS_I2C_TIMEOUT(TIMEOUT) ((TIMEOUT) <= (uint16_t)0x0FFF) + +/** + * @} + */ + +/** @defgroup I2C_registers + * @{ + */ + +#define I2C_Register_CR1 ((uint8_t)0x00) +#define I2C_Register_CR2 ((uint8_t)0x04) +#define I2C_Register_OAR1 ((uint8_t)0x08) +#define I2C_Register_OAR2 ((uint8_t)0x0C) +#define I2C_Register_TIMINGR ((uint8_t)0x10) +#define I2C_Register_TIMEOUTR ((uint8_t)0x14) +#define I2C_Register_ISR ((uint8_t)0x18) +#define I2C_Register_ICR ((uint8_t)0x1C) +#define I2C_Register_PECR ((uint8_t)0x20) +#define I2C_Register_RXDR ((uint8_t)0x24) +#define I2C_Register_TXDR ((uint8_t)0x28) + +#define IS_I2C_REGISTER(REGISTER) (((REGISTER) == I2C_Register_CR1) || \ + ((REGISTER) == I2C_Register_CR2) || \ + ((REGISTER) == I2C_Register_OAR1) || \ + ((REGISTER) == I2C_Register_OAR2) || \ + ((REGISTER) == I2C_Register_TIMINGR) || \ + ((REGISTER) == I2C_Register_TIMEOUTR) || \ + ((REGISTER) == I2C_Register_ISR) || \ + ((REGISTER) == I2C_Register_ICR) || \ + ((REGISTER) == I2C_Register_PECR) || \ + ((REGISTER) == I2C_Register_RXDR) || \ + ((REGISTER) == I2C_Register_TXDR)) +/** + * @} + */ + +/** @defgroup I2C_interrupts_definition + * @{ + */ + +#define I2C_IT_ERRI I2C_CR1_ERRIE +#define I2C_IT_TCI I2C_CR1_TCIE +#define I2C_IT_STOPI I2C_CR1_STOPIE +#define I2C_IT_NACKI I2C_CR1_NACKIE +#define I2C_IT_ADDRI I2C_CR1_ADDRIE +#define I2C_IT_RXI I2C_CR1_RXIE +#define I2C_IT_TXI I2C_CR1_TXIE + +#define IS_I2C_CONFIG_IT(IT) ((((IT) & (uint32_t)0xFFFFFF01) == 0x00) && ((IT) != 0x00)) + +/** + * @} + */ + +/** @defgroup I2C_flags_definition + * @{ + */ + +#define I2C_FLAG_TXE I2C_ISR_TXE +#define I2C_FLAG_TXIS I2C_ISR_TXIS +#define I2C_FLAG_RXNE I2C_ISR_RXNE +#define I2C_FLAG_ADDR I2C_ISR_ADDR +#define I2C_FLAG_NACKF I2C_ISR_NACKF +#define I2C_FLAG_STOPF I2C_ISR_STOPF +#define I2C_FLAG_TC I2C_ISR_TC +#define I2C_FLAG_TCR I2C_ISR_TCR +#define I2C_FLAG_BERR I2C_ISR_BERR +#define I2C_FLAG_ARLO I2C_ISR_ARLO +#define I2C_FLAG_OVR I2C_ISR_OVR +#define I2C_FLAG_PECERR I2C_ISR_PECERR +#define I2C_FLAG_TIMEOUT I2C_ISR_TIMEOUT +#define I2C_FLAG_ALERT I2C_ISR_ALERT +#define I2C_FLAG_BUSY I2C_ISR_BUSY + +#define IS_I2C_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFF4000) == 0x00) && ((FLAG) != 0x00)) + +#define IS_I2C_GET_FLAG(FLAG) (((FLAG) == I2C_FLAG_TXE) || ((FLAG) == I2C_FLAG_TXIS) || \ + ((FLAG) == I2C_FLAG_RXNE) || ((FLAG) == I2C_FLAG_ADDR) || \ + ((FLAG) == I2C_FLAG_NACKF) || ((FLAG) == I2C_FLAG_STOPF) || \ + ((FLAG) == I2C_FLAG_TC) || ((FLAG) == I2C_FLAG_TCR) || \ + ((FLAG) == I2C_FLAG_BERR) || ((FLAG) == I2C_FLAG_ARLO) || \ + ((FLAG) == I2C_FLAG_OVR) || ((FLAG) == I2C_FLAG_PECERR) || \ + ((FLAG) == I2C_FLAG_TIMEOUT) || ((FLAG) == I2C_FLAG_ALERT) || \ + ((FLAG) == I2C_FLAG_BUSY)) + +/** + * @} + */ + + +/** @defgroup I2C_interrupts_definition + * @{ + */ + +#define I2C_IT_TXIS I2C_ISR_TXIS +#define I2C_IT_RXNE I2C_ISR_RXNE +#define I2C_IT_ADDR I2C_ISR_ADDR +#define I2C_IT_NACKF I2C_ISR_NACKF +#define I2C_IT_STOPF I2C_ISR_STOPF +#define I2C_IT_TC I2C_ISR_TC +#define I2C_IT_TCR I2C_ISR_TCR +#define I2C_IT_BERR I2C_ISR_BERR +#define I2C_IT_ARLO I2C_ISR_ARLO +#define I2C_IT_OVR I2C_ISR_OVR +#define I2C_IT_PECERR I2C_ISR_PECERR +#define I2C_IT_TIMEOUT I2C_ISR_TIMEOUT +#define I2C_IT_ALERT I2C_ISR_ALERT + +#define IS_I2C_CLEAR_IT(IT) ((((IT) & (uint32_t)0xFFFFC001) == 0x00) && ((IT) != 0x00)) + +#define IS_I2C_GET_IT(IT) (((IT) == I2C_IT_TXIS) || ((IT) == I2C_IT_RXNE) || \ + ((IT) == I2C_IT_ADDR) || ((IT) == I2C_IT_NACKF) || \ + ((IT) == I2C_IT_STOPF) || ((IT) == I2C_IT_TC) || \ + ((IT) == I2C_IT_TCR) || ((IT) == I2C_IT_BERR) || \ + ((IT) == I2C_IT_ARLO) || ((IT) == I2C_IT_OVR) || \ + ((IT) == I2C_IT_PECERR) || ((IT) == I2C_IT_TIMEOUT) || \ + ((IT) == I2C_IT_ALERT)) + + +/** + * @} + */ + +/** @defgroup I2C_ReloadEndMode_definition + * @{ + */ + +#define I2C_Reload_Mode I2C_CR2_RELOAD +#define I2C_AutoEnd_Mode I2C_CR2_AUTOEND +#define I2C_SoftEnd_Mode ((uint32_t)0x00000000) + + +#define IS_RELOAD_END_MODE(MODE) (((MODE) == I2C_Reload_Mode) || \ + ((MODE) == I2C_AutoEnd_Mode) || \ + ((MODE) == I2C_SoftEnd_Mode)) + + +/** + * @} + */ + +/** @defgroup I2C_StartStopMode_definition + * @{ + */ + +#define I2C_No_StartStop ((uint32_t)0x00000000) +#define I2C_Generate_Stop I2C_CR2_STOP +#define I2C_Generate_Start_Read (uint32_t)(I2C_CR2_START | I2C_CR2_RD_WRN) +#define I2C_Generate_Start_Write I2C_CR2_START + + +#define IS_START_STOP_MODE(MODE) (((MODE) == I2C_Generate_Stop) || \ + ((MODE) == I2C_Generate_Start_Read) || \ + ((MODE) == I2C_Generate_Start_Write) || \ + ((MODE) == I2C_No_StartStop)) + + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + + +/* Initialization and Configuration functions *********************************/ +void I2C_DeInit(I2C_TypeDef* I2Cx); +void I2C_Init(I2C_TypeDef* I2Cx, I2C_InitTypeDef* I2C_InitStruct); +void I2C_StructInit(I2C_InitTypeDef* I2C_InitStruct); +void I2C_Cmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_SoftwareResetCmd(I2C_TypeDef* I2Cx); +void I2C_ITConfig(I2C_TypeDef* I2Cx, uint32_t I2C_IT, FunctionalState NewState); +void I2C_StretchClockCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_StopModeCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_DualAddressCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_OwnAddress2Config(I2C_TypeDef* I2Cx, uint16_t Address, uint8_t Mask); +void I2C_GeneralCallCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_SlaveByteControlCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_SlaveAddressConfig(I2C_TypeDef* I2Cx, uint16_t Address); +void I2C_10BitAddressingModeCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); + +/* Communications handling functions ******************************************/ +void I2C_AutoEndCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_ReloadCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_NumberOfBytesConfig(I2C_TypeDef* I2Cx, uint8_t Number_Bytes); +void I2C_MasterRequestConfig(I2C_TypeDef* I2Cx, uint16_t I2C_Direction); +void I2C_GenerateSTART(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_GenerateSTOP(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_10BitAddressHeaderCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_AcknowledgeConfig(I2C_TypeDef* I2Cx, FunctionalState NewState); +uint8_t I2C_GetAddressMatched(I2C_TypeDef* I2Cx); +uint16_t I2C_GetTransferDirection(I2C_TypeDef* I2Cx); +void I2C_TransferHandling(I2C_TypeDef* I2Cx, uint16_t Address, uint8_t Number_Bytes, uint32_t ReloadEndMode, uint32_t StartStopMode); + +/* SMBUS management functions ************************************************/ +void I2C_SMBusAlertCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_ClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_ExtendedClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_IdleClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_TimeoutAConfig(I2C_TypeDef* I2Cx, uint16_t Timeout); +void I2C_TimeoutBConfig(I2C_TypeDef* I2Cx, uint16_t Timeout); +void I2C_CalculatePEC(I2C_TypeDef* I2Cx, FunctionalState NewState); +void I2C_PECRequestCmd(I2C_TypeDef* I2Cx, FunctionalState NewState); +uint8_t I2C_GetPEC(I2C_TypeDef* I2Cx); + +/* I2C registers management functions *****************************************/ +uint32_t I2C_ReadRegister(I2C_TypeDef* I2Cx, uint8_t I2C_Register); + +/* Data transfers management functions ****************************************/ +void I2C_SendData(I2C_TypeDef* I2Cx, uint8_t Data); +uint8_t I2C_ReceiveData(I2C_TypeDef* I2Cx); + +/* DMA transfers management functions *****************************************/ +void I2C_DMACmd(I2C_TypeDef* I2Cx, uint32_t I2C_DMAReq, FunctionalState NewState); + +/* Interrupts and flags management functions **********************************/ +FlagStatus I2C_GetFlagStatus(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG); +void I2C_ClearFlag(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG); +ITStatus I2C_GetITStatus(I2C_TypeDef* I2Cx, uint32_t I2C_IT); +void I2C_ClearITPendingBit(I2C_TypeDef* I2Cx, uint32_t I2C_IT); + + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_I2C_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_iwdg.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_iwdg.h new file mode 100755 index 0000000000..519bda9f1e --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_iwdg.h @@ -0,0 +1,153 @@ +/** + ****************************************************************************** + * @file stm32f30x_iwdg.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the IWDG + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_IWDG_H +#define __STM32F30x_IWDG_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup IWDG + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup IWDG_Exported_Constants + * @{ + */ + +/** @defgroup IWDG_WriteAccess + * @{ + */ + +#define IWDG_WriteAccess_Enable ((uint16_t)0x5555) +#define IWDG_WriteAccess_Disable ((uint16_t)0x0000) +#define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \ + ((ACCESS) == IWDG_WriteAccess_Disable)) +/** + * @} + */ + +/** @defgroup IWDG_prescaler + * @{ + */ + +#define IWDG_Prescaler_4 ((uint8_t)0x00) +#define IWDG_Prescaler_8 ((uint8_t)0x01) +#define IWDG_Prescaler_16 ((uint8_t)0x02) +#define IWDG_Prescaler_32 ((uint8_t)0x03) +#define IWDG_Prescaler_64 ((uint8_t)0x04) +#define IWDG_Prescaler_128 ((uint8_t)0x05) +#define IWDG_Prescaler_256 ((uint8_t)0x06) +#define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \ + ((PRESCALER) == IWDG_Prescaler_8) || \ + ((PRESCALER) == IWDG_Prescaler_16) || \ + ((PRESCALER) == IWDG_Prescaler_32) || \ + ((PRESCALER) == IWDG_Prescaler_64) || \ + ((PRESCALER) == IWDG_Prescaler_128)|| \ + ((PRESCALER) == IWDG_Prescaler_256)) +/** + * @} + */ + +/** @defgroup IWDG_Flag + * @{ + */ + +#define IWDG_FLAG_PVU ((uint16_t)0x0001) +#define IWDG_FLAG_RVU ((uint16_t)0x0002) +#define IWDG_FLAG_WVU ((uint16_t)0x0002) +#define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU) || \ + ((FLAG) == IWDG_FLAG_WVU)) +/** + * @} + */ + +/** @defgroup IWDG_Reload_Value + * @{ + */ +#define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF) + +/** + * @} + */ + +/** @defgroup IWDG_CounterWindow_Value + * @{ + */ +#define IS_IWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0xFFF) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +/* Prescaler and Counter configuration functions ******************************/ +void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess); +void IWDG_SetPrescaler(uint8_t IWDG_Prescaler); +void IWDG_SetReload(uint16_t Reload); +void IWDG_ReloadCounter(void); +void IWDG_SetWindowValue(uint16_t WindowValue); + +/* IWDG activation function ***************************************************/ +void IWDG_Enable(void); + +/* Flag management function ***************************************************/ +FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_IWDG_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_misc.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_misc.h new file mode 100755 index 0000000000..7d66b7e0c6 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_misc.h @@ -0,0 +1,204 @@ +/** + ****************************************************************************** + * @file stm32f30x_misc.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the miscellaneous + * firmware library functions (add-on to CMSIS functions). + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_MISC_H +#define __STM32F30x_MISC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup MISC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief NVIC Init Structure definition + */ + +typedef struct +{ + uint8_t NVIC_IRQChannel; /*!< Specifies the IRQ channel to be enabled or disabled. + This parameter can be a value of @ref IRQn_Type (For + the complete STM32 Devices IRQ Channels list, please + refer to stm32f30x.h file) */ + + uint8_t NVIC_IRQChannelPreemptionPriority; /*!< Specifies the pre-emption priority for the IRQ channel + specified in NVIC_IRQChannel. This parameter can be a value + between 0 and 15. + A lower priority value indicates a higher priority */ + + + uint8_t NVIC_IRQChannelSubPriority; /*!< Specifies the subpriority level for the IRQ channel specified + in NVIC_IRQChannel. This parameter can be a value + between 0 and 15. + A lower priority value indicates a higher priority */ + + FunctionalState NVIC_IRQChannelCmd; /*!< Specifies whether the IRQ channel defined in NVIC_IRQChannel + will be enabled or disabled. + This parameter can be set either to ENABLE or DISABLE */ +} NVIC_InitTypeDef; + +/** + * +@verbatim + The table below gives the allowed values of the pre-emption priority and subpriority according + to the Priority Grouping configuration performed by NVIC_PriorityGroupConfig function + ============================================================================================================================ + NVIC_PriorityGroup | NVIC_IRQChannelPreemptionPriority | NVIC_IRQChannelSubPriority | Description + ============================================================================================================================ + NVIC_PriorityGroup_0 | 0 | 0-15 | 0 bits for pre-emption priority + | | | 4 bits for subpriority + ---------------------------------------------------------------------------------------------------------------------------- + NVIC_PriorityGroup_1 | 0-1 | 0-7 | 1 bits for pre-emption priority + | | | 3 bits for subpriority + ---------------------------------------------------------------------------------------------------------------------------- + NVIC_PriorityGroup_2 | 0-3 | 0-3 | 2 bits for pre-emption priority + | | | 2 bits for subpriority + ---------------------------------------------------------------------------------------------------------------------------- + NVIC_PriorityGroup_3 | 0-7 | 0-1 | 3 bits for pre-emption priority + | | | 1 bits for subpriority + ---------------------------------------------------------------------------------------------------------------------------- + NVIC_PriorityGroup_4 | 0-15 | 0 | 4 bits for pre-emption priority + | | | 0 bits for subpriority + ============================================================================================================================ +@endverbatim +*/ + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup MISC_Exported_Constants + * @{ + */ + +/** @defgroup MISC_Vector_Table_Base + * @{ + */ + +#define NVIC_VectTab_RAM ((uint32_t)0x20000000) +#define NVIC_VectTab_FLASH ((uint32_t)0x08000000) +#define IS_NVIC_VECTTAB(VECTTAB) (((VECTTAB) == NVIC_VectTab_RAM) || \ + ((VECTTAB) == NVIC_VectTab_FLASH)) +/** + * @} + */ + +/** @defgroup MISC_System_Low_Power + * @{ + */ + +#define NVIC_LP_SEVONPEND ((uint8_t)0x10) +#define NVIC_LP_SLEEPDEEP ((uint8_t)0x04) +#define NVIC_LP_SLEEPONEXIT ((uint8_t)0x02) +#define IS_NVIC_LP(LP) (((LP) == NVIC_LP_SEVONPEND) || \ + ((LP) == NVIC_LP_SLEEPDEEP) || \ + ((LP) == NVIC_LP_SLEEPONEXIT)) +/** + * @} + */ + +/** @defgroup MISC_Preemption_Priority_Group + * @{ + */ + +#define NVIC_PriorityGroup_0 ((uint32_t)0x700) /*!< 0 bits for pre-emption priority + 4 bits for subpriority */ +#define NVIC_PriorityGroup_1 ((uint32_t)0x600) /*!< 1 bits for pre-emption priority + 3 bits for subpriority */ +#define NVIC_PriorityGroup_2 ((uint32_t)0x500) /*!< 2 bits for pre-emption priority + 2 bits for subpriority */ +#define NVIC_PriorityGroup_3 ((uint32_t)0x400) /*!< 3 bits for pre-emption priority + 1 bits for subpriority */ +#define NVIC_PriorityGroup_4 ((uint32_t)0x300) /*!< 4 bits for pre-emption priority + 0 bits for subpriority */ + +#define IS_NVIC_PRIORITY_GROUP(GROUP) (((GROUP) == NVIC_PriorityGroup_0) || \ + ((GROUP) == NVIC_PriorityGroup_1) || \ + ((GROUP) == NVIC_PriorityGroup_2) || \ + ((GROUP) == NVIC_PriorityGroup_3) || \ + ((GROUP) == NVIC_PriorityGroup_4)) + +#define IS_NVIC_PREEMPTION_PRIORITY(PRIORITY) ((PRIORITY) < 0x10) + +#define IS_NVIC_SUB_PRIORITY(PRIORITY) ((PRIORITY) < 0x10) + +#define IS_NVIC_OFFSET(OFFSET) ((OFFSET) < 0x000FFFFF) + +/** + * @} + */ + +/** @defgroup MISC_SysTick_clock_source + */ + +#define SysTick_CLKSource_HCLK_Div8 ((uint32_t)0xFFFFFFFB) +#define SysTick_CLKSource_HCLK ((uint32_t)0x00000004) +#define IS_SYSTICK_CLK_SOURCE(SOURCE) (((SOURCE) == SysTick_CLKSource_HCLK) || \ + ((SOURCE) == SysTick_CLKSource_HCLK_Div8)) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup); +void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct); +void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset); +void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState); +void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_MISC_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_opamp.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_opamp.h new file mode 100755 index 0000000000..5edd6060ac --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_opamp.h @@ -0,0 +1,277 @@ +/** + ****************************************************************************** + * @file stm32f30x_opamp.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the operational + * amplifiers (OPAMP) firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_OPAMP_H +#define __STM32F30x_OPAMP_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup OPAMP + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief OPAMP Init structure definition + */ + +typedef struct +{ + + uint32_t OPAMP_InvertingInput; /*!< Selects the inverting input of the operational amplifier. + This parameter can be a value of @ref OPAMP_InvertingInput */ + + uint32_t OPAMP_NonInvertingInput; /*!< Selects the non inverting input of the operational amplifier. + This parameter can be a value of @ref OPAMP_NonInvertingInput */ + +}OPAMP_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup OPAMP_Exported_Constants + * @{ + */ + +/** @defgroup OPAMP_Selection + * @{ + */ + +#define OPAMP_Selection_OPAMP1 ((uint32_t)0x00000000) /*!< OPAMP1 Selection */ +#define OPAMP_Selection_OPAMP2 ((uint32_t)0x00000004) /*!< OPAMP2 Selection */ +#define OPAMP_Selection_OPAMP3 ((uint32_t)0x00000008) /*!< OPAMP3 Selection */ +#define OPAMP_Selection_OPAMP4 ((uint32_t)0x0000000C) /*!< OPAMP4 Selection */ + +#define IS_OPAMP_ALL_PERIPH(PERIPH) (((PERIPH) == OPAMP_Selection_OPAMP1) || \ + ((PERIPH) == OPAMP_Selection_OPAMP2) || \ + ((PERIPH) == OPAMP_Selection_OPAMP3) || \ + ((PERIPH) == OPAMP_Selection_OPAMP4)) + +/** + * @} + */ + +/** @defgroup OPAMP_InvertingInput + * @{ + */ + +#define OPAMP_InvertingInput_IO1 ((uint32_t)0x00000000) /*!< IO1 (PC5 for OPAMP1 and OPAMP2, PB10 for OPAMP3 and OPAMP4) + connected to OPAMPx inverting input */ +#define OPAMP_InvertingInput_IO2 OPAMP_CSR_VMSEL_0 /*!< IO2 (PA3 for OPAMP1, PA5 for OPAMP2, PB2 for OPAMP3, PD8 for OPAMP4) + connected to OPAMPx inverting input */ +#define OPAMP_InvertingInput_PGA OPAMP_CSR_VMSEL_1 /*!< Resistor feedback output connected to OPAMPx inverting input (PGA mode) */ +#define OPAMP_InvertingInput_Vout OPAMP_CSR_VMSEL /*!< Vout connected to OPAMPx inverting input (follower mode) */ + +#define IS_OPAMP_INVERTING_INPUT(INPUT) (((INPUT) == OPAMP_InvertingInput_IO1) || \ + ((INPUT) == OPAMP_InvertingInput_IO2) || \ + ((INPUT) == OPAMP_InvertingInput_PGA) || \ + ((INPUT) == OPAMP_InvertingInput_Vout)) +/** + * @} + */ + +/** @defgroup OPAMP_NonInvertingInput + * @{ + */ + +#define OPAMP_NonInvertingInput_IO1 ((uint32_t)0x00000000) /*!< IO1 (PA7 for OPAMP1, PD14 for OPAMP2, PB13 for OPAMP3, PD11 for OPAMP4) + connected to OPAMPx non inverting input */ +#define OPAMP_NonInvertingInput_IO2 OPAMP_CSR_VPSEL_0 /*!< IO2 (PA5 for OPAMP1, PB14 for OPAMP2, PA5 for OPAMP3, PB11 for OPAMP4) + connected to OPAMPx non inverting input */ +#define OPAMP_NonInvertingInput_IO3 OPAMP_CSR_VPSEL_1 /*!< IO3 (PA3 for OPAMP1, PB0 for OPAMP2, PA1 for OPAMP3, PA4 for OPAMP4) + connected to OPAMPx non inverting input */ +#define OPAMP_NonInvertingInput_IO4 OPAMP_CSR_VPSEL /*!< IO4 (PA1 for OPAMP1, PA7 for OPAMP2, PB0 for OPAMP3, PB13 for OPAMP4) + connected to OPAMPx non inverting input */ + +#define IS_OPAMP_NONINVERTING_INPUT(INPUT) (((INPUT) == OPAMP_NonInvertingInput_IO1) || \ + ((INPUT) == OPAMP_NonInvertingInput_IO2) || \ + ((INPUT) == OPAMP_NonInvertingInput_IO3) || \ + ((INPUT) == OPAMP_NonInvertingInput_IO4)) +/** + * @} + */ + +/** @defgroup OPAMP_PGAGain_Config + * @{ + */ + +#define OPAMP_OPAMP_PGAGain_2 ((uint32_t)0x00000000) +#define OPAMP_OPAMP_PGAGain_4 OPAMP_CSR_PGGAIN_0 +#define OPAMP_OPAMP_PGAGain_8 OPAMP_CSR_PGGAIN_1 +#define OPAMP_OPAMP_PGAGain_16 ((uint32_t)0x0000C000) + +#define IS_OPAMP_PGAGAIN(GAIN) (((GAIN) == OPAMP_OPAMP_PGAGain_2) || \ + ((GAIN) == OPAMP_OPAMP_PGAGain_4) || \ + ((GAIN) == OPAMP_OPAMP_PGAGain_8) || \ + ((GAIN) == OPAMP_OPAMP_PGAGain_16)) +/** + * @} + */ + +/** @defgroup OPAMP_PGAConnect_Config + * @{ + */ + +#define OPAMP_PGAConnect_No ((uint32_t)0x00000000) +#define OPAMP_PGAConnect_IO1 OPAMP_CSR_PGGAIN_3 +#define OPAMP_PGAConnect_IO2 ((uint32_t)0x00030000) + +#define IS_OPAMP_PGACONNECT(CONNECT) (((CONNECT) == OPAMP_PGAConnect_No) || \ + ((CONNECT) == OPAMP_PGAConnect_IO1) || \ + ((CONNECT) == OPAMP_PGAConnect_IO2)) +/** + * @} + */ + +/** @defgroup OPAMP_SecondaryInvertingInput + * @{ + */ + +#define IS_OPAMP_SECONDARY_INVINPUT(INVINPUT) (((INVINPUT) == OPAMP_InvertingInput_IO1) || \ + ((INVINPUT) == OPAMP_InvertingInput_IO2)) +/** + * @} + */ + +/** @defgroup OPAMP_Input + * @{ + */ + +#define OPAMP_Input_Inverting ((uint32_t)0x00000018) /*!< Inverting input */ +#define OPAMP_Input_NonInverting ((uint32_t)0x00000013) /*!< Non inverting input */ + +#define IS_OPAMP_INPUT(INPUT) (((INPUT) == OPAMP_Input_Inverting) || \ + ((INPUT) == OPAMP_Input_NonInverting)) + +/** + * @} + */ + +/** @defgroup OPAMP_Vref + * @{ + */ + +#define OPAMP_Vref_3VDDA ((uint32_t)0x00000000) /*!< OPMAP Vref = 3.3% VDDA */ +#define OPAMP_Vref_10VDDA OPAMP_CSR_CALSEL_0 /*!< OPMAP Vref = 10% VDDA */ +#define OPAMP_Vref_50VDDA OPAMP_CSR_CALSEL_1 /*!< OPMAP Vref = 50% VDDA */ +#define OPAMP_Vref_90VDDA OPAMP_CSR_CALSEL /*!< OPMAP Vref = 90% VDDA */ + +#define IS_OPAMP_VREF(VREF) (((VREF) == OPAMP_Vref_3VDDA) || \ + ((VREF) == OPAMP_Vref_10VDDA) || \ + ((VREF) == OPAMP_Vref_50VDDA) || \ + ((VREF) == OPAMP_Vref_90VDDA)) + +/** + * @} + */ + +/** @defgroup OPAMP_Trimming + */ + +#define OPAMP_Trimming_Factory ((uint32_t)0x00000000) /*!< Factory trimming */ +#define OPAMP_Trimming_User OPAMP_CSR_USERTRIM /*!< User trimming */ + +#define IS_OPAMP_TRIMMING(TRIMMING) (((TRIMMING) == OPAMP_Trimming_Factory) || \ + ((TRIMMING) == OPAMP_Trimming_User)) + +/** + * @} + */ + +/** @defgroup OPAMP_TrimValue + * @{ + */ + +#define IS_OPAMP_TRIMMINGVALUE(VALUE) ((VALUE) <= 0x0000001F) /*!< Trimming value */ + +/** + * @} + */ + +/** @defgroup OPAMP_OutputLevel + * @{ + */ + +#define OPAMP_OutputLevel_High OPAMP_CSR_OUTCAL +#define OPAMP_OutputLevel_Low ((uint32_t)0x00000000) + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the OPAMP configuration to the default reset state ***/ +void OPAMP_DeInit(uint32_t OPAMP_Selection); + +/* Initialization and Configuration functions *********************************/ +void OPAMP_Init(uint32_t OPAMP_Selection, OPAMP_InitTypeDef* OPAMP_InitStruct); +void OPAMP_StructInit(OPAMP_InitTypeDef* OPAMP_InitStruct); +void OPAMP_PGAConfig(uint32_t OPAMP_Selection, uint32_t OPAMP_PGAGain, uint32_t OPAMP_PGAConnect); +void OPAMP_VrefConfig(uint32_t OPAMP_Selection, uint32_t OPAMP_Vref); +void OPAMP_VrefConnectADCCmd(uint32_t OPAMP_Selection, FunctionalState NewState); +void OPAMP_TimerControlledMuxConfig(uint32_t OPAMP_Selection, OPAMP_InitTypeDef* OPAMP_InitStruct); +void OPAMP_TimerControlledMuxCmd(uint32_t OPAMP_Selection, FunctionalState NewState); +void OPAMP_Cmd(uint32_t OPAMP_Selection, FunctionalState NewState); +uint32_t OPAMP_GetOutputLevel(uint32_t OPAMP_Selection); + +/* Calibration functions ******************************************************/ +void OPAMP_VrefConnectNonInvertingInput(uint32_t OPAMP_Selection, FunctionalState NewState); +void OPAMP_OffsetTrimModeSelect(uint32_t OPAMP_Selection, uint32_t OPAMP_Trimming); +void OPAMP_OffsetTrimConfig(uint32_t OPAMP_Selection, uint32_t OPAMP_Input, uint32_t OPAMP_TrimValue); +void OPAMP_StartCalibration(uint32_t OPAMP_Selection, FunctionalState NewState); + +/* OPAMP configuration locking function ***************************************/ +void OPAMP_LockConfig(uint32_t OPAMP_Selection); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_OPAMP_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_pwr.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_pwr.h new file mode 100755 index 0000000000..dc96dedf7b --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_pwr.h @@ -0,0 +1,187 @@ +/** + ****************************************************************************** + * @file stm32f30x_pwr.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the PWR firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_PWR_H +#define __STM32F30x_PWR_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup PWR + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup PWR_Exported_Constants + * @{ + */ + +/** @defgroup PWR_PVD_detection_level + * @{ + */ + +#define PWR_PVDLevel_0 PWR_CR_PLS_LEV0 +#define PWR_PVDLevel_1 PWR_CR_PLS_LEV1 +#define PWR_PVDLevel_2 PWR_CR_PLS_LEV2 +#define PWR_PVDLevel_3 PWR_CR_PLS_LEV3 +#define PWR_PVDLevel_4 PWR_CR_PLS_LEV4 +#define PWR_PVDLevel_5 PWR_CR_PLS_LEV5 +#define PWR_PVDLevel_6 PWR_CR_PLS_LEV6 +#define PWR_PVDLevel_7 PWR_CR_PLS_LEV7 + +#define IS_PWR_PVD_LEVEL(LEVEL) (((LEVEL) == PWR_PVDLevel_0) || ((LEVEL) == PWR_PVDLevel_1)|| \ + ((LEVEL) == PWR_PVDLevel_2) || ((LEVEL) == PWR_PVDLevel_3)|| \ + ((LEVEL) == PWR_PVDLevel_4) || ((LEVEL) == PWR_PVDLevel_5)|| \ + ((LEVEL) == PWR_PVDLevel_6) || ((LEVEL) == PWR_PVDLevel_7)) +/** + * @} + */ + +/** @defgroup PWR_WakeUp_Pins + * @{ + */ + +#define PWR_WakeUpPin_1 PWR_CSR_EWUP1 +#define PWR_WakeUpPin_2 PWR_CSR_EWUP2 +#define PWR_WakeUpPin_3 PWR_CSR_EWUP3 +#define IS_PWR_WAKEUP_PIN(PIN) (((PIN) == PWR_WakeUpPin_1) || \ + ((PIN) == PWR_WakeUpPin_2) || \ + ((PIN) == PWR_WakeUpPin_3)) +/** + * @} + */ + + +/** @defgroup PWR_Regulator_state_is_Sleep_STOP_mode + * @{ + */ + +#define PWR_Regulator_ON ((uint32_t)0x00000000) +#define PWR_Regulator_LowPower PWR_CR_LPSDSR +#define IS_PWR_REGULATOR(REGULATOR) (((REGULATOR) == PWR_Regulator_ON) || \ + ((REGULATOR) == PWR_Regulator_LowPower)) +/** + * @} + */ + +/** @defgroup PWR_SLEEP_mode_entry + * @{ + */ + +#define PWR_SLEEPEntry_WFI ((uint8_t)0x01) +#define PWR_SLEEPEntry_WFE ((uint8_t)0x02) +#define IS_PWR_SLEEP_ENTRY(ENTRY) (((ENTRY) == PWR_SLEEPEntry_WFI) || ((ENTRY) == PWR_SLEEPEntry_WFE)) + +/** + * @} + */ + +/** @defgroup PWR_STOP_mode_entry + * @{ + */ + +#define PWR_STOPEntry_WFI ((uint8_t)0x01) +#define PWR_STOPEntry_WFE ((uint8_t)0x02) +#define IS_PWR_STOP_ENTRY(ENTRY) (((ENTRY) == PWR_STOPEntry_WFI) || ((ENTRY) == PWR_STOPEntry_WFE)) + +/** + * @} + */ + +/** @defgroup PWR_Flag + * @{ + */ + +#define PWR_FLAG_WU PWR_CSR_WUF +#define PWR_FLAG_SB PWR_CSR_SBF +#define PWR_FLAG_PVDO PWR_CSR_PVDO +#define PWR_FLAG_VREFINTRDY PWR_CSR_VREFINTRDYF + +#define IS_PWR_GET_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB) || \ + ((FLAG) == PWR_FLAG_PVDO) || ((FLAG) == PWR_FLAG_VREFINTRDY)) + +#define IS_PWR_CLEAR_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB)) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the PWR configuration to the default reset state ******/ +void PWR_DeInit(void); + +/* Backup Domain Access function **********************************************/ +void PWR_BackupAccessCmd(FunctionalState NewState); + +/* PVD configuration functions ************************************************/ +void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel); +void PWR_PVDCmd(FunctionalState NewState); + +/* WakeUp pins configuration functions ****************************************/ +void PWR_WakeUpPinCmd(uint32_t PWR_WakeUpPin, FunctionalState NewState); + +/* Low Power modes configuration functions ************************************/ +void PWR_EnterSleepMode(uint8_t PWR_SLEEPEntry); +void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry); +void PWR_EnterSTANDBYMode(void); + +/* Flags management functions *************************************************/ +FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG); +void PWR_ClearFlag(uint32_t PWR_FLAG); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_PWR_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_rcc.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_rcc.h new file mode 100755 index 0000000000..13391f6636 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_rcc.h @@ -0,0 +1,731 @@ +/** + ****************************************************************************** + * @file stm32f30x_rcc.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the RCC + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_RCC_H +#define __STM32F30x_RCC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup RCC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +typedef struct +{ + uint32_t SYSCLK_Frequency; + uint32_t HCLK_Frequency; + uint32_t PCLK1_Frequency; + uint32_t PCLK2_Frequency; + uint32_t ADC12CLK_Frequency; + uint32_t ADC34CLK_Frequency; + uint32_t I2C1CLK_Frequency; + uint32_t I2C2CLK_Frequency; + uint32_t I2C3CLK_Frequency; + uint32_t TIM1CLK_Frequency; + uint32_t HRTIM1CLK_Frequency; + uint32_t TIM8CLK_Frequency; + uint32_t TIM2CLK_Frequency; + uint32_t TIM3CLK_Frequency; + uint32_t USART1CLK_Frequency; + uint32_t USART2CLK_Frequency; + uint32_t USART3CLK_Frequency; + uint32_t UART4CLK_Frequency; + uint32_t UART5CLK_Frequency; + uint32_t TIM15CLK_Frequency; + uint32_t TIM16CLK_Frequency; + uint32_t TIM17CLK_Frequency; + uint32_t TIM20CLK_Frequency; +}RCC_ClocksTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup RCC_Exported_Constants + * @{ + */ + +/** @defgroup RCC_HSE_configuration + * @{ + */ + +#define RCC_HSE_OFF ((uint8_t)0x00) +#define RCC_HSE_ON ((uint8_t)0x01) +#define RCC_HSE_Bypass ((uint8_t)0x05) +#define IS_RCC_HSE(HSE) (((HSE) == RCC_HSE_OFF) || ((HSE) == RCC_HSE_ON) || \ + ((HSE) == RCC_HSE_Bypass)) + +/** + * @} + */ + +/** @defgroup RCC_PLL_Clock_Source + * @{ + */ +#define RCC_PLLSource_HSI RCC_CFGR_PLLSRC_HSI_PREDIV /*!< Only applicable for STM32F303xE devices */ +#define RCC_PLLSource_HSI_Div2 RCC_CFGR_PLLSRC_HSI_Div2 +#define RCC_PLLSource_PREDIV1 RCC_CFGR_PLLSRC_PREDIV1 + +#define IS_RCC_PLL_SOURCE(SOURCE) (((SOURCE) == RCC_PLLSource_HSI_Div2) || \ + ((SOURCE) == RCC_PLLSource_PREDIV1)|| \ + ((SOURCE) == RCC_PLLSource_HSI)) + +/** + * @} + */ + +/** @defgroup RCC_PLL_Multiplication_Factor + * @{ + */ + +#define RCC_PLLMul_2 RCC_CFGR_PLLMULL2 +#define RCC_PLLMul_3 RCC_CFGR_PLLMULL3 +#define RCC_PLLMul_4 RCC_CFGR_PLLMULL4 +#define RCC_PLLMul_5 RCC_CFGR_PLLMULL5 +#define RCC_PLLMul_6 RCC_CFGR_PLLMULL6 +#define RCC_PLLMul_7 RCC_CFGR_PLLMULL7 +#define RCC_PLLMul_8 RCC_CFGR_PLLMULL8 +#define RCC_PLLMul_9 RCC_CFGR_PLLMULL9 +#define RCC_PLLMul_10 RCC_CFGR_PLLMULL10 +#define RCC_PLLMul_11 RCC_CFGR_PLLMULL11 +#define RCC_PLLMul_12 RCC_CFGR_PLLMULL12 +#define RCC_PLLMul_13 RCC_CFGR_PLLMULL13 +#define RCC_PLLMul_14 RCC_CFGR_PLLMULL14 +#define RCC_PLLMul_15 RCC_CFGR_PLLMULL15 +#define RCC_PLLMul_16 RCC_CFGR_PLLMULL16 +#define IS_RCC_PLL_MUL(MUL) (((MUL) == RCC_PLLMul_2) || ((MUL) == RCC_PLLMul_3) || \ + ((MUL) == RCC_PLLMul_4) || ((MUL) == RCC_PLLMul_5) || \ + ((MUL) == RCC_PLLMul_6) || ((MUL) == RCC_PLLMul_7) || \ + ((MUL) == RCC_PLLMul_8) || ((MUL) == RCC_PLLMul_9) || \ + ((MUL) == RCC_PLLMul_10) || ((MUL) == RCC_PLLMul_11) || \ + ((MUL) == RCC_PLLMul_12) || ((MUL) == RCC_PLLMul_13) || \ + ((MUL) == RCC_PLLMul_14) || ((MUL) == RCC_PLLMul_15) || \ + ((MUL) == RCC_PLLMul_16)) +/** + * @} + */ + +/** @defgroup RCC_PREDIV1_division_factor + * @{ + */ +#define RCC_PREDIV1_Div1 RCC_CFGR2_PREDIV1_DIV1 +#define RCC_PREDIV1_Div2 RCC_CFGR2_PREDIV1_DIV2 +#define RCC_PREDIV1_Div3 RCC_CFGR2_PREDIV1_DIV3 +#define RCC_PREDIV1_Div4 RCC_CFGR2_PREDIV1_DIV4 +#define RCC_PREDIV1_Div5 RCC_CFGR2_PREDIV1_DIV5 +#define RCC_PREDIV1_Div6 RCC_CFGR2_PREDIV1_DIV6 +#define RCC_PREDIV1_Div7 RCC_CFGR2_PREDIV1_DIV7 +#define RCC_PREDIV1_Div8 RCC_CFGR2_PREDIV1_DIV8 +#define RCC_PREDIV1_Div9 RCC_CFGR2_PREDIV1_DIV9 +#define RCC_PREDIV1_Div10 RCC_CFGR2_PREDIV1_DIV10 +#define RCC_PREDIV1_Div11 RCC_CFGR2_PREDIV1_DIV11 +#define RCC_PREDIV1_Div12 RCC_CFGR2_PREDIV1_DIV12 +#define RCC_PREDIV1_Div13 RCC_CFGR2_PREDIV1_DIV13 +#define RCC_PREDIV1_Div14 RCC_CFGR2_PREDIV1_DIV14 +#define RCC_PREDIV1_Div15 RCC_CFGR2_PREDIV1_DIV15 +#define RCC_PREDIV1_Div16 RCC_CFGR2_PREDIV1_DIV16 + +#define IS_RCC_PREDIV1(PREDIV1) (((PREDIV1) == RCC_PREDIV1_Div1) || ((PREDIV1) == RCC_PREDIV1_Div2) || \ + ((PREDIV1) == RCC_PREDIV1_Div3) || ((PREDIV1) == RCC_PREDIV1_Div4) || \ + ((PREDIV1) == RCC_PREDIV1_Div5) || ((PREDIV1) == RCC_PREDIV1_Div6) || \ + ((PREDIV1) == RCC_PREDIV1_Div7) || ((PREDIV1) == RCC_PREDIV1_Div8) || \ + ((PREDIV1) == RCC_PREDIV1_Div9) || ((PREDIV1) == RCC_PREDIV1_Div10) || \ + ((PREDIV1) == RCC_PREDIV1_Div11) || ((PREDIV1) == RCC_PREDIV1_Div12) || \ + ((PREDIV1) == RCC_PREDIV1_Div13) || ((PREDIV1) == RCC_PREDIV1_Div14) || \ + ((PREDIV1) == RCC_PREDIV1_Div15) || ((PREDIV1) == RCC_PREDIV1_Div16)) +/** + * @} + */ + +/** @defgroup RCC_System_Clock_Source + * @{ + */ + +#define RCC_SYSCLKSource_HSI RCC_CFGR_SW_HSI +#define RCC_SYSCLKSource_HSE RCC_CFGR_SW_HSE +#define RCC_SYSCLKSource_PLLCLK RCC_CFGR_SW_PLL +#define IS_RCC_SYSCLK_SOURCE(SOURCE) (((SOURCE) == RCC_SYSCLKSource_HSI) || \ + ((SOURCE) == RCC_SYSCLKSource_HSE) || \ + ((SOURCE) == RCC_SYSCLKSource_PLLCLK)) +/** + * @} + */ + +/** @defgroup RCC_AHB_Clock_Source + * @{ + */ + +#define RCC_SYSCLK_Div1 RCC_CFGR_HPRE_DIV1 +#define RCC_SYSCLK_Div2 RCC_CFGR_HPRE_DIV2 +#define RCC_SYSCLK_Div4 RCC_CFGR_HPRE_DIV4 +#define RCC_SYSCLK_Div8 RCC_CFGR_HPRE_DIV8 +#define RCC_SYSCLK_Div16 RCC_CFGR_HPRE_DIV16 +#define RCC_SYSCLK_Div64 RCC_CFGR_HPRE_DIV64 +#define RCC_SYSCLK_Div128 RCC_CFGR_HPRE_DIV128 +#define RCC_SYSCLK_Div256 RCC_CFGR_HPRE_DIV256 +#define RCC_SYSCLK_Div512 RCC_CFGR_HPRE_DIV512 +#define IS_RCC_HCLK(HCLK) (((HCLK) == RCC_SYSCLK_Div1) || ((HCLK) == RCC_SYSCLK_Div2) || \ + ((HCLK) == RCC_SYSCLK_Div4) || ((HCLK) == RCC_SYSCLK_Div8) || \ + ((HCLK) == RCC_SYSCLK_Div16) || ((HCLK) == RCC_SYSCLK_Div64) || \ + ((HCLK) == RCC_SYSCLK_Div128) || ((HCLK) == RCC_SYSCLK_Div256) || \ + ((HCLK) == RCC_SYSCLK_Div512)) +/** + * @} + */ + +/** @defgroup RCC_APB1_APB2_clock_source + * @{ + */ + +#define RCC_HCLK_Div1 ((uint32_t)0x00000000) +#define RCC_HCLK_Div2 ((uint32_t)0x00000400) +#define RCC_HCLK_Div4 ((uint32_t)0x00000500) +#define RCC_HCLK_Div8 ((uint32_t)0x00000600) +#define RCC_HCLK_Div16 ((uint32_t)0x00000700) +#define IS_RCC_PCLK(PCLK) (((PCLK) == RCC_HCLK_Div1) || ((PCLK) == RCC_HCLK_Div2) || \ + ((PCLK) == RCC_HCLK_Div4) || ((PCLK) == RCC_HCLK_Div8) || \ + ((PCLK) == RCC_HCLK_Div16)) +/** + * @} + */ + +/** @defgroup RCC_ADC_clock_source + * @{ + */ + +/* ADC1 & ADC2 */ +#define RCC_ADC12PLLCLK_OFF ((uint32_t)0x00000000) +#define RCC_ADC12PLLCLK_Div1 ((uint32_t)0x00000100) +#define RCC_ADC12PLLCLK_Div2 ((uint32_t)0x00000110) +#define RCC_ADC12PLLCLK_Div4 ((uint32_t)0x00000120) +#define RCC_ADC12PLLCLK_Div6 ((uint32_t)0x00000130) +#define RCC_ADC12PLLCLK_Div8 ((uint32_t)0x00000140) +#define RCC_ADC12PLLCLK_Div10 ((uint32_t)0x00000150) +#define RCC_ADC12PLLCLK_Div12 ((uint32_t)0x00000160) +#define RCC_ADC12PLLCLK_Div16 ((uint32_t)0x00000170) +#define RCC_ADC12PLLCLK_Div32 ((uint32_t)0x00000180) +#define RCC_ADC12PLLCLK_Div64 ((uint32_t)0x00000190) +#define RCC_ADC12PLLCLK_Div128 ((uint32_t)0x000001A0) +#define RCC_ADC12PLLCLK_Div256 ((uint32_t)0x000001B0) + +/* ADC3 & ADC4 */ +#define RCC_ADC34PLLCLK_OFF ((uint32_t)0x10000000) +#define RCC_ADC34PLLCLK_Div1 ((uint32_t)0x10002000) +#define RCC_ADC34PLLCLK_Div2 ((uint32_t)0x10002200) +#define RCC_ADC34PLLCLK_Div4 ((uint32_t)0x10002400) +#define RCC_ADC34PLLCLK_Div6 ((uint32_t)0x10002600) +#define RCC_ADC34PLLCLK_Div8 ((uint32_t)0x10002800) +#define RCC_ADC34PLLCLK_Div10 ((uint32_t)0x10002A00) +#define RCC_ADC34PLLCLK_Div12 ((uint32_t)0x10002C00) +#define RCC_ADC34PLLCLK_Div16 ((uint32_t)0x10002E00) +#define RCC_ADC34PLLCLK_Div32 ((uint32_t)0x10003000) +#define RCC_ADC34PLLCLK_Div64 ((uint32_t)0x10003200) +#define RCC_ADC34PLLCLK_Div128 ((uint32_t)0x10003400) +#define RCC_ADC34PLLCLK_Div256 ((uint32_t)0x10003600) + +#define IS_RCC_ADCCLK(ADCCLK) (((ADCCLK) == RCC_ADC12PLLCLK_OFF) || ((ADCCLK) == RCC_ADC12PLLCLK_Div1) || \ + ((ADCCLK) == RCC_ADC12PLLCLK_Div2) || ((ADCCLK) == RCC_ADC12PLLCLK_Div4) || \ + ((ADCCLK) == RCC_ADC12PLLCLK_Div6) || ((ADCCLK) == RCC_ADC12PLLCLK_Div8) || \ + ((ADCCLK) == RCC_ADC12PLLCLK_Div10) || ((ADCCLK) == RCC_ADC12PLLCLK_Div12) || \ + ((ADCCLK) == RCC_ADC12PLLCLK_Div16) || ((ADCCLK) == RCC_ADC12PLLCLK_Div32) || \ + ((ADCCLK) == RCC_ADC12PLLCLK_Div64) || ((ADCCLK) == RCC_ADC12PLLCLK_Div128) || \ + ((ADCCLK) == RCC_ADC12PLLCLK_Div256) || ((ADCCLK) == RCC_ADC34PLLCLK_OFF) || \ + ((ADCCLK) == RCC_ADC34PLLCLK_Div1) || ((ADCCLK) == RCC_ADC34PLLCLK_Div2) || \ + ((ADCCLK) == RCC_ADC34PLLCLK_Div4) || ((ADCCLK) == RCC_ADC34PLLCLK_Div6) || \ + ((ADCCLK) == RCC_ADC34PLLCLK_Div8) || ((ADCCLK) == RCC_ADC34PLLCLK_Div10) || \ + ((ADCCLK) == RCC_ADC34PLLCLK_Div12) || ((ADCCLK) == RCC_ADC34PLLCLK_Div16) || \ + ((ADCCLK) == RCC_ADC34PLLCLK_Div32) || ((ADCCLK) == RCC_ADC34PLLCLK_Div64) || \ + ((ADCCLK) == RCC_ADC34PLLCLK_Div128) || ((ADCCLK) == RCC_ADC34PLLCLK_Div256)) + +/** + * @} + */ + +/** @defgroup RCC_TIM_clock_source + * @{ + */ + +#define RCC_TIM1CLK_PCLK ((uint32_t)0x00000000) +#define RCC_TIM1CLK_PLLCLK RCC_CFGR3_TIM1SW + +#define RCC_TIM8CLK_PCLK ((uint32_t)0x10000000) +#define RCC_TIM8CLK_PLLCLK ((uint32_t)0x10000200) + +#define RCC_TIM15CLK_PCLK ((uint32_t)0x20000000) +#define RCC_TIM15CLK_PLLCLK ((uint32_t)0x20000400) + +#define RCC_TIM16CLK_PCLK ((uint32_t)0x30000000) +#define RCC_TIM16CLK_PLLCLK ((uint32_t)0x30000800) + +#define RCC_TIM17CLK_PCLK ((uint32_t)0x40000000) +#define RCC_TIM17CLK_PLLCLK ((uint32_t)0x40002000) + +#define RCC_TIM20CLK_PCLK ((uint32_t)0x50000000) +#define RCC_TIM20CLK_PLLCLK ((uint32_t)0x50008000) + +#define RCC_TIM2CLK_PCLK ((uint32_t)0x60000000) +#define RCC_TIM2CLK_PLLCLK ((uint32_t)0x61000000) + +#define RCC_TIM3TIM4CLK_PCLK ((uint32_t)0x70000000) +#define RCC_TIM3TIM4CLK_PLLCLK ((uint32_t)0x72000000) + +#define IS_RCC_TIMCLK(TIMCLK) (((TIMCLK) == RCC_TIM1CLK_PCLK) || ((TIMCLK) == RCC_TIM1CLK_PLLCLK) || \ + ((TIMCLK) == RCC_TIM8CLK_PCLK) || ((TIMCLK) == RCC_TIM8CLK_PLLCLK) || \ + ((TIMCLK) == RCC_TIM15CLK_PCLK) || ((TIMCLK) == RCC_TIM15CLK_PLLCLK) || \ + ((TIMCLK) == RCC_TIM16CLK_PCLK) || ((TIMCLK) == RCC_TIM16CLK_PLLCLK) || \ + ((TIMCLK) == RCC_TIM17CLK_PCLK) || ((TIMCLK) == RCC_TIM17CLK_PLLCLK)|| \ + ((TIMCLK) == RCC_TIM20CLK_PCLK) || ((TIMCLK) == RCC_TIM20CLK_PLLCLK)|| \ + ((TIMCLK) == RCC_TIM2CLK_PCLK) || ((TIMCLK) == RCC_TIM2CLK_PLLCLK)|| \ + ((TIMCLK) == RCC_TIM3TIM4CLK_PCLK) || ((TIMCLK) == RCC_TIM3TIM4CLK_PLLCLK)) +/* legacy RCC_TIM_clock_source*/ +#define RCC_TIM1CLK_HCLK RCC_TIM1CLK_PCLK +#define RCC_TIM8CLK_HCLK RCC_TIM8CLK_PCLK +#define RCC_TIM15CLK_HCLK RCC_TIM15CLK_PCLK +#define RCC_TIM16CLK_HCLK RCC_TIM16CLK_PCLK +#define RCC_TIM17CLK_HCLK RCC_TIM17CLK_PCLK +#define RCC_TIM20CLK_HCLK RCC_TIM20CLK_PCLK +#define RCC_TIM2CLK_HCLK RCC_TIM2CLK_PCLK +#define RCC_TIM3CLK_HCLK RCC_TIM3TIM4CLK_PCLK +#define RCC_TIM3CLK_PLLCLK RCC_TIM3TIM4CLK_PLLCLK +/** + * @} + */ + +/** @defgroup RCC_HRTIM_clock_source + * @{ + */ + +#define RCC_HRTIM1CLK_HCLK ((uint32_t)0x00000000) +#define RCC_HRTIM1CLK_PLLCLK RCC_CFGR3_HRTIM1SW + +#define IS_RCC_HRTIMCLK(HRTIMCLK) (((HRTIMCLK) == RCC_HRTIM1CLK_HCLK) || ((HRTIMCLK) == RCC_HRTIM1CLK_PLLCLK)) + +/** + * @} + */ + +/** @defgroup RCC_I2C_clock_source + * @{ + */ + +#define RCC_I2C1CLK_HSI ((uint32_t)0x00000000) +#define RCC_I2C1CLK_SYSCLK RCC_CFGR3_I2C1SW + +#define RCC_I2C2CLK_HSI ((uint32_t)0x10000000) +#define RCC_I2C2CLK_SYSCLK ((uint32_t)0x10000020) + +#define RCC_I2C3CLK_HSI ((uint32_t)0x20000000) +#define RCC_I2C3CLK_SYSCLK ((uint32_t)0x20000040) + +#define IS_RCC_I2CCLK(I2CCLK) (((I2CCLK) == RCC_I2C1CLK_HSI) || ((I2CCLK) == RCC_I2C1CLK_SYSCLK) || \ + ((I2CCLK) == RCC_I2C2CLK_HSI) || ((I2CCLK) == RCC_I2C2CLK_SYSCLK) || \ + ((I2CCLK) == RCC_I2C3CLK_HSI) || ((I2CCLK) == RCC_I2C3CLK_SYSCLK)) + +/** + * @} + */ + +/** @defgroup RCC_USART_clock_source + * @{ + */ + +#define RCC_USART1CLK_PCLK ((uint32_t)0x10000000) +#define RCC_USART1CLK_SYSCLK ((uint32_t)0x10000001) +#define RCC_USART1CLK_LSE ((uint32_t)0x10000002) +#define RCC_USART1CLK_HSI ((uint32_t)0x10000003) + +#define RCC_USART2CLK_PCLK ((uint32_t)0x20000000) +#define RCC_USART2CLK_SYSCLK ((uint32_t)0x20010000) +#define RCC_USART2CLK_LSE ((uint32_t)0x20020000) +#define RCC_USART2CLK_HSI ((uint32_t)0x20030000) + +#define RCC_USART3CLK_PCLK ((uint32_t)0x30000000) +#define RCC_USART3CLK_SYSCLK ((uint32_t)0x30040000) +#define RCC_USART3CLK_LSE ((uint32_t)0x30080000) +#define RCC_USART3CLK_HSI ((uint32_t)0x300C0000) + +#define RCC_UART4CLK_PCLK ((uint32_t)0x40000000) +#define RCC_UART4CLK_SYSCLK ((uint32_t)0x40100000) +#define RCC_UART4CLK_LSE ((uint32_t)0x40200000) +#define RCC_UART4CLK_HSI ((uint32_t)0x40300000) + +#define RCC_UART5CLK_PCLK ((uint32_t)0x50000000) +#define RCC_UART5CLK_SYSCLK ((uint32_t)0x50400000) +#define RCC_UART5CLK_LSE ((uint32_t)0x50800000) +#define RCC_UART5CLK_HSI ((uint32_t)0x50C00000) + +#define IS_RCC_USARTCLK(USARTCLK) (((USARTCLK) == RCC_USART1CLK_PCLK) || ((USARTCLK) == RCC_USART1CLK_SYSCLK) || \ + ((USARTCLK) == RCC_USART1CLK_LSE) || ((USARTCLK) == RCC_USART1CLK_HSI) ||\ + ((USARTCLK) == RCC_USART2CLK_PCLK) || ((USARTCLK) == RCC_USART2CLK_SYSCLK) || \ + ((USARTCLK) == RCC_USART2CLK_LSE) || ((USARTCLK) == RCC_USART2CLK_HSI) || \ + ((USARTCLK) == RCC_USART3CLK_PCLK) || ((USARTCLK) == RCC_USART3CLK_SYSCLK) || \ + ((USARTCLK) == RCC_USART3CLK_LSE) || ((USARTCLK) == RCC_USART3CLK_HSI) || \ + ((USARTCLK) == RCC_UART4CLK_PCLK) || ((USARTCLK) == RCC_UART4CLK_SYSCLK) || \ + ((USARTCLK) == RCC_UART4CLK_LSE) || ((USARTCLK) == RCC_UART4CLK_HSI) || \ + ((USARTCLK) == RCC_UART5CLK_PCLK) || ((USARTCLK) == RCC_UART5CLK_SYSCLK) || \ + ((USARTCLK) == RCC_UART5CLK_LSE) || ((USARTCLK) == RCC_UART5CLK_HSI)) + +/** + * @} + */ + +/** @defgroup RCC_Interrupt_Source + * @{ + */ + +#define RCC_IT_LSIRDY ((uint8_t)0x01) +#define RCC_IT_LSERDY ((uint8_t)0x02) +#define RCC_IT_HSIRDY ((uint8_t)0x04) +#define RCC_IT_HSERDY ((uint8_t)0x08) +#define RCC_IT_PLLRDY ((uint8_t)0x10) +#define RCC_IT_CSS ((uint8_t)0x80) + +#define IS_RCC_IT(IT) ((((IT) & (uint8_t)0xC0) == 0x00) && ((IT) != 0x00)) + +#define IS_RCC_GET_IT(IT) (((IT) == RCC_IT_LSIRDY) || ((IT) == RCC_IT_LSERDY) || \ + ((IT) == RCC_IT_HSIRDY) || ((IT) == RCC_IT_HSERDY) || \ + ((IT) == RCC_IT_PLLRDY) || ((IT) == RCC_IT_CSS)) + + +#define IS_RCC_CLEAR_IT(IT) ((((IT) & (uint8_t)0x40) == 0x00) && ((IT) != 0x00)) + +/** + * @} + */ + +/** @defgroup RCC_LSE_configuration + * @{ + */ + +#define RCC_LSE_OFF ((uint32_t)0x00000000) +#define RCC_LSE_ON RCC_BDCR_LSEON +#define RCC_LSE_Bypass ((uint32_t)(RCC_BDCR_LSEON | RCC_BDCR_LSEBYP)) +#define IS_RCC_LSE(LSE) (((LSE) == RCC_LSE_OFF) || ((LSE) == RCC_LSE_ON) || \ + ((LSE) == RCC_LSE_Bypass)) +/** + * @} + */ + +/** @defgroup RCC_RTC_Clock_Source + * @{ + */ + +#define RCC_RTCCLKSource_LSE RCC_BDCR_RTCSEL_LSE +#define RCC_RTCCLKSource_LSI RCC_BDCR_RTCSEL_LSI +#define RCC_RTCCLKSource_HSE_Div32 RCC_BDCR_RTCSEL_HSE + +#define IS_RCC_RTCCLK_SOURCE(SOURCE) (((SOURCE) == RCC_RTCCLKSource_LSE) || \ + ((SOURCE) == RCC_RTCCLKSource_LSI) || \ + ((SOURCE) == RCC_RTCCLKSource_HSE_Div32)) +/** + * @} + */ + +/** @defgroup RCC_I2S_Clock_Source + * @{ + */ +#define RCC_I2S2CLKSource_SYSCLK ((uint8_t)0x00) +#define RCC_I2S2CLKSource_Ext ((uint8_t)0x01) + +#define IS_RCC_I2SCLK_SOURCE(SOURCE) (((SOURCE) == RCC_I2S2CLKSource_SYSCLK) || ((SOURCE) == RCC_I2S2CLKSource_Ext)) + +/** @defgroup RCC_LSE_Drive_Configuration + * @{ + */ + +#define RCC_LSEDrive_Low ((uint32_t)0x00000000) +#define RCC_LSEDrive_MediumLow RCC_BDCR_LSEDRV_0 +#define RCC_LSEDrive_MediumHigh RCC_BDCR_LSEDRV_1 +#define RCC_LSEDrive_High RCC_BDCR_LSEDRV +#define IS_RCC_LSE_DRIVE(DRIVE) (((DRIVE) == RCC_LSEDrive_Low) || ((DRIVE) == RCC_LSEDrive_MediumLow) || \ + ((DRIVE) == RCC_LSEDrive_MediumHigh) || ((DRIVE) == RCC_LSEDrive_High)) +/** + * @} + */ + +/** @defgroup RCC_AHB_Peripherals + * @{ + */ + +#define RCC_AHBPeriph_ADC34 RCC_AHBENR_ADC34EN +#define RCC_AHBPeriph_ADC12 RCC_AHBENR_ADC12EN +#define RCC_AHBPeriph_GPIOA RCC_AHBENR_GPIOAEN +#define RCC_AHBPeriph_GPIOB RCC_AHBENR_GPIOBEN +#define RCC_AHBPeriph_GPIOC RCC_AHBENR_GPIOCEN +#define RCC_AHBPeriph_GPIOD RCC_AHBENR_GPIODEN +#define RCC_AHBPeriph_GPIOE RCC_AHBENR_GPIOEEN +#define RCC_AHBPeriph_GPIOF RCC_AHBENR_GPIOFEN +#define RCC_AHBPeriph_GPIOG RCC_AHBENR_GPIOGEN +#define RCC_AHBPeriph_GPIOH RCC_AHBENR_GPIOHEN +#define RCC_AHBPeriph_TS RCC_AHBENR_TSEN +#define RCC_AHBPeriph_CRC RCC_AHBENR_CRCEN +#define RCC_AHBPeriph_FMC RCC_AHBENR_FMCEN +#define RCC_AHBPeriph_FLITF RCC_AHBENR_FLITFEN +#define RCC_AHBPeriph_SRAM RCC_AHBENR_SRAMEN +#define RCC_AHBPeriph_DMA2 RCC_AHBENR_DMA2EN +#define RCC_AHBPeriph_DMA1 RCC_AHBENR_DMA1EN + +#define IS_RCC_AHB_PERIPH(PERIPH) ((((PERIPH) & 0xCE00FF88) == 0x00) && ((PERIPH) != 0x00)) +#define IS_RCC_AHB_RST_PERIPH(PERIPH) ((((PERIPH) & 0xCE00FFDF) == 0x00) && ((PERIPH) != 0x00)) + +/** + * @} + */ + +/** @defgroup RCC_APB2_Peripherals + * @{ + */ + +#define RCC_APB2Periph_SYSCFG RCC_APB2ENR_SYSCFGEN +#define RCC_APB2Periph_TIM1 RCC_APB2ENR_TIM1EN +#define RCC_APB2Periph_SPI1 RCC_APB2ENR_SPI1EN +#define RCC_APB2Periph_TIM8 RCC_APB2ENR_TIM8EN +#define RCC_APB2Periph_USART1 RCC_APB2ENR_USART1EN +#define RCC_APB2Periph_SPI4 RCC_APB2ENR_SPI4EN +#define RCC_APB2Periph_TIM15 RCC_APB2ENR_TIM15EN +#define RCC_APB2Periph_TIM16 RCC_APB2ENR_TIM16EN +#define RCC_APB2Periph_TIM17 RCC_APB2ENR_TIM17EN +#define RCC_APB2Periph_TIM20 RCC_APB2ENR_TIM20EN +#define RCC_APB2Periph_HRTIM1 RCC_APB2ENR_HRTIM1 + +#define IS_RCC_APB2_PERIPH(PERIPH) ((((PERIPH) & 0xDFE807FE) == 0x00) && ((PERIPH) != 0x00)) + +/** + * @} + */ + +/** @defgroup RCC_APB1_Peripherals + * @{ + */ +#define RCC_APB1Periph_TIM2 RCC_APB1ENR_TIM2EN +#define RCC_APB1Periph_TIM3 RCC_APB1ENR_TIM3EN +#define RCC_APB1Periph_TIM4 RCC_APB1ENR_TIM4EN +#define RCC_APB1Periph_TIM6 RCC_APB1ENR_TIM6EN +#define RCC_APB1Periph_TIM7 RCC_APB1ENR_TIM7EN +#define RCC_APB1Periph_WWDG RCC_APB1ENR_WWDGEN +#define RCC_APB1Periph_SPI2 RCC_APB1ENR_SPI2EN +#define RCC_APB1Periph_SPI3 RCC_APB1ENR_SPI3EN +#define RCC_APB1Periph_USART2 RCC_APB1ENR_USART2EN +#define RCC_APB1Periph_USART3 RCC_APB1ENR_USART3EN +#define RCC_APB1Periph_UART4 RCC_APB1ENR_UART4EN +#define RCC_APB1Periph_UART5 RCC_APB1ENR_UART5EN +#define RCC_APB1Periph_I2C1 RCC_APB1ENR_I2C1EN +#define RCC_APB1Periph_I2C2 RCC_APB1ENR_I2C2EN +#define RCC_APB1Periph_USB RCC_APB1ENR_USBEN +#define RCC_APB1Periph_CAN1 RCC_APB1ENR_CAN1EN +#define RCC_APB1Periph_PWR RCC_APB1ENR_PWREN +#define RCC_APB1Periph_DAC1 RCC_APB1ENR_DAC1EN +#define RCC_APB1Periph_I2C3 RCC_APB1ENR_I2C3EN +#define RCC_APB1Periph_DAC2 RCC_APB1ENR_DAC2EN +#define RCC_APB1Periph_DAC RCC_APB1Periph_DAC1 + + +#define IS_RCC_APB1_PERIPH(PERIPH) ((((PERIPH) & 0x890137C8) == 0x00) && ((PERIPH) != 0x00)) +/** + * @} + */ + +/** @defgroup RCC_MCO_Clock_Source + * @{ + */ + +#define RCC_MCOSource_NoClock ((uint8_t)0x00) +#define RCC_MCOSource_LSI ((uint8_t)0x02) +#define RCC_MCOSource_LSE ((uint8_t)0x03) +#define RCC_MCOSource_SYSCLK ((uint8_t)0x04) +#define RCC_MCOSource_HSI ((uint8_t)0x05) +#define RCC_MCOSource_HSE ((uint8_t)0x06) +#define RCC_MCOSource_PLLCLK_Div2 ((uint8_t)0x07) +#define RCC_MCOSource_PLLCLK ((uint8_t)0x87) + +#define IS_RCC_MCO_SOURCE(SOURCE) (((SOURCE) == RCC_MCOSource_NoClock) ||((SOURCE) == RCC_MCOSource_SYSCLK) ||\ + ((SOURCE) == RCC_MCOSource_HSI) || ((SOURCE) == RCC_MCOSource_HSE) || \ + ((SOURCE) == RCC_MCOSource_LSI) || ((SOURCE) == RCC_MCOSource_LSE) || \ + ((SOURCE) == RCC_MCOSource_PLLCLK_Div2)|| ((SOURCE) == RCC_MCOSource_PLLCLK)) +/** + * @} + */ + +/** @defgroup RCC_MCOPrescaler + * @{ + */ + +#define RCC_MCOPrescaler_1 RCC_CFGR_MCO_PRE_1 +#define RCC_MCOPrescaler_2 RCC_CFGR_MCO_PRE_2 +#define RCC_MCOPrescaler_4 RCC_CFGR_MCO_PRE_4 +#define RCC_MCOPrescaler_8 RCC_CFGR_MCO_PRE_8 +#define RCC_MCOPrescaler_16 RCC_CFGR_MCO_PRE_16 +#define RCC_MCOPrescaler_32 RCC_CFGR_MCO_PRE_32 +#define RCC_MCOPrescaler_64 RCC_CFGR_MCO_PRE_64 +#define RCC_MCOPrescaler_128 RCC_CFGR_MCO_PRE_128 + +#define IS_RCC_MCO_PRESCALER(PRESCALER) (((PRESCALER) == RCC_MCOPrescaler_1) || \ + ((PRESCALER) == RCC_MCOPrescaler_2) || \ + ((PRESCALER) == RCC_MCOPrescaler_4) || \ + ((PRESCALER) == RCC_MCOPrescaler_8) || \ + ((PRESCALER) == RCC_MCOPrescaler_16) || \ + ((PRESCALER) == RCC_MCOPrescaler_32) || \ + ((PRESCALER) == RCC_MCOPrescaler_64) || \ + ((PRESCALER) == RCC_MCOPrescaler_128)) +/** + * @} + */ + +/** @defgroup RCC_USB_Device_clock_source + * @{ + */ + +#define RCC_USBCLKSource_PLLCLK_1Div5 ((uint8_t)0x00) +#define RCC_USBCLKSource_PLLCLK_Div1 ((uint8_t)0x01) + +#define IS_RCC_USBCLK_SOURCE(SOURCE) (((SOURCE) == RCC_USBCLKSource_PLLCLK_1Div5) || \ + ((SOURCE) == RCC_USBCLKSource_PLLCLK_Div1)) +/** + * @} + */ + +/** @defgroup RCC_Flag + * @{ + */ +#define RCC_FLAG_HSIRDY ((uint8_t)0x01) +#define RCC_FLAG_HSERDY ((uint8_t)0x11) +#define RCC_FLAG_PLLRDY ((uint8_t)0x19) +#define RCC_FLAG_MCOF ((uint8_t)0x9C) +#define RCC_FLAG_LSERDY ((uint8_t)0x21) +#define RCC_FLAG_LSIRDY ((uint8_t)0x41) +#define RCC_FLAG_OBLRST ((uint8_t)0x59) +#define RCC_FLAG_PINRST ((uint8_t)0x5A) +#define RCC_FLAG_PORRST ((uint8_t)0x5B) +#define RCC_FLAG_SFTRST ((uint8_t)0x5C) +#define RCC_FLAG_IWDGRST ((uint8_t)0x5D) +#define RCC_FLAG_WWDGRST ((uint8_t)0x5E) +#define RCC_FLAG_LPWRRST ((uint8_t)0x5F) + +#define IS_RCC_FLAG(FLAG) (((FLAG) == RCC_FLAG_HSIRDY) || ((FLAG) == RCC_FLAG_HSERDY) || \ + ((FLAG) == RCC_FLAG_PLLRDY) || ((FLAG) == RCC_FLAG_LSERDY) || \ + ((FLAG) == RCC_FLAG_LSIRDY) || ((FLAG) == RCC_FLAG_OBLRST) || \ + ((FLAG) == RCC_FLAG_PINRST) || ((FLAG) == RCC_FLAG_PORRST) || \ + ((FLAG) == RCC_FLAG_SFTRST) || ((FLAG) == RCC_FLAG_IWDGRST)|| \ + ((FLAG) == RCC_FLAG_WWDGRST)|| ((FLAG) == RCC_FLAG_LPWRRST)|| \ + ((FLAG) == RCC_FLAG_MCOF)) + +#define IS_RCC_HSI_CALIBRATION_VALUE(VALUE) ((VALUE) <= 0x1F) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the RCC clock configuration to the default reset state */ +void RCC_DeInit(void); + +/* Internal/external clocks, PLL, CSS and MCO configuration functions *********/ +void RCC_HSEConfig(uint8_t RCC_HSE); +ErrorStatus RCC_WaitForHSEStartUp(void); +void RCC_AdjustHSICalibrationValue(uint8_t HSICalibrationValue); +void RCC_HSICmd(FunctionalState NewState); +void RCC_LSEConfig(uint32_t RCC_LSE); +void RCC_LSEDriveConfig(uint32_t RCC_LSEDrive); +void RCC_LSICmd(FunctionalState NewState); +void RCC_PLLConfig(uint32_t RCC_PLLSource, uint32_t RCC_PLLMul); +void RCC_PLLCmd(FunctionalState NewState); +void RCC_PREDIV1Config(uint32_t RCC_PREDIV1_Div); +void RCC_ClockSecuritySystemCmd(FunctionalState NewState); +#ifdef STM32F303xC + void RCC_MCOConfig(uint8_t RCC_MCOSource); +#else + void RCC_MCOConfig(uint8_t RCC_MCOSource,uint32_t RCC_MCOPrescaler); +#endif /* STM32F303xC */ + +/* System, AHB and APB busses clocks configuration functions ******************/ +void RCC_SYSCLKConfig(uint32_t RCC_SYSCLKSource); +uint8_t RCC_GetSYSCLKSource(void); +void RCC_HCLKConfig(uint32_t RCC_SYSCLK); +void RCC_PCLK1Config(uint32_t RCC_HCLK); +void RCC_PCLK2Config(uint32_t RCC_HCLK); +void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks); + +/* Peripheral clocks configuration functions **********************************/ +void RCC_ADCCLKConfig(uint32_t RCC_PLLCLK); +void RCC_I2CCLKConfig(uint32_t RCC_I2CCLK); +void RCC_TIMCLKConfig(uint32_t RCC_TIMCLK); +void RCC_HRTIM1CLKConfig(uint32_t RCC_HRTIMCLK); +void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource); +void RCC_USARTCLKConfig(uint32_t RCC_USARTCLK); +void RCC_USBCLKConfig(uint32_t RCC_USBCLKSource); + +void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource); +void RCC_RTCCLKCmd(FunctionalState NewState); +void RCC_BackupResetCmd(FunctionalState NewState); + +void RCC_AHBPeriphClockCmd(uint32_t RCC_AHBPeriph, FunctionalState NewState); +void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState); +void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState NewState); + +void RCC_AHBPeriphResetCmd(uint32_t RCC_AHBPeriph, FunctionalState NewState); +void RCC_APB2PeriphResetCmd(uint32_t RCC_APB2Periph, FunctionalState NewState); +void RCC_APB1PeriphResetCmd(uint32_t RCC_APB1Periph, FunctionalState NewState); + +/* Interrupts and flags management functions **********************************/ +void RCC_ITConfig(uint8_t RCC_IT, FunctionalState NewState); +FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG); +void RCC_ClearFlag(void); +ITStatus RCC_GetITStatus(uint8_t RCC_IT); +void RCC_ClearITPendingBit(uint8_t RCC_IT); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_RCC_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_rtc.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_rtc.h new file mode 100755 index 0000000000..b6841a962a --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_rtc.h @@ -0,0 +1,852 @@ +/** + ****************************************************************************** + * @file stm32f30x_rtc.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the RTC firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_RTC_H +#define __STM32F30x_RTC_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup RTC + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief RTC Init structures definition + */ +typedef struct +{ + uint32_t RTC_HourFormat; /*!< Specifies the RTC Hour Format. + This parameter can be a value of @ref RTC_Hour_Formats */ + + uint32_t RTC_AsynchPrediv; /*!< Specifies the RTC Asynchronous Predivider value. + This parameter must be set to a value lower than 0x7F */ + + uint32_t RTC_SynchPrediv; /*!< Specifies the RTC Synchronous Predivider value. + This parameter must be set to a value lower than 0x1FFF */ +}RTC_InitTypeDef; + +/** + * @brief RTC Time structure definition + */ +typedef struct +{ + uint8_t RTC_Hours; /*!< Specifies the RTC Time Hour. + This parameter must be set to a value in the 0-12 range + if the RTC_HourFormat_12 is selected or 0-23 range if + the RTC_HourFormat_24 is selected. */ + + uint8_t RTC_Minutes; /*!< Specifies the RTC Time Minutes. + This parameter must be set to a value in the 0-59 range. */ + + uint8_t RTC_Seconds; /*!< Specifies the RTC Time Seconds. + This parameter must be set to a value in the 0-59 range. */ + + uint8_t RTC_H12; /*!< Specifies the RTC AM/PM Time. + This parameter can be a value of @ref RTC_AM_PM_Definitions */ +}RTC_TimeTypeDef; + +/** + * @brief RTC Date structure definition + */ +typedef struct +{ + uint8_t RTC_WeekDay; /*!< Specifies the RTC Date WeekDay. + This parameter can be a value of @ref RTC_WeekDay_Definitions */ + + uint8_t RTC_Month; /*!< Specifies the RTC Date Month (in BCD format). + This parameter can be a value of @ref RTC_Month_Date_Definitions */ + + uint8_t RTC_Date; /*!< Specifies the RTC Date. + This parameter must be set to a value in the 1-31 range. */ + + uint8_t RTC_Year; /*!< Specifies the RTC Date Year. + This parameter must be set to a value in the 0-99 range. */ +}RTC_DateTypeDef; + +/** + * @brief RTC Alarm structure definition + */ +typedef struct +{ + RTC_TimeTypeDef RTC_AlarmTime; /*!< Specifies the RTC Alarm Time members. */ + + uint32_t RTC_AlarmMask; /*!< Specifies the RTC Alarm Masks. + This parameter can be a value of @ref RTC_AlarmMask_Definitions */ + + uint32_t RTC_AlarmDateWeekDaySel; /*!< Specifies the RTC Alarm is on Date or WeekDay. + This parameter can be a value of @ref RTC_AlarmDateWeekDay_Definitions */ + + uint8_t RTC_AlarmDateWeekDay; /*!< Specifies the RTC Alarm Date/WeekDay. + If the Alarm Date is selected, this parameter + must be set to a value in the 1-31 range. + If the Alarm WeekDay is selected, this + parameter can be a value of @ref RTC_WeekDay_Definitions */ +}RTC_AlarmTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup RTC_Exported_Constants + * @{ + */ + + +/** @defgroup RTC_Hour_Formats + * @{ + */ +#define RTC_HourFormat_24 ((uint32_t)0x00000000) +#define RTC_HourFormat_12 ((uint32_t)0x00000040) +#define IS_RTC_HOUR_FORMAT(FORMAT) (((FORMAT) == RTC_HourFormat_12) || \ + ((FORMAT) == RTC_HourFormat_24)) +/** + * @} + */ + +/** @defgroup RTC_Asynchronous_Predivider + * @{ + */ +#define IS_RTC_ASYNCH_PREDIV(PREDIV) ((PREDIV) <= 0x7F) + +/** + * @} + */ + + +/** @defgroup RTC_Synchronous_Predivider + * @{ + */ +#define IS_RTC_SYNCH_PREDIV(PREDIV) ((PREDIV) <= 0x7FFF) + +/** + * @} + */ + +/** @defgroup RTC_Time_Definitions + * @{ + */ +#define IS_RTC_HOUR12(HOUR) (((HOUR) > 0) && ((HOUR) <= 12)) +#define IS_RTC_HOUR24(HOUR) ((HOUR) <= 23) +#define IS_RTC_MINUTES(MINUTES) ((MINUTES) <= 59) +#define IS_RTC_SECONDS(SECONDS) ((SECONDS) <= 59) + +/** + * @} + */ + +/** @defgroup RTC_AM_PM_Definitions + * @{ + */ +#define RTC_H12_AM ((uint8_t)0x00) +#define RTC_H12_PM ((uint8_t)0x40) +#define IS_RTC_H12(PM) (((PM) == RTC_H12_AM) || ((PM) == RTC_H12_PM)) + +/** + * @} + */ + +/** @defgroup RTC_Year_Date_Definitions + * @{ + */ +#define IS_RTC_YEAR(YEAR) ((YEAR) <= 99) + +/** + * @} + */ + +/** @defgroup RTC_Month_Date_Definitions + * @{ + */ + +/* Coded in BCD format */ +#define RTC_Month_January ((uint8_t)0x01) +#define RTC_Month_February ((uint8_t)0x02) +#define RTC_Month_March ((uint8_t)0x03) +#define RTC_Month_April ((uint8_t)0x04) +#define RTC_Month_May ((uint8_t)0x05) +#define RTC_Month_June ((uint8_t)0x06) +#define RTC_Month_July ((uint8_t)0x07) +#define RTC_Month_August ((uint8_t)0x08) +#define RTC_Month_September ((uint8_t)0x09) +#define RTC_Month_October ((uint8_t)0x10) +#define RTC_Month_November ((uint8_t)0x11) +#define RTC_Month_December ((uint8_t)0x12) +#define IS_RTC_MONTH(MONTH) (((MONTH) >= 1) && ((MONTH) <= 12)) +#define IS_RTC_DATE(DATE) (((DATE) >= 1) && ((DATE) <= 31)) + +/** + * @} + */ + +/** @defgroup RTC_WeekDay_Definitions + * @{ + */ + +#define RTC_Weekday_Monday ((uint8_t)0x01) +#define RTC_Weekday_Tuesday ((uint8_t)0x02) +#define RTC_Weekday_Wednesday ((uint8_t)0x03) +#define RTC_Weekday_Thursday ((uint8_t)0x04) +#define RTC_Weekday_Friday ((uint8_t)0x05) +#define RTC_Weekday_Saturday ((uint8_t)0x06) +#define RTC_Weekday_Sunday ((uint8_t)0x07) +#define IS_RTC_WEEKDAY(WEEKDAY) (((WEEKDAY) == RTC_Weekday_Monday) || \ + ((WEEKDAY) == RTC_Weekday_Tuesday) || \ + ((WEEKDAY) == RTC_Weekday_Wednesday) || \ + ((WEEKDAY) == RTC_Weekday_Thursday) || \ + ((WEEKDAY) == RTC_Weekday_Friday) || \ + ((WEEKDAY) == RTC_Weekday_Saturday) || \ + ((WEEKDAY) == RTC_Weekday_Sunday)) +/** + * @} + */ + + +/** @defgroup RTC_Alarm_Definitions + * @{ + */ +#define IS_RTC_ALARM_DATE_WEEKDAY_DATE(DATE) (((DATE) > 0) && ((DATE) <= 31)) +#define IS_RTC_ALARM_DATE_WEEKDAY_WEEKDAY(WEEKDAY) (((WEEKDAY) == RTC_Weekday_Monday) || \ + ((WEEKDAY) == RTC_Weekday_Tuesday) || \ + ((WEEKDAY) == RTC_Weekday_Wednesday) || \ + ((WEEKDAY) == RTC_Weekday_Thursday) || \ + ((WEEKDAY) == RTC_Weekday_Friday) || \ + ((WEEKDAY) == RTC_Weekday_Saturday) || \ + ((WEEKDAY) == RTC_Weekday_Sunday)) + +/** + * @} + */ + + +/** @defgroup RTC_AlarmDateWeekDay_Definitions + * @{ + */ +#define RTC_AlarmDateWeekDaySel_Date ((uint32_t)0x00000000) +#define RTC_AlarmDateWeekDaySel_WeekDay ((uint32_t)0x40000000) + +#define IS_RTC_ALARM_DATE_WEEKDAY_SEL(SEL) (((SEL) == RTC_AlarmDateWeekDaySel_Date) || \ + ((SEL) == RTC_AlarmDateWeekDaySel_WeekDay)) + +/** + * @} + */ + + +/** @defgroup RTC_AlarmMask_Definitions + * @{ + */ +#define RTC_AlarmMask_None ((uint32_t)0x00000000) +#define RTC_AlarmMask_DateWeekDay ((uint32_t)0x80000000) +#define RTC_AlarmMask_Hours ((uint32_t)0x00800000) +#define RTC_AlarmMask_Minutes ((uint32_t)0x00008000) +#define RTC_AlarmMask_Seconds ((uint32_t)0x00000080) +#define RTC_AlarmMask_All ((uint32_t)0x80808080) +#define IS_ALARM_MASK(MASK) (((MASK) & 0x7F7F7F7F) == (uint32_t)RESET) + +/** + * @} + */ + +/** @defgroup RTC_Alarms_Definitions + * @{ + */ +#define RTC_Alarm_A ((uint32_t)0x00000100) +#define RTC_Alarm_B ((uint32_t)0x00000200) +#define IS_RTC_ALARM(ALARM) (((ALARM) == RTC_Alarm_A) || ((ALARM) == RTC_Alarm_B)) +#define IS_RTC_CMD_ALARM(ALARM) (((ALARM) & (RTC_Alarm_A | RTC_Alarm_B)) != (uint32_t)RESET) + +/** + * @} + */ + +/** @defgroup RTC_Alarm_Sub_Seconds_Masks_Definitions + * @{ + */ +#define RTC_AlarmSubSecondMask_All ((uint32_t)0x00000000) /*!< All Alarm SS fields are masked. + There is no comparison on sub seconds + for Alarm */ +#define RTC_AlarmSubSecondMask_SS14_1 ((uint32_t)0x01000000) /*!< SS[14:1] are don't care in Alarm + comparison. Only SS[0] is compared. */ +#define RTC_AlarmSubSecondMask_SS14_2 ((uint32_t)0x02000000) /*!< SS[14:2] are don't care in Alarm + comparison. Only SS[1:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_3 ((uint32_t)0x03000000) /*!< SS[14:3] are don't care in Alarm + comparison. Only SS[2:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_4 ((uint32_t)0x04000000) /*!< SS[14:4] are don't care in Alarm + comparison. Only SS[3:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_5 ((uint32_t)0x05000000) /*!< SS[14:5] are don't care in Alarm + comparison. Only SS[4:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_6 ((uint32_t)0x06000000) /*!< SS[14:6] are don't care in Alarm + comparison. Only SS[5:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_7 ((uint32_t)0x07000000) /*!< SS[14:7] are don't care in Alarm + comparison. Only SS[6:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_8 ((uint32_t)0x08000000) /*!< SS[14:8] are don't care in Alarm + comparison. Only SS[7:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_9 ((uint32_t)0x09000000) /*!< SS[14:9] are don't care in Alarm + comparison. Only SS[8:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_10 ((uint32_t)0x0A000000) /*!< SS[14:10] are don't care in Alarm + comparison. Only SS[9:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_11 ((uint32_t)0x0B000000) /*!< SS[14:11] are don't care in Alarm + comparison. Only SS[10:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_12 ((uint32_t)0x0C000000) /*!< SS[14:12] are don't care in Alarm + comparison.Only SS[11:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14_13 ((uint32_t)0x0D000000) /*!< SS[14:13] are don't care in Alarm + comparison. Only SS[12:0] are compared */ +#define RTC_AlarmSubSecondMask_SS14 ((uint32_t)0x0E000000) /*!< SS[14] is don't care in Alarm + comparison.Only SS[13:0] are compared */ +#define RTC_AlarmSubSecondMask_None ((uint32_t)0x0F000000) /*!< SS[14:0] are compared and must match + to activate alarm. */ +#define IS_RTC_ALARM_SUB_SECOND_MASK(MASK) (((MASK) == RTC_AlarmSubSecondMask_All) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_1) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_2) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_3) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_4) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_5) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_6) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_7) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_8) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_9) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_10) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_11) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_12) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14_13) || \ + ((MASK) == RTC_AlarmSubSecondMask_SS14) || \ + ((MASK) == RTC_AlarmSubSecondMask_None)) +/** + * @} + */ + +/** @defgroup RTC_Alarm_Sub_Seconds_Value + * @{ + */ + +#define IS_RTC_ALARM_SUB_SECOND_VALUE(VALUE) ((VALUE) <= 0x00007FFF) + +/** + * @} + */ + +/** @defgroup RTC_Wakeup_Timer_Definitions + * @{ + */ +#define RTC_WakeUpClock_RTCCLK_Div16 ((uint32_t)0x00000000) +#define RTC_WakeUpClock_RTCCLK_Div8 ((uint32_t)0x00000001) +#define RTC_WakeUpClock_RTCCLK_Div4 ((uint32_t)0x00000002) +#define RTC_WakeUpClock_RTCCLK_Div2 ((uint32_t)0x00000003) +#define RTC_WakeUpClock_CK_SPRE_16bits ((uint32_t)0x00000004) +#define RTC_WakeUpClock_CK_SPRE_17bits ((uint32_t)0x00000006) +#define IS_RTC_WAKEUP_CLOCK(CLOCK) (((CLOCK) == RTC_WakeUpClock_RTCCLK_Div16) || \ + ((CLOCK) == RTC_WakeUpClock_RTCCLK_Div8) || \ + ((CLOCK) == RTC_WakeUpClock_RTCCLK_Div4) || \ + ((CLOCK) == RTC_WakeUpClock_RTCCLK_Div2) || \ + ((CLOCK) == RTC_WakeUpClock_CK_SPRE_16bits) || \ + ((CLOCK) == RTC_WakeUpClock_CK_SPRE_17bits)) +#define IS_RTC_WAKEUP_COUNTER(COUNTER) ((COUNTER) <= 0xFFFF) +/** + * @} + */ + +/** @defgroup RTC_Time_Stamp_Edges_definitions + * @{ + */ +#define RTC_TimeStampEdge_Rising ((uint32_t)0x00000000) +#define RTC_TimeStampEdge_Falling ((uint32_t)0x00000008) +#define IS_RTC_TIMESTAMP_EDGE(EDGE) (((EDGE) == RTC_TimeStampEdge_Rising) || \ + ((EDGE) == RTC_TimeStampEdge_Falling)) +/** + * @} + */ + +/** @defgroup RTC_Output_selection_Definitions + * @{ + */ +#define RTC_Output_Disable ((uint32_t)0x00000000) +#define RTC_Output_AlarmA ((uint32_t)0x00200000) +#define RTC_Output_AlarmB ((uint32_t)0x00400000) +#define RTC_Output_WakeUp ((uint32_t)0x00600000) + +#define IS_RTC_OUTPUT(OUTPUT) (((OUTPUT) == RTC_Output_Disable) || \ + ((OUTPUT) == RTC_Output_AlarmA) || \ + ((OUTPUT) == RTC_Output_AlarmB) || \ + ((OUTPUT) == RTC_Output_WakeUp)) + +/** + * @} + */ + +/** @defgroup RTC_Output_Polarity_Definitions + * @{ + */ +#define RTC_OutputPolarity_High ((uint32_t)0x00000000) +#define RTC_OutputPolarity_Low ((uint32_t)0x00100000) +#define IS_RTC_OUTPUT_POL(POL) (((POL) == RTC_OutputPolarity_High) || \ + ((POL) == RTC_OutputPolarity_Low)) +/** + * @} + */ + +/** @defgroup RTC_Digital_Calibration_Definitions + * @{ + */ +#define RTC_CalibSign_Positive ((uint32_t)0x00000000) +#define RTC_CalibSign_Negative ((uint32_t)0x00000080) +#define IS_RTC_CALIB_SIGN(SIGN) (((SIGN) == RTC_CalibSign_Positive) || \ + ((SIGN) == RTC_CalibSign_Negative)) +#define IS_RTC_CALIB_VALUE(VALUE) ((VALUE) < 0x20) + +/** + * @} + */ + + /** @defgroup RTC_Calib_Output_selection_Definitions + * @{ + */ +#define RTC_CalibOutput_512Hz ((uint32_t)0x00000000) +#define RTC_CalibOutput_1Hz ((uint32_t)0x00080000) +#define IS_RTC_CALIB_OUTPUT(OUTPUT) (((OUTPUT) == RTC_CalibOutput_512Hz) || \ + ((OUTPUT) == RTC_CalibOutput_1Hz)) +/** + * @} + */ + +/** @defgroup RTC_Smooth_calib_period_Definitions + * @{ + */ +#define RTC_SmoothCalibPeriod_32sec ((uint32_t)0x00000000) /*!< if RTCCLK = 32768 Hz, Smooth calibation + period is 32s, else 2exp20 RTCCLK seconds */ +#define RTC_SmoothCalibPeriod_16sec ((uint32_t)0x00002000) /*!< if RTCCLK = 32768 Hz, Smooth calibation + period is 16s, else 2exp19 RTCCLK seconds */ +#define RTC_SmoothCalibPeriod_8sec ((uint32_t)0x00004000) /*!< if RTCCLK = 32768 Hz, Smooth calibation + period is 8s, else 2exp18 RTCCLK seconds */ +#define IS_RTC_SMOOTH_CALIB_PERIOD(PERIOD) (((PERIOD) == RTC_SmoothCalibPeriod_32sec) || \ + ((PERIOD) == RTC_SmoothCalibPeriod_16sec) || \ + ((PERIOD) == RTC_SmoothCalibPeriod_8sec)) + +/** + * @} + */ + +/** @defgroup RTC_Smooth_calib_Plus_pulses_Definitions + * @{ + */ +#define RTC_SmoothCalibPlusPulses_Set ((uint32_t)0x00008000) /*!< The number of RTCCLK pulses added + during a X -second window = Y - CALM[8:0]. + with Y = 512, 256, 128 when X = 32, 16, 8 */ +#define RTC_SmoothCalibPlusPulses_Reset ((uint32_t)0x00000000) /*!< The number of RTCCLK pulses subbstited + during a 32-second window = CALM[8:0]. */ +#define IS_RTC_SMOOTH_CALIB_PLUS(PLUS) (((PLUS) == RTC_SmoothCalibPlusPulses_Set) || \ + ((PLUS) == RTC_SmoothCalibPlusPulses_Reset)) + +/** + * @} + */ + +/** @defgroup RTC_Smooth_calib_Minus_pulses_Definitions + * @{ + */ +#define IS_RTC_SMOOTH_CALIB_MINUS(VALUE) ((VALUE) <= 0x000001FF) + +/** + * @} + */ + +/** @defgroup RTC_DayLightSaving_Definitions + * @{ + */ +#define RTC_DayLightSaving_SUB1H ((uint32_t)0x00020000) +#define RTC_DayLightSaving_ADD1H ((uint32_t)0x00010000) +#define IS_RTC_DAYLIGHT_SAVING(SAVE) (((SAVE) == RTC_DayLightSaving_SUB1H) || \ + ((SAVE) == RTC_DayLightSaving_ADD1H)) + +#define RTC_StoreOperation_Reset ((uint32_t)0x00000000) +#define RTC_StoreOperation_Set ((uint32_t)0x00040000) +#define IS_RTC_STORE_OPERATION(OPERATION) (((OPERATION) == RTC_StoreOperation_Reset) || \ + ((OPERATION) == RTC_StoreOperation_Set)) +/** + * @} + */ + +/** @defgroup RTC_Tamper_Trigger_Definitions + * @{ + */ +#define RTC_TamperTrigger_RisingEdge ((uint32_t)0x00000000) +#define RTC_TamperTrigger_FallingEdge ((uint32_t)0x00000001) +#define RTC_TamperTrigger_LowLevel ((uint32_t)0x00000000) +#define RTC_TamperTrigger_HighLevel ((uint32_t)0x00000001) +#define IS_RTC_TAMPER_TRIGGER(TRIGGER) (((TRIGGER) == RTC_TamperTrigger_RisingEdge) || \ + ((TRIGGER) == RTC_TamperTrigger_FallingEdge) || \ + ((TRIGGER) == RTC_TamperTrigger_LowLevel) || \ + ((TRIGGER) == RTC_TamperTrigger_HighLevel)) + +/** + * @} + */ + +/** @defgroup RTC_Tamper_Filter_Definitions + * @{ + */ +#define RTC_TamperFilter_Disable ((uint32_t)0x00000000) /*!< Tamper filter is disabled */ + +#define RTC_TamperFilter_2Sample ((uint32_t)0x00000800) /*!< Tamper is activated after 2 + consecutive samples at the active level */ +#define RTC_TamperFilter_4Sample ((uint32_t)0x00001000) /*!< Tamper is activated after 4 + consecutive samples at the active level */ +#define RTC_TamperFilter_8Sample ((uint32_t)0x00001800) /*!< Tamper is activated after 8 + consecutive samples at the active leve. */ +#define IS_RTC_TAMPER_FILTER(FILTER) (((FILTER) == RTC_TamperFilter_Disable) || \ + ((FILTER) == RTC_TamperFilter_2Sample) || \ + ((FILTER) == RTC_TamperFilter_4Sample) || \ + ((FILTER) == RTC_TamperFilter_8Sample)) +/** + * @} + */ + +/** @defgroup RTC_Tamper_Sampling_Frequencies_Definitions + * @{ + */ +#define RTC_TamperSamplingFreq_RTCCLK_Div32768 ((uint32_t)0x00000000) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 32768 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div16384 ((uint32_t)0x000000100) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 16384 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div8192 ((uint32_t)0x00000200) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 8192 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div4096 ((uint32_t)0x00000300) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 4096 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div2048 ((uint32_t)0x00000400) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 2048 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div1024 ((uint32_t)0x00000500) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 1024 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div512 ((uint32_t)0x00000600) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 512 */ +#define RTC_TamperSamplingFreq_RTCCLK_Div256 ((uint32_t)0x00000700) /*!< Each of the tamper inputs are sampled + with a frequency = RTCCLK / 256 */ +#define IS_RTC_TAMPER_SAMPLING_FREQ(FREQ) (((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div32768) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div16384) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div8192) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div4096) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div2048) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div1024) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div512) || \ + ((FREQ) ==RTC_TamperSamplingFreq_RTCCLK_Div256)) + +/** + * @} + */ + + /** @defgroup RTC_Tamper_Pin_Precharge_Duration_Definitions + * @{ + */ +#define RTC_TamperPrechargeDuration_1RTCCLK ((uint32_t)0x00000000) /*!< Tamper pins are pre-charged before + sampling during 1 RTCCLK cycle */ +#define RTC_TamperPrechargeDuration_2RTCCLK ((uint32_t)0x00002000) /*!< Tamper pins are pre-charged before + sampling during 2 RTCCLK cycles */ +#define RTC_TamperPrechargeDuration_4RTCCLK ((uint32_t)0x00004000) /*!< Tamper pins are pre-charged before + sampling during 4 RTCCLK cycles */ +#define RTC_TamperPrechargeDuration_8RTCCLK ((uint32_t)0x00006000) /*!< Tamper pins are pre-charged before + sampling during 8 RTCCLK cycles */ + +#define IS_RTC_TAMPER_PRECHARGE_DURATION(DURATION) (((DURATION) == RTC_TamperPrechargeDuration_1RTCCLK) || \ + ((DURATION) == RTC_TamperPrechargeDuration_2RTCCLK) || \ + ((DURATION) == RTC_TamperPrechargeDuration_4RTCCLK) || \ + ((DURATION) == RTC_TamperPrechargeDuration_8RTCCLK)) +/** + * @} + */ + +/** @defgroup RTC_Tamper_Pins_Definitions + * @{ + */ +#define RTC_Tamper_1 RTC_TAFCR_TAMP1E /*!< Tamper detection enable for + input tamper 1 */ +#define RTC_Tamper_2 RTC_TAFCR_TAMP2E /*!< Tamper detection enable for + input tamper 2 */ +#define RTC_Tamper_3 RTC_TAFCR_TAMP3E /*!< Tamper detection enable for + input tamper 3 */ + +#define IS_RTC_TAMPER(TAMPER) ((((TAMPER) & (uint32_t)0xFFFFFFD6) == 0x00) && ((TAMPER) != (uint32_t)RESET)) + + +/** + * @} + */ + +/** @defgroup RTC_Output_Type_ALARM_OUT + * @{ + */ +#define RTC_OutputType_OpenDrain ((uint32_t)0x00000000) +#define RTC_OutputType_PushPull ((uint32_t)0x00040000) +#define IS_RTC_OUTPUT_TYPE(TYPE) (((TYPE) == RTC_OutputType_OpenDrain) || \ + ((TYPE) == RTC_OutputType_PushPull)) + +/** + * @} + */ + +/** @defgroup RTC_Add_1_Second_Parameter_Definitions + * @{ + */ +#define RTC_ShiftAdd1S_Reset ((uint32_t)0x00000000) +#define RTC_ShiftAdd1S_Set ((uint32_t)0x80000000) +#define IS_RTC_SHIFT_ADD1S(SEL) (((SEL) == RTC_ShiftAdd1S_Reset) || \ + ((SEL) == RTC_ShiftAdd1S_Set)) +/** + * @} + */ + +/** @defgroup RTC_Substract_Fraction_Of_Second_Value + * @{ + */ +#define IS_RTC_SHIFT_SUBFS(FS) ((FS) <= 0x00007FFF) + +/** + * @} + */ + +/** @defgroup RTC_Backup_Registers_Definitions + * @{ + */ + +#define RTC_BKP_DR0 ((uint32_t)0x00000000) +#define RTC_BKP_DR1 ((uint32_t)0x00000001) +#define RTC_BKP_DR2 ((uint32_t)0x00000002) +#define RTC_BKP_DR3 ((uint32_t)0x00000003) +#define RTC_BKP_DR4 ((uint32_t)0x00000004) +#define RTC_BKP_DR5 ((uint32_t)0x00000005) +#define RTC_BKP_DR6 ((uint32_t)0x00000006) +#define RTC_BKP_DR7 ((uint32_t)0x00000007) +#define RTC_BKP_DR8 ((uint32_t)0x00000008) +#define RTC_BKP_DR9 ((uint32_t)0x00000009) +#define RTC_BKP_DR10 ((uint32_t)0x0000000A) +#define RTC_BKP_DR11 ((uint32_t)0x0000000B) +#define RTC_BKP_DR12 ((uint32_t)0x0000000C) +#define RTC_BKP_DR13 ((uint32_t)0x0000000D) +#define RTC_BKP_DR14 ((uint32_t)0x0000000E) +#define RTC_BKP_DR15 ((uint32_t)0x0000000F) +#define IS_RTC_BKP(BKP) (((BKP) == RTC_BKP_DR0) || \ + ((BKP) == RTC_BKP_DR1) || \ + ((BKP) == RTC_BKP_DR2) || \ + ((BKP) == RTC_BKP_DR3) || \ + ((BKP) == RTC_BKP_DR4) || \ + ((BKP) == RTC_BKP_DR5) || \ + ((BKP) == RTC_BKP_DR6) || \ + ((BKP) == RTC_BKP_DR7) || \ + ((BKP) == RTC_BKP_DR8) || \ + ((BKP) == RTC_BKP_DR9) || \ + ((BKP) == RTC_BKP_DR10) || \ + ((BKP) == RTC_BKP_DR11) || \ + ((BKP) == RTC_BKP_DR12) || \ + ((BKP) == RTC_BKP_DR13) || \ + ((BKP) == RTC_BKP_DR14) || \ + ((BKP) == RTC_BKP_DR15)) +/** + * @} + */ + +/** @defgroup RTC_Input_parameter_format_definitions + * @{ + */ +#define RTC_Format_BIN ((uint32_t)0x000000000) +#define RTC_Format_BCD ((uint32_t)0x000000001) +#define IS_RTC_FORMAT(FORMAT) (((FORMAT) == RTC_Format_BIN) || ((FORMAT) == RTC_Format_BCD)) + +/** + * @} + */ + +/** @defgroup RTC_Flags_Definitions + * @{ + */ +#define RTC_FLAG_RECALPF ((uint32_t)0x00010000) +#define RTC_FLAG_TAMP3F ((uint32_t)0x00008000) +#define RTC_FLAG_TAMP2F ((uint32_t)0x00004000) +#define RTC_FLAG_TAMP1F ((uint32_t)0x00002000) +#define RTC_FLAG_TSOVF ((uint32_t)0x00001000) +#define RTC_FLAG_TSF ((uint32_t)0x00000800) +#define RTC_FLAG_WUTF ((uint32_t)0x00000400) +#define RTC_FLAG_ALRBF ((uint32_t)0x00000200) +#define RTC_FLAG_ALRAF ((uint32_t)0x00000100) +#define RTC_FLAG_INITF ((uint32_t)0x00000040) +#define RTC_FLAG_RSF ((uint32_t)0x00000020) +#define RTC_FLAG_INITS ((uint32_t)0x00000010) +#define RTC_FLAG_SHPF ((uint32_t)0x00000008) +#define RTC_FLAG_WUTWF ((uint32_t)0x00000004) +#define RTC_FLAG_ALRBWF ((uint32_t)0x00000002) +#define RTC_FLAG_ALRAWF ((uint32_t)0x00000001) +#define IS_RTC_GET_FLAG(FLAG) (((FLAG) == RTC_FLAG_TSOVF) || ((FLAG) == RTC_FLAG_TSF) || \ + ((FLAG) == RTC_FLAG_WUTF) || ((FLAG) == RTC_FLAG_ALRBF) || \ + ((FLAG) == RTC_FLAG_ALRAF) || ((FLAG) == RTC_FLAG_INITF) || \ + ((FLAG) == RTC_FLAG_RSF) || ((FLAG) == RTC_FLAG_WUTWF) || \ + ((FLAG) == RTC_FLAG_ALRBWF) || ((FLAG) == RTC_FLAG_ALRAWF) || \ + ((FLAG) == RTC_FLAG_TAMP1F) || ((FLAG) == RTC_FLAG_TAMP2F) || \ + ((FLAG) == RTC_FLAG_TAMP3F) || ((FLAG) == RTC_FLAG_RECALPF) || \ + ((FLAG) == RTC_FLAG_SHPF)) +#define IS_RTC_CLEAR_FLAG(FLAG) (((FLAG) != (uint32_t)RESET) && (((FLAG) & 0xFFFF00DF) == (uint32_t)RESET)) + +/** + * @} + */ + +/** @defgroup RTC_Interrupts_Definitions + * @{ + */ +#define RTC_IT_TS ((uint32_t)0x00008000) +#define RTC_IT_WUT ((uint32_t)0x00004000) +#define RTC_IT_ALRB ((uint32_t)0x00002000) +#define RTC_IT_ALRA ((uint32_t)0x00001000) +#define RTC_IT_TAMP ((uint32_t)0x00000004) /* Used only to Enable the Tamper Interrupt */ +#define RTC_IT_TAMP1 ((uint32_t)0x00020000) +#define RTC_IT_TAMP2 ((uint32_t)0x00040000) +#define RTC_IT_TAMP3 ((uint32_t)0x00080000) + + +#define IS_RTC_CONFIG_IT(IT) (((IT) != (uint32_t)RESET) && (((IT) & 0xFFFF0FFB) == (uint32_t)RESET)) +#define IS_RTC_GET_IT(IT) (((IT) == RTC_IT_TS) || ((IT) == RTC_IT_WUT) || \ + ((IT) == RTC_IT_ALRB) || ((IT) == RTC_IT_ALRA) || \ + ((IT) == RTC_IT_TAMP1) || ((IT) == RTC_IT_TAMP2) || \ + ((IT) == RTC_IT_TAMP3)) +#define IS_RTC_CLEAR_IT(IT) (((IT) != (uint32_t)RESET) && (((IT) & 0xFFF10FFF) == (uint32_t)RESET)) + +/** + * @} + */ + +/** + * @} + */ + + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the RTC configuration to the default reset state *****/ +ErrorStatus RTC_DeInit(void); + + +/* Initialization and Configuration functions *********************************/ +ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct); +void RTC_StructInit(RTC_InitTypeDef* RTC_InitStruct); +void RTC_WriteProtectionCmd(FunctionalState NewState); +ErrorStatus RTC_EnterInitMode(void); +void RTC_ExitInitMode(void); +ErrorStatus RTC_WaitForSynchro(void); +ErrorStatus RTC_RefClockCmd(FunctionalState NewState); +void RTC_BypassShadowCmd(FunctionalState NewState); + +/* Time and Date configuration functions **************************************/ +ErrorStatus RTC_SetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct); +void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct); +void RTC_GetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct); +uint32_t RTC_GetSubSecond(void); +ErrorStatus RTC_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct); +void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct); +void RTC_GetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct); + +/* Alarms (Alarm A and Alarm B) configuration functions **********************/ +void RTC_SetAlarm(uint32_t RTC_Format, uint32_t RTC_Alarm, RTC_AlarmTypeDef* RTC_AlarmStruct); +void RTC_AlarmStructInit(RTC_AlarmTypeDef* RTC_AlarmStruct); +void RTC_GetAlarm(uint32_t RTC_Format, uint32_t RTC_Alarm, RTC_AlarmTypeDef* RTC_AlarmStruct); +ErrorStatus RTC_AlarmCmd(uint32_t RTC_Alarm, FunctionalState NewState); +void RTC_AlarmSubSecondConfig(uint32_t RTC_Alarm, uint32_t RTC_AlarmSubSecondValue, uint32_t RTC_AlarmSubSecondMask); +uint32_t RTC_GetAlarmSubSecond(uint32_t RTC_Alarm); + +/* WakeUp Timer configuration functions ***************************************/ +void RTC_WakeUpClockConfig(uint32_t RTC_WakeUpClock); +void RTC_SetWakeUpCounter(uint32_t RTC_WakeUpCounter); +uint32_t RTC_GetWakeUpCounter(void); +ErrorStatus RTC_WakeUpCmd(FunctionalState NewState); + +/* Daylight Saving configuration functions ************************************/ +void RTC_DayLightSavingConfig(uint32_t RTC_DayLightSaving, uint32_t RTC_StoreOperation); +uint32_t RTC_GetStoreOperation(void); + +/* Output pin Configuration function ******************************************/ +void RTC_OutputConfig(uint32_t RTC_Output, uint32_t RTC_OutputPolarity); + +/* Digital Calibration configuration functions ********************************/ +void RTC_CalibOutputCmd(FunctionalState NewState); +void RTC_CalibOutputConfig(uint32_t RTC_CalibOutput); +ErrorStatus RTC_SmoothCalibConfig(uint32_t RTC_SmoothCalibPeriod, + uint32_t RTC_SmoothCalibPlusPulses, + uint32_t RTC_SmouthCalibMinusPulsesValue); + +/* TimeStamp configuration functions ******************************************/ +void RTC_TimeStampCmd(uint32_t RTC_TimeStampEdge, FunctionalState NewState); +void RTC_GetTimeStamp(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_StampTimeStruct, + RTC_DateTypeDef* RTC_StampDateStruct); +uint32_t RTC_GetTimeStampSubSecond(void); + +/* Tampers configuration functions ********************************************/ +void RTC_TamperTriggerConfig(uint32_t RTC_Tamper, uint32_t RTC_TamperTrigger); +void RTC_TamperCmd(uint32_t RTC_Tamper, FunctionalState NewState); +void RTC_TamperFilterConfig(uint32_t RTC_TamperFilter); +void RTC_TamperSamplingFreqConfig(uint32_t RTC_TamperSamplingFreq); +void RTC_TamperPinsPrechargeDuration(uint32_t RTC_TamperPrechargeDuration); +void RTC_TimeStampOnTamperDetectionCmd(FunctionalState NewState); +void RTC_TamperPullUpCmd(FunctionalState NewState); + +/* Backup Data Registers configuration functions ******************************/ +void RTC_WriteBackupRegister(uint32_t RTC_BKP_DR, uint32_t Data); +uint32_t RTC_ReadBackupRegister(uint32_t RTC_BKP_DR); + +/* Output Type Config configuration functions *********************************/ +void RTC_OutputTypeConfig(uint32_t RTC_OutputType); + +/* RTC_Shift_control_synchonisation_functions *********************************/ +ErrorStatus RTC_SynchroShiftConfig(uint32_t RTC_ShiftAdd1S, uint32_t RTC_ShiftSubFS); + +/* Interrupts and flags management functions **********************************/ +void RTC_ITConfig(uint32_t RTC_IT, FunctionalState NewState); +FlagStatus RTC_GetFlagStatus(uint32_t RTC_FLAG); +void RTC_ClearFlag(uint32_t RTC_FLAG); +ITStatus RTC_GetITStatus(uint32_t RTC_IT); +void RTC_ClearITPendingBit(uint32_t RTC_IT); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_RTC_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_spi.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_spi.h new file mode 100755 index 0000000000..088fb2a894 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_spi.h @@ -0,0 +1,608 @@ +/** + ****************************************************************************** + * @file stm32f30x_spi.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the SPI + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_SPI_H +#define __STM32F30x_SPI_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup SPI + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief SPI Init structure definition + */ + +typedef struct +{ + uint16_t SPI_Direction; /*!< Specifies the SPI unidirectional or bidirectional data mode. + This parameter can be a value of @ref SPI_data_direction */ + + uint16_t SPI_Mode; /*!< Specifies the SPI mode (Master/Slave). + This parameter can be a value of @ref SPI_mode */ + + uint16_t SPI_DataSize; /*!< Specifies the SPI data size. + This parameter can be a value of @ref SPI_data_size */ + + uint16_t SPI_CPOL; /*!< Specifies the serial clock steady state. + This parameter can be a value of @ref SPI_Clock_Polarity */ + + uint16_t SPI_CPHA; /*!< Specifies the clock active edge for the bit capture. + This parameter can be a value of @ref SPI_Clock_Phase */ + + uint16_t SPI_NSS; /*!< Specifies whether the NSS signal is managed by + hardware (NSS pin) or by software using the SSI bit. + This parameter can be a value of @ref SPI_Slave_Select_management */ + + uint16_t SPI_BaudRatePrescaler; /*!< Specifies the Baud Rate prescaler value which will be + used to configure the transmit and receive SCK clock. + This parameter can be a value of @ref SPI_BaudRate_Prescaler. + @note The communication clock is derived from the master + clock. The slave clock does not need to be set. */ + + uint16_t SPI_FirstBit; /*!< Specifies whether data transfers start from MSB or LSB bit. + This parameter can be a value of @ref SPI_MSB_LSB_transmission */ + + uint16_t SPI_CRCPolynomial; /*!< Specifies the polynomial used for the CRC calculation. */ +}SPI_InitTypeDef; + + +/** + * @brief I2S Init structure definition + */ + +typedef struct +{ + uint16_t I2S_Mode; /*!< Specifies the I2S operating mode. + This parameter can be a value of @ref I2S_Mode */ + + uint16_t I2S_Standard; /*!< Specifies the standard used for the I2S communication. + This parameter can be a value of @ref I2S_Standard */ + + uint16_t I2S_DataFormat; /*!< Specifies the data format for the I2S communication. + This parameter can be a value of @ref I2S_Data_Format */ + + uint16_t I2S_MCLKOutput; /*!< Specifies whether the I2S MCLK output is enabled or not. + This parameter can be a value of @ref I2S_MCLK_Output */ + + uint32_t I2S_AudioFreq; /*!< Specifies the frequency selected for the I2S communication. + This parameter can be a value of @ref I2S_Audio_Frequency */ + + uint16_t I2S_CPOL; /*!< Specifies the idle state of the I2S clock. + This parameter can be a value of @ref I2S_Clock_Polarity */ +}I2S_InitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup SPI_Exported_Constants + * @{ + */ + +#define IS_SPI_ALL_PERIPH(PERIPH) (((PERIPH) == SPI1) || \ + ((PERIPH) == SPI2) || \ + ((PERIPH) == SPI3) || \ + ((PERIPH) == SPI4)) + +#define IS_SPI_ALL_PERIPH_EXT(PERIPH) (((PERIPH) == SPI1) || \ + ((PERIPH) == SPI2) || \ + ((PERIPH) == SPI3) || \ + ((PERIPH) == SPI4) || \ + ((PERIPH) == I2S2ext) || \ + ((PERIPH) == I2S3ext)) + +#define IS_SPI_23_PERIPH(PERIPH) (((PERIPH) == SPI2) || \ + ((PERIPH) == SPI3)) + +#define IS_SPI_23_PERIPH_EXT(PERIPH) (((PERIPH) == SPI2) || \ + ((PERIPH) == SPI3) || \ + ((PERIPH) == I2S2ext) || \ + ((PERIPH) == I2S3ext)) + +#define IS_I2S_EXT_PERIPH(PERIPH) (((PERIPH) == I2S2ext) || \ + ((PERIPH) == I2S3ext)) + +/** @defgroup SPI_data_direction + * @{ + */ + +#define SPI_Direction_2Lines_FullDuplex ((uint16_t)0x0000) +#define SPI_Direction_2Lines_RxOnly ((uint16_t)0x0400) +#define SPI_Direction_1Line_Rx ((uint16_t)0x8000) +#define SPI_Direction_1Line_Tx ((uint16_t)0xC000) +#define IS_SPI_DIRECTION_MODE(MODE) (((MODE) == SPI_Direction_2Lines_FullDuplex) || \ + ((MODE) == SPI_Direction_2Lines_RxOnly) || \ + ((MODE) == SPI_Direction_1Line_Rx) || \ + ((MODE) == SPI_Direction_1Line_Tx)) +/** + * @} + */ + +/** @defgroup SPI_mode + * @{ + */ + +#define SPI_Mode_Master ((uint16_t)0x0104) +#define SPI_Mode_Slave ((uint16_t)0x0000) +#define IS_SPI_MODE(MODE) (((MODE) == SPI_Mode_Master) || \ + ((MODE) == SPI_Mode_Slave)) +/** + * @} + */ + +/** @defgroup SPI_data_size + * @{ + */ + +#define SPI_DataSize_4b ((uint16_t)0x0300) +#define SPI_DataSize_5b ((uint16_t)0x0400) +#define SPI_DataSize_6b ((uint16_t)0x0500) +#define SPI_DataSize_7b ((uint16_t)0x0600) +#define SPI_DataSize_8b ((uint16_t)0x0700) +#define SPI_DataSize_9b ((uint16_t)0x0800) +#define SPI_DataSize_10b ((uint16_t)0x0900) +#define SPI_DataSize_11b ((uint16_t)0x0A00) +#define SPI_DataSize_12b ((uint16_t)0x0B00) +#define SPI_DataSize_13b ((uint16_t)0x0C00) +#define SPI_DataSize_14b ((uint16_t)0x0D00) +#define SPI_DataSize_15b ((uint16_t)0x0E00) +#define SPI_DataSize_16b ((uint16_t)0x0F00) +#define IS_SPI_DATA_SIZE(SIZE) (((SIZE) == SPI_DataSize_4b) || \ + ((SIZE) == SPI_DataSize_5b) || \ + ((SIZE) == SPI_DataSize_6b) || \ + ((SIZE) == SPI_DataSize_7b) || \ + ((SIZE) == SPI_DataSize_8b) || \ + ((SIZE) == SPI_DataSize_9b) || \ + ((SIZE) == SPI_DataSize_10b) || \ + ((SIZE) == SPI_DataSize_11b) || \ + ((SIZE) == SPI_DataSize_12b) || \ + ((SIZE) == SPI_DataSize_13b) || \ + ((SIZE) == SPI_DataSize_14b) || \ + ((SIZE) == SPI_DataSize_15b) || \ + ((SIZE) == SPI_DataSize_16b)) +/** + * @} + */ + +/** @defgroup SPI_CRC_length + * @{ + */ + +#define SPI_CRCLength_8b ((uint16_t)0x0000) +#define SPI_CRCLength_16b ((uint16_t)0x0800) +#define IS_SPI_CRC_LENGTH(LENGTH) (((LENGTH) == SPI_CRCLength_8b) || \ + ((LENGTH) == SPI_CRCLength_16b)) +/** + * @} + */ + +/** @defgroup SPI_Clock_Polarity + * @{ + */ + +#define SPI_CPOL_Low ((uint16_t)0x0000) +#define SPI_CPOL_High ((uint16_t)0x0002) +#define IS_SPI_CPOL(CPOL) (((CPOL) == SPI_CPOL_Low) || \ + ((CPOL) == SPI_CPOL_High)) +/** + * @} + */ + +/** @defgroup SPI_Clock_Phase + * @{ + */ + +#define SPI_CPHA_1Edge ((uint16_t)0x0000) +#define SPI_CPHA_2Edge ((uint16_t)0x0001) +#define IS_SPI_CPHA(CPHA) (((CPHA) == SPI_CPHA_1Edge) || \ + ((CPHA) == SPI_CPHA_2Edge)) +/** + * @} + */ + +/** @defgroup SPI_Slave_Select_management + * @{ + */ + +#define SPI_NSS_Soft ((uint16_t)0x0200) +#define SPI_NSS_Hard ((uint16_t)0x0000) +#define IS_SPI_NSS(NSS) (((NSS) == SPI_NSS_Soft) || \ + ((NSS) == SPI_NSS_Hard)) +/** + * @} + */ + +/** @defgroup SPI_BaudRate_Prescaler + * @{ + */ + +#define SPI_BaudRatePrescaler_2 ((uint16_t)0x0000) +#define SPI_BaudRatePrescaler_4 ((uint16_t)0x0008) +#define SPI_BaudRatePrescaler_8 ((uint16_t)0x0010) +#define SPI_BaudRatePrescaler_16 ((uint16_t)0x0018) +#define SPI_BaudRatePrescaler_32 ((uint16_t)0x0020) +#define SPI_BaudRatePrescaler_64 ((uint16_t)0x0028) +#define SPI_BaudRatePrescaler_128 ((uint16_t)0x0030) +#define SPI_BaudRatePrescaler_256 ((uint16_t)0x0038) +#define IS_SPI_BAUDRATE_PRESCALER(PRESCALER) (((PRESCALER) == SPI_BaudRatePrescaler_2) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_4) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_8) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_16) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_32) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_64) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_128) || \ + ((PRESCALER) == SPI_BaudRatePrescaler_256)) +/** + * @} + */ + +/** @defgroup SPI_MSB_LSB_transmission + * @{ + */ + +#define SPI_FirstBit_MSB ((uint16_t)0x0000) +#define SPI_FirstBit_LSB ((uint16_t)0x0080) +#define IS_SPI_FIRST_BIT(BIT) (((BIT) == SPI_FirstBit_MSB) || \ + ((BIT) == SPI_FirstBit_LSB)) +/** + * @} + */ + +/** @defgroup I2S_Mode + * @{ + */ + +#define I2S_Mode_SlaveTx ((uint16_t)0x0000) +#define I2S_Mode_SlaveRx ((uint16_t)0x0100) +#define I2S_Mode_MasterTx ((uint16_t)0x0200) +#define I2S_Mode_MasterRx ((uint16_t)0x0300) +#define IS_I2S_MODE(MODE) (((MODE) == I2S_Mode_SlaveTx) || \ + ((MODE) == I2S_Mode_SlaveRx) || \ + ((MODE) == I2S_Mode_MasterTx)|| \ + ((MODE) == I2S_Mode_MasterRx)) +/** + * @} + */ + +/** @defgroup I2S_Standard + * @{ + */ + +#define I2S_Standard_Phillips ((uint16_t)0x0000) +#define I2S_Standard_MSB ((uint16_t)0x0010) +#define I2S_Standard_LSB ((uint16_t)0x0020) +#define I2S_Standard_PCMShort ((uint16_t)0x0030) +#define I2S_Standard_PCMLong ((uint16_t)0x00B0) +#define IS_I2S_STANDARD(STANDARD) (((STANDARD) == I2S_Standard_Phillips) || \ + ((STANDARD) == I2S_Standard_MSB) || \ + ((STANDARD) == I2S_Standard_LSB) || \ + ((STANDARD) == I2S_Standard_PCMShort) || \ + ((STANDARD) == I2S_Standard_PCMLong)) +/** + * @} + */ + +/** @defgroup I2S_Data_Format + * @{ + */ + +#define I2S_DataFormat_16b ((uint16_t)0x0000) +#define I2S_DataFormat_16bextended ((uint16_t)0x0001) +#define I2S_DataFormat_24b ((uint16_t)0x0003) +#define I2S_DataFormat_32b ((uint16_t)0x0005) +#define IS_I2S_DATA_FORMAT(FORMAT) (((FORMAT) == I2S_DataFormat_16b) || \ + ((FORMAT) == I2S_DataFormat_16bextended) || \ + ((FORMAT) == I2S_DataFormat_24b) || \ + ((FORMAT) == I2S_DataFormat_32b)) +/** + * @} + */ + +/** @defgroup I2S_MCLK_Output + * @{ + */ + +#define I2S_MCLKOutput_Enable ((uint16_t)0x0200) +#define I2S_MCLKOutput_Disable ((uint16_t)0x0000) +#define IS_I2S_MCLK_OUTPUT(OUTPUT) (((OUTPUT) == I2S_MCLKOutput_Enable) || \ + ((OUTPUT) == I2S_MCLKOutput_Disable)) +/** + * @} + */ + +/** @defgroup I2S_Audio_Frequency + * @{ + */ + +#define I2S_AudioFreq_192k ((uint32_t)192000) +#define I2S_AudioFreq_96k ((uint32_t)96000) +#define I2S_AudioFreq_48k ((uint32_t)48000) +#define I2S_AudioFreq_44k ((uint32_t)44100) +#define I2S_AudioFreq_32k ((uint32_t)32000) +#define I2S_AudioFreq_22k ((uint32_t)22050) +#define I2S_AudioFreq_16k ((uint32_t)16000) +#define I2S_AudioFreq_11k ((uint32_t)11025) +#define I2S_AudioFreq_8k ((uint32_t)8000) +#define I2S_AudioFreq_Default ((uint32_t)2) + +#define IS_I2S_AUDIO_FREQ(FREQ) ((((FREQ) >= I2S_AudioFreq_8k) && \ + ((FREQ) <= I2S_AudioFreq_192k)) || \ + ((FREQ) == I2S_AudioFreq_Default)) +/** + * @} + */ + +/** @defgroup I2S_Clock_Polarity + * @{ + */ + +#define I2S_CPOL_Low ((uint16_t)0x0000) +#define I2S_CPOL_High ((uint16_t)0x0008) +#define IS_I2S_CPOL(CPOL) (((CPOL) == I2S_CPOL_Low) || \ + ((CPOL) == I2S_CPOL_High)) +/** + * @} + */ + +/** @defgroup SPI_FIFO_reception_threshold + * @{ + */ + +#define SPI_RxFIFOThreshold_HF ((uint16_t)0x0000) +#define SPI_RxFIFOThreshold_QF ((uint16_t)0x1000) +#define IS_SPI_RX_FIFO_THRESHOLD(THRESHOLD) (((THRESHOLD) == SPI_RxFIFOThreshold_HF) || \ + ((THRESHOLD) == SPI_RxFIFOThreshold_QF)) +/** + * @} + */ + +/** @defgroup SPI_I2S_DMA_transfer_requests + * @{ + */ + +#define SPI_I2S_DMAReq_Tx ((uint16_t)0x0002) +#define SPI_I2S_DMAReq_Rx ((uint16_t)0x0001) +#define IS_SPI_I2S_DMA_REQ(REQ) ((((REQ) & (uint16_t)0xFFFC) == 0x00) && ((REQ) != 0x00)) +/** + * @} + */ + +/** @defgroup SPI_last_DMA_transfers + * @{ + */ + +#define SPI_LastDMATransfer_TxEvenRxEven ((uint16_t)0x0000) +#define SPI_LastDMATransfer_TxOddRxEven ((uint16_t)0x4000) +#define SPI_LastDMATransfer_TxEvenRxOdd ((uint16_t)0x2000) +#define SPI_LastDMATransfer_TxOddRxOdd ((uint16_t)0x6000) +#define IS_SPI_LAST_DMA_TRANSFER(TRANSFER) (((TRANSFER) == SPI_LastDMATransfer_TxEvenRxEven) || \ + ((TRANSFER) == SPI_LastDMATransfer_TxOddRxEven) || \ + ((TRANSFER) == SPI_LastDMATransfer_TxEvenRxOdd) || \ + ((TRANSFER) == SPI_LastDMATransfer_TxOddRxOdd)) +/** + * @} + */ +/** @defgroup SPI_NSS_internal_software_management + * @{ + */ + +#define SPI_NSSInternalSoft_Set ((uint16_t)0x0100) +#define SPI_NSSInternalSoft_Reset ((uint16_t)0xFEFF) +#define IS_SPI_NSS_INTERNAL(INTERNAL) (((INTERNAL) == SPI_NSSInternalSoft_Set) || \ + ((INTERNAL) == SPI_NSSInternalSoft_Reset)) +/** + * @} + */ + +/** @defgroup SPI_CRC_Transmit_Receive + * @{ + */ + +#define SPI_CRC_Tx ((uint8_t)0x00) +#define SPI_CRC_Rx ((uint8_t)0x01) +#define IS_SPI_CRC(CRC) (((CRC) == SPI_CRC_Tx) || ((CRC) == SPI_CRC_Rx)) +/** + * @} + */ + +/** @defgroup SPI_direction_transmit_receive + * @{ + */ + +#define SPI_Direction_Rx ((uint16_t)0xBFFF) +#define SPI_Direction_Tx ((uint16_t)0x4000) +#define IS_SPI_DIRECTION(DIRECTION) (((DIRECTION) == SPI_Direction_Rx) || \ + ((DIRECTION) == SPI_Direction_Tx)) +/** + * @} + */ + +/** @defgroup SPI_I2S_interrupts_definition + * @{ + */ + +#define SPI_I2S_IT_TXE ((uint8_t)0x71) +#define SPI_I2S_IT_RXNE ((uint8_t)0x60) +#define SPI_I2S_IT_ERR ((uint8_t)0x50) + +#define IS_SPI_I2S_CONFIG_IT(IT) (((IT) == SPI_I2S_IT_TXE) || \ + ((IT) == SPI_I2S_IT_RXNE) || \ + ((IT) == SPI_I2S_IT_ERR)) + +#define I2S_IT_UDR ((uint8_t)0x53) +#define SPI_IT_MODF ((uint8_t)0x55) +#define SPI_I2S_IT_OVR ((uint8_t)0x56) +#define SPI_I2S_IT_FRE ((uint8_t)0x58) + +#define IS_SPI_I2S_GET_IT(IT) (((IT) == SPI_I2S_IT_RXNE) || ((IT) == SPI_I2S_IT_TXE) || \ + ((IT) == SPI_I2S_IT_OVR) || ((IT) == SPI_IT_MODF) || \ + ((IT) == SPI_I2S_IT_FRE)|| ((IT) == I2S_IT_UDR)) +/** + * @} + */ + + +/** @defgroup SPI_transmission_fifo_status_level + * @{ + */ + +#define SPI_TransmissionFIFOStatus_Empty ((uint16_t)0x0000) +#define SPI_TransmissionFIFOStatus_1QuarterFull ((uint16_t)0x0800) +#define SPI_TransmissionFIFOStatus_HalfFull ((uint16_t)0x1000) +#define SPI_TransmissionFIFOStatus_Full ((uint16_t)0x1800) + +/** + * @} + */ + +/** @defgroup SPI_reception_fifo_status_level + * @{ + */ +#define SPI_ReceptionFIFOStatus_Empty ((uint16_t)0x0000) +#define SPI_ReceptionFIFOStatus_1QuarterFull ((uint16_t)0x0200) +#define SPI_ReceptionFIFOStatus_HalfFull ((uint16_t)0x0400) +#define SPI_ReceptionFIFOStatus_Full ((uint16_t)0x0600) + +/** + * @} + */ + + +/** @defgroup SPI_I2S_flags_definition + * @{ + */ + +#define SPI_I2S_FLAG_RXNE ((uint16_t)0x0001) +#define SPI_I2S_FLAG_TXE ((uint16_t)0x0002) +#define I2S_FLAG_CHSIDE ((uint16_t)0x0004) +#define I2S_FLAG_UDR ((uint16_t)0x0008) +#define SPI_FLAG_CRCERR ((uint16_t)0x0010) +#define SPI_FLAG_MODF ((uint16_t)0x0020) +#define SPI_I2S_FLAG_OVR ((uint16_t)0x0040) +#define SPI_I2S_FLAG_BSY ((uint16_t)0x0080) +#define SPI_I2S_FLAG_FRE ((uint16_t)0x0100) + + + +#define IS_SPI_CLEAR_FLAG(FLAG) (((FLAG) == SPI_FLAG_CRCERR)) +#define IS_SPI_I2S_GET_FLAG(FLAG) (((FLAG) == SPI_I2S_FLAG_BSY) || ((FLAG) == SPI_I2S_FLAG_OVR) || \ + ((FLAG) == SPI_FLAG_MODF) || ((FLAG) == SPI_FLAG_CRCERR) || \ + ((FLAG) == SPI_I2S_FLAG_TXE) || ((FLAG) == SPI_I2S_FLAG_RXNE)|| \ + ((FLAG) == SPI_I2S_FLAG_FRE)|| ((FLAG) == I2S_FLAG_CHSIDE)|| \ + ((FLAG) == I2S_FLAG_UDR)) +/** + * @} + */ + +/** @defgroup SPI_CRC_polynomial + * @{ + */ + +#define IS_SPI_CRC_POLYNOMIAL(POLYNOMIAL) ((POLYNOMIAL) >= 0x1) +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the SPI configuration to the default reset state*******/ +void SPI_I2S_DeInit(SPI_TypeDef* SPIx); + +/* Initialization and Configuration functions *********************************/ +void SPI_Init(SPI_TypeDef* SPIx, SPI_InitTypeDef* SPI_InitStruct); +void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct); +void SPI_StructInit(SPI_InitTypeDef* SPI_InitStruct); +void I2S_StructInit(I2S_InitTypeDef* I2S_InitStruct); +void SPI_TIModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState); +void SPI_NSSPulseModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState); +void SPI_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState); +void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState); +void SPI_DataSizeConfig(SPI_TypeDef* SPIx, uint16_t SPI_DataSize); +void SPI_RxFIFOThresholdConfig(SPI_TypeDef* SPIx, uint16_t SPI_RxFIFOThreshold); +void SPI_BiDirectionalLineConfig(SPI_TypeDef* SPIx, uint16_t SPI_Direction); +void SPI_NSSInternalSoftwareConfig(SPI_TypeDef* SPIx, uint16_t SPI_NSSInternalSoft); +void SPI_SSOutputCmd(SPI_TypeDef* SPIx, FunctionalState NewState); +void I2S_FullDuplexConfig(SPI_TypeDef* I2Sxext, I2S_InitTypeDef* I2S_InitStruct); + +/* Data transfers functions ***************************************************/ +void SPI_SendData8(SPI_TypeDef* SPIx, uint8_t Data); +void SPI_I2S_SendData16(SPI_TypeDef* SPIx, uint16_t Data); +uint8_t SPI_ReceiveData8(SPI_TypeDef* SPIx); +uint16_t SPI_I2S_ReceiveData16(SPI_TypeDef* SPIx); + +/* Hardware CRC Calculation functions *****************************************/ +void SPI_CRCLengthConfig(SPI_TypeDef* SPIx, uint16_t SPI_CRCLength); +void SPI_CalculateCRC(SPI_TypeDef* SPIx, FunctionalState NewState); +void SPI_TransmitCRC(SPI_TypeDef* SPIx); +uint16_t SPI_GetCRC(SPI_TypeDef* SPIx, uint8_t SPI_CRC); +uint16_t SPI_GetCRCPolynomial(SPI_TypeDef* SPIx); + +/* DMA transfers management functions *****************************************/ +void SPI_I2S_DMACmd(SPI_TypeDef* SPIx, uint16_t SPI_I2S_DMAReq, FunctionalState NewState); +void SPI_LastDMATransferCmd(SPI_TypeDef* SPIx, uint16_t SPI_LastDMATransfer); + +/* Interrupts and flags management functions **********************************/ +void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState); +uint16_t SPI_GetTransmissionFIFOStatus(SPI_TypeDef* SPIx); +uint16_t SPI_GetReceptionFIFOStatus(SPI_TypeDef* SPIx); +FlagStatus SPI_I2S_GetFlagStatus(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG); +void SPI_I2S_ClearFlag(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG); +ITStatus SPI_I2S_GetITStatus(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_SPI_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_syscfg.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_syscfg.h new file mode 100755 index 0000000000..cbf3bddc18 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_syscfg.h @@ -0,0 +1,427 @@ +/** + ****************************************************************************** + * @file stm32f30x_syscfg.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the SYSCFG firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/*!< Define to prevent recursive inclusion -----------------------------------*/ +#ifndef __STM32F30x_SYSCFG_H +#define __STM32F30x_SYSCFG_H + +#ifdef __cplusplus + extern "C" { +#endif + +/*!< Includes ----------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup SYSCFG + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup SYSCFG_Exported_Constants + * @{ + */ + +/** @defgroup SYSCFG_EXTI_Port_Sources + * @{ + */ +#define EXTI_PortSourceGPIOA ((uint8_t)0x00) +#define EXTI_PortSourceGPIOB ((uint8_t)0x01) +#define EXTI_PortSourceGPIOC ((uint8_t)0x02) +#define EXTI_PortSourceGPIOD ((uint8_t)0x03) +#define EXTI_PortSourceGPIOE ((uint8_t)0x04) +#define EXTI_PortSourceGPIOF ((uint8_t)0x05) +#define EXTI_PortSourceGPIOG ((uint8_t)0x06) +#define EXTI_PortSourceGPIOH ((uint8_t)0x07) + +#define IS_EXTI_PORT_SOURCE(PORTSOURCE) (((PORTSOURCE) == EXTI_PortSourceGPIOA) || \ + ((PORTSOURCE) == EXTI_PortSourceGPIOB) || \ + ((PORTSOURCE) == EXTI_PortSourceGPIOC) || \ + ((PORTSOURCE) == EXTI_PortSourceGPIOD) || \ + ((PORTSOURCE) == EXTI_PortSourceGPIOE) || \ + ((PORTSOURCE) == EXTI_PortSourceGPIOF)|| \ + ((PORTSOURCE) == EXTI_PortSourceGPIOG)|| \ + ((PORTSOURCE) == EXTI_PortSourceGPIOH)) +/** + * @} + */ + +/** @defgroup SYSCFG_EXTI_Pin_sources + * @{ + */ +#define EXTI_PinSource0 ((uint8_t)0x00) +#define EXTI_PinSource1 ((uint8_t)0x01) +#define EXTI_PinSource2 ((uint8_t)0x02) +#define EXTI_PinSource3 ((uint8_t)0x03) +#define EXTI_PinSource4 ((uint8_t)0x04) +#define EXTI_PinSource5 ((uint8_t)0x05) +#define EXTI_PinSource6 ((uint8_t)0x06) +#define EXTI_PinSource7 ((uint8_t)0x07) +#define EXTI_PinSource8 ((uint8_t)0x08) +#define EXTI_PinSource9 ((uint8_t)0x09) +#define EXTI_PinSource10 ((uint8_t)0x0A) +#define EXTI_PinSource11 ((uint8_t)0x0B) +#define EXTI_PinSource12 ((uint8_t)0x0C) +#define EXTI_PinSource13 ((uint8_t)0x0D) +#define EXTI_PinSource14 ((uint8_t)0x0E) +#define EXTI_PinSource15 ((uint8_t)0x0F) + +#define IS_EXTI_PIN_SOURCE(PINSOURCE) (((PINSOURCE) == EXTI_PinSource0) || \ + ((PINSOURCE) == EXTI_PinSource1) || \ + ((PINSOURCE) == EXTI_PinSource2) || \ + ((PINSOURCE) == EXTI_PinSource3) || \ + ((PINSOURCE) == EXTI_PinSource4) || \ + ((PINSOURCE) == EXTI_PinSource5) || \ + ((PINSOURCE) == EXTI_PinSource6) || \ + ((PINSOURCE) == EXTI_PinSource7) || \ + ((PINSOURCE) == EXTI_PinSource8) || \ + ((PINSOURCE) == EXTI_PinSource9) || \ + ((PINSOURCE) == EXTI_PinSource10) || \ + ((PINSOURCE) == EXTI_PinSource11) || \ + ((PINSOURCE) == EXTI_PinSource12) || \ + ((PINSOURCE) == EXTI_PinSource13) || \ + ((PINSOURCE) == EXTI_PinSource14) || \ + ((PINSOURCE) == EXTI_PinSource15)) +/** + * @} + */ + +/** @defgroup SYSCFG_Memory_Remap_Config + * @{ + */ +#define SYSCFG_MemoryRemap_Flash ((uint8_t)0x00) +#define SYSCFG_MemoryRemap_SystemMemory ((uint8_t)0x01) +#define SYSCFG_MemoryRemap_SRAM ((uint8_t)0x03) +#define SYSCFG_MemoryRemap_FMC ((uint8_t)0x04) + + +#define IS_SYSCFG_MEMORY_REMAP(REMAP) (((REMAP) == SYSCFG_MemoryRemap_Flash) || \ + ((REMAP) == SYSCFG_MemoryRemap_SystemMemory) || \ + ((REMAP) == SYSCFG_MemoryRemap_SRAM) || \ + ((REMAP) == SYSCFG_MemoryRemap_FMC)) + +/** + * @} + */ + +/** @defgroup SYSCFG_DMA_Remap_Config + * @{ + */ +#define SYSCFG_DMARemap_TIM17 SYSCFG_CFGR1_TIM17_DMA_RMP /*!< Remap TIM17 DMA requests from channel1 to channel2 */ +#define SYSCFG_DMARemap_TIM16 SYSCFG_CFGR1_TIM16_DMA_RMP /*!< Remap TIM16 DMA requests from channel3 to channel4 */ +#define SYSCFG_DMARemap_ADC2ADC4 SYSCFG_CFGR1_ADC24_DMA_RMP /*!< Remap ADC2 and ADC4 DMA requests */ + +#define SYSCFG_DMARemap_TIM6DAC1Ch1 SYSCFG_CFGR1_TIM6DAC1Ch1_DMA_RMP /* Remap TIM6/DAC1 Ch1 DMA requests */ +#define SYSCFG_DMARemap_TIM7DAC1Ch2 SYSCFG_CFGR1_TIM7DAC1Ch2_DMA_RMP /* Remap TIM7/DAC1 Ch2 DMA requests */ +#define SYSCFG_DMARemap_DAC2Ch1 SYSCFG_CFGR1_DAC2Ch1_DMA_RMP /* Remap DAC2 Ch1 DMA requests */ + +#define SYSCFG_DMARemapCh2_SPI1_RX ((uint32_t)0x80000003) /* Remap SPI1 RX DMA CH2 requests */ +#define SYSCFG_DMARemapCh4_SPI1_RX ((uint32_t)0x80000001) /* Remap SPI1 RX DMA CH4 requests */ +#define SYSCFG_DMARemapCh6_SPI1_RX ((uint32_t)0x80000002) /* Remap SPI1 RX DMA CH6 requests */ + +#define SYSCFG_DMARemapCh3_SPI1_TX ((uint32_t)0x8000000C) /* Remap SPI1 TX DMA CH2 requests */ +#define SYSCFG_DMARemapCh5_SPI1_TX ((uint32_t)0x80000004) /* Remap SPI1 TX DMA CH5 requests */ +#define SYSCFG_DMARemapCh7_SPI1_TX ((uint32_t)0x80000008) /* Remap SPI1 TX DMA CH7 requests */ + +#define SYSCFG_DMARemapCh7_I2C1_RX ((uint32_t)0x80000030) /* Remap I2C1 RX DMA CH7 requests */ +#define SYSCFG_DMARemapCh3_I2C1_RX ((uint32_t)0x80000010) /* Remap I2C1 RX DMA CH3 requests */ +#define SYSCFG_DMARemapCh5_I2C1_RX ((uint32_t)0x80000020) /* Remap I2C1 RX DMA CH5 requests */ + +#define SYSCFG_DMARemapCh6_I2C1_TX ((uint32_t)0x800000C0) /* Remap I2C1 TX DMA CH6 requests */ +#define SYSCFG_DMARemapCh2_I2C1_TX ((uint32_t)0x80000040) /* Remap I2C1 TX DMA CH2 requests */ +#define SYSCFG_DMARemapCh4_I2C1_TX ((uint32_t)0x80000080) /* Remap I2C1 TX DMA CH4 requests */ + +#define SYSCFG_DMARemapCh4_ADC2 ((uint32_t)0x80000300) /* Remap ADC2 DMA1 Ch4 requests */ +#define SYSCFG_DMARemapCh2_ADC2 ((uint32_t)0x80000200) /* Remap ADC2 DMA1 Ch2 requests */ + +/* SYSCFG_DMA_Remap_Legacy */ +#define SYSCFG_DMARemap_TIM6DAC1 SYSCFG_DMARemap_TIM6DAC1Ch1 /*!< Remap TIM6/DAC1 DMA requests */ +#define SYSCFG_DMARemap_TIM7DAC2 SYSCFG_DMARemap_TIM7DAC1Ch2 /*!< Remap TIM7/DAC2 DMA requests */ + +#define IS_SYSCFG_DMA_REMAP(REMAP) (((REMAP) == SYSCFG_DMARemap_TIM17) || \ + ((REMAP) == SYSCFG_DMARemap_TIM16) || \ + ((REMAP) == SYSCFG_DMARemap_ADC2ADC4) || \ + ((REMAP) == SYSCFG_DMARemap_TIM6DAC1Ch1) || \ + ((REMAP) == SYSCFG_DMARemap_TIM7DAC1Ch2) || \ + ((REMAP) == SYSCFG_DMARemap_DAC2Ch1) || \ + ((REMAP) == SYSCFG_DMARemapCh2_SPI1_RX) || \ + ((REMAP) == SYSCFG_DMARemapCh4_SPI1_RX) || \ + ((REMAP) == SYSCFG_DMARemapCh6_SPI1_RX) || \ + ((REMAP) == SYSCFG_DMARemapCh5_SPI1_TX) || \ + ((REMAP) == SYSCFG_DMARemapCh5_SPI1_TX) || \ + ((REMAP) == SYSCFG_DMARemapCh7_SPI1_TX) || \ + ((REMAP) == SYSCFG_DMARemapCh7_I2C1_RX) || \ + ((REMAP) == SYSCFG_DMARemapCh3_I2C1_RX) || \ + ((REMAP) == SYSCFG_DMARemapCh5_I2C1_RX) || \ + ((REMAP) == SYSCFG_DMARemapCh6_I2C1_TX) || \ + ((REMAP) == SYSCFG_DMARemapCh2_I2C1_TX) || \ + ((REMAP) == SYSCFG_DMARemapCh4_I2C1_TX) || \ + ((REMAP) == SYSCFG_DMARemapCh4_ADC2) || \ + ((REMAP) == SYSCFG_DMARemapCh2_ADC2)) + +/** + * @} + */ + +/** @defgroup SYSCFG_Trigger_Remap_Config + * @{ + */ +#define SYSCFG_TriggerRemap_DACTIM3 SYSCFG_CFGR1_DAC1_TRIG1_RMP /*!< Remap DAC trigger to TIM3 */ +#define SYSCFG_TriggerRemap_TIM1TIM17 SYSCFG_CFGR1_TIM1_ITR3_RMP /*!< Remap TIM1 ITR3 to TIM17 OC */ +#define SYSCFG_TriggerRemap_DACHRTIM1_TRIG1 ((uint32_t)0x80010000) /*!< Remap DAC trigger to HRTIM1 TRIG1 */ +#define SYSCFG_TriggerRemap_DACHRTIM1_TRIG2 ((uint32_t)0x80020000) /*!< Remap DAC trigger to HRTIM1 TRIG2 */ + +#define IS_SYSCFG_TRIGGER_REMAP(REMAP) (((REMAP) == SYSCFG_TriggerRemap_DACTIM3) || \ + ((REMAP) == SYSCFG_TriggerRemap_DACHRTIM1_TRIG1) || \ + ((REMAP) == SYSCFG_TriggerRemap_DACHRTIM1_TRIG2) || \ + ((REMAP) == SYSCFG_TriggerRemap_TIM1TIM17)) + +/** + * @} + */ + +/** @defgroup SYSCFG_EncoderRemap_Config + * @{ + */ +#define SYSCFG_EncoderRemap_No ((uint32_t)0x00000000) /*!< No redirection */ +#define SYSCFG_EncoderRemap_TIM2 SYSCFG_CFGR1_ENCODER_MODE_0 /*!< Timer 2 IC1 and IC2 connected to TIM15 IC1 and IC2 */ +#define SYSCFG_EncoderRemap_TIM3 SYSCFG_CFGR1_ENCODER_MODE_1 /*!< Timer 3 IC1 and IC2 connected to TIM15 IC1 and IC2 */ +#define SYSCFG_EncoderRemap_TIM4 SYSCFG_CFGR1_ENCODER_MODE /*!< Timer 4 IC1 and IC2 connected to TIM15 IC1 and IC2 */ + +#define IS_SYSCFG_ENCODER_REMAP(REMAP) (((REMAP) == SYSCFG_EncoderRemap_No) || \ + ((REMAP) == SYSCFG_EncoderRemap_TIM2) || \ + ((REMAP) == SYSCFG_EncoderRemap_TIM3) || \ + ((REMAP) == SYSCFG_EncoderRemap_TIM4)) + +/** + * @} + */ + +/** @defgroup SYSCFG_I2C_FastModePlus_Config + * @{ + */ +#define SYSCFG_I2CFastModePlus_PB6 SYSCFG_CFGR1_I2C_PB6_FMP /*!< Enable Fast Mode Plus on PB6 */ +#define SYSCFG_I2CFastModePlus_PB7 SYSCFG_CFGR1_I2C_PB7_FMP /*!< Enable Fast Mode Plus on PB7 */ +#define SYSCFG_I2CFastModePlus_PB8 SYSCFG_CFGR1_I2C_PB8_FMP /*!< Enable Fast Mode Plus on PB8 */ +#define SYSCFG_I2CFastModePlus_PB9 SYSCFG_CFGR1_I2C_PB9_FMP /*!< Enable Fast Mode Plus on PB9 */ +#define SYSCFG_I2CFastModePlus_I2C1 SYSCFG_CFGR1_I2C1_FMP /*!< Enable Fast Mode Plus on I2C1 pins */ +#define SYSCFG_I2CFastModePlus_I2C2 SYSCFG_CFGR1_I2C2_FMP /*!< Enable Fast Mode Plus on I2C2 pins */ +#define SYSCFG_I2CFastModePlus_I2C3 SYSCFG_CFGR1_I2C3_FMP /*!< Enable Fast Mode Plus on I2C3 pins */ + +#define IS_SYSCFG_I2C_FMP(PIN) (((PIN) == SYSCFG_I2CFastModePlus_PB6) || \ + ((PIN) == SYSCFG_I2CFastModePlus_PB7) || \ + ((PIN) == SYSCFG_I2CFastModePlus_PB8) || \ + ((PIN) == SYSCFG_I2CFastModePlus_PB9) || \ + ((PIN) == SYSCFG_I2CFastModePlus_I2C1) || \ + ((PIN) == SYSCFG_I2CFastModePlus_I2C2)|| \ + ((PIN) == SYSCFG_I2CFastModePlus_I2C3)) + +/** + * @} + */ + +/** @defgroup SYSCFG_FPU_Interrupt_Config + * @{ + */ +#define SYSCFG_IT_IXC SYSCFG_CFGR1_FPU_IE_5 /*!< Inexact Interrupt enable (interrupt disabled by default) */ +#define SYSCFG_IT_IDC SYSCFG_CFGR1_FPU_IE_4 /*!< Input denormal Interrupt enable */ +#define SYSCFG_IT_OFC SYSCFG_CFGR1_FPU_IE_3 /*!< Overflow Interrupt enable */ +#define SYSCFG_IT_UFC SYSCFG_CFGR1_FPU_IE_2 /*!< Underflow Interrupt enable */ +#define SYSCFG_IT_DZC SYSCFG_CFGR1_FPU_IE_1 /*!< Divide-by-zero Interrupt enable */ +#define SYSCFG_IT_IOC SYSCFG_CFGR1_FPU_IE_0 /*!< Invalid operation Interrupt enable */ + +#define IS_SYSCFG_IT(IT) ((((IT) & (uint32_t)0x03FFFFFF) == 0) && ((IT) != 0)) + +/** + * @} + */ + +/** @defgroup SYSCFG_Lock_Config + * @{ + */ +#define SYSCFG_Break_PVD SYSCFG_CFGR2_PVD_LOCK /*!< Enables and locks the PVD connection with TIM1/8/15/16/17 Break Input and also the PVD_EN and PVDSEL[2:0] bits of the Power Control Interface */ +#define SYSCFG_Break_SRAMParity SYSCFG_CFGR2_SRAM_PARITY_LOCK /*!< Enables and locks the SRAM_PARITY error signal with Break Input of TIM1/8/15/16/17 */ +#define SYSCFG_Break_Lockup SYSCFG_CFGR2_LOCKUP_LOCK /*!< Enables and locks the LOCKUP output of CortexM4 with Break Input of TIM1/8/15/16/17 */ + +#define IS_SYSCFG_LOCK_CONFIG(CONFIG) (((CONFIG) == SYSCFG_Break_PVD) || \ + ((CONFIG) == SYSCFG_Break_SRAMParity) || \ + ((CONFIG) == SYSCFG_Break_Lockup)) + +/** + * @} + */ + +/** @defgroup ADC_Trigger_Remapping + * @{ + */ +#define REMAPADCTRIGGER_ADC12_EXT2 SYSCFG_CFGR4_ADC12_EXT2_RMP /*!< Input trigger of ADC12 regular channel EXT2 + 0: No remap (TIM1_CC3) + 1: Remap (TIM20_TRGO) */ +#define REMAPADCTRIGGER_ADC12_EXT3 SYSCFG_CFGR4_ADC12_EXT3_RMP /*!< Input trigger of ADC12 regular channel EXT3 + 0: No remap (TIM2_CC2) + 1: Remap (TIM20_TRGO2) */ +#define REMAPADCTRIGGER_ADC12_EXT5 SYSCFG_CFGR4_ADC12_EXT5_RMP /*!< Input trigger of ADC12 regular channel EXT5 + 0: No remap (TIM4_CC4) + 1: Remap (TIM20_CC1) */ +#define REMAPADCTRIGGER_ADC12_EXT13 SYSCFG_CFGR4_ADC12_EXT13_RMP /*!< Input trigger of ADC12 regular channel EXT13 + 0: No remap (TIM6_TRGO) + 1: Remap (TIM20_CC2) */ +#define REMAPADCTRIGGER_ADC12_EXT15 SYSCFG_CFGR4_ADC12_EXT15_RMP /*!< Input trigger of ADC12 regular channel EXT15 + 0: No remap (TIM3_CC4) + 1: Remap (TIM20_CC3) */ +#define REMAPADCTRIGGER_ADC12_JEXT3 SYSCFG_CFGR4_ADC12_JEXT3_RMP /*!< Input trigger of ADC12 injected channel JEXT3 + 0: No remap (TIM2_CC1) + 1: Remap (TIM20_TRGO) */ +#define REMAPADCTRIGGER_ADC12_JEXT6 SYSCFG_CFGR4_ADC12_JEXT6_RMP /*!< Input trigger of ADC12 injected channel JEXT6 + 0: No remap (EXTI line 15) + 1: Remap (TIM20_TRGO2) */ +#define REMAPADCTRIGGER_ADC12_JEXT13 SYSCFG_CFGR4_ADC12_JEXT13_RMP /*!< Input trigger of ADC12 injected channel JEXT13 + 0: No remap (TIM3_CC1) + 1: Remap (TIM20_CC4) */ +#define REMAPADCTRIGGER_ADC34_EXT5 SYSCFG_CFGR4_ADC34_EXT5_RMP /*!< Input trigger of ADC34 regular channel EXT5 + 0: No remap (EXTI line 2) + 1: Remap (TIM20_TRGO) */ +#define REMAPADCTRIGGER_ADC34_EXT6 SYSCFG_CFGR4_ADC34_EXT6_RMP /*!< Input trigger of ADC34 regular channel EXT6 + 0: No remap (TIM4_CC1) + 1: Remap (TIM20_TRGO2) */ +#define REMAPADCTRIGGER_ADC34_EXT15 SYSCFG_CFGR4_ADC34_EXT15_RMP /*!< Input trigger of ADC34 regular channel EXT15 + 0: No remap (TIM2_CC1) + 1: Remap (TIM20_CC1) */ +#define REMAPADCTRIGGER_ADC34_JEXT5 SYSCFG_CFGR4_ADC34_JEXT5_RMP /*!< Input trigger of ADC34 injected channel JEXT5 + 0: No remap (TIM4_CC3) + 1: Remap (TIM20_TRGO) */ +#define REMAPADCTRIGGER_ADC34_JEXT11 SYSCFG_CFGR4_ADC34_JEXT11_RMP /*!< Input trigger of ADC34 injected channel JEXT11 + 0: No remap (TIM1_CC3) + 1: Remap (TIM20_TRGO2) */ +#define REMAPADCTRIGGER_ADC34_JEXT14 SYSCFG_CFGR4_ADC34_JEXT14_RMP /*!< Input trigger of ADC34 injected channel JEXT14 + 0: No remap (TIM7_TRGO) + 1: Remap (TIM20_CC2) */ + +#define IS_SYSCFG_ADC_TRIGGER_REMAP(RMP) (((RMP) == REMAPADCTRIGGER_ADC12_EXT2) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_EXT3) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_EXT5) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_EXT13) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_EXT15) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_JEXT3) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_JEXT6) || \ + ((RMP) == REMAPADCTRIGGER_ADC12_JEXT13) || \ + ((RMP) == REMAPADCTRIGGER_ADC34_EXT5) || \ + ((RMP) == REMAPADCTRIGGER_ADC34_EXT6) || \ + ((RMP) == REMAPADCTRIGGER_ADC34_EXT15) || \ + ((RMP) == REMAPADCTRIGGER_ADC34_JEXT5) || \ + ((RMP) == REMAPADCTRIGGER_ADC34_JEXT11) || \ + ((RMP) == REMAPADCTRIGGER_ADC34_JEXT14)) + +/** + * @} + */ + +/** @defgroup SYSCFG_SRAMWRP_Config + * @{ + */ +#define SYSCFG_SRAMWRP_Page0 SYSCFG_RCR_PAGE0 /*!< ICODE SRAM Write protection page 0 */ +#define SYSCFG_SRAMWRP_Page1 SYSCFG_RCR_PAGE1 /*!< ICODE SRAM Write protection page 1 */ +#define SYSCFG_SRAMWRP_Page2 SYSCFG_RCR_PAGE2 /*!< ICODE SRAM Write protection page 2 */ +#define SYSCFG_SRAMWRP_Page3 SYSCFG_RCR_PAGE3 /*!< ICODE SRAM Write protection page 3 */ +#define SYSCFG_SRAMWRP_Page4 SYSCFG_RCR_PAGE4 /*!< ICODE SRAM Write protection page 4 */ +#define SYSCFG_SRAMWRP_Page5 SYSCFG_RCR_PAGE5 /*!< ICODE SRAM Write protection page 5 */ +#define SYSCFG_SRAMWRP_Page6 SYSCFG_RCR_PAGE6 /*!< ICODE SRAM Write protection page 6 */ +#define SYSCFG_SRAMWRP_Page7 SYSCFG_RCR_PAGE7 /*!< ICODE SRAM Write protection page 7 */ +#define SYSCFG_SRAMWRP_Page8 SYSCFG_RCR_PAGE8 /*!< ICODE SRAM Write protection page 8 */ +#define SYSCFG_SRAMWRP_Page9 SYSCFG_RCR_PAGE9 /*!< ICODE SRAM Write protection page 9 */ +#define SYSCFG_SRAMWRP_Page10 SYSCFG_RCR_PAGE10 /*!< ICODE SRAM Write protection page 10 */ +#define SYSCFG_SRAMWRP_Page11 SYSCFG_RCR_PAGE11 /*!< ICODE SRAM Write protection page 11 */ +#define SYSCFG_SRAMWRP_Page12 SYSCFG_RCR_PAGE12 /*!< ICODE SRAM Write protection page 12 */ +#define SYSCFG_SRAMWRP_Page13 SYSCFG_RCR_PAGE13 /*!< ICODE SRAM Write protection page 13 */ +#define SYSCFG_SRAMWRP_Page14 SYSCFG_RCR_PAGE14 /*!< ICODE SRAM Write protection page 14 */ +#define SYSCFG_SRAMWRP_Page15 SYSCFG_RCR_PAGE15 /*!< ICODE SRAM Write protection page 15 */ + +#define IS_SYSCFG_PAGE(PAGE)((((PAGE) & (uint32_t)0xFFFF0000) == 0x00000000) && ((PAGE) != 0x00000000)) + +/** + * @} + */ + +/** @defgroup SYSCFG_flags_definition + * @{ + */ + +#define SYSCFG_FLAG_PE SYSCFG_CFGR2_SRAM_PE + +#define IS_SYSCFG_FLAG(FLAG) (((FLAG) == SYSCFG_FLAG_PE)) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Function used to set the SYSCFG configuration to the default reset state **/ +void SYSCFG_DeInit(void); + +/* SYSCFG configuration functions *********************************************/ +void SYSCFG_MemoryRemapConfig(uint32_t SYSCFG_MemoryRemap); +void SYSCFG_DMAChannelRemapConfig(uint32_t SYSCFG_DMARemap, FunctionalState NewState); +void SYSCFG_TriggerRemapConfig(uint32_t SYSCFG_TriggerRemap, FunctionalState NewState); +void SYSCFG_EncoderRemapConfig(uint32_t SYSCFG_EncoderRemap); +void SYSCFG_USBInterruptLineRemapCmd(FunctionalState NewState); +void SYSCFG_I2CFastModePlusConfig(uint32_t SYSCFG_I2CFastModePlus, FunctionalState NewState); +void SYSCFG_ITConfig(uint32_t SYSCFG_IT, FunctionalState NewState); +void SYSCFG_EXTILineConfig(uint8_t EXTI_PortSourceGPIOx, uint8_t EXTI_PinSourcex); +void SYSCFG_BreakConfig(uint32_t SYSCFG_Break); +void SYSCFG_BypassParityCheckDisable(void); +void SYSCFG_ADCTriggerRemapConfig(uint32_t SYSCFG_ADCTriggerRemap, FunctionalState NewState); +void SYSCFG_SRAMWRPEnable(uint32_t SYSCFG_SRAMWRP); +FlagStatus SYSCFG_GetFlagStatus(uint32_t SYSCFG_Flag); +void SYSCFG_ClearFlag(uint32_t SYSCFG_Flag); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_SYSCFG_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_tim.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_tim.h new file mode 100755 index 0000000000..4837de7514 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_tim.h @@ -0,0 +1,1360 @@ +/** + ****************************************************************************** + * @file stm32f30x_tim.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the TIM firmware + * library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_TIM_H +#define __STM32F30x_TIM_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup stm32f30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup TIM + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + +/** + * @brief TIM Time Base Init structure definition + * @note This structure is used with all TIMx except for TIM6 and TIM7. + */ + +typedef struct +{ + uint16_t TIM_Prescaler; /*!< Specifies the prescaler value used to divide the TIM clock. + This parameter can be a number between 0x0000 and 0xFFFF */ + + uint16_t TIM_CounterMode; /*!< Specifies the counter mode. + This parameter can be a value of @ref TIM_Counter_Mode */ + + uint32_t TIM_Period; /*!< Specifies the period value to be loaded into the active + Auto-Reload Register at the next update event. + This parameter must be a number between 0x0000 and 0xFFFF. */ + + uint16_t TIM_ClockDivision; /*!< Specifies the clock division. + This parameter can be a value of @ref TIM_Clock_Division_CKD */ + + uint16_t TIM_RepetitionCounter; /*!< Specifies the repetition counter value. Each time the RCR downcounter + reaches zero, an update event is generated and counting restarts + from the RCR value (N). + This means in PWM mode that (N+1) corresponds to: + - the number of PWM periods in edge-aligned mode + - the number of half PWM period in center-aligned mode + This parameter must be a number between 0x00 and 0xFF. + @note This parameter is valid only for TIM1 and TIM8. */ +} TIM_TimeBaseInitTypeDef; + +/** + * @brief TIM Output Compare Init structure definition + */ + +typedef struct +{ + uint32_t TIM_OCMode; /*!< Specifies the TIM mode. + This parameter can be a value of @ref TIM_Output_Compare_and_PWM_modes */ + + uint16_t TIM_OutputState; /*!< Specifies the TIM Output Compare state. + This parameter can be a value of @ref TIM_Output_Compare_State */ + + uint16_t TIM_OutputNState; /*!< Specifies the TIM complementary Output Compare state. + This parameter can be a value of @ref TIM_Output_Compare_N_State + @note This parameter is valid only for TIM1 and TIM8. */ + + uint32_t TIM_Pulse; /*!< Specifies the pulse value to be loaded into the Capture Compare Register. + This parameter can be a number between 0x0000 and 0xFFFF */ + + uint16_t TIM_OCPolarity; /*!< Specifies the output polarity. + This parameter can be a value of @ref TIM_Output_Compare_Polarity */ + + uint16_t TIM_OCNPolarity; /*!< Specifies the complementary output polarity. + This parameter can be a value of @ref TIM_Output_Compare_N_Polarity + @note This parameter is valid only for TIM1 and TIM8. */ + + uint16_t TIM_OCIdleState; /*!< Specifies the TIM Output Compare pin state during Idle state. + This parameter can be a value of @ref TIM_Output_Compare_Idle_State + @note This parameter is valid only for TIM1 and TIM8. */ + + uint16_t TIM_OCNIdleState; /*!< Specifies the TIM Output Compare pin state during Idle state. + This parameter can be a value of @ref TIM_Output_Compare_N_Idle_State + @note This parameter is valid only for TIM1 and TIM8. */ +} TIM_OCInitTypeDef; + +/** + * @brief TIM Input Capture Init structure definition + */ + +typedef struct +{ + + uint16_t TIM_Channel; /*!< Specifies the TIM channel. + This parameter can be a value of @ref TIM_Channel */ + + uint16_t TIM_ICPolarity; /*!< Specifies the active edge of the input signal. + This parameter can be a value of @ref TIM_Input_Capture_Polarity */ + + uint16_t TIM_ICSelection; /*!< Specifies the input. + This parameter can be a value of @ref TIM_Input_Capture_Selection */ + + uint16_t TIM_ICPrescaler; /*!< Specifies the Input Capture Prescaler. + This parameter can be a value of @ref TIM_Input_Capture_Prescaler */ + + uint16_t TIM_ICFilter; /*!< Specifies the input capture filter. + This parameter can be a number between 0x0 and 0xF */ +} TIM_ICInitTypeDef; + +/** + * @brief BDTR structure definition + * @note This structure is used only with TIM1 and TIM8. + */ + +typedef struct +{ + + uint16_t TIM_OSSRState; /*!< Specifies the Off-State selection used in Run mode. + This parameter can be a value of @ref TIM_OSSR_Off_State_Selection_for_Run_mode_state */ + + uint16_t TIM_OSSIState; /*!< Specifies the Off-State used in Idle state. + This parameter can be a value of @ref TIM_OSSI_Off_State_Selection_for_Idle_mode_state */ + + uint16_t TIM_LOCKLevel; /*!< Specifies the LOCK level parameters. + This parameter can be a value of @ref TIM_Lock_level */ + + uint16_t TIM_DeadTime; /*!< Specifies the delay time between the switching-off and the + switching-on of the outputs. + This parameter can be a number between 0x00 and 0xFF */ + + uint16_t TIM_Break; /*!< Specifies whether the TIM Break input is enabled or not. + This parameter can be a value of @ref TIM_Break_Input_enable_disable */ + + uint16_t TIM_BreakPolarity; /*!< Specifies the TIM Break Input pin polarity. + This parameter can be a value of @ref TIM_Break_Polarity */ + + uint16_t TIM_AutomaticOutput; /*!< Specifies whether the TIM Automatic Output feature is enabled or not. + This parameter can be a value of @ref TIM_AOE_Bit_Set_Reset */ +} TIM_BDTRInitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup TIM_Exported_constants + * @{ + */ + +#define IS_TIM_ALL_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM2) || \ + ((PERIPH) == TIM3) || \ + ((PERIPH) == TIM4) || \ + ((PERIPH) == TIM6) || \ + ((PERIPH) == TIM7) || \ + ((PERIPH) == TIM8) || \ + ((PERIPH) == TIM15) || \ + ((PERIPH) == TIM16) || \ + ((PERIPH) == TIM17) || \ + ((PERIPH) == TIM20)) + +/* LIST1: TIM1, TIM2, TIM3, TIM4, TIM8, TIM15, TIM16, TIM20 and TIM17 */ +#define IS_TIM_LIST1_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM2) || \ + ((PERIPH) == TIM3) || \ + ((PERIPH) == TIM4) || \ + ((PERIPH) == TIM8) || \ + ((PERIPH) == TIM15) || \ + ((PERIPH) == TIM16) || \ + ((PERIPH) == TIM17) || \ + ((PERIPH) == TIM20)) + +/* LIST2: TIM1, TIM2, TIM3, TIM4, TIM8, TIM20 and TIM15 */ +#define IS_TIM_LIST2_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM2) || \ + ((PERIPH) == TIM3) || \ + ((PERIPH) == TIM4) || \ + ((PERIPH) == TIM8) || \ + ((PERIPH) == TIM15)|| \ + ((PERIPH) == TIM20)) + +/* LIST3: TIM1, TIM2, TIM3, TIM4, TIM20 and TIM8 */ +#define IS_TIM_LIST3_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM2) || \ + ((PERIPH) == TIM3) || \ + ((PERIPH) == TIM4) || \ + ((PERIPH) == TIM8) || \ + ((PERIPH) == TIM20)) + +/* LIST4: TIM1, TIM20 and TIM8 */ +#define IS_TIM_LIST4_PERIPH(PERIPH) (((PERIPH) == TIM1) ||\ + ((PERIPH) == TIM8) ||\ + ((PERIPH) == TIM20)) + +/* LIST5: TIM1, TIM2, TIM3, TIM4, TIM5, TIM6, TIM7 and TIM8 */ +#define IS_TIM_LIST5_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM2) || \ + ((PERIPH) == TIM3) || \ + ((PERIPH) == TIM4) || \ + ((PERIPH) == TIM6) || \ + ((PERIPH) == TIM7) || \ + ((PERIPH) == TIM8)) +/* LIST6: TIM1, TIM8, TIM15, TIM16, TIM20 and TIM17 */ +#define IS_TIM_LIST6_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM8) || \ + ((PERIPH) == TIM15) || \ + ((PERIPH) == TIM16) || \ + ((PERIPH) == TIM17) || \ + ((PERIPH) == TIM20)) + +/* LIST5: TIM1, TIM2, TIM3, TIM4, TIM5, TIM6, TIM7, TIM20 and TIM8 */ +#define IS_TIM_LIST7_PERIPH(PERIPH) (((PERIPH) == TIM1) || \ + ((PERIPH) == TIM2) || \ + ((PERIPH) == TIM3) || \ + ((PERIPH) == TIM4) || \ + ((PERIPH) == TIM6) || \ + ((PERIPH) == TIM7) || \ + ((PERIPH) == TIM8) || \ + ((PERIPH) == TIM15)|| \ + ((PERIPH) == TIM20)) + +/* LIST8: TIM16 (option register) */ +#define IS_TIM_LIST8_PERIPH(PERIPH) (((PERIPH) == TIM16)|| \ + ((PERIPH) == TIM1) ||\ + ((PERIPH) == TIM8) ||\ + ((PERIPH) == TIM20)) + +/** @defgroup TIM_Output_Compare_and_PWM_modes + * @{ + */ + +#define TIM_OCMode_Timing ((uint32_t)0x00000) +#define TIM_OCMode_Active ((uint32_t)0x00010) +#define TIM_OCMode_Inactive ((uint32_t)0x00020) +#define TIM_OCMode_Toggle ((uint32_t)0x00030) +#define TIM_OCMode_PWM1 ((uint32_t)0x00060) +#define TIM_OCMode_PWM2 ((uint32_t)0x00070) + +#define TIM_OCMode_Retrigerrable_OPM1 ((uint32_t)0x10000) +#define TIM_OCMode_Retrigerrable_OPM2 ((uint32_t)0x10010) +#define TIM_OCMode_Combined_PWM1 ((uint32_t)0x10040) +#define TIM_OCMode_Combined_PWM2 ((uint32_t)0x10050) +#define TIM_OCMode_Asymmetric_PWM1 ((uint32_t)0x10060) +#define TIM_OCMode_Asymmetric_PWM2 ((uint32_t)0x10070) + +#define IS_TIM_OC_MODE(MODE) (((MODE) == TIM_OCMode_Timing) || \ + ((MODE) == TIM_OCMode_Active) || \ + ((MODE) == TIM_OCMode_Inactive) || \ + ((MODE) == TIM_OCMode_Toggle)|| \ + ((MODE) == TIM_OCMode_PWM1) || \ + ((MODE) == TIM_OCMode_PWM2) || \ + ((MODE) == TIM_OCMode_Retrigerrable_OPM1) || \ + ((MODE) == TIM_OCMode_Retrigerrable_OPM2) || \ + ((MODE) == TIM_OCMode_Combined_PWM1) || \ + ((MODE) == TIM_OCMode_Combined_PWM2) || \ + ((MODE) == TIM_OCMode_Asymmetric_PWM1) || \ + ((MODE) == TIM_OCMode_Asymmetric_PWM2)) + +#define IS_TIM_OCM(MODE) (((MODE) == TIM_OCMode_Timing) || \ + ((MODE) == TIM_OCMode_Active) || \ + ((MODE) == TIM_OCMode_Inactive) || \ + ((MODE) == TIM_OCMode_Toggle)|| \ + ((MODE) == TIM_OCMode_PWM1) || \ + ((MODE) == TIM_OCMode_PWM2) || \ + ((MODE) == TIM_ForcedAction_Active) || \ + ((MODE) == TIM_ForcedAction_InActive) || \ + ((MODE) == TIM_OCMode_Retrigerrable_OPM1) || \ + ((MODE) == TIM_OCMode_Retrigerrable_OPM2) || \ + ((MODE) == TIM_OCMode_Combined_PWM1) || \ + ((MODE) == TIM_OCMode_Combined_PWM2) || \ + ((MODE) == TIM_OCMode_Asymmetric_PWM1) || \ + ((MODE) == TIM_OCMode_Asymmetric_PWM2)) +/** + * @} + */ + +/** @defgroup TIM_One_Pulse_Mode + * @{ + */ + +#define TIM_OPMode_Single ((uint16_t)0x0008) +#define TIM_OPMode_Repetitive ((uint16_t)0x0000) +#define IS_TIM_OPM_MODE(MODE) (((MODE) == TIM_OPMode_Single) || \ + ((MODE) == TIM_OPMode_Repetitive)) +/** + * @} + */ + +/** @defgroup TIM_Channel + * @{ + */ + +#define TIM_Channel_1 ((uint16_t)0x0000) +#define TIM_Channel_2 ((uint16_t)0x0004) +#define TIM_Channel_3 ((uint16_t)0x0008) +#define TIM_Channel_4 ((uint16_t)0x000C) +#define TIM_Channel_5 ((uint16_t)0x0010) +#define TIM_Channel_6 ((uint16_t)0x0014) + +#define IS_TIM_CHANNEL(CHANNEL) (((CHANNEL) == TIM_Channel_1) || \ + ((CHANNEL) == TIM_Channel_2) || \ + ((CHANNEL) == TIM_Channel_3) || \ + ((CHANNEL) == TIM_Channel_4)) + +#define IS_TIM_PWMI_CHANNEL(CHANNEL) (((CHANNEL) == TIM_Channel_1) || \ + ((CHANNEL) == TIM_Channel_2)) +#define IS_TIM_COMPLEMENTARY_CHANNEL(CHANNEL) (((CHANNEL) == TIM_Channel_1) || \ + ((CHANNEL) == TIM_Channel_2) || \ + ((CHANNEL) == TIM_Channel_3)) +/** + * @} + */ + +/** @defgroup TIM_Clock_Division_CKD + * @{ + */ + +#define TIM_CKD_DIV1 ((uint16_t)0x0000) +#define TIM_CKD_DIV2 ((uint16_t)0x0100) +#define TIM_CKD_DIV4 ((uint16_t)0x0200) +#define IS_TIM_CKD_DIV(DIV) (((DIV) == TIM_CKD_DIV1) || \ + ((DIV) == TIM_CKD_DIV2) || \ + ((DIV) == TIM_CKD_DIV4)) +/** + * @} + */ + +/** @defgroup TIM_Counter_Mode + * @{ + */ + +#define TIM_CounterMode_Up ((uint16_t)0x0000) +#define TIM_CounterMode_Down ((uint16_t)0x0010) +#define TIM_CounterMode_CenterAligned1 ((uint16_t)0x0020) +#define TIM_CounterMode_CenterAligned2 ((uint16_t)0x0040) +#define TIM_CounterMode_CenterAligned3 ((uint16_t)0x0060) +#define IS_TIM_COUNTER_MODE(MODE) (((MODE) == TIM_CounterMode_Up) || \ + ((MODE) == TIM_CounterMode_Down) || \ + ((MODE) == TIM_CounterMode_CenterAligned1) || \ + ((MODE) == TIM_CounterMode_CenterAligned2) || \ + ((MODE) == TIM_CounterMode_CenterAligned3)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_Polarity + * @{ + */ + +#define TIM_OCPolarity_High ((uint16_t)0x0000) +#define TIM_OCPolarity_Low ((uint16_t)0x0002) +#define IS_TIM_OC_POLARITY(POLARITY) (((POLARITY) == TIM_OCPolarity_High) || \ + ((POLARITY) == TIM_OCPolarity_Low)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_N_Polarity + * @{ + */ + +#define TIM_OCNPolarity_High ((uint16_t)0x0000) +#define TIM_OCNPolarity_Low ((uint16_t)0x0008) +#define IS_TIM_OCN_POLARITY(POLARITY) (((POLARITY) == TIM_OCNPolarity_High) || \ + ((POLARITY) == TIM_OCNPolarity_Low)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_State + * @{ + */ + +#define TIM_OutputState_Disable ((uint16_t)0x0000) +#define TIM_OutputState_Enable ((uint16_t)0x0001) +#define IS_TIM_OUTPUT_STATE(STATE) (((STATE) == TIM_OutputState_Disable) || \ + ((STATE) == TIM_OutputState_Enable)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_N_State + * @{ + */ + +#define TIM_OutputNState_Disable ((uint16_t)0x0000) +#define TIM_OutputNState_Enable ((uint16_t)0x0004) +#define IS_TIM_OUTPUTN_STATE(STATE) (((STATE) == TIM_OutputNState_Disable) || \ + ((STATE) == TIM_OutputNState_Enable)) +/** + * @} + */ + +/** @defgroup TIM_Capture_Compare_State + * @{ + */ + +#define TIM_CCx_Enable ((uint16_t)0x0001) +#define TIM_CCx_Disable ((uint16_t)0x0000) +#define IS_TIM_CCX(CCX) (((CCX) == TIM_CCx_Enable) || \ + ((CCX) == TIM_CCx_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Capture_Compare_N_State + * @{ + */ + +#define TIM_CCxN_Enable ((uint16_t)0x0004) +#define TIM_CCxN_Disable ((uint16_t)0x0000) +#define IS_TIM_CCXN(CCXN) (((CCXN) == TIM_CCxN_Enable) || \ + ((CCXN) == TIM_CCxN_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Break_Input_enable_disable + * @{ + */ + +#define TIM_Break_Enable ((uint16_t)0x1000) +#define TIM_Break_Disable ((uint16_t)0x0000) +#define IS_TIM_BREAK_STATE(STATE) (((STATE) == TIM_Break_Enable) || \ + ((STATE) == TIM_Break_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Break1_Input_enable_disable + * @{ + */ + +#define TIM_Break1_Enable ((uint32_t)0x00001000) +#define TIM_Break1_Disable ((uint32_t)0x00000000) +#define IS_TIM_BREAK1_STATE(STATE) (((STATE) == TIM_Break1_Enable) || \ + ((STATE) == TIM_Break1_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Break2_Input_enable_disable + * @{ + */ + +#define TIM_Break2_Enable ((uint32_t)0x01000000) +#define TIM_Break2_Disable ((uint32_t)0x00000000) +#define IS_TIM_BREAK2_STATE(STATE) (((STATE) == TIM_Break2_Enable) || \ + ((STATE) == TIM_Break2_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Break_Polarity + * @{ + */ + +#define TIM_BreakPolarity_Low ((uint16_t)0x0000) +#define TIM_BreakPolarity_High ((uint16_t)0x2000) +#define IS_TIM_BREAK_POLARITY(POLARITY) (((POLARITY) == TIM_BreakPolarity_Low) || \ + ((POLARITY) == TIM_BreakPolarity_High)) +/** + * @} + */ + +/** @defgroup TIM_Break1_Polarity + * @{ + */ + +#define TIM_Break1Polarity_Low ((uint32_t)0x00000000) +#define TIM_Break1Polarity_High ((uint32_t)0x00002000) +#define IS_TIM_BREAK1_POLARITY(POLARITY) (((POLARITY) == TIM_Break1Polarity_Low) || \ + ((POLARITY) == TIM_Break1Polarity_High)) +/** + * @} + */ + +/** @defgroup TIM_Break2_Polarity + * @{ + */ + +#define TIM_Break2Polarity_Low ((uint32_t)0x00000000) +#define TIM_Break2Polarity_High ((uint32_t)0x02000000) +#define IS_TIM_BREAK2_POLARITY(POLARITY) (((POLARITY) == TIM_Break2Polarity_Low) || \ + ((POLARITY) == TIM_Break2Polarity_High)) +/** + * @} + */ + +/** @defgroup TIM_Break1_Filter + * @{ + */ + +#define IS_TIM_BREAK1_FILTER(FILTER) ((FILTER) <= 0xF) +/** + * @} + */ + +/** @defgroup TIM_Break2_Filter + * @{ + */ + +#define IS_TIM_BREAK2_FILTER(FILTER) ((FILTER) <= 0xF) +/** + * @} + */ + +/** @defgroup TIM_AOE_Bit_Set_Reset + * @{ + */ + +#define TIM_AutomaticOutput_Enable ((uint16_t)0x4000) +#define TIM_AutomaticOutput_Disable ((uint16_t)0x0000) +#define IS_TIM_AUTOMATIC_OUTPUT_STATE(STATE) (((STATE) == TIM_AutomaticOutput_Enable) || \ + ((STATE) == TIM_AutomaticOutput_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Lock_level + * @{ + */ + +#define TIM_LOCKLevel_OFF ((uint16_t)0x0000) +#define TIM_LOCKLevel_1 ((uint16_t)0x0100) +#define TIM_LOCKLevel_2 ((uint16_t)0x0200) +#define TIM_LOCKLevel_3 ((uint16_t)0x0300) +#define IS_TIM_LOCK_LEVEL(LEVEL) (((LEVEL) == TIM_LOCKLevel_OFF) || \ + ((LEVEL) == TIM_LOCKLevel_1) || \ + ((LEVEL) == TIM_LOCKLevel_2) || \ + ((LEVEL) == TIM_LOCKLevel_3)) +/** + * @} + */ + +/** @defgroup TIM_OSSI_Off_State_Selection_for_Idle_mode_state + * @{ + */ + +#define TIM_OSSIState_Enable ((uint16_t)0x0400) +#define TIM_OSSIState_Disable ((uint16_t)0x0000) +#define IS_TIM_OSSI_STATE(STATE) (((STATE) == TIM_OSSIState_Enable) || \ + ((STATE) == TIM_OSSIState_Disable)) +/** + * @} + */ + +/** @defgroup TIM_OSSR_Off_State_Selection_for_Run_mode_state + * @{ + */ + +#define TIM_OSSRState_Enable ((uint16_t)0x0800) +#define TIM_OSSRState_Disable ((uint16_t)0x0000) +#define IS_TIM_OSSR_STATE(STATE) (((STATE) == TIM_OSSRState_Enable) || \ + ((STATE) == TIM_OSSRState_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_Idle_State + * @{ + */ + +#define TIM_OCIdleState_Set ((uint16_t)0x0100) +#define TIM_OCIdleState_Reset ((uint16_t)0x0000) +#define IS_TIM_OCIDLE_STATE(STATE) (((STATE) == TIM_OCIdleState_Set) || \ + ((STATE) == TIM_OCIdleState_Reset)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_N_Idle_State + * @{ + */ + +#define TIM_OCNIdleState_Set ((uint16_t)0x0200) +#define TIM_OCNIdleState_Reset ((uint16_t)0x0000) +#define IS_TIM_OCNIDLE_STATE(STATE) (((STATE) == TIM_OCNIdleState_Set) || \ + ((STATE) == TIM_OCNIdleState_Reset)) +/** + * @} + */ + +/** @defgroup TIM_Input_Capture_Polarity + * @{ + */ + +#define TIM_ICPolarity_Rising ((uint16_t)0x0000) +#define TIM_ICPolarity_Falling ((uint16_t)0x0002) +#define TIM_ICPolarity_BothEdge ((uint16_t)0x000A) +#define IS_TIM_IC_POLARITY(POLARITY) (((POLARITY) == TIM_ICPolarity_Rising) || \ + ((POLARITY) == TIM_ICPolarity_Falling)|| \ + ((POLARITY) == TIM_ICPolarity_BothEdge)) +/** + * @} + */ + +/** @defgroup TIM_Input_Capture_Selection + * @{ + */ + +#define TIM_ICSelection_DirectTI ((uint16_t)0x0001) /*!< TIM Input 1, 2, 3 or 4 is selected to be + connected to IC1, IC2, IC3 or IC4, respectively */ +#define TIM_ICSelection_IndirectTI ((uint16_t)0x0002) /*!< TIM Input 1, 2, 3 or 4 is selected to be + connected to IC2, IC1, IC4 or IC3, respectively. */ +#define TIM_ICSelection_TRC ((uint16_t)0x0003) /*!< TIM Input 1, 2, 3 or 4 is selected to be connected to TRC. */ +#define IS_TIM_IC_SELECTION(SELECTION) (((SELECTION) == TIM_ICSelection_DirectTI) || \ + ((SELECTION) == TIM_ICSelection_IndirectTI) || \ + ((SELECTION) == TIM_ICSelection_TRC)) +/** + * @} + */ + +/** @defgroup TIM_Input_Capture_Prescaler + * @{ + */ + +#define TIM_ICPSC_DIV1 ((uint16_t)0x0000) /*!< Capture performed each time an edge is detected on the capture input. */ +#define TIM_ICPSC_DIV2 ((uint16_t)0x0004) /*!< Capture performed once every 2 events. */ +#define TIM_ICPSC_DIV4 ((uint16_t)0x0008) /*!< Capture performed once every 4 events. */ +#define TIM_ICPSC_DIV8 ((uint16_t)0x000C) /*!< Capture performed once every 8 events. */ +#define IS_TIM_IC_PRESCALER(PRESCALER) (((PRESCALER) == TIM_ICPSC_DIV1) || \ + ((PRESCALER) == TIM_ICPSC_DIV2) || \ + ((PRESCALER) == TIM_ICPSC_DIV4) || \ + ((PRESCALER) == TIM_ICPSC_DIV8)) +/** + * @} + */ + +/** @defgroup TIM_interrupt_sources + * @{ + */ + +#define TIM_IT_Update ((uint16_t)0x0001) +#define TIM_IT_CC1 ((uint16_t)0x0002) +#define TIM_IT_CC2 ((uint16_t)0x0004) +#define TIM_IT_CC3 ((uint16_t)0x0008) +#define TIM_IT_CC4 ((uint16_t)0x0010) +#define TIM_IT_COM ((uint16_t)0x0020) +#define TIM_IT_Trigger ((uint16_t)0x0040) +#define TIM_IT_Break ((uint16_t)0x0080) +#define IS_TIM_IT(IT) ((((IT) & (uint16_t)0xFF00) == 0x0000) && ((IT) != 0x0000)) + +#define IS_TIM_GET_IT(IT) (((IT) == TIM_IT_Update) || \ + ((IT) == TIM_IT_CC1) || \ + ((IT) == TIM_IT_CC2) || \ + ((IT) == TIM_IT_CC3) || \ + ((IT) == TIM_IT_CC4) || \ + ((IT) == TIM_IT_COM) || \ + ((IT) == TIM_IT_Trigger) || \ + ((IT) == TIM_IT_Break)) +/** + * @} + */ + +/** @defgroup TIM_DMA_Base_address + * @{ + */ + +#define TIM_DMABase_CR1 ((uint16_t)0x0000) +#define TIM_DMABase_CR2 ((uint16_t)0x0001) +#define TIM_DMABase_SMCR ((uint16_t)0x0002) +#define TIM_DMABase_DIER ((uint16_t)0x0003) +#define TIM_DMABase_SR ((uint16_t)0x0004) +#define TIM_DMABase_EGR ((uint16_t)0x0005) +#define TIM_DMABase_CCMR1 ((uint16_t)0x0006) +#define TIM_DMABase_CCMR2 ((uint16_t)0x0007) +#define TIM_DMABase_CCER ((uint16_t)0x0008) +#define TIM_DMABase_CNT ((uint16_t)0x0009) +#define TIM_DMABase_PSC ((uint16_t)0x000A) +#define TIM_DMABase_ARR ((uint16_t)0x000B) +#define TIM_DMABase_RCR ((uint16_t)0x000C) +#define TIM_DMABase_CCR1 ((uint16_t)0x000D) +#define TIM_DMABase_CCR2 ((uint16_t)0x000E) +#define TIM_DMABase_CCR3 ((uint16_t)0x000F) +#define TIM_DMABase_CCR4 ((uint16_t)0x0010) +#define TIM_DMABase_BDTR ((uint16_t)0x0011) +#define TIM_DMABase_DCR ((uint16_t)0x0012) +#define TIM_DMABase_OR ((uint16_t)0x0013) +#define TIM_DMABase_CCMR3 ((uint16_t)0x0014) +#define TIM_DMABase_CCR5 ((uint16_t)0x0015) +#define TIM_DMABase_CCR6 ((uint16_t)0x0016) +#define IS_TIM_DMA_BASE(BASE) (((BASE) == TIM_DMABase_CR1) || \ + ((BASE) == TIM_DMABase_CR2) || \ + ((BASE) == TIM_DMABase_SMCR) || \ + ((BASE) == TIM_DMABase_DIER) || \ + ((BASE) == TIM_DMABase_SR) || \ + ((BASE) == TIM_DMABase_EGR) || \ + ((BASE) == TIM_DMABase_CCMR1) || \ + ((BASE) == TIM_DMABase_CCMR2) || \ + ((BASE) == TIM_DMABase_CCER) || \ + ((BASE) == TIM_DMABase_CNT) || \ + ((BASE) == TIM_DMABase_PSC) || \ + ((BASE) == TIM_DMABase_ARR) || \ + ((BASE) == TIM_DMABase_RCR) || \ + ((BASE) == TIM_DMABase_CCR1) || \ + ((BASE) == TIM_DMABase_CCR2) || \ + ((BASE) == TIM_DMABase_CCR3) || \ + ((BASE) == TIM_DMABase_CCR4) || \ + ((BASE) == TIM_DMABase_BDTR) || \ + ((BASE) == TIM_DMABase_DCR) || \ + ((BASE) == TIM_DMABase_OR) || \ + ((BASE) == TIM_DMABase_CCMR3) || \ + ((BASE) == TIM_DMABase_CCR5) || \ + ((BASE) == TIM_DMABase_CCR6)) +/** + * @} + */ + +/** @defgroup TIM_DMA_Burst_Length + * @{ + */ + +#define TIM_DMABurstLength_1Transfer ((uint16_t)0x0000) +#define TIM_DMABurstLength_2Transfers ((uint16_t)0x0100) +#define TIM_DMABurstLength_3Transfers ((uint16_t)0x0200) +#define TIM_DMABurstLength_4Transfers ((uint16_t)0x0300) +#define TIM_DMABurstLength_5Transfers ((uint16_t)0x0400) +#define TIM_DMABurstLength_6Transfers ((uint16_t)0x0500) +#define TIM_DMABurstLength_7Transfers ((uint16_t)0x0600) +#define TIM_DMABurstLength_8Transfers ((uint16_t)0x0700) +#define TIM_DMABurstLength_9Transfers ((uint16_t)0x0800) +#define TIM_DMABurstLength_10Transfers ((uint16_t)0x0900) +#define TIM_DMABurstLength_11Transfers ((uint16_t)0x0A00) +#define TIM_DMABurstLength_12Transfers ((uint16_t)0x0B00) +#define TIM_DMABurstLength_13Transfers ((uint16_t)0x0C00) +#define TIM_DMABurstLength_14Transfers ((uint16_t)0x0D00) +#define TIM_DMABurstLength_15Transfers ((uint16_t)0x0E00) +#define TIM_DMABurstLength_16Transfers ((uint16_t)0x0F00) +#define TIM_DMABurstLength_17Transfers ((uint16_t)0x1000) +#define TIM_DMABurstLength_18Transfers ((uint16_t)0x1100) +#define IS_TIM_DMA_LENGTH(LENGTH) (((LENGTH) == TIM_DMABurstLength_1Transfer) || \ + ((LENGTH) == TIM_DMABurstLength_2Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_3Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_4Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_5Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_6Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_7Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_8Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_9Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_10Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_11Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_12Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_13Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_14Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_15Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_16Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_17Transfers) || \ + ((LENGTH) == TIM_DMABurstLength_18Transfers)) +/** + * @} + */ + +/** @defgroup TIM_DMA_sources + * @{ + */ + +#define TIM_DMA_Update ((uint16_t)0x0100) +#define TIM_DMA_CC1 ((uint16_t)0x0200) +#define TIM_DMA_CC2 ((uint16_t)0x0400) +#define TIM_DMA_CC3 ((uint16_t)0x0800) +#define TIM_DMA_CC4 ((uint16_t)0x1000) +#define TIM_DMA_COM ((uint16_t)0x2000) +#define TIM_DMA_Trigger ((uint16_t)0x4000) +#define IS_TIM_DMA_SOURCE(SOURCE) ((((SOURCE) & (uint16_t)0x80FF) == 0x0000) && ((SOURCE) != 0x0000)) + +/** + * @} + */ + +/** @defgroup TIM_External_Trigger_Prescaler + * @{ + */ + +#define TIM_ExtTRGPSC_OFF ((uint16_t)0x0000) +#define TIM_ExtTRGPSC_DIV2 ((uint16_t)0x1000) +#define TIM_ExtTRGPSC_DIV4 ((uint16_t)0x2000) +#define TIM_ExtTRGPSC_DIV8 ((uint16_t)0x3000) +#define IS_TIM_EXT_PRESCALER(PRESCALER) (((PRESCALER) == TIM_ExtTRGPSC_OFF) || \ + ((PRESCALER) == TIM_ExtTRGPSC_DIV2) || \ + ((PRESCALER) == TIM_ExtTRGPSC_DIV4) || \ + ((PRESCALER) == TIM_ExtTRGPSC_DIV8)) +/** + * @} + */ + +/** @defgroup TIM_Internal_Trigger_Selection + * @{ + */ + +#define TIM_TS_ITR0 ((uint16_t)0x0000) +#define TIM_TS_ITR1 ((uint16_t)0x0010) +#define TIM_TS_ITR2 ((uint16_t)0x0020) +#define TIM_TS_ITR3 ((uint16_t)0x0030) +#define TIM_TS_TI1F_ED ((uint16_t)0x0040) +#define TIM_TS_TI1FP1 ((uint16_t)0x0050) +#define TIM_TS_TI2FP2 ((uint16_t)0x0060) +#define TIM_TS_ETRF ((uint16_t)0x0070) +#define IS_TIM_TRIGGER_SELECTION(SELECTION) (((SELECTION) == TIM_TS_ITR0) || \ + ((SELECTION) == TIM_TS_ITR1) || \ + ((SELECTION) == TIM_TS_ITR2) || \ + ((SELECTION) == TIM_TS_ITR3) || \ + ((SELECTION) == TIM_TS_TI1F_ED) || \ + ((SELECTION) == TIM_TS_TI1FP1) || \ + ((SELECTION) == TIM_TS_TI2FP2) || \ + ((SELECTION) == TIM_TS_ETRF)) +#define IS_TIM_INTERNAL_TRIGGER_SELECTION(SELECTION) (((SELECTION) == TIM_TS_ITR0) || \ + ((SELECTION) == TIM_TS_ITR1) || \ + ((SELECTION) == TIM_TS_ITR2) || \ + ((SELECTION) == TIM_TS_ITR3)) +/** + * @} + */ + +/** @defgroup TIM_TIx_External_Clock_Source + * @{ + */ + +#define TIM_TIxExternalCLK1Source_TI1 ((uint16_t)0x0050) +#define TIM_TIxExternalCLK1Source_TI2 ((uint16_t)0x0060) +#define TIM_TIxExternalCLK1Source_TI1ED ((uint16_t)0x0040) + +/** + * @} + */ + +/** @defgroup TIM_External_Trigger_Polarity + * @{ + */ +#define TIM_ExtTRGPolarity_Inverted ((uint16_t)0x8000) +#define TIM_ExtTRGPolarity_NonInverted ((uint16_t)0x0000) +#define IS_TIM_EXT_POLARITY(POLARITY) (((POLARITY) == TIM_ExtTRGPolarity_Inverted) || \ + ((POLARITY) == TIM_ExtTRGPolarity_NonInverted)) +/** + * @} + */ + +/** @defgroup TIM_Prescaler_Reload_Mode + * @{ + */ + +#define TIM_PSCReloadMode_Update ((uint16_t)0x0000) +#define TIM_PSCReloadMode_Immediate ((uint16_t)0x0001) +#define IS_TIM_PRESCALER_RELOAD(RELOAD) (((RELOAD) == TIM_PSCReloadMode_Update) || \ + ((RELOAD) == TIM_PSCReloadMode_Immediate)) +/** + * @} + */ + +/** @defgroup TIM_Forced_Action + * @{ + */ + +#define TIM_ForcedAction_Active ((uint16_t)0x0050) +#define TIM_ForcedAction_InActive ((uint16_t)0x0040) +#define IS_TIM_FORCED_ACTION(ACTION) (((ACTION) == TIM_ForcedAction_Active) || \ + ((ACTION) == TIM_ForcedAction_InActive)) +/** + * @} + */ + +/** @defgroup TIM_Encoder_Mode + * @{ + */ + +#define TIM_EncoderMode_TI1 ((uint16_t)0x0001) +#define TIM_EncoderMode_TI2 ((uint16_t)0x0002) +#define TIM_EncoderMode_TI12 ((uint16_t)0x0003) +#define IS_TIM_ENCODER_MODE(MODE) (((MODE) == TIM_EncoderMode_TI1) || \ + ((MODE) == TIM_EncoderMode_TI2) || \ + ((MODE) == TIM_EncoderMode_TI12)) +/** + * @} + */ + + +/** @defgroup TIM_Event_Source + * @{ + */ + +#define TIM_EventSource_Update ((uint16_t)0x0001) +#define TIM_EventSource_CC1 ((uint16_t)0x0002) +#define TIM_EventSource_CC2 ((uint16_t)0x0004) +#define TIM_EventSource_CC3 ((uint16_t)0x0008) +#define TIM_EventSource_CC4 ((uint16_t)0x0010) +#define TIM_EventSource_COM ((uint16_t)0x0020) +#define TIM_EventSource_Trigger ((uint16_t)0x0040) +#define TIM_EventSource_Break ((uint16_t)0x0080) +#define TIM_EventSource_Break2 ((uint16_t)0x0100) +#define IS_TIM_EVENT_SOURCE(SOURCE) ((((SOURCE) & (uint16_t)0xFE00) == 0x0000) && ((SOURCE) != 0x0000)) + +/** + * @} + */ + +/** @defgroup TIM_Update_Source + * @{ + */ + +#define TIM_UpdateSource_Global ((uint16_t)0x0000) /*!< Source of update is the counter overflow/underflow + or the setting of UG bit, or an update generation + through the slave mode controller. */ +#define TIM_UpdateSource_Regular ((uint16_t)0x0001) /*!< Source of update is counter overflow/underflow. */ +#define IS_TIM_UPDATE_SOURCE(SOURCE) (((SOURCE) == TIM_UpdateSource_Global) || \ + ((SOURCE) == TIM_UpdateSource_Regular)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_Preload_State + * @{ + */ + +#define TIM_OCPreload_Enable ((uint16_t)0x0008) +#define TIM_OCPreload_Disable ((uint16_t)0x0000) +#define IS_TIM_OCPRELOAD_STATE(STATE) (((STATE) == TIM_OCPreload_Enable) || \ + ((STATE) == TIM_OCPreload_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_Fast_State + * @{ + */ + +#define TIM_OCFast_Enable ((uint16_t)0x0004) +#define TIM_OCFast_Disable ((uint16_t)0x0000) +#define IS_TIM_OCFAST_STATE(STATE) (((STATE) == TIM_OCFast_Enable) || \ + ((STATE) == TIM_OCFast_Disable)) + +/** + * @} + */ + +/** @defgroup TIM_Output_Compare_Clear_State + * @{ + */ + +#define TIM_OCClear_Enable ((uint16_t)0x0080) +#define TIM_OCClear_Disable ((uint16_t)0x0000) +#define IS_TIM_OCCLEAR_STATE(STATE) (((STATE) == TIM_OCClear_Enable) || \ + ((STATE) == TIM_OCClear_Disable)) +/** + * @} + */ + +/** @defgroup TIM_Trigger_Output_Source + * @{ + */ + +#define TIM_TRGOSource_Reset ((uint16_t)0x0000) +#define TIM_TRGOSource_Enable ((uint16_t)0x0010) +#define TIM_TRGOSource_Update ((uint16_t)0x0020) +#define TIM_TRGOSource_OC1 ((uint16_t)0x0030) +#define TIM_TRGOSource_OC1Ref ((uint16_t)0x0040) +#define TIM_TRGOSource_OC2Ref ((uint16_t)0x0050) +#define TIM_TRGOSource_OC3Ref ((uint16_t)0x0060) +#define TIM_TRGOSource_OC4Ref ((uint16_t)0x0070) +#define IS_TIM_TRGO_SOURCE(SOURCE) (((SOURCE) == TIM_TRGOSource_Reset) || \ + ((SOURCE) == TIM_TRGOSource_Enable) || \ + ((SOURCE) == TIM_TRGOSource_Update) || \ + ((SOURCE) == TIM_TRGOSource_OC1) || \ + ((SOURCE) == TIM_TRGOSource_OC1Ref) || \ + ((SOURCE) == TIM_TRGOSource_OC2Ref) || \ + ((SOURCE) == TIM_TRGOSource_OC3Ref) || \ + ((SOURCE) == TIM_TRGOSource_OC4Ref)) + + +#define TIM_TRGO2Source_Reset ((uint32_t)0x00000000) +#define TIM_TRGO2Source_Enable ((uint32_t)0x00100000) +#define TIM_TRGO2Source_Update ((uint32_t)0x00200000) +#define TIM_TRGO2Source_OC1 ((uint32_t)0x00300000) +#define TIM_TRGO2Source_OC1Ref ((uint32_t)0x00400000) +#define TIM_TRGO2Source_OC2Ref ((uint32_t)0x00500000) +#define TIM_TRGO2Source_OC3Ref ((uint32_t)0x00600000) +#define TIM_TRGO2Source_OC4Ref ((uint32_t)0x00700000) +#define TIM_TRGO2Source_OC5Ref ((uint32_t)0x00800000) +#define TIM_TRGO2Source_OC6Ref ((uint32_t)0x00900000) +#define TIM_TRGO2Source_OC4Ref_RisingFalling ((uint32_t)0x00A00000) +#define TIM_TRGO2Source_OC6Ref_RisingFalling ((uint32_t)0x00B00000) +#define TIM_TRGO2Source_OC4RefRising_OC6RefRising ((uint32_t)0x00C00000) +#define TIM_TRGO2Source_OC4RefRising_OC6RefFalling ((uint32_t)0x00D00000) +#define TIM_TRGO2Source_OC5RefRising_OC6RefRising ((uint32_t)0x00E00000) +#define TIM_TRGO2Source_OC5RefRising_OC6RefFalling ((uint32_t)0x00F00000) +#define IS_TIM_TRGO2_SOURCE(SOURCE) (((SOURCE) == TIM_TRGO2Source_Reset) || \ + ((SOURCE) == TIM_TRGO2Source_Enable) || \ + ((SOURCE) == TIM_TRGO2Source_Update) || \ + ((SOURCE) == TIM_TRGO2Source_OC1) || \ + ((SOURCE) == TIM_TRGO2Source_OC1Ref) || \ + ((SOURCE) == TIM_TRGO2Source_OC2Ref) || \ + ((SOURCE) == TIM_TRGO2Source_OC3Ref) || \ + ((SOURCE) == TIM_TRGO2Source_OC4Ref) || \ + ((SOURCE) == TIM_TRGO2Source_OC5Ref) || \ + ((SOURCE) == TIM_TRGO2Source_OC6Ref) || \ + ((SOURCE) == TIM_TRGO2Source_OC4Ref_RisingFalling) || \ + ((SOURCE) == TIM_TRGO2Source_OC6Ref_RisingFalling) || \ + ((SOURCE) == TIM_TRGO2Source_OC4RefRising_OC6RefRising) || \ + ((SOURCE) == TIM_TRGO2Source_OC4RefRising_OC6RefFalling) || \ + ((SOURCE) == TIM_TRGO2Source_OC5RefRising_OC6RefRising) || \ + ((SOURCE) == TIM_TRGO2Source_OC5RefRising_OC6RefFalling)) +/** + * @} + */ + +/** @defgroup TIM_Slave_Mode + * @{ + */ + +#define TIM_SlaveMode_Reset ((uint32_t)0x00004) +#define TIM_SlaveMode_Gated ((uint32_t)0x00005) +#define TIM_SlaveMode_Trigger ((uint32_t)0x00006) +#define TIM_SlaveMode_External1 ((uint32_t)0x00007) +#define TIM_SlaveMode_Combined_ResetTrigger ((uint32_t)0x10000) +#define IS_TIM_SLAVE_MODE(MODE) (((MODE) == TIM_SlaveMode_Reset) || \ + ((MODE) == TIM_SlaveMode_Gated) || \ + ((MODE) == TIM_SlaveMode_Trigger) || \ + ((MODE) == TIM_SlaveMode_External1) || \ + ((MODE) == TIM_SlaveMode_Combined_ResetTrigger)) +/** + * @} + */ + +/** @defgroup TIM_Master_Slave_Mode + * @{ + */ + +#define TIM_MasterSlaveMode_Enable ((uint16_t)0x0080) +#define TIM_MasterSlaveMode_Disable ((uint16_t)0x0000) +#define IS_TIM_MSM_STATE(STATE) (((STATE) == TIM_MasterSlaveMode_Enable) || \ + ((STATE) == TIM_MasterSlaveMode_Disable)) +/** + * @} + */ +/** @defgroup TIM_Remap + * @{ + */ +#define TIM16_GPIO ((uint16_t)0x0000) +#define TIM16_RTC_CLK ((uint16_t)0x0001) +#define TIM16_HSEDiv32 ((uint16_t)0x0002) +#define TIM16_MCO ((uint16_t)0x0003) + +#define TIM1_ADC1_AWDG1 ((uint16_t)0x0001) +#define TIM1_ADC1_AWDG2 ((uint16_t)0x0002) +#define TIM1_ADC1_AWDG3 ((uint16_t)0x0003) +#define TIM1_ADC4_AWDG1 ((uint16_t)0x0004) +#define TIM1_ADC4_AWDG2 ((uint16_t)0x0008) +#define TIM1_ADC4_AWDG3 ((uint16_t)0x000C) + +#define TIM8_ADC2_AWDG1 ((uint16_t)0x0001) +#define TIM8_ADC2_AWDG2 ((uint16_t)0x0002) +#define TIM8_ADC2_AWDG3 ((uint16_t)0x0003) +#define TIM8_ADC3_AWDG1 ((uint16_t)0x0004) +#define TIM8_ADC3_AWDG2 ((uint16_t)0x0008) +#define TIM8_ADC3_AWDG3 ((uint16_t)0x000C) + +#define TIM20_ADC3_AWDG1 ((uint16_t)0x0001) +#define TIM20_ADC3_AWDG2 ((uint16_t)0x0002) +#define TIM20_ADC3_AWDG3 ((uint16_t)0x0003) +#define TIM20_ADC4_AWDG1 ((uint16_t)0x0004) +#define TIM20_ADC4_AWDG2 ((uint16_t)0x0008) +#define TIM20_ADC4_AWDG3 ((uint16_t)0x000C) + +#define IS_TIM_REMAP(TIM_REMAP) (((TIM_REMAP) == TIM16_GPIO)|| \ + ((TIM_REMAP) == TIM16_RTC_CLK) || \ + ((TIM_REMAP) == TIM16_HSEDiv32) || \ + ((TIM_REMAP) == TIM16_MCO) ||\ + ((TIM_REMAP) == TIM1_ADC1_AWDG1) ||\ + ((TIM_REMAP) == TIM1_ADC1_AWDG2) ||\ + ((TIM_REMAP) == TIM1_ADC1_AWDG3) ||\ + ((TIM_REMAP) == TIM1_ADC4_AWDG1) ||\ + ((TIM_REMAP) == TIM1_ADC4_AWDG2) ||\ + ((TIM_REMAP) == TIM1_ADC4_AWDG3) ||\ + ((TIM_REMAP) == TIM8_ADC2_AWDG1) ||\ + ((TIM_REMAP) == TIM8_ADC2_AWDG2) ||\ + ((TIM_REMAP) == TIM8_ADC2_AWDG3) ||\ + ((TIM_REMAP) == TIM8_ADC3_AWDG1) ||\ + ((TIM_REMAP) == TIM8_ADC3_AWDG2) ||\ + ((TIM_REMAP) == TIM8_ADC3_AWDG3) ||\ + ((TIM_REMAP) == TIM20_ADC3_AWDG1)||\ + ((TIM_REMAP) == TIM20_ADC3_AWDG2)||\ + ((TIM_REMAP) == TIM20_ADC3_AWDG3)||\ + ((TIM_REMAP) == TIM20_ADC4_AWDG1)||\ + ((TIM_REMAP) == TIM20_ADC4_AWDG2)||\ + ((TIM_REMAP) == TIM20_ADC4_AWDG3)) + +/** + * @} + */ +/** @defgroup TIM_Flags + * @{ + */ + +#define TIM_FLAG_Update ((uint32_t)0x00001) +#define TIM_FLAG_CC1 ((uint32_t)0x00002) +#define TIM_FLAG_CC2 ((uint32_t)0x00004) +#define TIM_FLAG_CC3 ((uint32_t)0x00008) +#define TIM_FLAG_CC4 ((uint32_t)0x00010) +#define TIM_FLAG_COM ((uint32_t)0x00020) +#define TIM_FLAG_Trigger ((uint32_t)0x00040) +#define TIM_FLAG_Break ((uint32_t)0x00080) +#define TIM_FLAG_Break2 ((uint32_t)0x00100) +#define TIM_FLAG_CC1OF ((uint32_t)0x00200) +#define TIM_FLAG_CC2OF ((uint32_t)0x00400) +#define TIM_FLAG_CC3OF ((uint32_t)0x00800) +#define TIM_FLAG_CC4OF ((uint32_t)0x01000) +#define TIM_FLAG_CC5 ((uint32_t)0x10000) +#define TIM_FLAG_CC6 ((uint32_t)0x20000) +#define IS_TIM_GET_FLAG(FLAG) (((FLAG) == TIM_FLAG_Update) || \ + ((FLAG) == TIM_FLAG_CC1) || \ + ((FLAG) == TIM_FLAG_CC2) || \ + ((FLAG) == TIM_FLAG_CC3) || \ + ((FLAG) == TIM_FLAG_CC4) || \ + ((FLAG) == TIM_FLAG_COM) || \ + ((FLAG) == TIM_FLAG_Trigger) || \ + ((FLAG) == TIM_FLAG_Break) || \ + ((FLAG) == TIM_FLAG_Break2) || \ + ((FLAG) == TIM_FLAG_CC1OF) || \ + ((FLAG) == TIM_FLAG_CC2OF) || \ + ((FLAG) == TIM_FLAG_CC3OF) || \ + ((FLAG) == TIM_FLAG_CC4OF) ||\ + ((FLAG) == TIM_FLAG_CC5) ||\ + ((FLAG) == TIM_FLAG_CC6)) + +#define IS_TIM_CLEAR_FLAG(TIM_FLAG) ((((TIM_FLAG) & (uint32_t)0xE000) == 0x0000) && ((TIM_FLAG) != 0x0000)) +/** + * @} + */ + +/** @defgroup TIM_OCReferenceClear + * @{ + */ +#define TIM_OCReferenceClear_ETRF ((uint16_t)0x0008) +#define TIM_OCReferenceClear_OCREFCLR ((uint16_t)0x0000) +#define TIM_OCREFERENCECECLEAR_SOURCE(SOURCE) (((SOURCE) == TIM_OCReferenceClear_ETRF) || \ + ((SOURCE) == TIM_OCReferenceClear_OCREFCLR)) + +/** @defgroup TIM_Input_Capture_Filer_Value + * @{ + */ + +#define IS_TIM_IC_FILTER(ICFILTER) ((ICFILTER) <= 0xF) +/** + * @} + */ + +/** @defgroup TIM_External_Trigger_Filter + * @{ + */ + +#define IS_TIM_EXT_FILTER(EXTFILTER) ((EXTFILTER) <= 0xF) +/** + * @} + */ + +/** @defgroup TIM_Legacy + * @{ + */ + +#define TIM_DMABurstLength_1Byte TIM_DMABurstLength_1Transfer +#define TIM_DMABurstLength_2Bytes TIM_DMABurstLength_2Transfers +#define TIM_DMABurstLength_3Bytes TIM_DMABurstLength_3Transfers +#define TIM_DMABurstLength_4Bytes TIM_DMABurstLength_4Transfers +#define TIM_DMABurstLength_5Bytes TIM_DMABurstLength_5Transfers +#define TIM_DMABurstLength_6Bytes TIM_DMABurstLength_6Transfers +#define TIM_DMABurstLength_7Bytes TIM_DMABurstLength_7Transfers +#define TIM_DMABurstLength_8Bytes TIM_DMABurstLength_8Transfers +#define TIM_DMABurstLength_9Bytes TIM_DMABurstLength_9Transfers +#define TIM_DMABurstLength_10Bytes TIM_DMABurstLength_10Transfers +#define TIM_DMABurstLength_11Bytes TIM_DMABurstLength_11Transfers +#define TIM_DMABurstLength_12Bytes TIM_DMABurstLength_12Transfers +#define TIM_DMABurstLength_13Bytes TIM_DMABurstLength_13Transfers +#define TIM_DMABurstLength_14Bytes TIM_DMABurstLength_14Transfers +#define TIM_DMABurstLength_15Bytes TIM_DMABurstLength_15Transfers +#define TIM_DMABurstLength_16Bytes TIM_DMABurstLength_16Transfers +#define TIM_DMABurstLength_17Bytes TIM_DMABurstLength_17Transfers +#define TIM_DMABurstLength_18Bytes TIM_DMABurstLength_18Transfers +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions --------------------------------------------------------*/ + +/* TimeBase management ********************************************************/ +void TIM_DeInit(TIM_TypeDef* TIMx); +void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct); +void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct); +void TIM_PrescalerConfig(TIM_TypeDef* TIMx, uint16_t Prescaler, uint16_t TIM_PSCReloadMode); +void TIM_CounterModeConfig(TIM_TypeDef* TIMx, uint16_t TIM_CounterMode); +void TIM_SetCounter(TIM_TypeDef* TIMx, uint32_t Counter); +void TIM_SetAutoreload(TIM_TypeDef* TIMx, uint32_t Autoreload); +uint32_t TIM_GetCounter(TIM_TypeDef* TIMx); +uint16_t TIM_GetPrescaler(TIM_TypeDef* TIMx); +void TIM_UpdateDisableConfig(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_UpdateRequestConfig(TIM_TypeDef* TIMx, uint16_t TIM_UpdateSource); +void TIM_UIFRemap(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_ARRPreloadConfig(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_SelectOnePulseMode(TIM_TypeDef* TIMx, uint16_t TIM_OPMode); +void TIM_SetClockDivision(TIM_TypeDef* TIMx, uint16_t TIM_CKD); +void TIM_Cmd(TIM_TypeDef* TIMx, FunctionalState NewState); + +/* Output Compare management **************************************************/ +void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC5Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC6Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_SelectGC5C1(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_SelectGC5C2(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_SelectGC5C3(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_OCStructInit(TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_SelectOCxM(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode); +void TIM_SetCompare1(TIM_TypeDef* TIMx, uint32_t Compare1); +void TIM_SetCompare2(TIM_TypeDef* TIMx, uint32_t Compare2); +void TIM_SetCompare3(TIM_TypeDef* TIMx, uint32_t Compare3); +void TIM_SetCompare4(TIM_TypeDef* TIMx, uint32_t Compare4); +void TIM_SetCompare5(TIM_TypeDef* TIMx, uint32_t Compare5); +void TIM_SetCompare6(TIM_TypeDef* TIMx, uint32_t Compare6); +void TIM_ForcedOC1Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction); +void TIM_ForcedOC2Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction); +void TIM_ForcedOC3Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction); +void TIM_ForcedOC4Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction); +void TIM_ForcedOC5Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction); +void TIM_ForcedOC6Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction); +void TIM_OC1PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload); +void TIM_OC2PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload); +void TIM_OC3PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload); +void TIM_OC4PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload); +void TIM_OC5PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload); +void TIM_OC6PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload); +void TIM_OC1FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast); +void TIM_OC2FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast); +void TIM_OC3FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast); +void TIM_OC4FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast); +void TIM_ClearOC1Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear); +void TIM_ClearOC2Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear); +void TIM_ClearOC3Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear); +void TIM_ClearOC4Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear); +void TIM_ClearOC5Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear); +void TIM_ClearOC6Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear); +void TIM_SelectOCREFClear(TIM_TypeDef* TIMx, uint16_t TIM_OCReferenceClear); +void TIM_OC1PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity); +void TIM_OC1NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity); +void TIM_OC2PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity); +void TIM_OC2NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity); +void TIM_OC3PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity); +void TIM_OC3NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity); +void TIM_OC4PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity); +void TIM_OC5PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity); +void TIM_OC6PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity); +void TIM_CCxCmd(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_CCx); +void TIM_CCxNCmd(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_CCxN); + +/* Input Capture management ***************************************************/ +void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct); +void TIM_ICStructInit(TIM_ICInitTypeDef* TIM_ICInitStruct); +void TIM_PWMIConfig(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct); +uint32_t TIM_GetCapture1(TIM_TypeDef* TIMx); +uint32_t TIM_GetCapture2(TIM_TypeDef* TIMx); +uint32_t TIM_GetCapture3(TIM_TypeDef* TIMx); +uint32_t TIM_GetCapture4(TIM_TypeDef* TIMx); +void TIM_SetIC1Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC); +void TIM_SetIC2Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC); +void TIM_SetIC3Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC); +void TIM_SetIC4Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC); + +/* Advanced-control timers (TIM1 and TIM8) specific features ******************/ +void TIM_BDTRConfig(TIM_TypeDef* TIMx, TIM_BDTRInitTypeDef *TIM_BDTRInitStruct); +void TIM_Break1Config(TIM_TypeDef* TIMx, uint32_t TIM_Break1Polarity, uint8_t TIM_Break1Filter); +void TIM_Break2Config(TIM_TypeDef* TIMx, uint32_t TIM_Break2Polarity, uint8_t TIM_Break2Filter); +void TIM_Break1Cmd(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_Break2Cmd(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_BDTRStructInit(TIM_BDTRInitTypeDef* TIM_BDTRInitStruct); +void TIM_CtrlPWMOutputs(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_SelectCOM(TIM_TypeDef* TIMx, FunctionalState NewState); +void TIM_CCPreloadControl(TIM_TypeDef* TIMx, FunctionalState NewState); + +/* Interrupts, DMA and flags management ***************************************/ +void TIM_ITConfig(TIM_TypeDef* TIMx, uint16_t TIM_IT, FunctionalState NewState); +void TIM_GenerateEvent(TIM_TypeDef* TIMx, uint16_t TIM_EventSource); +FlagStatus TIM_GetFlagStatus(TIM_TypeDef* TIMx, uint32_t TIM_FLAG); +void TIM_ClearFlag(TIM_TypeDef* TIMx, uint16_t TIM_FLAG); +ITStatus TIM_GetITStatus(TIM_TypeDef* TIMx, uint16_t TIM_IT); +void TIM_ClearITPendingBit(TIM_TypeDef* TIMx, uint16_t TIM_IT); +void TIM_DMAConfig(TIM_TypeDef* TIMx, uint16_t TIM_DMABase, uint16_t TIM_DMABurstLength); +void TIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState); +void TIM_SelectCCDMA(TIM_TypeDef* TIMx, FunctionalState NewState); + +/* Clocks management **********************************************************/ +void TIM_InternalClockConfig(TIM_TypeDef* TIMx); +void TIM_ITRxExternalClockConfig(TIM_TypeDef* TIMx, uint16_t TIM_InputTriggerSource); +void TIM_TIxExternalClockConfig(TIM_TypeDef* TIMx, uint16_t TIM_TIxExternalCLKSource, + uint16_t TIM_ICPolarity, uint16_t ICFilter); +void TIM_ETRClockMode1Config(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, uint16_t TIM_ExtTRGPolarity, + uint16_t ExtTRGFilter); +void TIM_ETRClockMode2Config(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, + uint16_t TIM_ExtTRGPolarity, uint16_t ExtTRGFilter); + +/* Synchronization management *************************************************/ +void TIM_SelectInputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_InputTriggerSource); +void TIM_SelectOutputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_TRGOSource); +void TIM_SelectOutputTrigger2(TIM_TypeDef* TIMx, uint32_t TIM_TRGO2Source); +void TIM_SelectSlaveMode(TIM_TypeDef* TIMx, uint32_t TIM_SlaveMode); +void TIM_SelectMasterSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_MasterSlaveMode); +void TIM_ETRConfig(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, uint16_t TIM_ExtTRGPolarity, + uint16_t ExtTRGFilter); + +/* Specific interface management **********************************************/ +void TIM_EncoderInterfaceConfig(TIM_TypeDef* TIMx, uint16_t TIM_EncoderMode, + uint16_t TIM_IC1Polarity, uint16_t TIM_IC2Polarity); +void TIM_SelectHallSensor(TIM_TypeDef* TIMx, FunctionalState NewState); + +/* Specific remapping management **********************************************/ +void TIM_RemapConfig(TIM_TypeDef* TIMx, uint16_t TIM_Remap); + +#ifdef __cplusplus +} +#endif + +#endif /*__STM32F30x_TIM_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_usart.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_usart.h new file mode 100755 index 0000000000..4a62acd878 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_usart.h @@ -0,0 +1,607 @@ +/** + ****************************************************************************** + * @file stm32f30x_usart.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the USART + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_USART_H +#define __STM32F30x_USART_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup USART + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ + + + +/** + * @brief USART Init Structure definition + */ + +typedef struct +{ + uint32_t USART_BaudRate; /*!< This member configures the USART communication baud rate. + The baud rate is computed using the following formula: + - IntegerDivider = ((PCLKx) / (16 * (USART_InitStruct->USART_BaudRate))) + - FractionalDivider = ((IntegerDivider - ((uint32_t) IntegerDivider)) * 16) + 0.5 */ + + uint32_t USART_WordLength; /*!< Specifies the number of data bits transmitted or received in a frame. + This parameter can be a value of @ref USART_Word_Length */ + + uint32_t USART_StopBits; /*!< Specifies the number of stop bits transmitted. + This parameter can be a value of @ref USART_Stop_Bits */ + + uint32_t USART_Parity; /*!< Specifies the parity mode. + This parameter can be a value of @ref USART_Parity + @note When parity is enabled, the computed parity is inserted + at the MSB position of the transmitted data (9th bit when + the word length is set to 9 data bits; 8th bit when the + word length is set to 8 data bits). */ + + uint32_t USART_Mode; /*!< Specifies whether the Receive or Transmit mode is enabled or disabled. + This parameter can be a value of @ref USART_Mode */ + + uint32_t USART_HardwareFlowControl; /*!< Specifies whether the hardware flow control mode is enabled + or disabled. + This parameter can be a value of @ref USART_Hardware_Flow_Control*/ +} USART_InitTypeDef; + +/** + * @brief USART Clock Init Structure definition + */ + +typedef struct +{ + uint32_t USART_Clock; /*!< Specifies whether the USART clock is enabled or disabled. + This parameter can be a value of @ref USART_Clock */ + + uint32_t USART_CPOL; /*!< Specifies the steady state of the serial clock. + This parameter can be a value of @ref USART_Clock_Polarity */ + + uint32_t USART_CPHA; /*!< Specifies the clock transition on which the bit capture is made. + This parameter can be a value of @ref USART_Clock_Phase */ + + uint32_t USART_LastBit; /*!< Specifies whether the clock pulse corresponding to the last transmitted + data bit (MSB) has to be output on the SCLK pin in synchronous mode. + This parameter can be a value of @ref USART_Last_Bit */ +} USART_ClockInitTypeDef; + +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup USART_Exported_Constants + * @{ + */ + +#define IS_USART_ALL_PERIPH(PERIPH) (((PERIPH) == USART1) || \ + ((PERIPH) == USART2) || \ + ((PERIPH) == USART3) || \ + ((PERIPH) == UART4) || \ + ((PERIPH) == UART5)) + +#define IS_USART_123_PERIPH(PERIPH) (((PERIPH) == USART1) || \ + ((PERIPH) == USART2) || \ + ((PERIPH) == USART3)) + +#define IS_USART_1234_PERIPH(PERIPH) (((PERIPH) == USART1) || \ + ((PERIPH) == USART2) || \ + ((PERIPH) == USART3) || \ + ((PERIPH) == UART4)) + + +/** @defgroup USART_Word_Length + * @{ + */ + +#define USART_WordLength_8b ((uint32_t)0x00000000) +#define USART_WordLength_9b USART_CR1_M +#define IS_USART_WORD_LENGTH(LENGTH) (((LENGTH) == USART_WordLength_8b) || \ + ((LENGTH) == USART_WordLength_9b)) +/** + * @} + */ + +/** @defgroup USART_Stop_Bits + * @{ + */ + +#define USART_StopBits_1 ((uint32_t)0x00000000) +#define USART_StopBits_2 USART_CR2_STOP_1 +#define USART_StopBits_1_5 (USART_CR2_STOP_0 | USART_CR2_STOP_1) +#define IS_USART_STOPBITS(STOPBITS) (((STOPBITS) == USART_StopBits_1) || \ + ((STOPBITS) == USART_StopBits_2) || \ + ((STOPBITS) == USART_StopBits_1_5)) +/** + * @} + */ + +/** @defgroup USART_Parity + * @{ + */ + +#define USART_Parity_No ((uint32_t)0x00000000) +#define USART_Parity_Even USART_CR1_PCE +#define USART_Parity_Odd (USART_CR1_PCE | USART_CR1_PS) +#define IS_USART_PARITY(PARITY) (((PARITY) == USART_Parity_No) || \ + ((PARITY) == USART_Parity_Even) || \ + ((PARITY) == USART_Parity_Odd)) +/** + * @} + */ + +/** @defgroup USART_Mode + * @{ + */ + +#define USART_Mode_Rx USART_CR1_RE +#define USART_Mode_Tx USART_CR1_TE +#define IS_USART_MODE(MODE) ((((MODE) & (uint32_t)0xFFFFFFF3) == 0x00) && \ + ((MODE) != (uint32_t)0x00)) +/** + * @} + */ + +/** @defgroup USART_Hardware_Flow_Control + * @{ + */ + +#define USART_HardwareFlowControl_None ((uint32_t)0x00000000) +#define USART_HardwareFlowControl_RTS USART_CR3_RTSE +#define USART_HardwareFlowControl_CTS USART_CR3_CTSE +#define USART_HardwareFlowControl_RTS_CTS (USART_CR3_RTSE | USART_CR3_CTSE) +#define IS_USART_HARDWARE_FLOW_CONTROL(CONTROL)\ + (((CONTROL) == USART_HardwareFlowControl_None) || \ + ((CONTROL) == USART_HardwareFlowControl_RTS) || \ + ((CONTROL) == USART_HardwareFlowControl_CTS) || \ + ((CONTROL) == USART_HardwareFlowControl_RTS_CTS)) +/** + * @} + */ + +/** @defgroup USART_Clock + * @{ + */ + +#define USART_Clock_Disable ((uint32_t)0x00000000) +#define USART_Clock_Enable USART_CR2_CLKEN +#define IS_USART_CLOCK(CLOCK) (((CLOCK) == USART_Clock_Disable) || \ + ((CLOCK) == USART_Clock_Enable)) +/** + * @} + */ + +/** @defgroup USART_Clock_Polarity + * @{ + */ + +#define USART_CPOL_Low ((uint32_t)0x00000000) +#define USART_CPOL_High USART_CR2_CPOL +#define IS_USART_CPOL(CPOL) (((CPOL) == USART_CPOL_Low) || ((CPOL) == USART_CPOL_High)) + +/** + * @} + */ + +/** @defgroup USART_Clock_Phase + * @{ + */ + +#define USART_CPHA_1Edge ((uint32_t)0x00000000) +#define USART_CPHA_2Edge USART_CR2_CPHA +#define IS_USART_CPHA(CPHA) (((CPHA) == USART_CPHA_1Edge) || ((CPHA) == USART_CPHA_2Edge)) + +/** + * @} + */ + +/** @defgroup USART_Last_Bit + * @{ + */ + +#define USART_LastBit_Disable ((uint32_t)0x00000000) +#define USART_LastBit_Enable USART_CR2_LBCL +#define IS_USART_LASTBIT(LASTBIT) (((LASTBIT) == USART_LastBit_Disable) || \ + ((LASTBIT) == USART_LastBit_Enable)) +/** + * @} + */ + +/** @defgroup USART_DMA_Requests + * @{ + */ + +#define USART_DMAReq_Tx USART_CR3_DMAT +#define USART_DMAReq_Rx USART_CR3_DMAR +#define IS_USART_DMAREQ(DMAREQ) ((((DMAREQ) & (uint32_t)0xFFFFFF3F) == 0x00) && \ + ((DMAREQ) != (uint32_t)0x00)) + +/** + * @} + */ + +/** @defgroup USART_DMA_Recception_Error + * @{ + */ + +#define USART_DMAOnError_Enable ((uint32_t)0x00000000) +#define USART_DMAOnError_Disable USART_CR3_DDRE +#define IS_USART_DMAONERROR(DMAERROR) (((DMAERROR) == USART_DMAOnError_Disable)|| \ + ((DMAERROR) == USART_DMAOnError_Enable)) +/** + * @} + */ + +/** @defgroup USART_MuteMode_WakeUp_methods + * @{ + */ + +#define USART_WakeUp_IdleLine ((uint32_t)0x00000000) +#define USART_WakeUp_AddressMark USART_CR1_WAKE +#define IS_USART_MUTEMODE_WAKEUP(WAKEUP) (((WAKEUP) == USART_WakeUp_IdleLine) || \ + ((WAKEUP) == USART_WakeUp_AddressMark)) +/** + * @} + */ + +/** @defgroup USART_Address_Detection + * @{ + */ + +#define USART_AddressLength_4b ((uint32_t)0x00000000) +#define USART_AddressLength_7b USART_CR2_ADDM7 +#define IS_USART_ADDRESS_DETECTION(ADDRESS) (((ADDRESS) == USART_AddressLength_4b) || \ + ((ADDRESS) == USART_AddressLength_7b)) +/** + * @} + */ + +/** @defgroup USART_StopMode_WakeUp_methods + * @{ + */ + +#define USART_WakeUpSource_AddressMatch ((uint32_t)0x00000000) +#define USART_WakeUpSource_StartBit USART_CR3_WUS_1 +#define USART_WakeUpSource_RXNE (uint32_t)(USART_CR3_WUS_0 | USART_CR3_WUS_1) +#define IS_USART_STOPMODE_WAKEUPSOURCE(SOURCE) (((SOURCE) == USART_WakeUpSource_AddressMatch) || \ + ((SOURCE) == USART_WakeUpSource_StartBit) || \ + ((SOURCE) == USART_WakeUpSource_RXNE)) +/** + * @} + */ + +/** @defgroup USART_LIN_Break_Detection_Length + * @{ + */ + +#define USART_LINBreakDetectLength_10b ((uint32_t)0x00000000) +#define USART_LINBreakDetectLength_11b USART_CR2_LBDL +#define IS_USART_LIN_BREAK_DETECT_LENGTH(LENGTH) \ + (((LENGTH) == USART_LINBreakDetectLength_10b) || \ + ((LENGTH) == USART_LINBreakDetectLength_11b)) +/** + * @} + */ + +/** @defgroup USART_IrDA_Low_Power + * @{ + */ + +#define USART_IrDAMode_LowPower USART_CR3_IRLP +#define USART_IrDAMode_Normal ((uint32_t)0x00000000) +#define IS_USART_IRDA_MODE(MODE) (((MODE) == USART_IrDAMode_LowPower) || \ + ((MODE) == USART_IrDAMode_Normal)) +/** + * @} + */ + +/** @defgroup USART_DE_Polarity + * @{ + */ + +#define USART_DEPolarity_High ((uint32_t)0x00000000) +#define USART_DEPolarity_Low USART_CR3_DEP +#define IS_USART_DE_POLARITY(POLARITY) (((POLARITY) == USART_DEPolarity_Low) || \ + ((POLARITY) == USART_DEPolarity_High)) +/** + * @} + */ + +/** @defgroup USART_Inversion_Pins + * @{ + */ + +#define USART_InvPin_Tx USART_CR2_TXINV +#define USART_InvPin_Rx USART_CR2_RXINV +#define IS_USART_INVERSTION_PIN(PIN) ((((PIN) & (uint32_t)0xFFFCFFFF) == 0x00) && \ + ((PIN) != (uint32_t)0x00)) + +/** + * @} + */ + +/** @defgroup USART_AutoBaudRate_Mode + * @{ + */ + +#define USART_AutoBaudRate_StartBit ((uint32_t)0x00000000) +#define USART_AutoBaudRate_FallingEdge USART_CR2_ABRMODE_0 +#define USART_AutoBaudRate_0x7FFrame USART_CR2_ABRMODE_1 +#define USART_AutoBaudRate_0x55Frame (USART_CR2_ABRMODE_0 | USART_CR2_ABRMODE_1) +#define IS_USART_AUTOBAUDRATE_MODE(MODE) (((MODE) == USART_AutoBaudRate_StartBit) || \ + ((MODE) == USART_AutoBaudRate_FallingEdge) || \ + ((MODE) == USART_AutoBaudRate_0x7FFrame) || \ + ((MODE) == USART_AutoBaudRate_0x55Frame)) +/** + * @} + */ + +/** @defgroup USART_OVR_DETECTION + * @{ + */ + +#define USART_OVRDetection_Enable ((uint32_t)0x00000000) +#define USART_OVRDetection_Disable USART_CR3_OVRDIS +#define IS_USART_OVRDETECTION(OVR) (((OVR) == USART_OVRDetection_Enable)|| \ + ((OVR) == USART_OVRDetection_Disable)) +/** + * @} + */ +/** @defgroup USART_Request + * @{ + */ + +#define USART_Request_ABRRQ USART_RQR_ABRRQ +#define USART_Request_SBKRQ USART_RQR_SBKRQ +#define USART_Request_MMRQ USART_RQR_MMRQ +#define USART_Request_RXFRQ USART_RQR_RXFRQ +#define USART_Request_TXFRQ USART_RQR_TXFRQ + +#define IS_USART_REQUEST(REQUEST) (((REQUEST) == USART_Request_TXFRQ) || \ + ((REQUEST) == USART_Request_RXFRQ) || \ + ((REQUEST) == USART_Request_MMRQ) || \ + ((REQUEST) == USART_Request_SBKRQ) || \ + ((REQUEST) == USART_Request_ABRRQ)) +/** + * @} + */ + +/** @defgroup USART_Flags + * @{ + */ +#define USART_FLAG_REACK USART_ISR_REACK +#define USART_FLAG_TEACK USART_ISR_TEACK +#define USART_FLAG_WU USART_ISR_WUF +#define USART_FLAG_RWU USART_ISR_RWU +#define USART_FLAG_SBK USART_ISR_SBKF +#define USART_FLAG_CM USART_ISR_CMF +#define USART_FLAG_BUSY USART_ISR_BUSY +#define USART_FLAG_ABRF USART_ISR_ABRF +#define USART_FLAG_ABRE USART_ISR_ABRE +#define USART_FLAG_EOB USART_ISR_EOBF +#define USART_FLAG_RTO USART_ISR_RTOF +#define USART_FLAG_nCTSS USART_ISR_CTS +#define USART_FLAG_CTS USART_ISR_CTSIF +#define USART_FLAG_LBD USART_ISR_LBD +#define USART_FLAG_TXE USART_ISR_TXE +#define USART_FLAG_TC USART_ISR_TC +#define USART_FLAG_RXNE USART_ISR_RXNE +#define USART_FLAG_IDLE USART_ISR_IDLE +#define USART_FLAG_ORE USART_ISR_ORE +#define USART_FLAG_NE USART_ISR_NE +#define USART_FLAG_FE USART_ISR_FE +#define USART_FLAG_PE USART_ISR_PE +#define IS_USART_FLAG(FLAG) (((FLAG) == USART_FLAG_PE) || ((FLAG) == USART_FLAG_TXE) || \ + ((FLAG) == USART_FLAG_TC) || ((FLAG) == USART_FLAG_RXNE) || \ + ((FLAG) == USART_FLAG_IDLE) || ((FLAG) == USART_FLAG_LBD) || \ + ((FLAG) == USART_FLAG_CTS) || ((FLAG) == USART_FLAG_ORE) || \ + ((FLAG) == USART_FLAG_NE) || ((FLAG) == USART_FLAG_FE) || \ + ((FLAG) == USART_FLAG_nCTSS) || ((FLAG) == USART_FLAG_RTO) || \ + ((FLAG) == USART_FLAG_EOB) || ((FLAG) == USART_FLAG_ABRE) || \ + ((FLAG) == USART_FLAG_ABRF) || ((FLAG) == USART_FLAG_BUSY) || \ + ((FLAG) == USART_FLAG_CM) || ((FLAG) == USART_FLAG_SBK) || \ + ((FLAG) == USART_FLAG_RWU) || ((FLAG) == USART_FLAG_WU) || \ + ((FLAG) == USART_FLAG_TEACK)|| ((FLAG) == USART_FLAG_REACK)) + +#define IS_USART_CLEAR_FLAG(FLAG) (((FLAG) == USART_FLAG_WU) || ((FLAG) == USART_FLAG_TC) || \ + ((FLAG) == USART_FLAG_IDLE) || ((FLAG) == USART_FLAG_ORE) || \ + ((FLAG) == USART_FLAG_NE) || ((FLAG) == USART_FLAG_FE) || \ + ((FLAG) == USART_FLAG_LBD) || ((FLAG) == USART_FLAG_CTS) || \ + ((FLAG) == USART_FLAG_RTO) || ((FLAG) == USART_FLAG_EOB) || \ + ((FLAG) == USART_FLAG_CM) || ((FLAG) == USART_FLAG_PE)) +/** + * @} + */ + +/** @defgroup USART_Interrupt_definition + * @brief USART Interrupt definition + * USART_IT possible values + * Elements values convention: 0xZZZZYYXX + * XX: Position of the corresponding Interrupt + * YY: Register index + * ZZZZ: Flag position + * @{ + */ + +#define USART_IT_WU ((uint32_t)0x00140316) +#define USART_IT_CM ((uint32_t)0x0011010E) +#define USART_IT_EOB ((uint32_t)0x000C011B) +#define USART_IT_RTO ((uint32_t)0x000B011A) +#define USART_IT_PE ((uint32_t)0x00000108) +#define USART_IT_TXE ((uint32_t)0x00070107) +#define USART_IT_TC ((uint32_t)0x00060106) +#define USART_IT_RXNE ((uint32_t)0x00050105) +#define USART_IT_IDLE ((uint32_t)0x00040104) +#define USART_IT_LBD ((uint32_t)0x00080206) +#define USART_IT_CTS ((uint32_t)0x0009030A) +#define USART_IT_ERR ((uint32_t)0x00000300) +#define USART_IT_ORE ((uint32_t)0x00030300) +#define USART_IT_NE ((uint32_t)0x00020300) +#define USART_IT_FE ((uint32_t)0x00010300) + +#define IS_USART_CONFIG_IT(IT) (((IT) == USART_IT_PE) || ((IT) == USART_IT_TXE) || \ + ((IT) == USART_IT_TC) || ((IT) == USART_IT_RXNE) || \ + ((IT) == USART_IT_IDLE) || ((IT) == USART_IT_LBD) || \ + ((IT) == USART_IT_CTS) || ((IT) == USART_IT_ERR) || \ + ((IT) == USART_IT_RTO) || ((IT) == USART_IT_EOB) || \ + ((IT) == USART_IT_CM) || ((IT) == USART_IT_WU)) + +#define IS_USART_GET_IT(IT) (((IT) == USART_IT_PE) || ((IT) == USART_IT_TXE) || \ + ((IT) == USART_IT_TC) || ((IT) == USART_IT_RXNE) || \ + ((IT) == USART_IT_IDLE) || ((IT) == USART_IT_LBD) || \ + ((IT) == USART_IT_CTS) || ((IT) == USART_IT_ORE) || \ + ((IT) == USART_IT_NE) || ((IT) == USART_IT_FE) || \ + ((IT) == USART_IT_RTO) || ((IT) == USART_IT_EOB) || \ + ((IT) == USART_IT_CM) || ((IT) == USART_IT_WU)) + +#define IS_USART_CLEAR_IT(IT) (((IT) == USART_IT_TC) || ((IT) == USART_IT_PE) || \ + ((IT) == USART_IT_FE) || ((IT) == USART_IT_NE) || \ + ((IT) == USART_IT_ORE) || ((IT) == USART_IT_IDLE) || \ + ((IT) == USART_IT_LBD) || ((IT) == USART_IT_CTS) || \ + ((IT) == USART_IT_RTO) || ((IT) == USART_IT_EOB) || \ + ((IT) == USART_IT_CM) || ((IT) == USART_IT_WU)) +/** + * @} + */ + +/** @defgroup USART_Global_definition + * @{ + */ + +#define IS_USART_BAUDRATE(BAUDRATE) (((BAUDRATE) > 0) && ((BAUDRATE) < 0x005B8D81)) +#define IS_USART_DE_ASSERTION_DEASSERTION_TIME(TIME) ((TIME) <= 0x1F) +#define IS_USART_AUTO_RETRY_COUNTER(COUNTER) ((COUNTER) <= 0x7) +#define IS_USART_TIMEOUT(TIMEOUT) ((TIMEOUT) <= 0x00FFFFFF) +#define IS_USART_DATA(DATA) ((DATA) <= 0x1FF) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/* Initialization and Configuration functions *********************************/ +void USART_DeInit(USART_TypeDef* USARTx); +void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct); +void USART_StructInit(USART_InitTypeDef* USART_InitStruct); +void USART_ClockInit(USART_TypeDef* USARTx, USART_ClockInitTypeDef* USART_ClockInitStruct); +void USART_ClockStructInit(USART_ClockInitTypeDef* USART_ClockInitStruct); +void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_DirectionModeCmd(USART_TypeDef* USARTx, uint32_t USART_DirectionMode, FunctionalState NewState); +void USART_SetPrescaler(USART_TypeDef* USARTx, uint8_t USART_Prescaler); +void USART_OverSampling8Cmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_OneBitMethodCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_MSBFirstCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_DataInvCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_InvPinCmd(USART_TypeDef* USARTx, uint32_t USART_InvPin, FunctionalState NewState); +void USART_SWAPPinCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_ReceiverTimeOutCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_SetReceiverTimeOut(USART_TypeDef* USARTx, uint32_t USART_ReceiverTimeOut); + +/* STOP Mode functions ********************************************************/ +void USART_STOPModeCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_StopModeWakeUpSourceConfig(USART_TypeDef* USARTx, uint32_t USART_WakeUpSource); + +/* AutoBaudRate functions *****************************************************/ +void USART_AutoBaudRateCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_AutoBaudRateConfig(USART_TypeDef* USARTx, uint32_t USART_AutoBaudRate); + +/* Data transfers functions ***************************************************/ +void USART_SendData(USART_TypeDef* USARTx, uint16_t Data); +uint16_t USART_ReceiveData(USART_TypeDef* USARTx); + +/* Multi-Processor Communication functions ************************************/ +void USART_SetAddress(USART_TypeDef* USARTx, uint8_t USART_Address); +void USART_MuteModeWakeUpConfig(USART_TypeDef* USARTx, uint32_t USART_WakeUp); +void USART_MuteModeCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_AddressDetectionConfig(USART_TypeDef* USARTx, uint32_t USART_AddressLength); +/* LIN mode functions *********************************************************/ +void USART_LINBreakDetectLengthConfig(USART_TypeDef* USARTx, uint32_t USART_LINBreakDetectLength); +void USART_LINCmd(USART_TypeDef* USARTx, FunctionalState NewState); + +/* Half-duplex mode function **************************************************/ +void USART_HalfDuplexCmd(USART_TypeDef* USARTx, FunctionalState NewState); + +/* Smartcard mode functions ***************************************************/ +void USART_SmartCardCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_SmartCardNACKCmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_SetGuardTime(USART_TypeDef* USARTx, uint8_t USART_GuardTime); +void USART_SetAutoRetryCount(USART_TypeDef* USARTx, uint8_t USART_AutoCount); +void USART_SetBlockLength(USART_TypeDef* USARTx, uint8_t USART_BlockLength); + +/* IrDA mode functions ********************************************************/ +void USART_IrDAConfig(USART_TypeDef* USARTx, uint32_t USART_IrDAMode); +void USART_IrDACmd(USART_TypeDef* USARTx, FunctionalState NewState); + +/* RS485 mode functions *******************************************************/ +void USART_DECmd(USART_TypeDef* USARTx, FunctionalState NewState); +void USART_DEPolarityConfig(USART_TypeDef* USARTx, uint32_t USART_DEPolarity); +void USART_SetDEAssertionTime(USART_TypeDef* USARTx, uint32_t USART_DEAssertionTime); +void USART_SetDEDeassertionTime(USART_TypeDef* USARTx, uint32_t USART_DEDeassertionTime); + +/* DMA transfers management functions *****************************************/ +void USART_DMACmd(USART_TypeDef* USARTx, uint32_t USART_DMAReq, FunctionalState NewState); +void USART_DMAReceptionErrorConfig(USART_TypeDef* USARTx, uint32_t USART_DMAOnError); + +/* Interrupts and flags management functions **********************************/ +void USART_ITConfig(USART_TypeDef* USARTx, uint32_t USART_IT, FunctionalState NewState); +void USART_RequestCmd(USART_TypeDef* USARTx, uint32_t USART_Request, FunctionalState NewState); +void USART_OverrunDetectionConfig(USART_TypeDef* USARTx, uint32_t USART_OVRDetection); +FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint32_t USART_FLAG); +void USART_ClearFlag(USART_TypeDef* USARTx, uint32_t USART_FLAG); +ITStatus USART_GetITStatus(USART_TypeDef* USARTx, uint32_t USART_IT); +void USART_ClearITPendingBit(USART_TypeDef* USARTx, uint32_t USART_IT); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_USART_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_wwdg.h b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_wwdg.h new file mode 100755 index 0000000000..b3ddd36c90 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/inc/stm32f30x_wwdg.h @@ -0,0 +1,109 @@ +/** + ****************************************************************************** + * @file stm32f30x_wwdg.h + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file contains all the functions prototypes for the WWDG + * firmware library. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F30x_WWDG_H +#define __STM32F30x_WWDG_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup WWDG + * @{ + */ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/** @defgroup WWDG_Exported_Constants + * @{ + */ + +/** @defgroup WWDG_Prescaler + * @{ + */ + +#define WWDG_Prescaler_1 ((uint32_t)0x00000000) +#define WWDG_Prescaler_2 ((uint32_t)0x00000080) +#define WWDG_Prescaler_4 ((uint32_t)0x00000100) +#define WWDG_Prescaler_8 ((uint32_t)0x00000180) +#define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \ + ((PRESCALER) == WWDG_Prescaler_2) || \ + ((PRESCALER) == WWDG_Prescaler_4) || \ + ((PRESCALER) == WWDG_Prescaler_8)) +#define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F) +#define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F)) + +/** + * @} + */ + +/** + * @} + */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* Function used to set the WWDG configuration to the default reset state ****/ +void WWDG_DeInit(void); + +/* Prescaler, Refresh window and Counter configuration functions **************/ +void WWDG_SetPrescaler(uint32_t WWDG_Prescaler); +void WWDG_SetWindowValue(uint8_t WindowValue); +void WWDG_EnableIT(void); +void WWDG_SetCounter(uint8_t Counter); + +/* WWDG activation functions **************************************************/ +void WWDG_Enable(uint8_t Counter); + +/* Interrupts and flags management functions **********************************/ +FlagStatus WWDG_GetFlagStatus(void); +void WWDG_ClearFlag(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F30x_WWDG_H */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_adc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_adc.c new file mode 100755 index 0000000000..af90b23c9c --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_adc.c @@ -0,0 +1,2401 @@ +/** + ****************************************************************************** + * @file stm32f30x_adc.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Analog to Digital Convertor (ADC) peripheral: + * + Initialization and Configuration + * + Analog Watchdog configuration + * + Temperature Sensor, Vbat & Vrefint (Internal Reference Voltage) management + * + Regular Channels Configuration + * + Regular Channels DMA Configuration + * + Injected channels Configuration + * + Interrupts and flags management + * + Dual mode configuration + * + @verbatim + ============================================================================== + ##### How to use this driver ##### + ============================================================================== + [..] + (#) select the ADC clock using the function RCC_ADCCLKConfig() + (#) Enable the ADC interface clock using RCC_AHBPeriphClockCmd(); + (#) ADC pins configuration + (++) Enable the clock for the ADC GPIOs using the following function: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOx, ENABLE); + (++) Configure these ADC pins in analog mode using GPIO_Init(); + (#) Configure the ADC conversion resolution, data alignment, external + trigger and edge, sequencer lenght and Enable/Disable the continuous mode + using the ADC_Init() function. + (#) Activate the ADC peripheral using ADC_Cmd() function. + + *** ADC channels group configuration *** + ======================================== + [..] + (+) To configure the ADC channels features, use ADC_Init(), ADC_InjectedInit() + and/or ADC_RegularChannelConfig() functions. + (+) To activate the continuous mode, use the ADC_ContinuousModeCmd() + function. + (+) To activate the Discontinuous mode, use the ADC_DiscModeCmd() functions. + (+) To activate the overrun mode, use the ADC_OverrunModeCmd() functions. + (+) To activate the calibration mode, use the ADC_StartCalibration() functions. + (+) To read the ADC converted values, use the ADC_GetConversionValue() + function. + + *** DMA for ADC channels features configuration *** + =================================================== + [..] + (+) To enable the DMA mode for ADC channels group, use the ADC_DMACmd() function. + (+) To configure the DMA transfer request, use ADC_DMAConfig() function. + + @endverbatim + * + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_adc.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup ADC + * @brief ADC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* CFGR register Mask */ +#define CFGR_CLEAR_Mask ((uint32_t)0xFDFFC007) + +/* JSQR register Mask */ +#define JSQR_CLEAR_Mask ((uint32_t)0x00000000) + +/* ADC ADON mask */ +#define CCR_CLEAR_MASK ((uint32_t)0xFFFC10E0) + +/* ADC JDRx registers offset */ +#define JDR_Offset ((uint8_t)0x80) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup ADC_Private_Functions + * @{ + */ + +/** @defgroup ADC_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + [..] + This section provides functions allowing to: + (#) Initialize and configure the ADC injected and/or regular channels and dual mode. + (#) Management of the calibration process + (#) ADC Power-on Power-off + (#) Single ended or differential mode + (#) Enabling the queue of context and the auto delay mode + (#) The number of ADC conversions that will be done using the sequencer for regular + channel group + (#) Enable or disable the ADC peripheral + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the ADCx peripheral registers to their default reset values. + * @param ADCx: where x can be 1, 2,3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_DeInit(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + + if((ADCx == ADC1) || (ADCx == ADC2)) + { + /* Enable ADC1/ADC2 reset state */ + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_ADC12, ENABLE); + /* Release ADC1/ADC2 from reset state */ + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_ADC12, DISABLE); + } + else if((ADCx == ADC3) || (ADCx == ADC4)) + { + /* Enable ADC3/ADC4 reset state */ + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_ADC34, ENABLE); + /* Release ADC3/ADC4 from reset state */ + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_ADC34, DISABLE); + } +} +/** + * @brief Initializes the ADCx peripheral according to the specified parameters + * in the ADC_InitStruct. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_InitStruct: pointer to an ADC_InitTypeDef structure that contains + * the configuration information for the specified ADC peripheral. + * @retval None + */ +void ADC_Init(ADC_TypeDef* ADCx, ADC_InitTypeDef* ADC_InitStruct) +{ + uint32_t tmpreg1 = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CONVMODE(ADC_InitStruct->ADC_ContinuousConvMode)); + assert_param(IS_ADC_RESOLUTION(ADC_InitStruct->ADC_Resolution)); + assert_param(IS_ADC_EXT_TRIG(ADC_InitStruct->ADC_ExternalTrigConvEvent)); + assert_param(IS_EXTERNALTRIG_EDGE(ADC_InitStruct->ADC_ExternalTrigEventEdge)); + assert_param(IS_ADC_DATA_ALIGN(ADC_InitStruct->ADC_DataAlign)); + assert_param(IS_ADC_OVRUNMODE(ADC_InitStruct->ADC_OverrunMode)); + assert_param(IS_ADC_AUTOINJECMODE(ADC_InitStruct->ADC_AutoInjMode)); + assert_param(IS_ADC_REGULAR_LENGTH(ADC_InitStruct->ADC_NbrOfRegChannel)); + + /*---------------------------- ADCx CFGR Configuration -----------------*/ + /* Get the ADCx CFGR value */ + tmpreg1 = ADCx->CFGR; + /* Clear SCAN bit */ + tmpreg1 &= CFGR_CLEAR_Mask; + /* Configure ADCx: scan conversion mode */ + /* Set SCAN bit according to ADC_ScanConvMode value */ + tmpreg1 |= (uint32_t)ADC_InitStruct->ADC_ContinuousConvMode | + ADC_InitStruct->ADC_Resolution| + ADC_InitStruct->ADC_ExternalTrigConvEvent| + ADC_InitStruct->ADC_ExternalTrigEventEdge| + ADC_InitStruct->ADC_DataAlign| + ADC_InitStruct->ADC_OverrunMode| + ADC_InitStruct->ADC_AutoInjMode; + + /* Write to ADCx CFGR */ + ADCx->CFGR = tmpreg1; + + /*---------------------------- ADCx SQR1 Configuration -----------------*/ + /* Get the ADCx SQR1 value */ + tmpreg1 = ADCx->SQR1; + /* Clear L bits */ + tmpreg1 &= ~(uint32_t)(ADC_SQR1_L); + /* Configure ADCx: regular channel sequence length */ + /* Set L bits according to ADC_NbrOfRegChannel value */ + tmpreg1 |= (uint32_t) (ADC_InitStruct->ADC_NbrOfRegChannel - 1); + /* Write to ADCx SQR1 */ + ADCx->SQR1 = tmpreg1; + +} + +/** + * @brief Fills each ADC_InitStruct member with its default value. + * @param ADC_InitStruct : pointer to an ADC_InitTypeDef structure which will be initialized. + * @retval None + */ +void ADC_StructInit(ADC_InitTypeDef* ADC_InitStruct) +{ + /* Reset ADC init structure parameters values */ + ADC_InitStruct->ADC_ContinuousConvMode = DISABLE; + ADC_InitStruct->ADC_Resolution = ADC_Resolution_12b; + ADC_InitStruct->ADC_ExternalTrigConvEvent = ADC_ExternalTrigConvEvent_0; + ADC_InitStruct->ADC_ExternalTrigEventEdge = ADC_ExternalTrigEventEdge_None; + ADC_InitStruct->ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStruct->ADC_OverrunMode = DISABLE; + ADC_InitStruct->ADC_AutoInjMode = DISABLE; + ADC_InitStruct->ADC_NbrOfRegChannel = 1; +} + +/** + * @brief Initializes the ADCx peripheral according to the specified parameters + * in the ADC_InitStruct. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_InjectInitStruct: pointer to an ADC_InjecInitTypeDef structure that contains + * the configuration information for the specified ADC injected channel. + * @retval None + */ +void ADC_InjectedInit(ADC_TypeDef* ADCx, ADC_InjectedInitTypeDef* ADC_InjectedInitStruct) +{ + uint32_t tmpreg1 = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_EXT_INJEC_TRIG(ADC_InjectedInitStruct->ADC_ExternalTrigInjecConvEvent)); + assert_param(IS_EXTERNALTRIGINJ_EDGE(ADC_InjectedInitStruct->ADC_ExternalTrigInjecEventEdge)); + assert_param(IS_ADC_INJECTED_LENGTH(ADC_InjectedInitStruct->ADC_NbrOfInjecChannel)); + assert_param(IS_ADC_INJECTED_CHANNEL(ADC_InjectedInitStruct->ADC_InjecSequence1)); + assert_param(IS_ADC_INJECTED_CHANNEL(ADC_InjectedInitStruct->ADC_InjecSequence2)); + assert_param(IS_ADC_INJECTED_CHANNEL(ADC_InjectedInitStruct->ADC_InjecSequence3)); + assert_param(IS_ADC_INJECTED_CHANNEL(ADC_InjectedInitStruct->ADC_InjecSequence4)); + + /*---------------------------- ADCx JSQR Configuration -----------------*/ + /* Get the ADCx JSQR value */ + tmpreg1 = ADCx->JSQR; + /* Clear L bits */ + tmpreg1 &= JSQR_CLEAR_Mask; + /* Configure ADCx: Injected channel sequence length, external trigger, + external trigger edge and sequences + */ + tmpreg1 = (uint32_t) ((ADC_InjectedInitStruct->ADC_NbrOfInjecChannel - (uint8_t)1) | + ADC_InjectedInitStruct->ADC_ExternalTrigInjecConvEvent | + ADC_InjectedInitStruct->ADC_ExternalTrigInjecEventEdge | + (uint32_t)((ADC_InjectedInitStruct->ADC_InjecSequence1) << 8) | + (uint32_t)((ADC_InjectedInitStruct->ADC_InjecSequence2) << 14) | + (uint32_t)((ADC_InjectedInitStruct->ADC_InjecSequence3) << 20) | + (uint32_t)((ADC_InjectedInitStruct->ADC_InjecSequence4) << 26)); + /* Write to ADCx SQR1 */ + ADCx->JSQR = tmpreg1; +} + +/** + * @brief Fills each ADC_InjectedInitStruct member with its default value. + * @param ADC_InjectedInitStruct : pointer to an ADC_InjectedInitTypeDef structure which will be initialized. + * @retval None + */ +void ADC_InjectedStructInit(ADC_InjectedInitTypeDef* ADC_InjectedInitStruct) +{ + ADC_InjectedInitStruct->ADC_ExternalTrigInjecConvEvent = ADC_ExternalTrigInjecConvEvent_0; + ADC_InjectedInitStruct->ADC_ExternalTrigInjecEventEdge = ADC_ExternalTrigInjecEventEdge_None; + ADC_InjectedInitStruct->ADC_NbrOfInjecChannel = 1; + ADC_InjectedInitStruct->ADC_InjecSequence1 = ADC_InjectedChannel_1; + ADC_InjectedInitStruct->ADC_InjecSequence2 = ADC_InjectedChannel_1; + ADC_InjectedInitStruct->ADC_InjecSequence3 = ADC_InjectedChannel_1; + ADC_InjectedInitStruct->ADC_InjecSequence4 = ADC_InjectedChannel_1; +} + +/** + * @brief Initializes the ADCs peripherals according to the specified parameters + * in the ADC_CommonInitStruct. + * @param ADCx: where x can be 1 or 4 to select the ADC peripheral. + * @param ADC_CommonInitStruct: pointer to an ADC_CommonInitTypeDef structure + * that contains the configuration information for All ADCs peripherals. + * @retval None + */ +void ADC_CommonInit(ADC_TypeDef* ADCx, ADC_CommonInitTypeDef* ADC_CommonInitStruct) +{ + uint32_t tmpreg1 = 0; + /* Check the parameters */ + assert_param(IS_ADC_MODE(ADC_CommonInitStruct->ADC_Mode)); + assert_param(IS_ADC_CLOCKMODE(ADC_CommonInitStruct->ADC_Clock)); + assert_param(IS_ADC_DMA_MODE(ADC_CommonInitStruct->ADC_DMAMode)); + assert_param(IS_ADC_DMA_ACCESS_MODE(ADC_CommonInitStruct->ADC_DMAAccessMode)); + assert_param(IS_ADC_TWOSAMPLING_DELAY(ADC_CommonInitStruct->ADC_TwoSamplingDelay)); + + if((ADCx == ADC1) || (ADCx == ADC2)) + { + /* Get the ADC CCR value */ + tmpreg1 = ADC1_2->CCR; + + /* Clear MULTI, DELAY, DMA and ADCPRE bits */ + tmpreg1 &= CCR_CLEAR_MASK; + } + else + { + /* Get the ADC CCR value */ + tmpreg1 = ADC3_4->CCR; + + /* Clear MULTI, DELAY, DMA and ADCPRE bits */ + tmpreg1 &= CCR_CLEAR_MASK; + } + /*---------------------------- ADC CCR Configuration -----------------*/ + /* Configure ADCx: Multi mode, Delay between two sampling time, ADC clock, DMA mode + and DMA access mode for dual mode */ + /* Set MULTI bits according to ADC_Mode value */ + /* Set CKMODE bits according to ADC_Clock value */ + /* Set MDMA bits according to ADC_DMAAccessMode value */ + /* Set DMACFG bits according to ADC_DMAMode value */ + /* Set DELAY bits according to ADC_TwoSamplingDelay value */ + tmpreg1 |= (uint32_t)(ADC_CommonInitStruct->ADC_Mode | + ADC_CommonInitStruct->ADC_Clock | + ADC_CommonInitStruct->ADC_DMAAccessMode | + (uint32_t)(ADC_CommonInitStruct->ADC_DMAMode << 12) | + (uint32_t)((uint32_t)ADC_CommonInitStruct->ADC_TwoSamplingDelay << 8)); + + if((ADCx == ADC1) || (ADCx == ADC2)) + { + /* Write to ADC CCR */ + ADC1_2->CCR = tmpreg1; + } + else + { + /* Write to ADC CCR */ + ADC3_4->CCR = tmpreg1; + } +} + +/** + * @brief Fills each ADC_CommonInitStruct member with its default value. + * @param ADC_CommonInitStruct: pointer to an ADC_CommonInitTypeDef structure + * which will be initialized. + * @retval None + */ +void ADC_CommonStructInit(ADC_CommonInitTypeDef* ADC_CommonInitStruct) +{ + /* Initialize the ADC_Mode member */ + ADC_CommonInitStruct->ADC_Mode = ADC_Mode_Independent; + + /* initialize the ADC_Clock member */ + ADC_CommonInitStruct->ADC_Clock = ADC_Clock_AsynClkMode; + + /* Initialize the ADC_DMAAccessMode member */ + ADC_CommonInitStruct->ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + + /* Initialize the ADC_DMAMode member */ + ADC_CommonInitStruct->ADC_DMAMode = ADC_DMAMode_OneShot; + + /* Initialize the ADC_TwoSamplingDelay member */ + ADC_CommonInitStruct->ADC_TwoSamplingDelay = 0; + +} + +/** + * @brief Enables or disables the specified ADC peripheral. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADCx peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_Cmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the ADEN bit */ + ADCx->CR |= ADC_CR_ADEN; + } + else + { + /* Disable the selected ADC peripheral: Set the ADDIS bit */ + ADCx->CR |= ADC_CR_ADDIS; + } +} + +/** + * @brief Starts the selected ADC calibration process. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_StartCalibration(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADCAL bit */ + ADCx->CR |= ADC_CR_ADCAL; +} + +/** + * @brief Returns the ADCx calibration value. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +uint32_t ADC_GetCalibrationValue(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Return the selected ADC calibration value */ + return (uint32_t)ADCx->CALFACT; +} + +/** + * @brief Sets the ADCx calibration register. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_SetCalibrationValue(ADC_TypeDef* ADCx, uint32_t ADC_Calibration) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADC calibration register value */ + ADCx->CALFACT = ADC_Calibration; +} + +/** + * @brief Select the ADC calibration mode. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_CalibrationMode: the ADC calibration mode. + * This parameter can be one of the following values: + * @arg ADC_CalibrationMode_Single: to select the calibration for single channel + * @arg ADC_CalibrationMode_Differential: to select the calibration for differential channel + * @retval None + */ +void ADC_SelectCalibrationMode(ADC_TypeDef* ADCx, uint32_t ADC_CalibrationMode) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CALIBRATION_MODE(ADC_CalibrationMode)); + /* Set or Reset the ADCALDIF bit */ + ADCx->CR &= (~ADC_CR_ADCALDIF); + ADCx->CR |= ADC_CalibrationMode; + +} + +/** + * @brief Gets the selected ADC calibration status. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval The new state of ADC calibration (SET or RESET). + */ +FlagStatus ADC_GetCalibrationStatus(ADC_TypeDef* ADCx) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + /* Check the status of CAL bit */ + if ((ADCx->CR & ADC_CR_ADCAL) != (uint32_t)RESET) + { + /* CAL bit is set: calibration on going */ + bitstatus = SET; + } + else + { + /* CAL bit is reset: end of calibration */ + bitstatus = RESET; + } + /* Return the CAL bit status */ + return bitstatus; +} + +/** + * @brief ADC Disable Command. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_DisableCmd(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADDIS bit */ + ADCx->CR |= ADC_CR_ADDIS; +} + + +/** + * @brief Gets the selected ADC disable command Status. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval The new state of ADC ADC disable command (SET or RESET). + */ +FlagStatus ADC_GetDisableCmdStatus(ADC_TypeDef* ADCx) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Check the status of ADDIS bit */ + if ((ADCx->CR & ADC_CR_ADDIS) != (uint32_t)RESET) + { + /* ADDIS bit is set */ + bitstatus = SET; + } + else + { + /* ADDIS bit is reset */ + bitstatus = RESET; + } + /* Return the ADDIS bit status */ + return bitstatus; +} + +/** + * @brief Enables or disables the specified ADC Voltage Regulator. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADCx Voltage Regulator. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_VoltageRegulatorCmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* set the intermediate state before moving the ADC voltage regulator + from enable state to disable state or from disable state to enable state */ + ADCx->CR &= ~(ADC_CR_ADVREGEN); + + if (NewState != DISABLE) + { + /* Set the ADVREGEN bit 0 */ + ADCx->CR |= ADC_CR_ADVREGEN_0; + } + else + { + /* Set the ADVREGEN bit 1 */ + ADCx->CR |=ADC_CR_ADVREGEN_1; + } +} + +/** + * @brief Selects the differential mode for a specific channel + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure for the analog watchdog. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @note : Channel 15, 16 and 17 are fixed to single-ended inputs mode. + * @retval None + */ +void ADC_SelectDifferentialMode(ADC_TypeDef* ADCx, uint8_t ADC_Channel, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_DIFFCHANNEL(ADC_Channel)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the DIFSEL bit */ + ADCx->DIFSEL |= (uint32_t)(1 << ADC_Channel ); + } + else + { + /* Reset the DIFSEL bit */ + ADCx->DIFSEL &= ~(uint32_t)(1 << ADC_Channel); + } +} + +/** + * @brief Selects the Queue Of Context Mode for injected channels. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the Queue Of Context Mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_SelectQueueOfContextMode(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the JQM bit */ + ADCx->CFGR |= (uint32_t)(ADC_CFGR_JQM ); + } + else + { + /* Reset the JQM bit */ + ADCx->CFGR &= ~(uint32_t)(ADC_CFGR_JQM); + } +} + +/** + * @brief Selects the ADC Delayed Conversion Mode. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADC Delayed Conversion Mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_AutoDelayCmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the AUTDLY bit */ + ADCx->CFGR |= (uint32_t)(ADC_CFGR_AUTDLY ); + } + else + { + /* Reset the AUTDLY bit */ + ADCx->CFGR &= ~(uint32_t)(ADC_CFGR_AUTDLY); + } +} + +/** + * @} + */ + +/** @defgroup ADC_Group2 Analog Watchdog configuration functions + * @brief Analog Watchdog configuration functions + * +@verbatim + =============================================================================== + ##### Analog Watchdog configuration functions ##### + =============================================================================== + + [..] This section provides functions allowing to configure the 3 Analog Watchdogs + (AWDG1, AWDG2 and AWDG3) in the ADC. + + [..] A typical configuration Analog Watchdog is done following these steps : + (#) The ADC guarded channel(s) is (are) selected using the functions: + (++) ADC_AnalogWatchdog1SingleChannelConfig(). + (++) ADC_AnalogWatchdog2SingleChannelConfig(). + (++) ADC_AnalogWatchdog3SingleChannelConfig(). + + (#) The Analog watchdog lower and higher threshold are configured using the functions: + (++) ADC_AnalogWatchdog1ThresholdsConfig(). + (++) ADC_AnalogWatchdog2ThresholdsConfig(). + (++) ADC_AnalogWatchdog3ThresholdsConfig(). + + (#) The Analog watchdog is enabled and configured to enable the check, on one + or more channels, using the function: + (++) ADC_AnalogWatchdogCmd(). + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the analog watchdog on single/all regular + * or injected channels + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_AnalogWatchdog: the ADC analog watchdog configuration. + * This parameter can be one of the following values: + * @arg ADC_AnalogWatchdog_SingleRegEnable: Analog watchdog on a single regular channel + * @arg ADC_AnalogWatchdog_SingleInjecEnable: Analog watchdog on a single injected channel + * @arg ADC_AnalogWatchdog_SingleRegOrInjecEnable: Analog watchdog on a single regular or injected channel + * @arg ADC_AnalogWatchdog_AllRegEnable: Analog watchdog on all regular channel + * @arg ADC_AnalogWatchdog_AllInjecEnable: Analog watchdog on all injected channel + * @arg ADC_AnalogWatchdog_AllRegAllInjecEnable: Analog watchdog on all regular and injected channels + * @arg ADC_AnalogWatchdog_None: No channel guarded by the analog watchdog + * @retval None + */ +void ADC_AnalogWatchdogCmd(ADC_TypeDef* ADCx, uint32_t ADC_AnalogWatchdog) +{ + uint32_t tmpreg = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_ANALOG_WATCHDOG(ADC_AnalogWatchdog)); + /* Get the old register value */ + tmpreg = ADCx->CFGR; + /* Clear AWDEN, AWDENJ and AWDSGL bits */ + tmpreg &= ~(uint32_t)(ADC_CFGR_AWD1SGL|ADC_CFGR_AWD1EN|ADC_CFGR_JAWD1EN); + /* Set the analog watchdog enable mode */ + tmpreg |= ADC_AnalogWatchdog; + /* Store the new register value */ + ADCx->CFGR = tmpreg; +} + +/** + * @brief Configures the high and low thresholds of the analog watchdog1. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param HighThreshold: the ADC analog watchdog High threshold value. + * This parameter must be a 12bit value. + * @param LowThreshold: the ADC analog watchdog Low threshold value. + * This parameter must be a 12bit value. + * @retval None + */ +void ADC_AnalogWatchdog1ThresholdsConfig(ADC_TypeDef* ADCx, uint16_t HighThreshold, + uint16_t LowThreshold) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_THRESHOLD(HighThreshold)); + assert_param(IS_ADC_THRESHOLD(LowThreshold)); + /* Set the ADCx high threshold */ + ADCx->TR1 &= ~(uint32_t)ADC_TR1_HT1; + ADCx->TR1 |= (uint32_t)((uint32_t)HighThreshold << 16); + + /* Set the ADCx low threshold */ + ADCx->TR1 &= ~(uint32_t)ADC_TR1_LT1; + ADCx->TR1 |= LowThreshold; +} + +/** + * @brief Configures the high and low thresholds of the analog watchdog2. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param HighThreshold: the ADC analog watchdog High threshold value. + * This parameter must be a 8bit value. + * @param LowThreshold: the ADC analog watchdog Low threshold value. + * This parameter must be a 8bit value. + * @retval None + */ +void ADC_AnalogWatchdog2ThresholdsConfig(ADC_TypeDef* ADCx, uint8_t HighThreshold, + uint8_t LowThreshold) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADCx high threshold */ + ADCx->TR2 &= ~(uint32_t)ADC_TR2_HT2; + ADCx->TR2 |= (uint32_t)((uint32_t)HighThreshold << 16); + + /* Set the ADCx low threshold */ + ADCx->TR2 &= ~(uint32_t)ADC_TR2_LT2; + ADCx->TR2 |= LowThreshold; +} + +/** + * @brief Configures the high and low thresholds of the analog watchdog3. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param HighThreshold: the ADC analog watchdog High threshold value. + * This parameter must be a 8bit value. + * @param LowThreshold: the ADC analog watchdog Low threshold value. + * This parameter must be a 8bit value. + * @retval None + */ +void ADC_AnalogWatchdog3ThresholdsConfig(ADC_TypeDef* ADCx, uint8_t HighThreshold, + uint8_t LowThreshold) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADCx high threshold */ + ADCx->TR3 &= ~(uint32_t)ADC_TR3_HT3; + ADCx->TR3 |= (uint32_t)((uint32_t)HighThreshold << 16); + + /* Set the ADCx low threshold */ + ADCx->TR3 &= ~(uint32_t)ADC_TR3_LT3; + ADCx->TR3 |= LowThreshold; +} + +/** + * @brief Configures the analog watchdog 2 guarded single channel + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure for the analog watchdog. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @retval None + */ +void ADC_AnalogWatchdog1SingleChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel) +{ + uint32_t tmpreg = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + /* Get the old register value */ + tmpreg = ADCx->CFGR; + /* Clear the Analog watchdog channel select bits */ + tmpreg &= ~(uint32_t)ADC_CFGR_AWD1CH; + /* Set the Analog watchdog channel */ + tmpreg |= (uint32_t)((uint32_t)ADC_Channel << 26); + /* Store the new register value */ + ADCx->CFGR = tmpreg; +} + +/** + * @brief Configures the analog watchdog 2 guarded single channel + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure for the analog watchdog. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @retval None + */ +void ADC_AnalogWatchdog2SingleChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel) +{ + uint32_t tmpreg = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + /* Get the old register value */ + tmpreg = ADCx->AWD2CR; + /* Clear the Analog watchdog channel select bits */ + tmpreg &= ~(uint32_t)ADC_AWD2CR_AWD2CH; + /* Set the Analog watchdog channel */ + tmpreg |= (uint32_t)1 << (ADC_Channel); + /* Store the new register value */ + ADCx->AWD2CR |= tmpreg; +} + +/** + * @brief Configures the analog watchdog 3 guarded single channel + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure for the analog watchdog. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @retval None + */ +void ADC_AnalogWatchdog3SingleChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel) +{ + uint32_t tmpreg = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + /* Get the old register value */ + tmpreg = ADCx->AWD3CR; + /* Clear the Analog watchdog channel select bits */ + tmpreg &= ~(uint32_t)ADC_AWD3CR_AWD3CH; + /* Set the Analog watchdog channel */ + tmpreg |= (uint32_t)1 << (ADC_Channel); + /* Store the new register value */ + ADCx->AWD3CR |= tmpreg; +} + +/** + * @} + */ + +/** @defgroup ADC_Group3 Temperature Sensor - Vrefint (Internal Reference Voltage) and VBAT management functions + * @brief Vbat, Temperature Sensor & Vrefint (Internal Reference Voltage) management function + * +@verbatim + ==================================================================================================== + ##### Temperature Sensor - Vrefint (Internal Reference Voltage) and VBAT management functions ##### + ==================================================================================================== + + [..] This section provides a function allowing to enable/ disable the internal + connections between the ADC and the Vbat/2, Temperature Sensor and the Vrefint source. + + [..] A typical configuration to get the Temperature sensor and Vrefint channels + voltages is done following these steps : + (#) Enable the internal connection of Vbat/2, Temperature sensor and Vrefint sources + with the ADC channels using: + (++) ADC_TempSensorCmd() + (++) ADC_VrefintCmd() + (++) ADC_VbatCmd() + + (#) select the ADC_Channel_TempSensor and/or ADC_Channel_Vrefint and/or ADC_Channel_Vbat using + (++) ADC_RegularChannelConfig() or + (++) ADC_InjectedInit() functions + + (#) Get the voltage values, using: + (++) ADC_GetConversionValue() or + (++) ADC_GetInjectedConversionValue(). + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the temperature sensor channel. + * @param ADCx: where x can be 1 to select the ADC peripheral. + * @param NewState: new state of the temperature sensor. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_TempSensorCmd(__attribute__((unused)) ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the temperature sensor channel*/ + ADC1_2->CCR |= ADC12_CCR_TSEN; + } + else + { + /* Disable the temperature sensor channel*/ + ADC1_2->CCR &= ~(uint32_t)ADC12_CCR_TSEN; + } +} + +/** + * @brief Enables or disables the Vrefint channel. + * @param ADCx: where x can be 1 or 4 to select the ADC peripheral. + * @param NewState: new state of the Vrefint. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_VrefintCmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if((ADCx == ADC1) || (ADCx == ADC2)) + { + if (NewState != DISABLE) + { + /* Enable the Vrefint channel*/ + ADC1_2->CCR |= ADC12_CCR_VREFEN; + } + else + { + /* Disable the Vrefint channel*/ + ADC1_2->CCR &= ~(uint32_t)ADC12_CCR_VREFEN; + } + } + else + { + if (NewState != DISABLE) + { + /* Enable the Vrefint channel*/ + ADC3_4->CCR |= ADC34_CCR_VREFEN; + } + else + { + /* Disable the Vrefint channel*/ + ADC3_4->CCR &= ~(uint32_t)ADC34_CCR_VREFEN; + } + } +} + +/** + * @brief Enables or disables the Vbat channel. + * @param ADCx: where x can be 1 to select the ADC peripheral. + * @param NewState: new state of the Vbat. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_VbatCmd(__attribute__((unused)) ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the Vbat channel*/ + ADC1_2->CCR |= ADC12_CCR_VBATEN; + } + else + { + /* Disable the Vbat channel*/ + ADC1_2->CCR &= ~(uint32_t)ADC12_CCR_VBATEN; + } +} + +/** + * @} + */ + +/** @defgroup ADC_Group4 Regular Channels Configuration functions + * @brief Regular Channels Configuration functions + * +@verbatim + =============================================================================== + ##### Channels Configuration functions ##### + =============================================================================== + + [..] This section provides functions allowing to manage the ADC regular channels. + + [..] To configure a regular sequence of channels use: + (#) ADC_RegularChannelConfig() + this function allows: + (++) Configure the rank in the regular group sequencer for each channel + (++) Configure the sampling time for each channel + + (#) ADC_RegularChannelSequencerLengthConfig() to set the length of the regular sequencer + + [..] The regular trigger is configured using the following functions: + (#) ADC_SelectExternalTrigger() + (#) ADC_ExternalTriggerPolarityConfig() + + [..] The start and the stop conversion are controlled by: + (#) ADC_StartConversion() + (#) ADC_StopConversion() + + [..] + (@)Please Note that the following features for regular channels are configured + using the ADC_Init() function : + (++) continuous mode activation + (++) Resolution + (++) Data Alignement + (++) Overrun Mode. + + [..] Get the conversion data: This subsection provides an important function in + the ADC peripheral since it returns the converted data of the current + regular channel. When the Conversion value is read, the EOC Flag is + automatically cleared. + + [..] To configure the discontinuous mode, the following functions should be used: + (#) ADC_DiscModeChannelCountConfig() to configure the number of discontinuous channel to be converted. + (#) ADC_DiscModeCmd() to enable the discontinuous mode. + + [..] To configure and enable/disable the Channel offset use the functions: + (++) ADC_SetChannelOffset1() + (++) ADC_SetChannelOffset2() + (++) ADC_SetChannelOffset3() + (++) ADC_SetChannelOffset4() + (++) ADC_ChannelOffset1Cmd() + (++) ADC_ChannelOffset2Cmd() + (++) ADC_ChannelOffset3Cmd() + (++) ADC_ChannelOffset4Cmd() + +@endverbatim + * @{ + */ + +/** + * @brief Configures for the selected ADC regular channel its corresponding + * rank in the sequencer and its sample time. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @param Rank: The rank in the regular group sequencer. This parameter must be between 1 to 16. + * @param ADC_SampleTime: The sample time value to be set for the selected channel. + * This parameter can be one of the following values: + * @arg ADC_SampleTime_1Cycles5: Sample time equal to 1.5 cycles + * @arg ADC_SampleTime_2Cycles5: Sample time equal to 2.5 cycles + * @arg ADC_SampleTime_4Cycles5: Sample time equal to 4.5 cycles + * @arg ADC_SampleTime_7Cycles5: Sample time equal to 7.5 cycles + * @arg ADC_SampleTime_19Cycles5: Sample time equal to 19.5 cycles + * @arg ADC_SampleTime_61Cycles5: Sample time equal to 61.5 cycles + * @arg ADC_SampleTime_181Cycles5: Sample time equal to 181.5 cycles + * @arg ADC_SampleTime_601Cycles5: Sample time equal to 601.5 cycles + * @retval None + */ +void ADC_RegularChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint8_t Rank, uint8_t ADC_SampleTime) +{ + uint32_t tmpreg1 = 0, tmpreg2 = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + assert_param(IS_ADC_SAMPLE_TIME(ADC_SampleTime)); + + /* Regular sequence configuration */ + /* For Rank 1 to 4 */ + if (Rank < 5) + { + /* Get the old register value */ + tmpreg1 = ADCx->SQR1; + /* Calculate the mask to clear */ + tmpreg2 = 0x1F << (6 * (Rank )); + /* Clear the old SQx bits for the selected rank */ + tmpreg1 &= ~tmpreg2; + /* Calculate the mask to set */ + tmpreg2 = (uint32_t)(ADC_Channel) << (6 * (Rank)); + /* Set the SQx bits for the selected rank */ + tmpreg1 |= tmpreg2; + /* Store the new register value */ + ADCx->SQR1 = tmpreg1; + } + /* For Rank 5 to 9 */ + else if (Rank < 10) + { + /* Get the old register value */ + tmpreg1 = ADCx->SQR2; + /* Calculate the mask to clear */ + tmpreg2 = ADC_SQR2_SQ5 << (6 * (Rank - 5)); + /* Clear the old SQx bits for the selected rank */ + tmpreg1 &= ~tmpreg2; + /* Calculate the mask to set */ + tmpreg2 = (uint32_t)(ADC_Channel) << (6 * (Rank - 5)); + /* Set the SQx bits for the selected rank */ + tmpreg1 |= tmpreg2; + /* Store the new register value */ + ADCx->SQR2 = tmpreg1; + } + /* For Rank 10 to 14 */ + else if (Rank < 15) + { + /* Get the old register value */ + tmpreg1 = ADCx->SQR3; + /* Calculate the mask to clear */ + tmpreg2 = ADC_SQR3_SQ10 << (6 * (Rank - 10)); + /* Clear the old SQx bits for the selected rank */ + tmpreg1 &= ~tmpreg2; + /* Calculate the mask to set */ + tmpreg2 = (uint32_t)(ADC_Channel) << (6 * (Rank - 10)); + /* Set the SQx bits for the selected rank */ + tmpreg1 |= tmpreg2; + /* Store the new register value */ + ADCx->SQR3 = tmpreg1; + } + else + { + /* Get the old register value */ + tmpreg1 = ADCx->SQR4; + /* Calculate the mask to clear */ + tmpreg2 = ADC_SQR3_SQ15 << (6 * (Rank - 15)); + /* Clear the old SQx bits for the selected rank */ + tmpreg1 &= ~tmpreg2; + /* Calculate the mask to set */ + tmpreg2 = (uint32_t)(ADC_Channel) << (6 * (Rank - 15)); + /* Set the SQx bits for the selected rank */ + tmpreg1 |= tmpreg2; + /* Store the new register value */ + ADCx->SQR4 = tmpreg1; + } + + /* Channel sampling configuration */ + /* if ADC_Channel_10 ... ADC_Channel_18 is selected */ + if (ADC_Channel > ADC_Channel_9) + { + /* Get the old register value */ + tmpreg1 = ADCx->SMPR2; + /* Calculate the mask to clear */ + tmpreg2 = ADC_SMPR2_SMP10 << (3 * (ADC_Channel - 10)); + /* Clear the old channel sample time */ + ADCx->SMPR2 &= ~tmpreg2; + /* Calculate the mask to set */ + ADCx->SMPR2 |= (uint32_t)ADC_SampleTime << (3 * (ADC_Channel - 10)); + + } + else /* ADC_Channel include in ADC_Channel_[0..9] */ + { + /* Get the old register value */ + tmpreg1 = ADCx->SMPR1; + /* Calculate the mask to clear */ + tmpreg2 = ADC_SMPR1_SMP1 << (3 * (ADC_Channel - 1)); + /* Clear the old channel sample time */ + ADCx->SMPR1 &= ~tmpreg2; + /* Calculate the mask to set */ + ADCx->SMPR1 |= (uint32_t)ADC_SampleTime << (3 * (ADC_Channel)); + } +} + +/** + * @brief Sets the ADC regular channel sequence lenght. + * @param ADCx: where x can be 1, 2 or 3 to select the ADC peripheral. + * @param SequenceLength: The Regular sequence length. This parameter must be between 1 to 16. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_RegularChannelSequencerLengthConfig(ADC_TypeDef* ADCx, uint8_t SequencerLength) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Configure the ADC sequence lenght */ + ADCx->SQR1 &= ~(uint32_t)ADC_SQR1_L; + ADCx->SQR1 |= (uint32_t)(SequencerLength - 1); +} + +/** + * @brief External Trigger Enable and Polarity Selection for regular channels. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_ExternalTrigConvEvent: ADC external Trigger source. + * This parameter can be one of the following values: + * @arg ADC_ExternalTrigger_Event0: External trigger event 0 + * @arg ADC_ExternalTrigger_Event1: External trigger event 1 + * @arg ADC_ExternalTrigger_Event2: External trigger event 2 + * @arg ADC_ExternalTrigger_Event3: External trigger event 3 + * @arg ADC_ExternalTrigger_Event4: External trigger event 4 + * @arg ADC_ExternalTrigger_Event5: External trigger event 5 + * @arg ADC_ExternalTrigger_Event6: External trigger event 6 + * @arg ADC_ExternalTrigger_Event7: External trigger event 7 + * @arg ADC_ExternalTrigger_Event8: External trigger event 8 + * @arg ADC_ExternalTrigger_Event9: External trigger event 9 + * @arg ADC_ExternalTrigger_Event10: External trigger event 10 + * @arg ADC_ExternalTrigger_Event11: External trigger event 11 + * @arg ADC_ExternalTrigger_Event12: External trigger event 12 + * @arg ADC_ExternalTrigger_Event13: External trigger event 13 + * @arg ADC_ExternalTrigger_Event14: External trigger event 14 + * @arg ADC_ExternalTrigger_Event15: External trigger event 15 + * @param ADC_ExternalTrigEventEdge: ADC external Trigger Polarity. + * This parameter can be one of the following values: + * @arg ADC_ExternalTrigEventEdge_OFF: Hardware trigger detection disabled + * (conversions can be launched by software) + * @arg ADC_ExternalTrigEventEdge_RisingEdge: Hardware trigger detection on the rising edge + * @arg ADC_ExternalTrigEventEdge_FallingEdge: Hardware trigger detection on the falling edge + * @arg ADC_ExternalTrigEventEdge_BothEdge: Hardware trigger detection on both the rising and falling edges + * @retval None + */ +void ADC_ExternalTriggerConfig(ADC_TypeDef* ADCx, uint16_t ADC_ExternalTrigConvEvent, uint16_t ADC_ExternalTrigEventEdge) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_EXT_TRIG(ADC_ExternalTrigConvEvent)); + assert_param(IS_EXTERNALTRIG_EDGE(ADC_ExternalTrigEventEdge)); + + /* Disable the selected ADC conversion on external event */ + ADCx->CFGR &= ~(ADC_CFGR_EXTEN | ADC_CFGR_EXTSEL); + ADCx->CFGR |= (uint32_t)(ADC_ExternalTrigEventEdge | ADC_ExternalTrigConvEvent); +} + +/** + * @brief Enables or disables the selected ADC start conversion . + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_StartConversion(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADSTART bit */ + ADCx->CR |= ADC_CR_ADSTART; +} + +/** + * @brief Gets the selected ADC start conversion Status. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval The new state of ADC start conversion (SET or RESET). + */ +FlagStatus ADC_GetStartConversionStatus(ADC_TypeDef* ADCx) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + /* Check the status of ADSTART bit */ + if ((ADCx->CR & ADC_CR_ADSTART) != (uint32_t)RESET) + { + /* ADSTART bit is set */ + bitstatus = SET; + } + else + { + /* ADSTART bit is reset */ + bitstatus = RESET; + } + /* Return the ADSTART bit status */ + return bitstatus; +} + +/** + * @brief Stops the selected ADC ongoing conversion. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_StopConversion(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the ADSTP bit */ + ADCx->CR |= ADC_CR_ADSTP; +} + + +/** + * @brief Configures the discontinuous mode for the selected ADC regular + * group channel. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param Number: specifies the discontinuous mode regular channel + * count value. This number must be between 1 and 8. + * @retval None + */ +void ADC_DiscModeChannelCountConfig(ADC_TypeDef* ADCx, uint8_t Number) +{ + uint32_t tmpreg1 = 0; + uint32_t tmpreg2 = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_REGULAR_DISC_NUMBER(Number)); + /* Get the old register value */ + tmpreg1 = ADCx->CFGR; + /* Clear the old discontinuous mode channel count */ + tmpreg1 &= ~(uint32_t)(ADC_CFGR_DISCNUM); + /* Set the discontinuous mode channel count */ + tmpreg2 = Number - 1; + tmpreg1 |= tmpreg2 << 17; + /* Store the new register value */ + ADCx->CFGR = tmpreg1; +} + +/** + * @brief Enables or disables the discontinuous mode on regular group + * channel for the specified ADC + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the selected ADC discontinuous mode + * on regular group channel. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_DiscModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected ADC regular discontinuous mode */ + ADCx->CFGR |= ADC_CFGR_DISCEN; + } + else + { + /* Disable the selected ADC regular discontinuous mode */ + ADCx->CFGR &= ~(uint32_t)(ADC_CFGR_DISCEN); + } +} + +/** + * @brief Returns the last ADCx conversion result data for regular channel. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval The Data conversion value. + */ +uint16_t ADC_GetConversionValue(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + /* Return the selected ADC conversion value */ + return (uint16_t) ADCx->DR; +} + +/** + * @brief Returns the last ADC1, ADC2, ADC3 and ADC4 regular conversions results + * data in the selected dual mode. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval The Data conversion value. + * @note In dual mode, the value returned by this function is as following + * Data[15:0] : these bits contain the regular data of the Master ADC. + * Data[31:16]: these bits contain the regular data of the Slave ADC. + */ +uint32_t ADC_GetDualModeConversionValue(ADC_TypeDef* ADCx) +{ + uint32_t tmpreg1 = 0; + + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + if((ADCx == ADC1) || (ADCx== ADC2)) + { + /* Get the dual mode conversion value */ + tmpreg1 = ADC1_2->CDR; + } + else + { + /* Get the dual mode conversion value */ + tmpreg1 = ADC3_4->CDR; + } + /* Return the dual mode conversion value */ + return (uint32_t) tmpreg1; +} + +/** + * @brief Set the ADC channels conversion value offset1 + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @param Offset: the offset value for the selected ADC Channel + * This parameter must be a 12bit value. + * @retval None + */ +void ADC_SetChannelOffset1(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + assert_param(IS_ADC_OFFSET(Offset)); + + /* Select the Channel */ + ADCx->OFR1 &= ~ (uint32_t) ADC_OFR1_OFFSET1_CH; + ADCx->OFR1 |= (uint32_t)((uint32_t)ADC_Channel << 26); + + /* Set the data offset */ + ADCx->OFR1 &= ~ (uint32_t) ADC_OFR1_OFFSET1; + ADCx->OFR1 |= (uint32_t)Offset; +} + +/** + * @brief Set the ADC channels conversion value offset2 + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @param Offset: the offset value for the selected ADC Channel + * This parameter must be a 12bit value. + * @retval None + */ +void ADC_SetChannelOffset2(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + assert_param(IS_ADC_OFFSET(Offset)); + + /* Select the Channel */ + ADCx->OFR2 &= ~ (uint32_t) ADC_OFR2_OFFSET2_CH; + ADCx->OFR2 |= (uint32_t)((uint32_t)ADC_Channel << 26); + + /* Set the data offset */ + ADCx->OFR2 &= ~ (uint32_t) ADC_OFR2_OFFSET2; + ADCx->OFR2 |= (uint32_t)Offset; +} + +/** + * @brief Set the ADC channels conversion value offset3 + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @param Offset: the offset value for the selected ADC Channel + * This parameter must be a 12bit value. + * @retval None + */ +void ADC_SetChannelOffset3(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + assert_param(IS_ADC_OFFSET(Offset)); + + /* Select the Channel */ + ADCx->OFR3 &= ~ (uint32_t) ADC_OFR3_OFFSET3_CH; + ADCx->OFR3 |= (uint32_t)((uint32_t)ADC_Channel << 26); + + /* Set the data offset */ + ADCx->OFR3 &= ~ (uint32_t) ADC_OFR3_OFFSET3; + ADCx->OFR3 |= (uint32_t)Offset; +} + +/** + * @brief Set the ADC channels conversion value offset4 + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure. + * This parameter can be one of the following values: + * @arg ADC_Channel_1: ADC Channel1 selected + * @arg ADC_Channel_2: ADC Channel2 selected + * @arg ADC_Channel_3: ADC Channel3 selected + * @arg ADC_Channel_4: ADC Channel4 selected + * @arg ADC_Channel_5: ADC Channel5 selected + * @arg ADC_Channel_6: ADC Channel6 selected + * @arg ADC_Channel_7: ADC Channel7 selected + * @arg ADC_Channel_8: ADC Channel8 selected + * @arg ADC_Channel_9: ADC Channel9 selected + * @arg ADC_Channel_10: ADC Channel10 selected + * @arg ADC_Channel_11: ADC Channel11 selected + * @arg ADC_Channel_12: ADC Channel12 selected + * @arg ADC_Channel_13: ADC Channel13 selected + * @arg ADC_Channel_14: ADC Channel14 selected + * @arg ADC_Channel_15: ADC Channel15 selected + * @arg ADC_Channel_16: ADC Channel16 selected + * @arg ADC_Channel_17: ADC Channel17 selected + * @arg ADC_Channel_18: ADC Channel18 selected + * @param Offset: the offset value for the selected ADC Channel + * This parameter must be a 12bit value. + * @retval None + */ +void ADC_SetChannelOffset4(ADC_TypeDef* ADCx, uint8_t ADC_Channel, uint16_t Offset) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CHANNEL(ADC_Channel)); + assert_param(IS_ADC_OFFSET(Offset)); + + /* Select the Channel */ + ADCx->OFR4 &= ~ (uint32_t) ADC_OFR4_OFFSET4_CH; + ADCx->OFR4 |= (uint32_t)((uint32_t)ADC_Channel << 26); + + /* Set the data offset */ + ADCx->OFR4 &= ~ (uint32_t) ADC_OFR4_OFFSET4; + ADCx->OFR4 |= (uint32_t)Offset; +} + +/** + * @brief Enables or disables the Offset1. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADCx offset1. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_ChannelOffset1Cmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the OFFSET1_EN bit */ + ADCx->OFR1 |= ADC_OFR1_OFFSET1_EN; + } + else + { + /* Reset the OFFSET1_EN bit */ + ADCx->OFR1 &= ~(ADC_OFR1_OFFSET1_EN); + } +} + +/** + * @brief Enables or disables the Offset2. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADCx offset2. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_ChannelOffset2Cmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the OFFSET1_EN bit */ + ADCx->OFR2 |= ADC_OFR2_OFFSET2_EN; + } + else + { + /* Reset the OFFSET1_EN bit */ + ADCx->OFR2 &= ~(ADC_OFR2_OFFSET2_EN); + } +} + +/** + * @brief Enables or disables the Offset3. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADCx offset3. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_ChannelOffset3Cmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the OFFSET1_EN bit */ + ADCx->OFR3 |= ADC_OFR3_OFFSET3_EN; + } + else + { + /* Reset the OFFSET1_EN bit */ + ADCx->OFR3 &= ~(ADC_OFR3_OFFSET3_EN); + } +} + +/** + * @brief Enables or disables the Offset4. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the ADCx offset4. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_ChannelOffset4Cmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the OFFSET1_EN bit */ + ADCx->OFR4 |= ADC_OFR4_OFFSET4_EN; + } + else + { + /* Reset the OFFSET1_EN bit */ + ADCx->OFR4 &= ~(ADC_OFR4_OFFSET4_EN); + } +} + +/** + * @} + */ + +/** @defgroup ADC_Group5 Regular Channels DMA Configuration functions + * @brief Regular Channels DMA Configuration functions + * +@verbatim + =============================================================================== + ##### Regular Channels DMA Configuration functions ##### + =============================================================================== + + [..] This section provides functions allowing to configure the DMA for ADC regular + channels. Since converted regular channel values are stored into a unique data register, + it is useful to use DMA for conversion of more than one regular channel. This + avoids the loss of the data already stored in the ADC Data register. + + (#) ADC_DMACmd() function is used to enable the ADC DMA mode, after each + conversion of a regular channel, a DMA request is generated. + (#) ADC_DMAConfig() function is used to select between the one shot DMA mode + or the circular DMA mode + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified ADC DMA request. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the selected ADC DMA transfer. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_DMACmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_DMA_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected ADC DMA request */ + ADCx->CFGR |= ADC_CFGR_DMAEN; + } + else + { + /* Disable the selected ADC DMA request */ + ADCx->CFGR &= ~(uint32_t)ADC_CFGR_DMAEN; + } +} + +/** + * @brief Configure ADC DMA mode. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_DMAMode: select the ADC DMA mode. + * This parameter can be one of the following values: + * @arg ADC_DMAMode_OneShot: ADC DMA Oneshot mode + * @arg ADC_DMAMode_Circular: ADC DMA circular mode + * @retval None + */ +void ADC_DMAConfig(ADC_TypeDef* ADCx, uint32_t ADC_DMAMode) +{ + /* Check the parameters */ + assert_param(IS_ADC_DMA_PERIPH(ADCx)); + assert_param(IS_ADC_DMA_MODE(ADC_DMAMode)); + + /* Set or reset the DMACFG bit */ + ADCx->CFGR &= ~(uint32_t)ADC_CFGR_DMACFG; + ADCx->CFGR |= ADC_DMAMode; +} + +/** + * @} + */ + +/** @defgroup ADC_Group6 Injected channels Configuration functions + * @brief Injected channels Configuration functions + * +@verbatim + =============================================================================== + ##### Injected channels Configuration functions ##### + =============================================================================== + + [..] This section provide functions allowing to manage the ADC Injected channels, + it is composed of : + + (#) Configuration functions for Injected channels sample time + (#) Functions to start and stop the injected conversion + (#) unction to select the discontinuous mode + (#) Function to get the Specified Injected channel conversion data: This subsection + provides an important function in the ADC peripheral since it returns the + converted data of the specific injected channel. + +@endverbatim + * @{ + */ + +/** + * @brief Configures for the selected ADC injected channel its corresponding + * sample time. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_Channel: the ADC channel to configure. + * This parameter can be one of the following values: + * @arg ADC_InjectedChannel_1: ADC Channel1 selected + * @arg ADC_InjectedChannel_2: ADC Channel2 selected + * @arg ADC_InjectedChannel_3: ADC Channel3 selected + * @arg ADC_InjectedChannel_4: ADC Channel4 selected + * @arg ADC_InjectedChannel_5: ADC Channel5 selected + * @arg ADC_InjectedChannel_6: ADC Channel6 selected + * @arg ADC_InjectedChannel_7: ADC Channel7 selected + * @arg ADC_InjectedChannel_8: ADC Channel8 selected + * @arg ADC_InjectedChannel_9: ADC Channel9 selected + * @arg ADC_InjectedChannel_10: ADC Channel10 selected + * @arg ADC_InjectedChannel_11: ADC Channel11 selected + * @arg ADC_InjectedChannel_12: ADC Channel12 selected + * @arg ADC_InjectedChannel_13: ADC Channel13 selected + * @arg ADC_InjectedChannel_14: ADC Channel14 selected + * @arg ADC_InjectedChannel_15: ADC Channel15 selected + * @arg ADC_InjectedChannel_16: ADC Channel16 selected + * @arg ADC_InjectedChannel_17: ADC Channel17 selected + * @arg ADC_InjectedChannel_18: ADC Channel18 selected + * @param ADC_SampleTime: The sample time value to be set for the selected channel. + * This parameter can be one of the following values: + * @arg ADC_SampleTime_1Cycles5: Sample time equal to 1.5 cycles + * @arg ADC_SampleTime_2Cycles5: Sample time equal to 2.5 cycles + * @arg ADC_SampleTime_4Cycles5: Sample time equal to 4.5 cycles + * @arg ADC_SampleTime_7Cycles5: Sample time equal to 7.5 cycles + * @arg ADC_SampleTime_19Cycles5: Sample time equal to 19.5 cycles + * @arg ADC_SampleTime_61Cycles5: Sample time equal to 61.5 cycles + * @arg ADC_SampleTime_181Cycles5: Sample time equal to 181.5 cycles + * @arg ADC_SampleTime_601Cycles5: Sample time equal to 601.5 cycles + * @retval None + */ +void ADC_InjectedChannelSampleTimeConfig(ADC_TypeDef* ADCx, uint8_t ADC_InjectedChannel, uint8_t ADC_SampleTime) +{ + uint32_t tmpreg1 = 0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_INJECTED_CHANNEL(ADC_InjectedChannel)); + assert_param(IS_ADC_SAMPLE_TIME(ADC_SampleTime)); + + /* Channel sampling configuration */ + /* if ADC_InjectedChannel_10 ... ADC_InjectedChannel_18 is selected */ + if (ADC_InjectedChannel > ADC_InjectedChannel_9) + { + /* Calculate the mask to clear */ + tmpreg1 = ADC_SMPR2_SMP10 << (3 * (ADC_InjectedChannel - 10)); + /* Clear the old channel sample time */ + ADCx->SMPR2 &= ~tmpreg1; + /* Calculate the mask to set */ + ADCx->SMPR2 |= (uint32_t)ADC_SampleTime << (3 * (ADC_InjectedChannel - 10)); + + } + else /* ADC_InjectedChannel include in ADC_InjectedChannel_[0..9] */ + { + /* Calculate the mask to clear */ + tmpreg1 = ADC_SMPR1_SMP1 << (3 * (ADC_InjectedChannel - 1)); + /* Clear the old channel sample time */ + ADCx->SMPR1 &= ~tmpreg1; + /* Calculate the mask to set */ + ADCx->SMPR1 |= (uint32_t)ADC_SampleTime << (3 * (ADC_InjectedChannel)); + } +} + +/** + * @brief Enables or disables the selected ADC start of the injected + * channels conversion. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the selected ADC software start injected conversion. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_StartInjectedConversion(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Enable the selected ADC conversion for injected group on external event and start the selected + ADC injected conversion */ + ADCx->CR |= ADC_CR_JADSTART; +} + +/** + * @brief Stops the selected ADC ongoing injected conversion. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval None + */ +void ADC_StopInjectedConversion(ADC_TypeDef* ADCx) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Set the JADSTP bit */ + ADCx->CR |= ADC_CR_JADSTP; +} + +/** + * @brief Gets the selected ADC Software start injected conversion Status. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @retval The new state of ADC start injected conversion (SET or RESET). + */ +FlagStatus ADC_GetStartInjectedConversionStatus(ADC_TypeDef* ADCx) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + + /* Check the status of JADSTART bit */ + if ((ADCx->CR & ADC_CR_JADSTART) != (uint32_t)RESET) + { + /* JADSTART bit is set */ + bitstatus = SET; + } + else + { + /* JADSTART bit is reset */ + bitstatus = RESET; + } + /* Return the JADSTART bit status */ + return bitstatus; +} + +/** + * @brief Enables or disables the selected ADC automatic injected group + * conversion after regular one. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the selected ADC auto injected conversion + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_AutoInjectedConvCmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected ADC automatic injected group conversion */ + ADCx->CFGR |= ADC_CFGR_JAUTO; + } + else + { + /* Disable the selected ADC automatic injected group conversion */ + ADCx->CFGR &= ~ADC_CFGR_JAUTO; + } +} + +/** + * @brief Enables or disables the discontinuous mode for injected group + * channel for the specified ADC + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param NewState: new state of the selected ADC discontinuous mode + * on injected group channel. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_InjectedDiscModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected ADC injected discontinuous mode */ + ADCx->CFGR |= ADC_CFGR_JDISCEN; + } + else + { + /* Disable the selected ADC injected discontinuous mode */ + ADCx->CFGR &= ~ADC_CFGR_JDISCEN; + } +} + +/** + * @brief Returns the ADC injected channel conversion result + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_InjectedSequence: the converted ADC injected sequence. + * This parameter can be one of the following values: + * @arg ADC_InjectedSequence_1: Injected Sequence1 selected + * @arg ADC_InjectedSequence_2: Injected Sequence2 selected + * @arg ADC_InjectedSequence_3: Injected Sequence3 selected + * @arg ADC_InjectedSequence_4: Injected Sequence4 selected + * @retval The Data conversion value. + */ +uint16_t ADC_GetInjectedConversionValue(ADC_TypeDef* ADCx, uint8_t ADC_InjectedSequence) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_INJECTED_SEQUENCE(ADC_InjectedSequence)); + + tmp = (uint32_t)ADCx; + tmp += ((ADC_InjectedSequence - 1 )<< 2) + JDR_Offset; + + /* Returns the selected injected channel conversion data value */ + return (uint16_t) (*(__IO uint32_t*) tmp); +} + +/** + * @} + */ + +/** @defgroup ADC_Group7 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + + [..] This section provides functions allowing to configure the ADC Interrupts, get + the status and clear flags and Interrupts pending bits. + + [..] The ADC provide 11 Interrupts sources and 11 Flags which can be divided into 3 groups: + + (#) Flags and Interrupts for ADC regular channels + (##)Flags + (+) ADC_FLAG_RDY: ADC Ready flag + (+) ADC_FLAG_EOSMP: ADC End of Sampling flag + (+) ADC_FLAG_EOC: ADC End of Regular Conversion flag. + (+) ADC_FLAG_EOS: ADC End of Regular sequence of Conversions flag + (+) ADC_FLAG_OVR: ADC overrun flag + + (##) Interrupts + (+) ADC_IT_RDY: ADC Ready interrupt source + (+) ADC_IT_EOSMP: ADC End of Sampling interrupt source + (+) ADC_IT_EOC: ADC End of Regular Conversion interrupt source + (+) ADC_IT_EOS: ADC End of Regular sequence of Conversions interrupt + (+) ADC_IT_OVR: ADC overrun interrupt source + + + (#) Flags and Interrupts for ADC regular channels + (##)Flags + (+) ADC_FLAG_JEOC: ADC Ready flag + (+) ADC_FLAG_JEOS: ADC End of Sampling flag + (+) ADC_FLAG_JQOVF: ADC End of Regular Conversion flag. + + (##) Interrupts + (+) ADC_IT_JEOC: ADC End of Injected Conversion interrupt source + (+) ADC_IT_JEOS: ADC End of Injected sequence of Conversions interrupt source + (+) ADC_IT_JQOVF: ADC Injected Context Queue Overflow interrupt source + + (#) General Flags and Interrupts for the ADC + (##)Flags + (+) ADC_FLAG_AWD1: ADC Analog watchdog 1 flag + (+) ADC_FLAG_AWD2: ADC Analog watchdog 2 flag + (+) ADC_FLAG_AWD3: ADC Analog watchdog 3 flag + + (##)Flags + (+) ADC_IT_AWD1: ADC Analog watchdog 1 interrupt source + (+) ADC_IT_AWD2: ADC Analog watchdog 2 interrupt source + (+) ADC_IT_AWD3: ADC Analog watchdog 3 interrupt source + + (#) Flags for ADC dual mode + (##)Flags for Master + (+) ADC_FLAG_MSTRDY: ADC master Ready (ADRDY) flag + (+) ADC_FLAG_MSTEOSMP: ADC master End of Sampling flag + (+) ADC_FLAG_MSTEOC: ADC master End of Regular Conversion flag + (+) ADC_FLAG_MSTEOS: ADC master End of Regular sequence of Conversions flag + (+) ADC_FLAG_MSTOVR: ADC master overrun flag + (+) ADC_FLAG_MSTJEOC: ADC master End of Injected Conversion flag + (+) ADC_FLAG_MSTJEOS: ADC master End of Injected sequence of Conversions flag + (+) ADC_FLAG_MSTAWD1: ADC master Analog watchdog 1 flag + (+) ADC_FLAG_MSTAWD2: ADC master Analog watchdog 2 flag + (+) ADC_FLAG_MSTAWD3: ADC master Analog watchdog 3 flag + (+) ADC_FLAG_MSTJQOVF: ADC master Injected Context Queue Overflow flag + + (##) Flags for Slave + (+) ADC_FLAG_SLVRDY: ADC slave Ready (ADRDY) flag + (+) ADC_FLAG_SLVEOSMP: ADC slave End of Sampling flag + (+) ADC_FLAG_SLVEOC: ADC slave End of Regular Conversion flag + (+) ADC_FLAG_SLVEOS: ADC slave End of Regular sequence of Conversions flag + (+) ADC_FLAG_SLVOVR: ADC slave overrun flag + (+) ADC_FLAG_SLVJEOC: ADC slave End of Injected Conversion flag + (+) ADC_FLAG_SLVJEOS: ADC slave End of Injected sequence of Conversions flag + (+) ADC_FLAG_SLVAWD1: ADC slave Analog watchdog 1 flag + (+) ADC_FLAG_SLVAWD2: ADC slave Analog watchdog 2 flag + (+) ADC_FLAG_SLVAWD3: ADC slave Analog watchdog 3 flag + (+) ADC_FLAG_SLVJQOVF: ADC slave Injected Context Queue Overflow flag + + The user should identify which mode will be used in his application to manage + the ADC controller events: Polling mode or Interrupt mode. + + In the Polling Mode it is advised to use the following functions: + - ADC_GetFlagStatus() : to check if flags events occur. + - ADC_ClearFlag() : to clear the flags events. + + In the Interrupt Mode it is advised to use the following functions: + - ADC_ITConfig() : to enable or disable the interrupt source. + - ADC_GetITStatus() : to check if Interrupt occurs. + - ADC_ClearITPendingBit() : to clear the Interrupt pending Bit + (corresponding Flag). +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified ADC interrupts. + * @param ADCx: where x can be 1, 2 or 3 to select the ADC peripheral. + * @param ADC_IT: specifies the ADC interrupt sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg ADC_IT_RDY: ADC Ready (ADRDY) interrupt source + * @arg ADC_IT_EOSMP: ADC End of Sampling interrupt source + * @arg ADC_IT_EOC: ADC End of Regular Conversion interrupt source + * @arg ADC_IT_EOS: ADC End of Regular sequence of Conversions interrupt source + * @arg ADC_IT_OVR: ADC overrun interrupt source + * @arg ADC_IT_JEOC: ADC End of Injected Conversion interrupt source + * @arg ADC_IT_JEOS: ADC End of Injected sequence of Conversions interrupt source + * @arg ADC_IT_AWD1: ADC Analog watchdog 1 interrupt source + * @arg ADC_IT_AWD2: ADC Analog watchdog 2 interrupt source + * @arg ADC_IT_AWD3: ADC Analog watchdog 3 interrupt source + * @arg ADC_IT_JQOVF: ADC Injected Context Queue Overflow interrupt source + * @param NewState: new state of the specified ADC interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void ADC_ITConfig(ADC_TypeDef* ADCx, uint32_t ADC_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_ADC_IT(ADC_IT)); + + if (NewState != DISABLE) + { + /* Enable the selected ADC interrupts */ + ADCx->IER |= ADC_IT; + } + else + { + /* Disable the selected ADC interrupts */ + ADCx->IER &= (~(uint32_t)ADC_IT); + } +} + +/** + * @brief Checks whether the specified ADC flag is set or not. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg ADC_FLAG_RDY: ADC Ready (ADRDY) flag + * @arg ADC_FLAG_EOSMP: ADC End of Sampling flag + * @arg ADC_FLAG_EOC: ADC End of Regular Conversion flag + * @arg ADC_FLAG_EOS: ADC End of Regular sequence of Conversions flag + * @arg ADC_FLAG_OVR: ADC overrun flag + * @arg ADC_FLAG_JEOC: ADC End of Injected Conversion flag + * @arg ADC_FLAG_JEOS: ADC End of Injected sequence of Conversions flag + * @arg ADC_FLAG_AWD1: ADC Analog watchdog 1 flag + * @arg ADC_FLAG_AWD2: ADC Analog watchdog 2 flag + * @arg ADC_FLAG_AWD3: ADC Analog watchdog 3 flag + * @arg ADC_FLAG_JQOVF: ADC Injected Context Queue Overflow flag + * @retval The new state of ADC_FLAG (SET or RESET). + */ +FlagStatus ADC_GetFlagStatus(ADC_TypeDef* ADCx, uint32_t ADC_FLAG) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_GET_FLAG(ADC_FLAG)); + + /* Check the status of the specified ADC flag */ + if ((ADCx->ISR & ADC_FLAG) != (uint32_t)RESET) + { + /* ADC_FLAG is set */ + bitstatus = SET; + } + else + { + /* ADC_FLAG is reset */ + bitstatus = RESET; + } + /* Return the ADC_FLAG status */ + return bitstatus; +} + +/** + * @brief Clears the ADCx's pending flags. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_FLAG: specifies the flag to clear. + * This parameter can be any combination of the following values: + * @arg ADC_FLAG_RDY: ADC Ready (ADRDY) flag + * @arg ADC_FLAG_EOSMP: ADC End of Sampling flag + * @arg ADC_FLAG_EOC: ADC End of Regular Conversion flag + * @arg ADC_FLAG_EOS: ADC End of Regular sequence of Conversions flag + * @arg ADC_FLAG_OVR: ADC overrun flag + * @arg ADC_FLAG_JEOC: ADC End of Injected Conversion flag + * @arg ADC_FLAG_JEOS: ADC End of Injected sequence of Conversions flag + * @arg ADC_FLAG_AWD1: ADC Analog watchdog 1 flag + * @arg ADC_FLAG_AWD2: ADC Analog watchdog 2 flag + * @arg ADC_FLAG_AWD3: ADC Analog watchdog 3 flag + * @arg ADC_FLAG_JQOVF: ADC Injected Context Queue Overflow flag + * @retval None + */ +void ADC_ClearFlag(ADC_TypeDef* ADCx, uint32_t ADC_FLAG) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CLEAR_FLAG(ADC_FLAG)); + /* Clear the selected ADC flags */ + ADCx->ISR = (uint32_t)ADC_FLAG; +} + +/** + * @brief Checks whether the specified ADC flag is set or not. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_FLAG: specifies the master or slave flag to check. + * This parameter can be one of the following values: + * @arg ADC_FLAG_MSTRDY: ADC master Ready (ADRDY) flag + * @arg ADC_FLAG_MSTEOSMP: ADC master End of Sampling flag + * @arg ADC_FLAG_MSTEOC: ADC master End of Regular Conversion flag + * @arg ADC_FLAG_MSTEOS: ADC master End of Regular sequence of Conversions flag + * @arg ADC_FLAG_MSTOVR: ADC master overrun flag + * @arg ADC_FLAG_MSTJEOC: ADC master End of Injected Conversion flag + * @arg ADC_FLAG_MSTJEOS: ADC master End of Injected sequence of Conversions flag + * @arg ADC_FLAG_MSTAWD1: ADC master Analog watchdog 1 flag + * @arg ADC_FLAG_MSTAWD2: ADC master Analog watchdog 2 flag + * @arg ADC_FLAG_MSTAWD3: ADC master Analog watchdog 3 flag + * @arg ADC_FLAG_MSTJQOVF: ADC master Injected Context Queue Overflow flag + * @arg ADC_FLAG_SLVRDY: ADC slave Ready (ADRDY) flag + * @arg ADC_FLAG_SLVEOSMP: ADC slave End of Sampling flag + * @arg ADC_FLAG_SLVEOC: ADC slave End of Regular Conversion flag + * @arg ADC_FLAG_SLVEOS: ADC slave End of Regular sequence of Conversions flag + * @arg ADC_FLAG_SLVOVR: ADC slave overrun flag + * @arg ADC_FLAG_SLVJEOC: ADC slave End of Injected Conversion flag + * @arg ADC_FLAG_SLVJEOS: ADC slave End of Injected sequence of Conversions flag + * @arg ADC_FLAG_SLVAWD1: ADC slave Analog watchdog 1 flag + * @arg ADC_FLAG_SLVAWD2: ADC slave Analog watchdog 2 flag + * @arg ADC_FLAG_SLVAWD3: ADC slave Analog watchdog 3 flag + * @arg ADC_FLAG_SLVJQOVF: ADC slave Injected Context Queue Overflow flag + * @retval The new state of ADC_FLAG (SET or RESET). + */ +FlagStatus ADC_GetCommonFlagStatus(ADC_TypeDef* ADCx, uint32_t ADC_FLAG) +{ + uint32_t tmpreg1 = 0; + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_GET_COMMONFLAG(ADC_FLAG)); + + if((ADCx == ADC1) || (ADCx == ADC2)) + { + tmpreg1 = ADC1_2->CSR; + } + else + { + tmpreg1 = ADC3_4->CSR; + } + /* Check the status of the specified ADC flag */ + if ((tmpreg1 & ADC_FLAG) != (uint32_t)RESET) + { + /* ADC_FLAG is set */ + bitstatus = SET; + } + else + { + /* ADC_FLAG is reset */ + bitstatus = RESET; + } + /* Return the ADC_FLAG status */ + return bitstatus; +} + +/** + * @brief Clears the ADCx's pending flags. + * @param ADCx: where x can be 1, 2, 3 or 4 to select the ADC peripheral. + * @param ADC_FLAG: specifies the master or slave flag to clear. + * This parameter can be one of the following values: + * @arg ADC_FLAG_MSTRDY: ADC master Ready (ADRDY) flag + * @arg ADC_FLAG_MSTEOSMP: ADC master End of Sampling flag + * @arg ADC_FLAG_MSTEOC: ADC master End of Regular Conversion flag + * @arg ADC_FLAG_MSTEOS: ADC master End of Regular sequence of Conversions flag + * @arg ADC_FLAG_MSTOVR: ADC master overrun flag + * @arg ADC_FLAG_MSTJEOC: ADC master End of Injected Conversion flag + * @arg ADC_FLAG_MSTJEOS: ADC master End of Injected sequence of Conversions flag + * @arg ADC_FLAG_MSTAWD1: ADC master Analog watchdog 1 flag + * @arg ADC_FLAG_MSTAWD2: ADC master Analog watchdog 2 flag + * @arg ADC_FLAG_MSTAWD3: ADC master Analog watchdog 3 flag + * @arg ADC_FLAG_MSTJQOVF: ADC master Injected Context Queue Overflow flag + * @arg ADC_FLAG_SLVRDY: ADC slave Ready (ADRDY) flag + * @arg ADC_FLAG_SLVEOSMP: ADC slave End of Sampling flag + * @arg ADC_FLAG_SLVEOC: ADC slave End of Regular Conversion flag + * @arg ADC_FLAG_SLVEOS: ADC slave End of Regular sequence of Conversions flag + * @arg ADC_FLAG_SLVOVR: ADC slave overrun flag + * @arg ADC_FLAG_SLVJEOC: ADC slave End of Injected Conversion flag + * @arg ADC_FLAG_SLVJEOS: ADC slave End of Injected sequence of Conversions flag + * @arg ADC_FLAG_SLVAWD1: ADC slave Analog watchdog 1 flag + * @arg ADC_FLAG_SLVAWD2: ADC slave Analog watchdog 2 flag + * @arg ADC_FLAG_SLVAWD3: ADC slave Analog watchdog 3 flag + * @arg ADC_FLAG_SLVJQOVF: ADC slave Injected Context Queue Overflow flag + * @retval None + */ +void ADC_ClearCommonFlag(ADC_TypeDef* ADCx, uint32_t ADC_FLAG) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_CLEAR_COMMONFLAG(ADC_FLAG)); + + if((ADCx == ADC1) || (ADCx == ADC2)) + { + /* Clear the selected ADC flags */ + ADC1_2->CSR |= (uint32_t)ADC_FLAG; + } + else + { + /* Clear the selected ADC flags */ + ADC3_4->CSR |= (uint32_t)ADC_FLAG; + } +} + +/** + * @brief Checks whether the specified ADC interrupt has occurred or not. + * @param ADCx: where x can be 1, 2 or 3 to select the ADC peripheral. + * @param ADC_IT: specifies the ADC interrupt source to check. + * This parameter can be one of the following values: + * @arg ADC_IT_RDY: ADC Ready (ADRDY) interrupt source + * @arg ADC_IT_EOSMP: ADC End of Sampling interrupt source + * @arg ADC_IT_EOC: ADC End of Regular Conversion interrupt source + * @arg ADC_IT_EOS: ADC End of Regular sequence of Conversions interrupt source + * @arg ADC_IT_OVR: ADC overrun interrupt source + * @arg ADC_IT_JEOC: ADC End of Injected Conversion interrupt source + * @arg ADC_IT_JEOS: ADC End of Injected sequence of Conversions interrupt source + * @arg ADC_IT_AWD1: ADC Analog watchdog 1 interrupt source + * @arg ADC_IT_AWD2: ADC Analog watchdog 2 interrupt source + * @arg ADC_IT_AWD3: ADC Analog watchdog 3 interrupt source + * @arg ADC_IT_JQOVF: ADC Injected Context Queue Overflow interrupt source + * @retval The new state of ADC_IT (SET or RESET). + */ +ITStatus ADC_GetITStatus(ADC_TypeDef* ADCx, uint32_t ADC_IT) +{ + ITStatus bitstatus = RESET; + uint16_t itstatus = 0x0, itenable = 0x0; + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_GET_IT(ADC_IT)); + + itstatus = ADCx->ISR & ADC_IT; + + itenable = ADCx->IER & ADC_IT; + if ((itstatus != (uint32_t)RESET) && (itenable != (uint32_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the ADCx's interrupt pending bits. + * @param ADCx: where x can be 1, 2 or 3 to select the ADC peripheral. + * @param ADC_IT: specifies the ADC interrupt pending bit to clear. + * This parameter can be any combination of the following values: + * @arg ADC_IT_RDY: ADC Ready (ADRDY) interrupt source + * @arg ADC_IT_EOSMP: ADC End of Sampling interrupt source + * @arg ADC_IT_EOC: ADC End of Regular Conversion interrupt source + * @arg ADC_IT_EOS: ADC End of Regular sequence of Conversions interrupt source + * @arg ADC_IT_OVR: ADC overrun interrupt source + * @arg ADC_IT_JEOC: ADC End of Injected Conversion interrupt source + * @arg ADC_IT_JEOS: ADC End of Injected sequence of Conversions interrupt source + * @arg ADC_IT_AWD1: ADC Analog watchdog 1 interrupt source + * @arg ADC_IT_AWD2: ADC Analog watchdog 2 interrupt source + * @arg ADC_IT_AWD3: ADC Analog watchdog 3 interrupt source + * @arg ADC_IT_JQOVF: ADC Injected Context Queue Overflow interrupt source + * @retval None + */ +void ADC_ClearITPendingBit(ADC_TypeDef* ADCx, uint32_t ADC_IT) +{ + /* Check the parameters */ + assert_param(IS_ADC_ALL_PERIPH(ADCx)); + assert_param(IS_ADC_IT(ADC_IT)); + /* Clear the selected ADC interrupt pending bit */ + ADCx->ISR = (uint32_t)ADC_IT; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_can.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_can.c new file mode 100755 index 0000000000..40b5a90751 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_can.c @@ -0,0 +1,1629 @@ +/** + ****************************************************************************** + * @file stm32f30x_can.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Controller area network (CAN) peripheral: + * + Initialization and Configuration + * + CAN Frames Transmission + * + CAN Frames Reception + * + Operation modes switch + * + Error management + * + Interrupts and flags + * + @verbatim + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] + (#) Enable the CAN controller interface clock using + RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE); + (#) CAN pins configuration: + (++) Enable the clock for the CAN GPIOs using the following function: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOx, ENABLE); + (++) Connect the involved CAN pins to AF9 using the following function + GPIO_PinAFConfig(GPIOx, GPIO_PinSourcex, GPIO_AF_CANx); + (++) Configure these CAN pins in alternate function mode by calling + the function GPIO_Init(); + (#) Initialize and configure the CAN using CAN_Init() and + CAN_FilterInit() functions. + (#) Transmit the desired CAN frame using CAN_Transmit() function. + (#) Check the transmission of a CAN frame using CAN_TransmitStatus() function. + (#) Cancel the transmission of a CAN frame using CAN_CancelTransmit() function. + (#) Receive a CAN frame using CAN_Recieve() function. + (#) Release the receive FIFOs using CAN_FIFORelease() function. + (#) Return the number of pending received frames using CAN_MessagePending() function. + (#) To control CAN events you can use one of the following two methods: + (++) Check on CAN flags using the CAN_GetFlagStatus() function. + (++) Use CAN interrupts through the function CAN_ITConfig() at initialization + phase and CAN_GetITStatus() function into interrupt routines to check + if the event has occurred or not. + After checking on a flag you should clear it using CAN_ClearFlag() + function. And after checking on an interrupt event you should clear it + using CAN_ClearITPendingBit() function. + + @endverbatim + * + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_can.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup CAN + * @brief CAN driver modules + * @{ + */ +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* CAN Master Control Register bits */ +#define MCR_DBF ((uint32_t)0x00010000) /* software master reset */ + +/* CAN Mailbox Transmit Request */ +#define TMIDxR_TXRQ ((uint32_t)0x00000001) /* Transmit mailbox request */ + +/* CAN Filter Master Register bits */ +#define FMR_FINIT ((uint32_t)0x00000001) /* Filter init mode */ + +/* Time out for INAK bit */ +#define INAK_TIMEOUT ((uint32_t)0x00FFFFFF) +/* Time out for SLAK bit */ +#define SLAK_TIMEOUT ((uint32_t)0x00FFFFFF) + +/* Flags in TSR register */ +#define CAN_FLAGS_TSR ((uint32_t)0x08000000) +/* Flags in RF1R register */ +#define CAN_FLAGS_RF1R ((uint32_t)0x04000000) +/* Flags in RF0R register */ +#define CAN_FLAGS_RF0R ((uint32_t)0x02000000) +/* Flags in MSR register */ +#define CAN_FLAGS_MSR ((uint32_t)0x01000000) +/* Flags in ESR register */ +#define CAN_FLAGS_ESR ((uint32_t)0x00F00000) + +/* Mailboxes definition */ +#define CAN_TXMAILBOX_0 ((uint8_t)0x00) +#define CAN_TXMAILBOX_1 ((uint8_t)0x01) +#define CAN_TXMAILBOX_2 ((uint8_t)0x02) + +#define CAN_MODE_MASK ((uint32_t) 0x00000003) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ +static ITStatus CheckITStatus(uint32_t CAN_Reg, uint32_t It_Bit); + +/** @defgroup CAN_Private_Functions + * @{ + */ + +/** @defgroup CAN_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + [..] This section provides functions allowing to: + (+) Initialize the CAN peripherals : Prescaler, operating mode, the maximum + number of time quanta to perform resynchronization, the number of time + quanta in Bit Segment 1 and 2 and many other modes. + (+) Configure the CAN reception filter. + (+) Select the start bank filter for slave CAN. + (+) Enable or disable the Debug Freeze mode for CAN. + (+) Enable or disable the CAN Time Trigger Operation communication mode. + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the CAN peripheral registers to their default reset values. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @retval None. + */ +void CAN_DeInit(CAN_TypeDef* CANx) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + + /* Enable CAN1 reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_CAN1, ENABLE); + /* Release CAN1 from reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_CAN1, DISABLE); +} + +/** + * @brief Initializes the CAN peripheral according to the specified + * parameters in the CAN_InitStruct. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @param CAN_InitStruct: pointer to a CAN_InitTypeDef structure that contains + * the configuration information for the CAN peripheral. + * @retval Constant indicates initialization succeed which will be + * CAN_InitStatus_Failed or CAN_InitStatus_Success. + */ +uint8_t CAN_Init(CAN_TypeDef* CANx, CAN_InitTypeDef* CAN_InitStruct) +{ + uint8_t InitStatus = CAN_InitStatus_Failed; + __IO uint32_t wait_ack = 0x00000000; + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_FUNCTIONAL_STATE(CAN_InitStruct->CAN_TTCM)); + assert_param(IS_FUNCTIONAL_STATE(CAN_InitStruct->CAN_ABOM)); + assert_param(IS_FUNCTIONAL_STATE(CAN_InitStruct->CAN_AWUM)); + assert_param(IS_FUNCTIONAL_STATE(CAN_InitStruct->CAN_NART)); + assert_param(IS_FUNCTIONAL_STATE(CAN_InitStruct->CAN_RFLM)); + assert_param(IS_FUNCTIONAL_STATE(CAN_InitStruct->CAN_TXFP)); + assert_param(IS_CAN_MODE(CAN_InitStruct->CAN_Mode)); + assert_param(IS_CAN_SJW(CAN_InitStruct->CAN_SJW)); + assert_param(IS_CAN_BS1(CAN_InitStruct->CAN_BS1)); + assert_param(IS_CAN_BS2(CAN_InitStruct->CAN_BS2)); + assert_param(IS_CAN_PRESCALER(CAN_InitStruct->CAN_Prescaler)); + + /* Exit from sleep mode */ + CANx->MCR &= (~(uint32_t)CAN_MCR_SLEEP); + + /* Request initialisation */ + CANx->MCR |= CAN_MCR_INRQ ; + + /* Wait the acknowledge */ + while (((CANx->MSR & CAN_MSR_INAK) != CAN_MSR_INAK) && (wait_ack != INAK_TIMEOUT)) + { + wait_ack++; + } + + /* Check acknowledge */ + if ((CANx->MSR & CAN_MSR_INAK) != CAN_MSR_INAK) + { + InitStatus = CAN_InitStatus_Failed; + } + else + { + /* Set the time triggered communication mode */ + if (CAN_InitStruct->CAN_TTCM == ENABLE) + { + CANx->MCR |= CAN_MCR_TTCM; + } + else + { + CANx->MCR &= ~(uint32_t)CAN_MCR_TTCM; + } + + /* Set the automatic bus-off management */ + if (CAN_InitStruct->CAN_ABOM == ENABLE) + { + CANx->MCR |= CAN_MCR_ABOM; + } + else + { + CANx->MCR &= ~(uint32_t)CAN_MCR_ABOM; + } + + /* Set the automatic wake-up mode */ + if (CAN_InitStruct->CAN_AWUM == ENABLE) + { + CANx->MCR |= CAN_MCR_AWUM; + } + else + { + CANx->MCR &= ~(uint32_t)CAN_MCR_AWUM; + } + + /* Set the no automatic retransmission */ + if (CAN_InitStruct->CAN_NART == ENABLE) + { + CANx->MCR |= CAN_MCR_NART; + } + else + { + CANx->MCR &= ~(uint32_t)CAN_MCR_NART; + } + + /* Set the receive FIFO locked mode */ + if (CAN_InitStruct->CAN_RFLM == ENABLE) + { + CANx->MCR |= CAN_MCR_RFLM; + } + else + { + CANx->MCR &= ~(uint32_t)CAN_MCR_RFLM; + } + + /* Set the transmit FIFO priority */ + if (CAN_InitStruct->CAN_TXFP == ENABLE) + { + CANx->MCR |= CAN_MCR_TXFP; + } + else + { + CANx->MCR &= ~(uint32_t)CAN_MCR_TXFP; + } + + /* Set the bit timing register */ + CANx->BTR = (uint32_t)((uint32_t)CAN_InitStruct->CAN_Mode << 30) | \ + ((uint32_t)CAN_InitStruct->CAN_SJW << 24) | \ + ((uint32_t)CAN_InitStruct->CAN_BS1 << 16) | \ + ((uint32_t)CAN_InitStruct->CAN_BS2 << 20) | \ + ((uint32_t)CAN_InitStruct->CAN_Prescaler - 1); + + /* Request leave initialisation */ + CANx->MCR &= ~(uint32_t)CAN_MCR_INRQ; + + /* Wait the acknowledge */ + wait_ack = 0; + + while (((CANx->MSR & CAN_MSR_INAK) == CAN_MSR_INAK) && (wait_ack != INAK_TIMEOUT)) + { + wait_ack++; + } + + /* ...and check acknowledged */ + if ((CANx->MSR & CAN_MSR_INAK) == CAN_MSR_INAK) + { + InitStatus = CAN_InitStatus_Failed; + } + else + { + InitStatus = CAN_InitStatus_Success ; + } + } + + /* At this step, return the status of initialization */ + return InitStatus; +} + +/** + * @brief Configures the CAN reception filter according to the specified + * parameters in the CAN_FilterInitStruct. + * @param CAN_FilterInitStruct: pointer to a CAN_FilterInitTypeDef structure that + * contains the configuration information. + * @retval None + */ +void CAN_FilterInit(CAN_FilterInitTypeDef* CAN_FilterInitStruct) +{ + uint32_t filter_number_bit_pos = 0; + /* Check the parameters */ + assert_param(IS_CAN_FILTER_NUMBER(CAN_FilterInitStruct->CAN_FilterNumber)); + assert_param(IS_CAN_FILTER_MODE(CAN_FilterInitStruct->CAN_FilterMode)); + assert_param(IS_CAN_FILTER_SCALE(CAN_FilterInitStruct->CAN_FilterScale)); + assert_param(IS_CAN_FILTER_FIFO(CAN_FilterInitStruct->CAN_FilterFIFOAssignment)); + assert_param(IS_FUNCTIONAL_STATE(CAN_FilterInitStruct->CAN_FilterActivation)); + + filter_number_bit_pos = ((uint32_t)1) << CAN_FilterInitStruct->CAN_FilterNumber; + + /* Initialisation mode for the filter */ + CAN1->FMR |= FMR_FINIT; + + /* Filter Deactivation */ + CAN1->FA1R &= ~(uint32_t)filter_number_bit_pos; + + /* Filter Scale */ + if (CAN_FilterInitStruct->CAN_FilterScale == CAN_FilterScale_16bit) + { + /* 16-bit scale for the filter */ + CAN1->FS1R &= ~(uint32_t)filter_number_bit_pos; + + /* First 16-bit identifier and First 16-bit mask */ + /* Or First 16-bit identifier and Second 16-bit identifier */ + CAN1->sFilterRegister[CAN_FilterInitStruct->CAN_FilterNumber].FR1 = + ((0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterMaskIdLow) << 16) | + (0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterIdLow); + + /* Second 16-bit identifier and Second 16-bit mask */ + /* Or Third 16-bit identifier and Fourth 16-bit identifier */ + CAN1->sFilterRegister[CAN_FilterInitStruct->CAN_FilterNumber].FR2 = + ((0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterMaskIdHigh) << 16) | + (0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterIdHigh); + } + + if (CAN_FilterInitStruct->CAN_FilterScale == CAN_FilterScale_32bit) + { + /* 32-bit scale for the filter */ + CAN1->FS1R |= filter_number_bit_pos; + /* 32-bit identifier or First 32-bit identifier */ + CAN1->sFilterRegister[CAN_FilterInitStruct->CAN_FilterNumber].FR1 = + ((0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterIdHigh) << 16) | + (0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterIdLow); + /* 32-bit mask or Second 32-bit identifier */ + CAN1->sFilterRegister[CAN_FilterInitStruct->CAN_FilterNumber].FR2 = + ((0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterMaskIdHigh) << 16) | + (0x0000FFFF & (uint32_t)CAN_FilterInitStruct->CAN_FilterMaskIdLow); + } + + /* Filter Mode */ + if (CAN_FilterInitStruct->CAN_FilterMode == CAN_FilterMode_IdMask) + { + /*Id/Mask mode for the filter*/ + CAN1->FM1R &= ~(uint32_t)filter_number_bit_pos; + } + else /* CAN_FilterInitStruct->CAN_FilterMode == CAN_FilterMode_IdList */ + { + /*Identifier list mode for the filter*/ + CAN1->FM1R |= (uint32_t)filter_number_bit_pos; + } + + /* Filter FIFO assignment */ + if (CAN_FilterInitStruct->CAN_FilterFIFOAssignment == CAN_Filter_FIFO0) + { + /* FIFO 0 assignation for the filter */ + CAN1->FFA1R &= ~(uint32_t)filter_number_bit_pos; + } + + if (CAN_FilterInitStruct->CAN_FilterFIFOAssignment == CAN_Filter_FIFO1) + { + /* FIFO 1 assignation for the filter */ + CAN1->FFA1R |= (uint32_t)filter_number_bit_pos; + } + + /* Filter activation */ + if (CAN_FilterInitStruct->CAN_FilterActivation == ENABLE) + { + CAN1->FA1R |= filter_number_bit_pos; + } + + /* Leave the initialisation mode for the filter */ + CAN1->FMR &= ~FMR_FINIT; +} + +/** + * @brief Fills each CAN_InitStruct member with its default value. + * @param CAN_InitStruct: pointer to a CAN_InitTypeDef structure which ill be initialized. + * @retval None + */ +void CAN_StructInit(CAN_InitTypeDef* CAN_InitStruct) +{ + /* Reset CAN init structure parameters values */ + + /* Initialize the time triggered communication mode */ + CAN_InitStruct->CAN_TTCM = DISABLE; + + /* Initialize the automatic bus-off management */ + CAN_InitStruct->CAN_ABOM = DISABLE; + + /* Initialize the automatic wake-up mode */ + CAN_InitStruct->CAN_AWUM = DISABLE; + + /* Initialize the no automatic retransmission */ + CAN_InitStruct->CAN_NART = DISABLE; + + /* Initialize the receive FIFO locked mode */ + CAN_InitStruct->CAN_RFLM = DISABLE; + + /* Initialize the transmit FIFO priority */ + CAN_InitStruct->CAN_TXFP = DISABLE; + + /* Initialize the CAN_Mode member */ + CAN_InitStruct->CAN_Mode = CAN_Mode_Normal; + + /* Initialize the CAN_SJW member */ + CAN_InitStruct->CAN_SJW = CAN_SJW_1tq; + + /* Initialize the CAN_BS1 member */ + CAN_InitStruct->CAN_BS1 = CAN_BS1_4tq; + + /* Initialize the CAN_BS2 member */ + CAN_InitStruct->CAN_BS2 = CAN_BS2_3tq; + + /* Initialize the CAN_Prescaler member */ + CAN_InitStruct->CAN_Prescaler = 1; +} + +/** + * @brief Select the start bank filter for slave CAN. + * @param CAN_BankNumber: Select the start slave bank filter from 1..27. + * @retval None + */ +void CAN_SlaveStartBank(uint8_t CAN_BankNumber) +{ + /* Check the parameters */ + assert_param(IS_CAN_BANKNUMBER(CAN_BankNumber)); + + /* Enter Initialisation mode for the filter */ + CAN1->FMR |= FMR_FINIT; + + /* Select the start slave bank */ + CAN1->FMR &= (uint32_t)0xFFFFC0F1 ; + CAN1->FMR |= (uint32_t)(CAN_BankNumber)<<8; + + /* Leave Initialisation mode for the filter */ + CAN1->FMR &= ~FMR_FINIT; +} + +/** + * @brief Enables or disables the DBG Freeze for CAN. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param NewState: new state of the CAN peripheral. + * This parameter can be: ENABLE (CAN reception/transmission is frozen + * during debug. Reception FIFOs can still be accessed/controlled normally) + * or DISABLE (CAN is working during debug). + * @retval None + */ +void CAN_DBGFreeze(CAN_TypeDef* CANx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable Debug Freeze */ + CANx->MCR |= MCR_DBF; + } + else + { + /* Disable Debug Freeze */ + CANx->MCR &= ~MCR_DBF; + } +} + +/** + * @brief Enables or disables the CAN Time TriggerOperation communication mode. + * @note DLC must be programmed as 8 in order Time Stamp (2 bytes) to be + * sent over the CAN bus. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param NewState: Mode new state. This parameter can be: ENABLE or DISABLE. + * When enabled, Time stamp (TIME[15:0]) value is sent in the last two + * data bytes of the 8-byte message: TIME[7:0] in data byte 6 and TIME[15:8] + * in data byte 7. + * @retval None + */ +void CAN_TTComModeCmd(CAN_TypeDef* CANx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the TTCM mode */ + CANx->MCR |= CAN_MCR_TTCM; + + /* Set TGT bits */ + CANx->sTxMailBox[0].TDTR |= ((uint32_t)CAN_TDT0R_TGT); + CANx->sTxMailBox[1].TDTR |= ((uint32_t)CAN_TDT1R_TGT); + CANx->sTxMailBox[2].TDTR |= ((uint32_t)CAN_TDT2R_TGT); + } + else + { + /* Disable the TTCM mode */ + CANx->MCR &= (uint32_t)(~(uint32_t)CAN_MCR_TTCM); + + /* Reset TGT bits */ + CANx->sTxMailBox[0].TDTR &= ((uint32_t)~CAN_TDT0R_TGT); + CANx->sTxMailBox[1].TDTR &= ((uint32_t)~CAN_TDT1R_TGT); + CANx->sTxMailBox[2].TDTR &= ((uint32_t)~CAN_TDT2R_TGT); + } +} +/** + * @} + */ + + +/** @defgroup CAN_Group2 CAN Frames Transmission functions + * @brief CAN Frames Transmission functions + * +@verbatim + =============================================================================== + ##### CAN Frames Transmission functions ##### + =============================================================================== + [..] This section provides functions allowing to + (+) Initiate and transmit a CAN frame message (if there is an empty mailbox). + (+) Check the transmission status of a CAN Frame. + (+) Cancel a transmit request. + +@endverbatim + * @{ + */ + +/** + * @brief Initiates and transmits a CAN frame message. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param TxMessage: pointer to a structure which contains CAN Id, CAN DLC and CAN data. + * @retval The number of the mailbox that is used for transmission or + * CAN_TxStatus_NoMailBox if there is no empty mailbox. + */ +uint8_t CAN_Transmit(CAN_TypeDef* CANx, CanTxMsg* TxMessage) +{ + uint8_t transmit_mailbox = 0; + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_IDTYPE(TxMessage->IDE)); + assert_param(IS_CAN_RTR(TxMessage->RTR)); + assert_param(IS_CAN_DLC(TxMessage->DLC)); + + /* Select one empty transmit mailbox */ + if ((CANx->TSR&CAN_TSR_TME0) == CAN_TSR_TME0) + { + transmit_mailbox = 0; + } + else if ((CANx->TSR&CAN_TSR_TME1) == CAN_TSR_TME1) + { + transmit_mailbox = 1; + } + else if ((CANx->TSR&CAN_TSR_TME2) == CAN_TSR_TME2) + { + transmit_mailbox = 2; + } + else + { + transmit_mailbox = CAN_TxStatus_NoMailBox; + } + + if (transmit_mailbox != CAN_TxStatus_NoMailBox) + { + /* Set up the Id */ + CANx->sTxMailBox[transmit_mailbox].TIR &= TMIDxR_TXRQ; + if (TxMessage->IDE == CAN_Id_Standard) + { + assert_param(IS_CAN_STDID(TxMessage->StdId)); + CANx->sTxMailBox[transmit_mailbox].TIR |= ((TxMessage->StdId << 21) | \ + TxMessage->RTR); + } + else + { + assert_param(IS_CAN_EXTID(TxMessage->ExtId)); + CANx->sTxMailBox[transmit_mailbox].TIR |= ((TxMessage->ExtId << 3) | \ + TxMessage->IDE | \ + TxMessage->RTR); + } + + /* Set up the DLC */ + TxMessage->DLC &= (uint8_t)0x0000000F; + CANx->sTxMailBox[transmit_mailbox].TDTR &= (uint32_t)0xFFFFFFF0; + CANx->sTxMailBox[transmit_mailbox].TDTR |= TxMessage->DLC; + + /* Set up the data field */ + CANx->sTxMailBox[transmit_mailbox].TDLR = (((uint32_t)TxMessage->Data[3] << 24) | + ((uint32_t)TxMessage->Data[2] << 16) | + ((uint32_t)TxMessage->Data[1] << 8) | + ((uint32_t)TxMessage->Data[0])); + CANx->sTxMailBox[transmit_mailbox].TDHR = (((uint32_t)TxMessage->Data[7] << 24) | + ((uint32_t)TxMessage->Data[6] << 16) | + ((uint32_t)TxMessage->Data[5] << 8) | + ((uint32_t)TxMessage->Data[4])); + /* Request transmission */ + CANx->sTxMailBox[transmit_mailbox].TIR |= TMIDxR_TXRQ; + } + return transmit_mailbox; +} + +/** + * @brief Checks the transmission status of a CAN Frame. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @param TransmitMailbox: the number of the mailbox that is used for transmission. + * @retval CAN_TxStatus_Ok if the CAN driver transmits the message, + * CAN_TxStatus_Failed in an other case. + */ +uint8_t CAN_TransmitStatus(CAN_TypeDef* CANx, uint8_t TransmitMailbox) +{ + uint32_t state = 0; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_TRANSMITMAILBOX(TransmitMailbox)); + + switch (TransmitMailbox) + { + case (CAN_TXMAILBOX_0): + state = CANx->TSR & (CAN_TSR_RQCP0 | CAN_TSR_TXOK0 | CAN_TSR_TME0); + break; + case (CAN_TXMAILBOX_1): + state = CANx->TSR & (CAN_TSR_RQCP1 | CAN_TSR_TXOK1 | CAN_TSR_TME1); + break; + case (CAN_TXMAILBOX_2): + state = CANx->TSR & (CAN_TSR_RQCP2 | CAN_TSR_TXOK2 | CAN_TSR_TME2); + break; + default: + state = CAN_TxStatus_Failed; + break; + } + switch (state) + { + /* transmit pending */ + case (0x0): state = CAN_TxStatus_Pending; + break; + /* transmit failed */ + case (CAN_TSR_RQCP0 | CAN_TSR_TME0): state = CAN_TxStatus_Failed; + break; + case (CAN_TSR_RQCP1 | CAN_TSR_TME1): state = CAN_TxStatus_Failed; + break; + case (CAN_TSR_RQCP2 | CAN_TSR_TME2): state = CAN_TxStatus_Failed; + break; + /* transmit succeeded */ + case (CAN_TSR_RQCP0 | CAN_TSR_TXOK0 | CAN_TSR_TME0):state = CAN_TxStatus_Ok; + break; + case (CAN_TSR_RQCP1 | CAN_TSR_TXOK1 | CAN_TSR_TME1):state = CAN_TxStatus_Ok; + break; + case (CAN_TSR_RQCP2 | CAN_TSR_TXOK2 | CAN_TSR_TME2):state = CAN_TxStatus_Ok; + break; + default: state = CAN_TxStatus_Failed; + break; + } + return (uint8_t) state; +} + +/** + * @brief Cancels a transmit request. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @param Mailbox: Mailbox number. + * @retval None + */ +void CAN_CancelTransmit(CAN_TypeDef* CANx, uint8_t Mailbox) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_TRANSMITMAILBOX(Mailbox)); + /* abort transmission */ + switch (Mailbox) + { + case (CAN_TXMAILBOX_0): CANx->TSR |= CAN_TSR_ABRQ0; + break; + case (CAN_TXMAILBOX_1): CANx->TSR |= CAN_TSR_ABRQ1; + break; + case (CAN_TXMAILBOX_2): CANx->TSR |= CAN_TSR_ABRQ2; + break; + default: + break; + } +} +/** + * @} + */ + + +/** @defgroup CAN_Group3 CAN Frames Reception functions + * @brief CAN Frames Reception functions + * +@verbatim + =============================================================================== + ##### CAN Frames Reception functions ##### + =============================================================================== + [..] This section provides functions allowing to + (+) Receive a correct CAN frame. + (+) Release a specified receive FIFO (2 FIFOs are available). + (+) Return the number of the pending received CAN frames. + +@endverbatim + * @{ + */ + +/** + * @brief Receives a correct CAN frame. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @param FIFONumber: Receive FIFO number, CAN_FIFO0 or CAN_FIFO1. + * @param RxMessage: pointer to a structure receive frame which contains CAN Id, + * CAN DLC, CAN data and FMI number. + * @retval None + */ +void CAN_Receive(CAN_TypeDef* CANx, uint8_t FIFONumber, CanRxMsg* RxMessage) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_FIFO(FIFONumber)); + /* Get the Id */ + RxMessage->IDE = (uint8_t)0x04 & CANx->sFIFOMailBox[FIFONumber].RIR; + if (RxMessage->IDE == CAN_Id_Standard) + { + RxMessage->StdId = (uint32_t)0x000007FF & (CANx->sFIFOMailBox[FIFONumber].RIR >> 21); + } + else + { + RxMessage->ExtId = (uint32_t)0x1FFFFFFF & (CANx->sFIFOMailBox[FIFONumber].RIR >> 3); + } + + RxMessage->RTR = (uint8_t)0x02 & CANx->sFIFOMailBox[FIFONumber].RIR; + /* Get the DLC */ + RxMessage->DLC = (uint8_t)0x0F & CANx->sFIFOMailBox[FIFONumber].RDTR; + /* Get the FMI */ + RxMessage->FMI = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDTR >> 8); + /* Get the data field */ + RxMessage->Data[0] = (uint8_t)0xFF & CANx->sFIFOMailBox[FIFONumber].RDLR; + RxMessage->Data[1] = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDLR >> 8); + RxMessage->Data[2] = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDLR >> 16); + RxMessage->Data[3] = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDLR >> 24); + RxMessage->Data[4] = (uint8_t)0xFF & CANx->sFIFOMailBox[FIFONumber].RDHR; + RxMessage->Data[5] = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDHR >> 8); + RxMessage->Data[6] = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDHR >> 16); + RxMessage->Data[7] = (uint8_t)0xFF & (CANx->sFIFOMailBox[FIFONumber].RDHR >> 24); + /* Release the FIFO */ + /* Release FIFO0 */ + if (FIFONumber == CAN_FIFO0) + { + CANx->RF0R |= CAN_RF0R_RFOM0; + } + /* Release FIFO1 */ + else /* FIFONumber == CAN_FIFO1 */ + { + CANx->RF1R |= CAN_RF1R_RFOM1; + } +} + +/** + * @brief Releases the specified receive FIFO. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @param FIFONumber: FIFO to release, CAN_FIFO0 or CAN_FIFO1. + * @retval None + */ +void CAN_FIFORelease(CAN_TypeDef* CANx, uint8_t FIFONumber) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_FIFO(FIFONumber)); + /* Release FIFO0 */ + if (FIFONumber == CAN_FIFO0) + { + CANx->RF0R |= CAN_RF0R_RFOM0; + } + /* Release FIFO1 */ + else /* FIFONumber == CAN_FIFO1 */ + { + CANx->RF1R |= CAN_RF1R_RFOM1; + } +} + +/** + * @brief Returns the number of pending received messages. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @param FIFONumber: Receive FIFO number, CAN_FIFO0 or CAN_FIFO1. + * @retval NbMessage : which is the number of pending message. + */ +uint8_t CAN_MessagePending(CAN_TypeDef* CANx, uint8_t FIFONumber) +{ + uint8_t message_pending=0; + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_FIFO(FIFONumber)); + if (FIFONumber == CAN_FIFO0) + { + message_pending = (uint8_t)(CANx->RF0R&(uint32_t)0x03); + } + else if (FIFONumber == CAN_FIFO1) + { + message_pending = (uint8_t)(CANx->RF1R&(uint32_t)0x03); + } + else + { + message_pending = 0; + } + return message_pending; +} +/** + * @} + */ + + +/** @defgroup CAN_Group4 CAN Operation modes functions + * @brief CAN Operation modes functions + * +@verbatim + =============================================================================== + ##### CAN Operation modes functions ##### + =============================================================================== + [..] This section provides functions allowing to select the CAN Operation modes: + (+) sleep mode. + (+) normal mode. + (+) initialization mode. + +@endverbatim + * @{ + */ + + +/** + * @brief Selects the CAN Operation mode. + * @param CAN_OperatingMode: CAN Operating Mode. + * This parameter can be one of @ref CAN_OperatingMode_TypeDef enumeration. + * @retval status of the requested mode which can be: + * - CAN_ModeStatus_Failed: CAN failed entering the specific mode + * - CAN_ModeStatus_Success: CAN Succeed entering the specific mode + */ +uint8_t CAN_OperatingModeRequest(CAN_TypeDef* CANx, uint8_t CAN_OperatingMode) +{ + uint8_t status = CAN_ModeStatus_Failed; + + /* Timeout for INAK or also for SLAK bits*/ + uint32_t timeout = INAK_TIMEOUT; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_OPERATING_MODE(CAN_OperatingMode)); + + if (CAN_OperatingMode == CAN_OperatingMode_Initialization) + { + /* Request initialisation */ + CANx->MCR = (uint32_t)((CANx->MCR & (uint32_t)(~(uint32_t)CAN_MCR_SLEEP)) | CAN_MCR_INRQ); + + /* Wait the acknowledge */ + while (((CANx->MSR & CAN_MODE_MASK) != CAN_MSR_INAK) && (timeout != 0)) + { + timeout--; + } + if ((CANx->MSR & CAN_MODE_MASK) != CAN_MSR_INAK) + { + status = CAN_ModeStatus_Failed; + } + else + { + status = CAN_ModeStatus_Success; + } + } + else if (CAN_OperatingMode == CAN_OperatingMode_Normal) + { + /* Request leave initialisation and sleep mode and enter Normal mode */ + CANx->MCR &= (uint32_t)(~(CAN_MCR_SLEEP|CAN_MCR_INRQ)); + + /* Wait the acknowledge */ + while (((CANx->MSR & CAN_MODE_MASK) != 0) && (timeout!=0)) + { + timeout--; + } + if ((CANx->MSR & CAN_MODE_MASK) != 0) + { + status = CAN_ModeStatus_Failed; + } + else + { + status = CAN_ModeStatus_Success; + } + } + else if (CAN_OperatingMode == CAN_OperatingMode_Sleep) + { + /* Request Sleep mode */ + CANx->MCR = (uint32_t)((CANx->MCR & (uint32_t)(~(uint32_t)CAN_MCR_INRQ)) | CAN_MCR_SLEEP); + + /* Wait the acknowledge */ + while (((CANx->MSR & CAN_MODE_MASK) != CAN_MSR_SLAK) && (timeout!=0)) + { + timeout--; + } + if ((CANx->MSR & CAN_MODE_MASK) != CAN_MSR_SLAK) + { + status = CAN_ModeStatus_Failed; + } + else + { + status = CAN_ModeStatus_Success; + } + } + else + { + status = CAN_ModeStatus_Failed; + } + + return (uint8_t) status; +} + +/** + * @brief Enters the Sleep (low power) mode. + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @retval CAN_Sleep_Ok if sleep entered, CAN_Sleep_Failed otherwise. + */ +uint8_t CAN_Sleep(CAN_TypeDef* CANx) +{ + uint8_t sleepstatus = CAN_Sleep_Failed; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + + /* Request Sleep mode */ + CANx->MCR = (((CANx->MCR) & (uint32_t)(~(uint32_t)CAN_MCR_INRQ)) | CAN_MCR_SLEEP); + + /* Sleep mode status */ + if ((CANx->MSR & (CAN_MSR_SLAK|CAN_MSR_INAK)) == CAN_MSR_SLAK) + { + /* Sleep mode not entered */ + sleepstatus = CAN_Sleep_Ok; + } + /* return sleep mode status */ + return (uint8_t)sleepstatus; +} + +/** + * @brief Wakes up the CAN peripheral from sleep mode . + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @retval CAN_WakeUp_Ok if sleep mode left, CAN_WakeUp_Failed otherwise. + */ +uint8_t CAN_WakeUp(CAN_TypeDef* CANx) +{ + uint32_t wait_slak = SLAK_TIMEOUT; + uint8_t wakeupstatus = CAN_WakeUp_Failed; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + + /* Wake up request */ + CANx->MCR &= ~(uint32_t)CAN_MCR_SLEEP; + + /* Sleep mode status */ + while(((CANx->MSR & CAN_MSR_SLAK) == CAN_MSR_SLAK)&&(wait_slak!=0x00)) + { + wait_slak--; + } + if((CANx->MSR & CAN_MSR_SLAK) != CAN_MSR_SLAK) + { + /* wake up done : Sleep mode exited */ + wakeupstatus = CAN_WakeUp_Ok; + } + /* return wakeup status */ + return (uint8_t)wakeupstatus; +} +/** + * @} + */ + + +/** @defgroup CAN_Group5 CAN Bus Error management functions + * @brief CAN Bus Error management functions + * +@verbatim + =============================================================================== + ##### CAN Bus Error management functions ##### + =============================================================================== + [..] This section provides functions allowing to + (+) Return the CANx's last error code (LEC). + (+) Return the CANx Receive Error Counter (REC). + (+) Return the LSB of the 9-bit CANx Transmit Error Counter(TEC). + [..] + (@) If TEC is greater than 255, The CAN is in bus-off state. + (@) If REC or TEC are greater than 96, an Error warning flag occurs. + (@) If REC or TEC are greater than 127, an Error Passive Flag occurs. + +@endverbatim + * @{ + */ + +/** + * @brief Returns the CANx's last error code (LEC). + * @param CANx: where x can be 1 to select the CAN1 peripheral. + * @retval Error code: + * - CAN_ERRORCODE_NoErr: No Error + * - CAN_ERRORCODE_StuffErr: Stuff Error + * - CAN_ERRORCODE_FormErr: Form Error + * - CAN_ERRORCODE_ACKErr : Acknowledgment Error + * - CAN_ERRORCODE_BitRecessiveErr: Bit Recessive Error + * - CAN_ERRORCODE_BitDominantErr: Bit Dominant Error + * - CAN_ERRORCODE_CRCErr: CRC Error + * - CAN_ERRORCODE_SoftwareSetErr: Software Set Error + */ +uint8_t CAN_GetLastErrorCode(CAN_TypeDef* CANx) +{ + uint8_t errorcode=0; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + + /* Get the error code*/ + errorcode = (((uint8_t)CANx->ESR) & (uint8_t)CAN_ESR_LEC); + + /* Return the error code*/ + return errorcode; +} + +/** + * @brief Returns the CANx Receive Error Counter (REC). + * @note In case of an error during reception, this counter is incremented + * by 1 or by 8 depending on the error condition as defined by the CAN + * standard. After every successful reception, the counter is + * decremented by 1 or reset to 120 if its value was higher than 128. + * When the counter value exceeds 127, the CAN controller enters the + * error passive state. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @retval CAN Receive Error Counter. + */ +uint8_t CAN_GetReceiveErrorCounter(CAN_TypeDef* CANx) +{ + uint8_t counter=0; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + + /* Get the Receive Error Counter*/ + counter = (uint8_t)((CANx->ESR & CAN_ESR_REC)>> 24); + + /* Return the Receive Error Counter*/ + return counter; +} + + +/** + * @brief Returns the LSB of the 9-bit CANx Transmit Error Counter(TEC). + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @retval LSB of the 9-bit CAN Transmit Error Counter. + */ +uint8_t CAN_GetLSBTransmitErrorCounter(CAN_TypeDef* CANx) +{ + uint8_t counter=0; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + + /* Get the LSB of the 9-bit CANx Transmit Error Counter(TEC) */ + counter = (uint8_t)((CANx->ESR & CAN_ESR_TEC)>> 16); + + /* Return the LSB of the 9-bit CANx Transmit Error Counter(TEC) */ + return counter; +} +/** + * @} + */ + +/** @defgroup CAN_Group6 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] This section provides functions allowing to configure the CAN Interrupts + and to get the status and clear flags and Interrupts pending bits. + [..] The CAN provides 14 Interrupts sources and 15 Flags: + + *** Flags *** + ============= + [..] The 15 flags can be divided on 4 groups: + (+) Transmit Flags: + (++) CAN_FLAG_RQCP0. + (++) CAN_FLAG_RQCP1. + (++) CAN_FLAG_RQCP2: Request completed MailBoxes 0, 1 and 2 Flags + Set when the last request (transmit or abort) has + been performed. + (+) Receive Flags: + (++) CAN_FLAG_FMP0. + (++) CAN_FLAG_FMP1: FIFO 0 and 1 Message Pending Flags; + Set to signal that messages are pending in the receive FIFO. + These Flags are cleared only by hardware. + (++) CAN_FLAG_FF0. + (++) CAN_FLAG_FF1: FIFO 0 and 1 Full Flags; + Set when three messages are stored in the selected FIFO. + (++) CAN_FLAG_FOV0. + (++) CAN_FLAG_FOV1: FIFO 0 and 1 Overrun Flags; + Set when a new message has been received and passed the filter + while the FIFO was full. + (+) Operating Mode Flags: + (++) CAN_FLAG_WKU: Wake up Flag; + Set to signal that a SOF bit has been detected while the CAN + hardware was in Sleep mode. + (++) CAN_FLAG_SLAK: Sleep acknowledge Flag; + Set to signal that the CAN has entered Sleep Mode. + (+) Error Flags: + (++) CAN_FLAG_EWG: Error Warning Flag; + Set when the warning limit has been reached (Receive Error Counter + or Transmit Error Counter greater than 96). + This Flag is cleared only by hardware. + (++) CAN_FLAG_EPV: Error Passive Flag; + Set when the Error Passive limit has been reached (Receive Error + Counter or Transmit Error Counter greater than 127). + This Flag is cleared only by hardware. + (++) CAN_FLAG_BOF: Bus-Off Flag; + Set when CAN enters the bus-off state. The bus-off state is + entered on TEC overflow, greater than 255. + This Flag is cleared only by hardware. + (++) CAN_FLAG_LEC: Last error code Flag; + Set If a message has been transferred (reception or transmission) + with error, and the error code is hold. + + *** Interrupts *** + ================== + [..] The 14 interrupts can be divided on 4 groups: + (+) Transmit interrupt: + (++) CAN_IT_TME: Transmit mailbox empty Interrupt; + If enabled, this interrupt source is pending when no transmit + request are pending for Tx mailboxes. + (+) Receive Interrupts: + (++) CAN_IT_FMP0. + (++) CAN_IT_FMP1: FIFO 0 and FIFO1 message pending Interrupts; + If enabled, these interrupt sources are pending when messages + are pending in the receive FIFO. + The corresponding interrupt pending bits are cleared only by hardware. + (++) CAN_IT_FF0. + (++) CAN_IT_FF1: FIFO 0 and FIFO1 full Interrupts; + If enabled, these interrupt sources are pending when three messages + are stored in the selected FIFO. + (++) CAN_IT_FOV0. + (++) CAN_IT_FOV1: FIFO 0 and FIFO1 overrun Interrupts; + If enabled, these interrupt sources are pending when a new message + has been received and passed the filter while the FIFO was full. + (+) Operating Mode Interrupts: + (++) CAN_IT_WKU: Wake-up Interrupt; + If enabled, this interrupt source is pending when a SOF bit has + been detected while the CAN hardware was in Sleep mode. + (++) CAN_IT_SLK: Sleep acknowledge Interrupt: + If enabled, this interrupt source is pending when the CAN has + entered Sleep Mode. + (+) Error Interrupts: + (++) CAN_IT_EWG: Error warning Interrupt; + If enabled, this interrupt source is pending when the warning limit + has been reached (Receive Error Counter or Transmit Error Counter=96). + (++) CAN_IT_EPV: Error passive Interrupt; + If enabled, this interrupt source is pending when the Error Passive + limit has been reached (Receive Error Counter or Transmit Error Counter>127). + (++) CAN_IT_BOF: Bus-off Interrupt; + If enabled, this interrupt source is pending when CAN enters + the bus-off state. The bus-off state is entered on TEC overflow, + greater than 255. + This Flag is cleared only by hardware. + (++) CAN_IT_LEC: Last error code Interrupt; + If enabled, this interrupt source is pending when a message has + been transferred (reception or transmission) with error and the + error code is hold. + (++) CAN_IT_ERR: Error Interrupt; + If enabled, this interrupt source is pending when an error condition + is pending. + [..] Managing the CAN controller events: + The user should identify which mode will be used in his application to manage + the CAN controller events: Polling mode or Interrupt mode. + (+) In the Polling Mode it is advised to use the following functions: + (++) CAN_GetFlagStatus() : to check if flags events occur. + (++) CAN_ClearFlag() : to clear the flags events. + (+) In the Interrupt Mode it is advised to use the following functions: + (++) CAN_ITConfig() : to enable or disable the interrupt source. + (++) CAN_GetITStatus() : to check if Interrupt occurs. + (++) CAN_ClearITPendingBit() : to clear the Interrupt pending Bit + (corresponding Flag). + This function has no impact on CAN_IT_FMP0 and CAN_IT_FMP1 Interrupts + pending bits since there are cleared only by hardware. + +@endverbatim + * @{ + */ +/** + * @brief Enables or disables the specified CANx interrupts. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param CAN_IT: specifies the CAN interrupt sources to be enabled or disabled. + * This parameter can be: + * @arg CAN_IT_TME: Transmit mailbox empty Interrupt + * @arg CAN_IT_FMP0: FIFO 0 message pending Interrupt + * @arg CAN_IT_FF0: FIFO 0 full Interrupt + * @arg CAN_IT_FOV0: FIFO 0 overrun Interrupt + * @arg CAN_IT_FMP1: FIFO 1 message pending Interrupt + * @arg CAN_IT_FF1: FIFO 1 full Interrupt + * @arg CAN_IT_FOV1: FIFO 1 overrun Interrupt + * @arg CAN_IT_WKU: Wake-up Interrupt + * @arg CAN_IT_SLK: Sleep acknowledge Interrupt + * @arg CAN_IT_EWG: Error warning Interrupt + * @arg CAN_IT_EPV: Error passive Interrupt + * @arg CAN_IT_BOF: Bus-off Interrupt + * @arg CAN_IT_LEC: Last error code Interrupt + * @arg CAN_IT_ERR: Error Interrupt + * @param NewState: new state of the CAN interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void CAN_ITConfig(CAN_TypeDef* CANx, uint32_t CAN_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_IT(CAN_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected CANx interrupt */ + CANx->IER |= CAN_IT; + } + else + { + /* Disable the selected CANx interrupt */ + CANx->IER &= ~CAN_IT; + } +} +/** + * @brief Checks whether the specified CAN flag is set or not. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param CAN_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg CAN_FLAG_RQCP0: Request MailBox0 Flag + * @arg CAN_FLAG_RQCP1: Request MailBox1 Flag + * @arg CAN_FLAG_RQCP2: Request MailBox2 Flag + * @arg CAN_FLAG_FMP0: FIFO 0 Message Pending Flag + * @arg CAN_FLAG_FF0: FIFO 0 Full Flag + * @arg CAN_FLAG_FOV0: FIFO 0 Overrun Flag + * @arg CAN_FLAG_FMP1: FIFO 1 Message Pending Flag + * @arg CAN_FLAG_FF1: FIFO 1 Full Flag + * @arg CAN_FLAG_FOV1: FIFO 1 Overrun Flag + * @arg CAN_FLAG_WKU: Wake up Flag + * @arg CAN_FLAG_SLAK: Sleep acknowledge Flag + * @arg CAN_FLAG_EWG: Error Warning Flag + * @arg CAN_FLAG_EPV: Error Passive Flag + * @arg CAN_FLAG_BOF: Bus-Off Flag + * @arg CAN_FLAG_LEC: Last error code Flag + * @retval The new state of CAN_FLAG (SET or RESET). + */ +FlagStatus CAN_GetFlagStatus(CAN_TypeDef* CANx, uint32_t CAN_FLAG) +{ + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_GET_FLAG(CAN_FLAG)); + + + if((CAN_FLAG & CAN_FLAGS_ESR) != (uint32_t)RESET) + { + /* Check the status of the specified CAN flag */ + if ((CANx->ESR & (CAN_FLAG & 0x000FFFFF)) != (uint32_t)RESET) + { + /* CAN_FLAG is set */ + bitstatus = SET; + } + else + { + /* CAN_FLAG is reset */ + bitstatus = RESET; + } + } + else if((CAN_FLAG & CAN_FLAGS_MSR) != (uint32_t)RESET) + { + /* Check the status of the specified CAN flag */ + if ((CANx->MSR & (CAN_FLAG & 0x000FFFFF)) != (uint32_t)RESET) + { + /* CAN_FLAG is set */ + bitstatus = SET; + } + else + { + /* CAN_FLAG is reset */ + bitstatus = RESET; + } + } + else if((CAN_FLAG & CAN_FLAGS_TSR) != (uint32_t)RESET) + { + /* Check the status of the specified CAN flag */ + if ((CANx->TSR & (CAN_FLAG & 0x000FFFFF)) != (uint32_t)RESET) + { + /* CAN_FLAG is set */ + bitstatus = SET; + } + else + { + /* CAN_FLAG is reset */ + bitstatus = RESET; + } + } + else if((CAN_FLAG & CAN_FLAGS_RF0R) != (uint32_t)RESET) + { + /* Check the status of the specified CAN flag */ + if ((CANx->RF0R & (CAN_FLAG & 0x000FFFFF)) != (uint32_t)RESET) + { + /* CAN_FLAG is set */ + bitstatus = SET; + } + else + { + /* CAN_FLAG is reset */ + bitstatus = RESET; + } + } + else /* If(CAN_FLAG & CAN_FLAGS_RF1R != (uint32_t)RESET) */ + { + /* Check the status of the specified CAN flag */ + if ((uint32_t)(CANx->RF1R & (CAN_FLAG & 0x000FFFFF)) != (uint32_t)RESET) + { + /* CAN_FLAG is set */ + bitstatus = SET; + } + else + { + /* CAN_FLAG is reset */ + bitstatus = RESET; + } + } + /* Return the CAN_FLAG status */ + return bitstatus; +} + +/** + * @brief Clears the CAN's pending flags. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param CAN_FLAG: specifies the flag to clear. + * This parameter can be one of the following values: + * @arg CAN_FLAG_RQCP0: Request MailBox0 Flag + * @arg CAN_FLAG_RQCP1: Request MailBox1 Flag + * @arg CAN_FLAG_RQCP2: Request MailBox2 Flag + * @arg CAN_FLAG_FF0: FIFO 0 Full Flag + * @arg CAN_FLAG_FOV0: FIFO 0 Overrun Flag + * @arg CAN_FLAG_FF1: FIFO 1 Full Flag + * @arg CAN_FLAG_FOV1: FIFO 1 Overrun Flag + * @arg CAN_FLAG_WKU: Wake up Flag + * @arg CAN_FLAG_SLAK: Sleep acknowledge Flag + * @arg CAN_FLAG_LEC: Last error code Flag + * @retval None + */ +void CAN_ClearFlag(CAN_TypeDef* CANx, uint32_t CAN_FLAG) +{ + uint32_t flagtmp=0; + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_CLEAR_FLAG(CAN_FLAG)); + + if (CAN_FLAG == CAN_FLAG_LEC) /* ESR register */ + { + /* Clear the selected CAN flags */ + CANx->ESR = (uint32_t)RESET; + } + else /* MSR or TSR or RF0R or RF1R */ + { + flagtmp = CAN_FLAG & 0x000FFFFF; + + if ((CAN_FLAG & CAN_FLAGS_RF0R)!=(uint32_t)RESET) + { + /* Receive Flags */ + CANx->RF0R = (uint32_t)(flagtmp); + } + else if ((CAN_FLAG & CAN_FLAGS_RF1R)!=(uint32_t)RESET) + { + /* Receive Flags */ + CANx->RF1R = (uint32_t)(flagtmp); + } + else if ((CAN_FLAG & CAN_FLAGS_TSR)!=(uint32_t)RESET) + { + /* Transmit Flags */ + CANx->TSR = (uint32_t)(flagtmp); + } + else /* If((CAN_FLAG & CAN_FLAGS_MSR)!=(uint32_t)RESET) */ + { + /* Operating mode Flags */ + CANx->MSR = (uint32_t)(flagtmp); + } + } +} + +/** + * @brief Checks whether the specified CANx interrupt has occurred or not. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param CAN_IT: specifies the CAN interrupt source to check. + * This parameter can be one of the following values: + * @arg CAN_IT_TME: Transmit mailbox empty Interrupt + * @arg CAN_IT_FMP0: FIFO 0 message pending Interrupt + * @arg CAN_IT_FF0: FIFO 0 full Interrupt + * @arg CAN_IT_FOV0: FIFO 0 overrun Interrupt + * @arg CAN_IT_FMP1: FIFO 1 message pending Interrupt + * @arg CAN_IT_FF1: FIFO 1 full Interrupt + * @arg CAN_IT_FOV1: FIFO 1 overrun Interrupt + * @arg CAN_IT_WKU: Wake-up Interrupt + * @arg CAN_IT_SLK: Sleep acknowledge Interrupt + * @arg CAN_IT_EWG: Error warning Interrupt + * @arg CAN_IT_EPV: Error passive Interrupt + * @arg CAN_IT_BOF: Bus-off Interrupt + * @arg CAN_IT_LEC: Last error code Interrupt + * @arg CAN_IT_ERR: Error Interrupt + * @retval The current state of CAN_IT (SET or RESET). + */ +ITStatus CAN_GetITStatus(CAN_TypeDef* CANx, uint32_t CAN_IT) +{ + ITStatus itstatus = RESET; + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_IT(CAN_IT)); + + /* check the interrupt enable bit */ + if((CANx->IER & CAN_IT) != RESET) + { + /* in case the Interrupt is enabled, .... */ + switch (CAN_IT) + { + case CAN_IT_TME: + /* Check CAN_TSR_RQCPx bits */ + itstatus = CheckITStatus(CANx->TSR, CAN_TSR_RQCP0|CAN_TSR_RQCP1|CAN_TSR_RQCP2); + break; + case CAN_IT_FMP0: + /* Check CAN_RF0R_FMP0 bit */ + itstatus = CheckITStatus(CANx->RF0R, CAN_RF0R_FMP0); + break; + case CAN_IT_FF0: + /* Check CAN_RF0R_FULL0 bit */ + itstatus = CheckITStatus(CANx->RF0R, CAN_RF0R_FULL0); + break; + case CAN_IT_FOV0: + /* Check CAN_RF0R_FOVR0 bit */ + itstatus = CheckITStatus(CANx->RF0R, CAN_RF0R_FOVR0); + break; + case CAN_IT_FMP1: + /* Check CAN_RF1R_FMP1 bit */ + itstatus = CheckITStatus(CANx->RF1R, CAN_RF1R_FMP1); + break; + case CAN_IT_FF1: + /* Check CAN_RF1R_FULL1 bit */ + itstatus = CheckITStatus(CANx->RF1R, CAN_RF1R_FULL1); + break; + case CAN_IT_FOV1: + /* Check CAN_RF1R_FOVR1 bit */ + itstatus = CheckITStatus(CANx->RF1R, CAN_RF1R_FOVR1); + break; + case CAN_IT_WKU: + /* Check CAN_MSR_WKUI bit */ + itstatus = CheckITStatus(CANx->MSR, CAN_MSR_WKUI); + break; + case CAN_IT_SLK: + /* Check CAN_MSR_SLAKI bit */ + itstatus = CheckITStatus(CANx->MSR, CAN_MSR_SLAKI); + break; + case CAN_IT_EWG: + /* Check CAN_ESR_EWGF bit */ + itstatus = CheckITStatus(CANx->ESR, CAN_ESR_EWGF); + break; + case CAN_IT_EPV: + /* Check CAN_ESR_EPVF bit */ + itstatus = CheckITStatus(CANx->ESR, CAN_ESR_EPVF); + break; + case CAN_IT_BOF: + /* Check CAN_ESR_BOFF bit */ + itstatus = CheckITStatus(CANx->ESR, CAN_ESR_BOFF); + break; + case CAN_IT_LEC: + /* Check CAN_ESR_LEC bit */ + itstatus = CheckITStatus(CANx->ESR, CAN_ESR_LEC); + break; + case CAN_IT_ERR: + /* Check CAN_MSR_ERRI bit */ + itstatus = CheckITStatus(CANx->MSR, CAN_MSR_ERRI); + break; + default: + /* in case of error, return RESET */ + itstatus = RESET; + break; + } + } + else + { + /* in case the Interrupt is not enabled, return RESET */ + itstatus = RESET; + } + + /* Return the CAN_IT status */ + return itstatus; +} + +/** + * @brief Clears the CANx's interrupt pending bits. + * @param CANx: where x can be 1 or 2 to select the CAN peripheral. + * @param CAN_IT: specifies the interrupt pending bit to clear. + * This parameter can be one of the following values: + * @arg CAN_IT_TME: Transmit mailbox empty Interrupt + * @arg CAN_IT_FF0: FIFO 0 full Interrupt + * @arg CAN_IT_FOV0: FIFO 0 overrun Interrupt + * @arg CAN_IT_FF1: FIFO 1 full Interrupt + * @arg CAN_IT_FOV1: FIFO 1 overrun Interrupt + * @arg CAN_IT_WKU: Wake-up Interrupt + * @arg CAN_IT_SLK: Sleep acknowledge Interrupt + * @arg CAN_IT_EWG: Error warning Interrupt + * @arg CAN_IT_EPV: Error passive Interrupt + * @arg CAN_IT_BOF: Bus-off Interrupt + * @arg CAN_IT_LEC: Last error code Interrupt + * @arg CAN_IT_ERR: Error Interrupt + * @retval None + */ +void CAN_ClearITPendingBit(CAN_TypeDef* CANx, uint32_t CAN_IT) +{ + /* Check the parameters */ + assert_param(IS_CAN_ALL_PERIPH(CANx)); + assert_param(IS_CAN_CLEAR_IT(CAN_IT)); + + switch (CAN_IT) + { + case CAN_IT_TME: + /* Clear CAN_TSR_RQCPx (rc_w1)*/ + CANx->TSR = CAN_TSR_RQCP0|CAN_TSR_RQCP1|CAN_TSR_RQCP2; + break; + case CAN_IT_FF0: + /* Clear CAN_RF0R_FULL0 (rc_w1)*/ + CANx->RF0R = CAN_RF0R_FULL0; + break; + case CAN_IT_FOV0: + /* Clear CAN_RF0R_FOVR0 (rc_w1)*/ + CANx->RF0R = CAN_RF0R_FOVR0; + break; + case CAN_IT_FF1: + /* Clear CAN_RF1R_FULL1 (rc_w1)*/ + CANx->RF1R = CAN_RF1R_FULL1; + break; + case CAN_IT_FOV1: + /* Clear CAN_RF1R_FOVR1 (rc_w1)*/ + CANx->RF1R = CAN_RF1R_FOVR1; + break; + case CAN_IT_WKU: + /* Clear CAN_MSR_WKUI (rc_w1)*/ + CANx->MSR = CAN_MSR_WKUI; + break; + case CAN_IT_SLK: + /* Clear CAN_MSR_SLAKI (rc_w1)*/ + CANx->MSR = CAN_MSR_SLAKI; + break; + case CAN_IT_EWG: + /* Clear CAN_MSR_ERRI (rc_w1) */ + CANx->MSR = CAN_MSR_ERRI; + /* @note the corresponding Flag is cleared by hardware depending on the CAN Bus status*/ + break; + case CAN_IT_EPV: + /* Clear CAN_MSR_ERRI (rc_w1) */ + CANx->MSR = CAN_MSR_ERRI; + /* @note the corresponding Flag is cleared by hardware depending on the CAN Bus status*/ + break; + case CAN_IT_BOF: + /* Clear CAN_MSR_ERRI (rc_w1) */ + CANx->MSR = CAN_MSR_ERRI; + /* @note the corresponding Flag is cleared by hardware depending on the CAN Bus status*/ + break; + case CAN_IT_LEC: + /* Clear LEC bits */ + CANx->ESR = RESET; + /* Clear CAN_MSR_ERRI (rc_w1) */ + CANx->MSR = CAN_MSR_ERRI; + break; + case CAN_IT_ERR: + /*Clear LEC bits */ + CANx->ESR = RESET; + /* Clear CAN_MSR_ERRI (rc_w1) */ + CANx->MSR = CAN_MSR_ERRI; + /* @note BOFF, EPVF and EWGF Flags are cleared by hardware depending on the CAN Bus status*/ + break; + default: + break; + } +} + /** + * @} + */ + +/** + * @brief Checks whether the CAN interrupt has occurred or not. + * @param CAN_Reg: specifies the CAN interrupt register to check. + * @param It_Bit: specifies the interrupt source bit to check. + * @retval The new state of the CAN Interrupt (SET or RESET). + */ +static ITStatus CheckITStatus(uint32_t CAN_Reg, uint32_t It_Bit) +{ + ITStatus pendingbitstatus = RESET; + + if ((CAN_Reg & It_Bit) != (uint32_t)RESET) + { + /* CAN_IT is set */ + pendingbitstatus = SET; + } + else + { + /* CAN_IT is reset */ + pendingbitstatus = RESET; + } + return pendingbitstatus; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_comp.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_comp.c new file mode 100755 index 0000000000..847a05f2bd --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_comp.c @@ -0,0 +1,507 @@ +/** + ****************************************************************************** + * @file stm32f30x_comp.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the 7 analog comparators (COMP1, COMP2...COMP7) peripheral: + * + Comparators configuration + * + Window mode control + * + @verbatim + + ============================================================================== + ##### COMP Peripheral features ##### + ============================================================================== + [..] + The device integrates 7 analog comparators COMP1, COMP2...COMP7: + (#) The non inverting input and inverting input can be set to GPIO pins + as shown in table1. COMP Inputs below. + + (#) The COMP output is internally is available using COMP_GetOutputLevel() + and can be set on GPIO pins. Refer to table 2. COMP Outputs below. + + (#) The COMP output can be redirected to embedded timers (TIM1, TIM2, TIM3...) + Refer to table 3. COMP Outputs redirection to embedded timers below. + + (#) The comparators COMP1 and COMP2, COMP3 and COMP4, COMP5 and COMP6 can be combined in window + mode and only COMP1, COMP3 and COMP5 non inverting input can be used as non-inverting input. + + (#) The seven comparators have interrupt capability with wake-up + from Sleep and Stop modes (through the EXTI controller): + (++) COMP1 is internally connected to EXTI Line 21 + (++) COMP2 is internally connected to EXTI Line 22 + (++) COMP3 is internally connected to EXTI Line 29 + (++) COMP4 is internally connected to EXTI Line 30 + (++) COMP5 is internally connected to EXTI Line 31 + (++) COMP6 is internally connected to EXTI Line 32 + (++) COMP7 is internally connected to EXTI Line 33 + + [..] Table 1. COMP Inputs + +------------------------------------------------------------------------------------------+ + | | | COMP1 | COMP2 | COMP3 | COMP4 | COMP5 | COMP6 | COMP7 | + |-----------------|----------------|---------------|---------------------------------------| + | | 1/4 VREFINT | OK | OK | OK | OK | OK | OK | OK | + | | 1/2 VREFINT | OK | OK | OK | OK | OK | OK | OK | + | | 3/4 VREFINT | OK | OK | OK | OK | OK | OK | OK | + | Inverting Input | VREFINT | OK | OK | OK | OK | OK | OK | OK | + | | DAC1 OUT1(PA4) | OK | OK | OK | OK | OK | OK | OK | + | | DAC1 OUT2(PA5) | OK | OK | OK | OK | OK | OK | OK | + | | IO1 | PA0 | PA2 | PD15 | PE8 | PD13 | PD10 | PC0 | + | | IO2 | --- | --- | PB12 | PB2 | PB10 | PB15 | --- | + | | DAC2 OUT1(PA6) | --- | OK | --- | OK | --- | OK | --- | + |-----------------|----------------|-------|-------|-------|-------|-------|-------|-------| + | Non Inverting | IO1 | PA1 | PA7 | PB14 | PB0 | PD12 | PD11 | PA0 | + | Input | IO2 | --- | PA3 | PD14 | PE7 | PB13 | PB11 | PC1 | + +------------------------------------------------------------------------------------------+ + + [..] Table 2. COMP Outputs + +-------------------------------------------------------+ + | COMP1 | COMP2 | COMP3 | COMP4 | COMP5 | COMP6 | COMP7 | + |-------|-------|-------|-------|-------|-------|-------| + | PA0 | PA2 | PB1 | PC8 | PC7 | PA10 | PC2 | + | PF4 | PA7 | --- | PA8 | PA9 | PC6 | --- | + | PA6 | PA12 | --- | --- | --- | --- | --- | + | PA11 | PB9 | --- | --- | --- | --- | --- | + | PB8 | --- | --- | --- | --- | --- | --- | + +-------------------------------------------------------+ + + [..] Table 3. COMP Outputs redirection to embedded timers + +----------------------------------------------------------------------------------------------------------------------+ + | COMP1 | COMP2 | COMP3 | COMP4 | COMP5 | COMP6 | COMP7 | + |----------------|----------------|----------------|----------------|----------------|----------------|----------------| + | TIM1 BKIN | TIM1 BKIN | TIM1 BKIN | TIM1 BKIN | TIM1 BKIN | TIM1 BKIN | TIM1 BKIN | + | | | | | | | | + | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | + | | | | | | | | + | TIM8 BKIN | TIM8 BKIN | TIM8 BKIN | TIM8 BKIN | TIM8 BKIN | TIM8 BKIN | TIM8 BKIN | + | | | | | | | | + | TIM8 BKIN2 | TIM8 BKIN2 | TIM8 BKIN2 | TIM8 BKIN2 | TIM8 BKIN2 | TIM8 BKIN2 | TIM8 BKIN2 | + | | | | | | | | + | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | TIM1 BKIN2 | + | + | + | + | + | + | + | + | + | TIM8BKIN2 | TIM8BKIN2 | TIM8BKIN2 | TIM8BKIN2 | TIM8BKIN2 | TIM8BKIN2 | TIM8BKIN2 | + | | | | | | | | + | TIM1 OCREFCLR | TIM1 OCREFCLR | TIM1 OCREFCLR | TIM8 OCREFCLR | TIM8 OCREFCLR | TIM8 OCREFCLR | TIM1 OCREFCLR | + | | | | | | | | + | TIM1 IC1 | TIM1 IC1 | TIM2 OCREFCLR | TIM3 IC3 | TIM2 IC1 | TIM2 IC2 | TIM8 OCREFCLR | + | | | | | | | | + | TIM2 IC4 | TIM2 IC4 | TIM3 IC2 | TIM3 OCREFCLR | TIM3 OCREFCLR | TIM2 OCREFCLR | TIM2 IC3 | + | | | | | | | | + | TIM2 OCREFCLR | TIM2 OCREFCLR | TIM4 IC1 | TIM4 IC2 | TIM4 IC3 | TIM16 OCREFCLR| TIM1 IC2 | + | | | | | | | | + | TIM3 IC1 | TIM3 IC1 | TIM15 IC1 | TIM15 OCREFCLR| TIM16 BKIN | TIM16 IC1 | TIM17 OCREFCLR| + | | | | | | | | + | TIM3 OCREFCLR | TIM3 OCREFCLR | TIM15 BKIN | TIM15 IC2 | TIM17 IC1 | TIM4 IC4 | TIM17 BKIN | + +----------------------------------------------------------------------------------------------------------------------+ + + [..] Table 4. COMP Outputs blanking sources + +----------------------------------------------------------------------------------------------------------------------+ + | COMP1 | COMP2 | COMP3 | COMP4 | COMP5 | COMP6 | COMP7 | + |----------------|----------------|----------------|----------------|----------------|----------------|----------------| + | TIM1 OC5 | TIM1 OC5 | TIM1 OC5 | TIM3 OC4 | TIM3 OC3 | TIM2 OC4 | TIM1 OC5 | + | | | | | | | | + | TIM2 OC3 | TIM2 OC3 | -------- | TIM8 OC5 | TIM8 OC5 | TIM8 OC5 | TIM8 OC5 | + | | | | | | | | + | TIM3 OC3 | TIM3 OC3 | TIM2 OC4 | TIM15 OC1 | TIM8 BKIN | TIM15 OC2 | TIM15 OC2 | + | | | | | | | | + +----------------------------------------------------------------------------------------------------------------------+ + + + ##### How to use this driver ##### + ============================================================================== + [..] + This driver provides functions to configure and program the Comparators + of all STM32F30x devices. + + To use the comparator, perform the following steps: + + (#) Enable the SYSCFG APB clock to get write access to comparator + register using RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + + (#) Configure the comparator input in analog mode using GPIO_Init() + + (#) Configure the comparator output in alternate function mode + using GPIO_Init() and use GPIO_PinAFConfig() function to map the + comparator output to the GPIO pin + + (#) Configure the comparator using COMP_Init() function: + (++) Select the inverting input + (++) Select the non-inverting input + (++) Select the output polarity + (++) Select the output redirection + (++) Select the hysteresis level + (++) Select the power mode + + (#) Enable the comparator using COMP_Cmd() function + + (#) If required enable the COMP interrupt by configuring and enabling + EXTI line in Interrupt mode and selecting the desired sensitivity + level using EXTI_Init() function. After that enable the comparator + interrupt vector using NVIC_Init() function. + + @endverbatim + * + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_comp.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup COMP + * @brief COMP driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* CSR register Mask */ +#define COMP_CSR_CLEAR_MASK ((uint32_t)0x00000003) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup COMP_Private_Functions + * @{ + */ + +/** @defgroup COMP_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes COMP peripheral registers to their default reset values. + * @note Deinitialization can't be performed if the COMP configuration is locked. + * To unlock the configuration, perform a system reset. + * @param COMP_Selection: the selected comparator. + * This parameter can be COMP_Selection_COMPx where x can be 1 to 7 + * to select the COMP peripheral. + * @param None + * @retval None + */ +void COMP_DeInit(uint32_t COMP_Selection) +{ + /*!< Set COMP_CSR register to reset value */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) = ((uint32_t)0x00000000); +} + +/** + * @brief Initializes the COMP peripheral according to the specified parameters + * in COMP_InitStruct + * @note If the selected comparator is locked, initialization can't be performed. + * To unlock the configuration, perform a system reset. + * @note By default, PA1 is selected as COMP1 non inverting input. + * To use PA4 as COMP1 non inverting input call COMP_SwitchCmd() after COMP_Init() + * @param COMP_Selection: the selected comparator. + * This parameter can be COMP_Selection_COMPx where x can be 1 to 7 + * to select the COMP peripheral. + * @param COMP_InitStruct: pointer to an COMP_InitTypeDef structure that contains + * the configuration information for the specified COMP peripheral. + * - COMP_InvertingInput specifies the inverting input of COMP + * - COMP_NonInvertingInput specifies the non inverting input of COMP + * - COMP_Output connect COMP output to selected timer + * input (Input capture / Output Compare Reference Clear / Break Input) + * - COMP_BlankingSrce specifies the blanking source of COMP + * - COMP_OutputPol select output polarity + * - COMP_Hysteresis configures COMP hysteresis value + * - COMP_Mode configures COMP power mode + * @note COMP_Hysteresis must be configured only for STM32F303xC. Otherwise, COMP_Hysteresis + * must be kept at reset value(COMP_Hysteresis_No). + * @note COMP_Mode field is only applicable for STM32F303xC devices. + * @retval None + */ +void COMP_Init(uint32_t COMP_Selection, COMP_InitTypeDef* COMP_InitStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_COMP_ALL_PERIPH(COMP_Selection)); + assert_param(IS_COMP_INVERTING_INPUT(COMP_InitStruct->COMP_InvertingInput)); + assert_param(IS_COMP_NONINVERTING_INPUT(COMP_InitStruct->COMP_NonInvertingInput)); + assert_param(IS_COMP_OUTPUT(COMP_InitStruct->COMP_Output)); + assert_param(IS_COMP_BLANKING_SOURCE(COMP_InitStruct->COMP_BlankingSrce)); + assert_param(IS_COMP_OUTPUT_POL(COMP_InitStruct->COMP_OutputPol)); + assert_param(IS_COMP_HYSTERESIS(COMP_InitStruct->COMP_Hysteresis)); + assert_param(IS_COMP_MODE(COMP_InitStruct->COMP_Mode)); + + /*!< Get the COMPx_CSR register value */ + tmpreg = *(__IO uint32_t *) (COMP_BASE + COMP_Selection); + + /*!< Clear the COMP1SW1, COMPxINSEL, COMPxOUTSEL, COMPxPOL, COMPxHYST and COMPxMODE bits */ + tmpreg &= (uint32_t) (COMP_CSR_CLEAR_MASK); + + /*!< Configure COMP: inverting input, output redirection, hysteresis value and power mode */ + /*!< Set COMPxINSEL bits according to COMP_InitStruct->COMP_InvertingInput value */ + /*!< Set COMPxNONINSEL bits according to COMP_InitStruct->COMP_NonInvertingInput value */ + /*!< Set COMPxBLANKING bits according to COMP_InitStruct->COMP_BlankingSrce value */ + /*!< Set COMPxOUTSEL bits according to COMP_InitStruct->COMP_Output value */ + /*!< Set COMPxPOL bit according to COMP_InitStruct->COMP_OutputPol value */ + /*!< Set COMPxHYST bits according to COMP_InitStruct->COMP_Hysteresis value */ + /*!< Set COMPxMODE bits according to COMP_InitStruct->COMP_Mode value */ + tmpreg |= (uint32_t)(COMP_InitStruct->COMP_InvertingInput | COMP_InitStruct->COMP_NonInvertingInput | + COMP_InitStruct->COMP_Output | COMP_InitStruct->COMP_OutputPol | COMP_InitStruct->COMP_BlankingSrce | + COMP_InitStruct->COMP_Hysteresis | COMP_InitStruct->COMP_Mode); + + /*!< Write to COMPx_CSR register */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) = tmpreg; +} + +/** + * @brief Fills each COMP_InitStruct member with its default value. + * @param COMP_InitStruct: pointer to an COMP_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void COMP_StructInit(COMP_InitTypeDef* COMP_InitStruct) +{ + COMP_InitStruct->COMP_InvertingInput = COMP_InvertingInput_1_4VREFINT; + COMP_InitStruct->COMP_NonInvertingInput = COMP_NonInvertingInput_IO1; + COMP_InitStruct->COMP_Output = COMP_Output_None; + COMP_InitStruct->COMP_BlankingSrce = COMP_BlankingSrce_None; + COMP_InitStruct->COMP_OutputPol = COMP_OutputPol_NonInverted; + COMP_InitStruct->COMP_Hysteresis = COMP_Hysteresis_No; + COMP_InitStruct->COMP_Mode = COMP_Mode_UltraLowPower; +} + +/** + * @brief Enable or disable the COMP peripheral. + * @note If the selected comparator is locked, enable/disable can't be performed. + * To unlock the configuration, perform a system reset. + * @param COMP_Selection: the selected comparator. + * This parameter can be COMP_Selection_COMPx where x can be 1 to 7 + * to select the COMP peripheral. + * @param NewState: new state of the COMP peripheral. + * This parameter can be: ENABLE or DISABLE. + * When enabled, the comparator compares the non inverting input with + * the inverting input and the comparison result is available + * on comparator output. + * When disabled, the comparator doesn't perform comparison and the + * output level is low. + * @retval None + */ +void COMP_Cmd(uint32_t COMP_Selection, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_COMP_ALL_PERIPH(COMP_Selection)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected COMPx peripheral */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) |= (uint32_t) (COMP_CSR_COMPxEN); + } + else + { + /* Disable the selected COMP peripheral */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) &= (uint32_t)(~COMP_CSR_COMPxEN); + } +} + +/** + * @brief Close or Open the SW1 switch. + * @note If the COMP1 is locked, Close/Open the SW1 switch can't be performed. + * To unlock the configuration, perform a system reset. + * @note This switch is solely intended to redirect signals onto high + * impedance input, such as COMP1 non-inverting input (highly resistive switch) + * @param NewState: New state of the analog switch. + * This parameter can be + * ENABLE so the SW1 is closed; PA1 is connected to PA4 + * or DISABLE so the SW1 switch is open; PA1 is disconnected from PA4 + * @retval None + */ +void COMP_SwitchCmd(uint32_t COMP_Selection, FunctionalState NewState) +{ + /* Check the parameter */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Close SW1 switch */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) |= (uint32_t) (COMP_CSR_COMP1SW1); + } + else + { + /* Open SW1 switch */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) &= (uint32_t)(~COMP_CSR_COMP1SW1); + } +} + +/** + * @brief Return the output level (high or low) of the selected comparator. + * The output level depends on the selected polarity. + * If the polarity is not inverted: + * - Comparator output is low when the non-inverting input is at a lower + * voltage than the inverting input + * - Comparator output is high when the non-inverting input is at a higher + * voltage than the inverting input + * If the polarity is inverted: + * - Comparator output is high when the non-inverting input is at a lower + * voltage than the inverting input + * - Comparator output is low when the non-inverting input is at a higher + * voltage than the inverting input + * @param COMP_Selection: the selected comparator. + * This parameter can be COMP_Selection_COMPx where x can be 1 to 7 + * to select the COMP peripheral. + * @retval Returns the selected comparator output level: low or high. + * + */ +uint32_t COMP_GetOutputLevel(uint32_t COMP_Selection) +{ + uint32_t compout = 0x0; + + /* Check the parameters */ + assert_param(IS_COMP_ALL_PERIPH(COMP_Selection)); + + /* Check if selected comparator output is high */ + if ((*(__IO uint32_t *) (COMP_BASE + COMP_Selection) & (COMP_CSR_COMPxOUT)) != 0) + { + compout = COMP_OutputLevel_High; + } + else + { + compout = COMP_OutputLevel_Low; + } + + /* Return the comparator output level */ + return (uint32_t)(compout); +} + +/** + * @} + */ + +/** @defgroup COMP_Group2 Window mode control function + * @brief Window mode control function + * +@verbatim + =============================================================================== + ##### Window mode control function ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the window mode. + * Window mode for comparators makes use of two comparators: + * COMP1 and COM2, COMP3 and COMP4, COMP5 and COMP6. + * In window mode, COMPx and COMPx-1 (where x can be 2, 4 or 6) + * non inverting inputs are connected together and only COMPx-1 non + * inverting input can be used. + * e.g When window mode enabled for COMP4, COMP3 non inverting input (PB14 or PD14) + * is to be used. + * @note If the COMPx is locked, ENABLE/DISABLE the window mode can't be performed. + * To unlock the configuration, perform a system reset. + * @param COMP_Selection: the selected comparator. + * This parameter can be COMP_Selection_COMPx where x can be 2, 4 or 6 + * to select the COMP peripheral. + * param NewState: new state of the window mode. + * This parameter can be ENABLE or DISABLE. + * When enbaled, COMPx and COMPx-1 non inverting inputs are connected together. + * When disabled, COMPx and COMPx-1 non inverting inputs are disconnected. + * @retval None + */ +void COMP_WindowCmd(uint32_t COMP_Selection, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_COMP_WINDOW(COMP_Selection)); + + if (NewState != DISABLE) + { + /* Enable the window mode */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) |= (uint32_t) COMP_CSR_COMPxWNDWEN; + } + else + { + /* Disable the window mode */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) &= (uint32_t)(~COMP_CSR_COMPxWNDWEN); + } +} + +/** + * @} + */ + +/** @defgroup COMP_Group3 COMP configuration locking function + * @brief COMP1, COMP2,...COMP7 configuration locking function + * COMP1, COMP2,...COMP7 configuration can be locked each separately. + * Unlocking is performed by system reset. + * +@verbatim + =============================================================================== + ##### Configuration Lock function ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Lock the selected comparator (COMP1/COMP2) configuration. + * @note Locking the configuration means that all control bits are read-only. + * To unlock the comparator configuration, perform a system reset. + * @param COMP_Selection: the selected comparator. + * This parameter can be COMP_Selection_COMPx where x can be 1 to 7 + * to select the COMP peripheral. + * @retval None + */ +void COMP_LockConfig(uint32_t COMP_Selection) +{ + /* Check the parameter */ + assert_param(IS_COMP_ALL_PERIPH(COMP_Selection)); + + /* Set the lock bit corresponding to selected comparator */ + *(__IO uint32_t *) (COMP_BASE + COMP_Selection) |= (uint32_t) (COMP_CSR_COMPxLOCK); +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_crc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_crc.c new file mode 100755 index 0000000000..ea9d5e9d87 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_crc.c @@ -0,0 +1,354 @@ +/** + ****************************************************************************** + * @file stm32f30x_crc.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of CRC computation unit peripheral: + * + Configuration of the CRC computation unit + * + CRC computation of one/many 32-bit data + * + CRC Independent register (IDR) access + * + @verbatim + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] + (#) Enable CRC AHB clock using RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE) + function. + (#) Select the polynomial size: 7-bit, 8-bit, 16-bit or 32-bit. + (#) Set the polynomial coefficients using CRC_SetPolynomial(); + (#) If required, select the reverse operation on input data + using CRC_ReverseInputDataSelect(); + (#) If required, enable the reverse operation on output data + using CRC_ReverseOutputDataCmd(Enable); + (#) If required, set the initialization remainder value using + CRC_SetInitRegister(); + (#) use CRC_CalcCRC() function to compute the CRC of a 32-bit data + or use CRC_CalcBlockCRC() function to compute the CRC if a 32-bit + data buffer. + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_crc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup CRC + * @brief CRC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup CRC_Private_Functions + * @{ + */ + +/** @defgroup CRC_Group1 Configuration of the CRC computation unit functions + * @brief Configuration of the CRC computation unit functions + * +@verbatim + =============================================================================== + ##### CRC configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes CRC peripheral registers to their default reset values. + * @param None + * @retval None + */ +void CRC_DeInit(void) +{ + /* Set DR register to reset value */ + CRC->DR = 0xFFFFFFFF; + /* Set the POL register to the reset value: 0x04C11DB7 */ + CRC->POL = 0x04C11DB7; + /* Reset IDR register */ + CRC->IDR = 0x00; + /* Set INIT register to reset value */ + CRC->INIT = 0xFFFFFFFF; + /* Reset the CRC calculation unit */ + CRC->CR = CRC_CR_RESET; +} + +/** + * @brief Resets the CRC calculation unit and sets INIT register content in DR register. + * @param None + * @retval None + */ +void CRC_ResetDR(void) +{ + /* Reset CRC generator */ + CRC->CR |= CRC_CR_RESET; +} + +/** + * @brief Selects the polynomial size. + * @param CRC_PolSize: Specifies the polynomial size. + * This parameter can be: + * @arg CRC_PolSize_7: 7-bit polynomial for CRC calculation + * @arg CRC_PolSize_8: 8-bit polynomial for CRC calculation + * @arg CRC_PolSize_16: 16-bit polynomial for CRC calculation + * @arg CRC_PolSize_32: 32-bit polynomial for CRC calculation + * @retval None + */ +void CRC_PolynomialSizeSelect(uint32_t CRC_PolSize) +{ + uint32_t tmpcr = 0; + + /* Check the parameter */ + assert_param(IS_CRC_POL_SIZE(CRC_PolSize)); + + /* Get CR register value */ + tmpcr = CRC->CR; + + /* Reset POL_SIZE bits */ + tmpcr &= (uint32_t)~((uint32_t)CRC_CR_POLSIZE); + /* Set the polynomial size */ + tmpcr |= (uint32_t)CRC_PolSize; + + /* Write to CR register */ + CRC->CR = (uint32_t)tmpcr; +} + +/** + * @brief Selects the reverse operation to be performed on input data. + * @param CRC_ReverseInputData: Specifies the reverse operation on input data. + * This parameter can be: + * @arg CRC_ReverseInputData_No: No reverse operation is performed + * @arg CRC_ReverseInputData_8bits: reverse operation performed on 8 bits + * @arg CRC_ReverseInputData_16bits: reverse operation performed on 16 bits + * @arg CRC_ReverseInputData_32bits: reverse operation performed on 32 bits + * @retval None + */ +void CRC_ReverseInputDataSelect(uint32_t CRC_ReverseInputData) +{ + uint32_t tmpcr = 0; + + /* Check the parameter */ + assert_param(IS_CRC_REVERSE_INPUT_DATA(CRC_ReverseInputData)); + + /* Get CR register value */ + tmpcr = CRC->CR; + + /* Reset REV_IN bits */ + tmpcr &= (uint32_t)~((uint32_t)CRC_CR_REV_IN); + /* Set the reverse operation */ + tmpcr |= (uint32_t)CRC_ReverseInputData; + + /* Write to CR register */ + CRC->CR = (uint32_t)tmpcr; +} + +/** + * @brief Enables or disable the reverse operation on output data. + * The reverse operation on output data is performed on 32-bit. + * @param NewState: new state of the reverse operation on output data. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void CRC_ReverseOutputDataCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable reverse operation on output data */ + CRC->CR |= CRC_CR_REV_OUT; + } + else + { + /* Disable reverse operation on output data */ + CRC->CR &= (uint32_t)~((uint32_t)CRC_CR_REV_OUT); + } +} + +/** + * @brief Initializes the INIT register. + * @note After resetting CRC calculation unit, CRC_InitValue is stored in DR register + * @param CRC_InitValue: Programmable initial CRC value + * @retval None + */ +void CRC_SetInitRegister(uint32_t CRC_InitValue) +{ + CRC->INIT = CRC_InitValue; +} + +/** + * @brief Initializes the polynomial coefficients. + * @param CRC_Pol: Polynomial to be used for CRC calculation. + * @retval None + */ +void CRC_SetPolynomial(uint32_t CRC_Pol) +{ + CRC->POL = CRC_Pol; +} + +/** + * @} + */ + +/** @defgroup CRC_Group2 CRC computation of one/many 32-bit data functions + * @brief CRC computation of one/many 32-bit data functions + * +@verbatim + =============================================================================== + ##### CRC computation functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Computes the 32-bit CRC of a given data word(32-bit). + * @param CRC_Data: data word(32-bit) to compute its CRC + * @retval 32-bit CRC + */ +uint32_t CRC_CalcCRC(uint32_t CRC_Data) +{ + CRC->DR = CRC_Data; + + return (CRC->DR); +} + +/** + * @brief Computes the 16-bit CRC of a given 16-bit data. + * @param CRC_Data: data half-word(16-bit) to compute its CRC + * @retval 16-bit CRC + */ +uint32_t CRC_CalcCRC16bits(uint16_t CRC_Data) +{ + *(uint16_t*)(CRC_BASE) = (uint16_t) CRC_Data; + + return (CRC->DR); +} + +/** + * @brief Computes the 8-bit CRC of a given 8-bit data. + * @param CRC_Data: 8-bit data to compute its CRC + * @retval 8-bit CRC + */ +uint32_t CRC_CalcCRC8bits(uint8_t CRC_Data) +{ + *(uint8_t*)(CRC_BASE) = (uint8_t) CRC_Data; + + return (CRC->DR); +} + +/** + * @brief Computes the 32-bit CRC of a given buffer of data word(32-bit). + * @param pBuffer: pointer to the buffer containing the data to be computed + * @param BufferLength: length of the buffer to be computed + * @retval 32-bit CRC + */ +uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength) +{ + uint32_t index = 0; + + for(index = 0; index < BufferLength; index++) + { + CRC->DR = pBuffer[index]; + } + return (CRC->DR); +} + +/** + * @brief Returns the current CRC value. + * @param None + * @retval 32-bit CRC + */ +uint32_t CRC_GetCRC(void) +{ + return (CRC->DR); +} + +/** + * @} + */ + +/** @defgroup CRC_Group3 CRC Independent Register (IDR) access functions + * @brief CRC Independent Register (IDR) access (write/read) functions + * +@verbatim + =============================================================================== + ##### CRC Independent Register (IDR) access functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Stores an 8-bit data in the Independent Data(ID) register. + * @param CRC_IDValue: 8-bit value to be stored in the ID register + * @retval None + */ +void CRC_SetIDRegister(uint8_t CRC_IDValue) +{ + CRC->IDR = CRC_IDValue; +} + +/** + * @brief Returns the 8-bit data stored in the Independent Data(ID) register + * @param None + * @retval 8-bit value of the ID register + */ +uint8_t CRC_GetIDRegister(void) +{ + return (CRC->IDR); +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dac.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dac.c new file mode 100755 index 0000000000..971ee88b95 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dac.c @@ -0,0 +1,754 @@ +/** + ****************************************************************************** + * @file stm32f30x_dac.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Digital-to-Analog Converter (DAC) peripheral: + * + DAC channels configuration: trigger, output buffer, data format + * + DMA management + * + Interrupts and flags management + * + @verbatim + + =============================================================================== + ##### DAC Peripheral features ##### + =============================================================================== + [..] The device integrates two 12-bit Digital Analog Converters that can + be used independently or simultaneously (dual mode): + (#) DAC1 integrates two DAC channels: + (++) DAC1 channel 1 with DAC1_OUT1 as output + (++) DAC1 channel 2 with DAC1_OUT2 as output + (++) The two channels can be used independently or simultaneously (dual mode) + + (#) DAC2 integrates only one channel DAC2 channel 1 with DAC2_OUT1 as output + + [..] Digital to Analog conversion can be non-triggered using DAC_Trigger_None + and DAC_OUT1/DAC_OUT2 is available once writing to DHRx register using + DAC_SetChannel1Data()/DAC_SetChannel2Data. + + [..] Digital to Analog conversion can be triggered by: + (#) External event: EXTI Line 9 (any GPIOx_Pin9) using DAC_Trigger_Ext_IT9. + The used pin (GPIOx_Pin9) must be configured in input mode. + + (#) Timers TRGO: TIM2, TIM8/TIM3, TIM4, TIM6, TIM7, and TIM15 + (DAC_Trigger_T2_TRGO, DAC_Trigger_T4_TRGO...) + The timer TRGO event should be selected using TIM_SelectOutputTrigger() + (++) To trigger DAC conversions by TIM3 instead of TIM8 follow + this sequence: + (+++) Enable SYSCFG APB clock by calling + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + (+++) Select DAC_Trigger_T3_TRGO when calling DAC_Init() + (+++) Remap the DAC trigger from TIM8 to TIM3 by calling + SYSCFG_TriggerRemapConfig(SYSCFG_TriggerRemap_DACTIM3, ENABLE) + (#) Software using DAC_Trigger_Software + + [..] Each DAC channel integrates an output buffer that can be used to + reduce the output impedance, and to drive external loads directly + without having to add an external operational amplifier. + To enable, the output buffer use + DAC_InitStructure.DAC_OutputBuffer = DAC_OutputBuffer_Enable; + + [..] Refer to the device datasheet for more details about output impedance + value with and without output buffer. + + [..] Both DAC channels can be used to generate: + (+) Noise wave using DAC_WaveGeneration_Noise + (+) Triangle wave using DAC_WaveGeneration_Triangle + + [..] Wave generation can be disabled using DAC_WaveGeneration_None + + [..] The DAC data format can be: + (+) 8-bit right alignment using DAC_Align_8b_R + (+) 12-bit left alignment using DAC_Align_12b_L + (+) 12-bit right alignment using DAC_Align_12b_R + + [..] The analog output voltage on each DAC channel pin is determined + by the following equation: + (+) DAC_OUTx = VREF+ * DOR / 4095 with DOR is the Data Output Register. + VREF+ is the input voltage reference (refer to the device datasheet) + e.g. To set DAC_OUT1 to 0.7V, use DAC_SetChannel1Data(DAC_Align_12b_R, 868); + Assuming that VREF+ = 3.3, DAC_OUT1 = (3.3 * 868) / 4095 = 0.7V + + [..] A DMA1 request can be generated when an external trigger (but not + a software trigger) occurs if DMA1 requests are enabled using + DAC_DMACmd() + DMA1 requests are mapped as following: + (+) DAC channel1 is mapped on DMA1 channel3 which must be already + configured + (+) DAC channel2 is mapped on DMA1 channel4 which must be already + configured + + ##### How to use this driver ##### + =============================================================================== + [..] + (+) Enable DAC APB1 clock to get write access to DAC registers + using RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE) + + (+) Configure DACx_OUTy (DAC1_OUT1: PA4, DAC1_OUT2: PA5, DAC2_OUT1: PA6) + in analog mode. + + (+) Configure the DAC channel using DAC_Init() + + (+) Enable the DAC channel using DAC_Cmd() + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_dac.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup DAC + * @brief DAC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* CR register Mask */ +#define CR_CLEAR_MASK ((uint32_t)0x00000FFE) + +/* DAC Dual Channels SWTRIG masks */ +#define DUAL_SWTRIG_SET ((uint32_t)0x00000003) +#define DUAL_SWTRIG_RESET ((uint32_t)0xFFFFFFFC) + +/* DHR registers offsets */ +#define DHR12R1_OFFSET ((uint32_t)0x00000008) +#define DHR12R2_OFFSET ((uint32_t)0x00000014) +#define DHR12RD_OFFSET ((uint32_t)0x00000020) + +/* DOR register offset */ +#define DOR_OFFSET ((uint32_t)0x0000002C) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup DAC_Private_Functions + * @{ + */ + +/** @defgroup DAC_Group1 DAC channels configuration + * @brief DAC channels configuration: trigger, output buffer, data format + * +@verbatim + =============================================================================== + ##### DAC channels configuration: trigger, output buffer, data format ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the DAC peripheral registers to their default reset values. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @retval None + */ +void DAC_DeInit(DAC_TypeDef* DACx) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + + if (DACx == DAC1) + { + /* Enable DAC1 reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_DAC1, ENABLE); + /* Release DAC1 from reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_DAC1, DISABLE); + } + else + { + /* Enable DAC2 reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_DAC2, ENABLE); + /* Release DAC2 from reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_DAC2, DISABLE); + } +} + +/** + * @brief Initializes the DAC peripheral according to the specified + * parameters in the DAC_InitStruct. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_InitStruct: pointer to a DAC_InitTypeDef structure that + * contains the configuration information for the specified DAC channel. + * @retval None + */ +void DAC_Init(DAC_TypeDef* DACx, uint32_t DAC_Channel, DAC_InitTypeDef* DAC_InitStruct) +{ + uint32_t tmpreg1 = 0, tmpreg2 = 0; + + /* Check the DAC parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_TRIGGER(DAC_InitStruct->DAC_Trigger)); + assert_param(IS_DAC_GENERATE_WAVE(DAC_InitStruct->DAC_WaveGeneration)); + assert_param(IS_DAC_LFSR_UNMASK_TRIANGLE_AMPLITUDE(DAC_InitStruct->DAC_LFSRUnmask_TriangleAmplitude)); + assert_param(IS_DAC_BUFFER_SWITCH_STATE(DAC_InitStruct->DAC_Buffer_Switch)); + +/*---------------------------- DAC CR Configuration --------------------------*/ + /* Get the DAC CR value */ + tmpreg1 = DACx->CR; + /* Clear BOFFx, TENx, TSELx, WAVEx and MAMPx bits */ + tmpreg1 &= ~(CR_CLEAR_MASK << DAC_Channel); + /* Configure for the selected DAC channel: buffer output, trigger, wave generation, + mask/amplitude for wave generation */ + + /* Set TSELx and TENx bits according to DAC_Trigger value */ + /* Set WAVEx bits according to DAC_WaveGeneration value */ + /* Set MAMPx bits according to DAC_LFSRUnmask_TriangleAmplitude value */ + /* Set BOFFx OUTENx bit according to DAC_Buffer_Switch value */ + tmpreg2 = (DAC_InitStruct->DAC_Trigger | DAC_InitStruct->DAC_WaveGeneration | + DAC_InitStruct->DAC_LFSRUnmask_TriangleAmplitude | DAC_InitStruct->DAC_Buffer_Switch); + + /* Calculate CR register value depending on DAC_Channel */ + tmpreg1 |= tmpreg2 << DAC_Channel; + /* Write to DAC CR */ + DACx->CR = tmpreg1; +} + +/** + * @brief Fills each DAC_InitStruct member with its default value. + * @param DAC_InitStruct: pointer to a DAC_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void DAC_StructInit(DAC_InitTypeDef* DAC_InitStruct) +{ +/*--------------- Reset DAC init structure parameters values -----------------*/ + /* Initialize the DAC_Trigger member */ + DAC_InitStruct->DAC_Trigger = DAC_Trigger_None; + /* Initialize the DAC_WaveGeneration member */ + DAC_InitStruct->DAC_WaveGeneration = DAC_WaveGeneration_None; + /* Initialize the DAC_LFSRUnmask_TriangleAmplitude member */ + DAC_InitStruct->DAC_LFSRUnmask_TriangleAmplitude = DAC_LFSRUnmask_Bit0; + /* Initialize the DAC_Buffer_Switch member */ + DAC_InitStruct->DAC_Buffer_Switch = DAC_BufferSwitch_Enable; +} + +/** + * @brief Enables or disables the specified DAC channel. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: The selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param NewState: new state of the DAC channel. + * This parameter can be: ENABLE or DISABLE. + * @note When the DAC channel is enabled the trigger source can no more + * be modified. + * @retval None + */ +void DAC_Cmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected DAC channel */ + DACx->CR |= (DAC_CR_EN1 << DAC_Channel); + } + else + { + /* Disable the selected DAC channel */ + DACx->CR &= (~(DAC_CR_EN1 << DAC_Channel)); + } +} + +/** + * @brief Enables or disables the selected DAC channel software trigger. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param NewState: new state of the selected DAC channel software trigger. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DAC_SoftwareTriggerCmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable software trigger for the selected DAC channel */ + DACx->SWTRIGR |= (uint32_t)DAC_SWTRIGR_SWTRIG1 << (DAC_Channel >> 4); + } + else + { + /* Disable software trigger for the selected DAC channel */ + DACx->SWTRIGR &= ~((uint32_t)DAC_SWTRIGR_SWTRIG1 << (DAC_Channel >> 4)); + } +} + +/** + * @brief Enables or disables simultaneously the two DAC channels software + * triggers. + * @param DACx: where x can be 1 to select the DAC1 peripheral. + * @note Dual trigger is not applicable for DAC2 (DAC2 integrates one channel). + * @param NewState: new state of the DAC channels software triggers. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DAC_DualSoftwareTriggerCmd(DAC_TypeDef* DACx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DAC_LIST1_PERIPH(DACx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable software trigger for both DAC channels */ + DACx->SWTRIGR |= DUAL_SWTRIG_SET; + } + else + { + /* Disable software trigger for both DAC channels */ + DACx->SWTRIGR &= DUAL_SWTRIG_RESET; + } +} + +/** + * @brief Enables or disables the selected DAC channel wave generation. + * @param DACx: where x can be 1 to select the DAC1 peripheral. + * @note Wave generation is not available in DAC2. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_Wave: Specifies the wave type to enable or disable. + * This parameter can be one of the following values: + * @arg DAC_Wave_Noise: noise wave generation + * @arg DAC_Wave_Triangle: triangle wave generation + * @param NewState: new state of the selected DAC channel wave generation. + * This parameter can be: ENABLE or DISABLE. + * @note + * @retval None + */ +void DAC_WaveGenerationCmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_Wave, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DAC_LIST1_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_DAC_WAVE(DAC_Wave)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected wave generation for the selected DAC channel */ + DACx->CR |= DAC_Wave << DAC_Channel; + } + else + { + /* Disable the selected wave generation for the selected DAC channel */ + DACx->CR &= ~(DAC_Wave << DAC_Channel); + } +} + +/** + * @brief Set the specified data holding register value for DAC channel1. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Align: Specifies the data alignment for DAC channel1. + * This parameter can be one of the following values: + * @arg DAC_Align_8b_R: 8bit right data alignment selected + * @arg DAC_Align_12b_L: 12bit left data alignment selected + * @arg DAC_Align_12b_R: 12bit right data alignment selected + * @param Data: Data to be loaded in the selected data holding register. + * @retval None + */ +void DAC_SetChannel1Data(DAC_TypeDef* DACx, uint32_t DAC_Align, uint16_t Data) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_ALIGN(DAC_Align)); + assert_param(IS_DAC_DATA(Data)); + + tmp = (uint32_t)DACx; + tmp += DHR12R1_OFFSET + DAC_Align; + + /* Set the DAC channel1 selected data holding register */ + *(__IO uint32_t *) tmp = Data; +} + +/** + * @brief Set the specified data holding register value for DAC channel2. + * @param DACx: where x can be 1 to select the DAC peripheral. + * @note This function is available only for DAC1. + * @param DAC_Align: Specifies the data alignment for DAC channel2. + * This parameter can be one of the following values: + * @arg DAC_Align_8b_R: 8bit right data alignment selected + * @arg DAC_Align_12b_L: 12bit left data alignment selected + * @arg DAC_Align_12b_R: 12bit right data alignment selected + * @param Data : Data to be loaded in the selected data holding register. + * @retval None + */ +void DAC_SetChannel2Data(DAC_TypeDef* DACx, uint32_t DAC_Align, uint16_t Data) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_DAC_LIST1_PERIPH(DACx)); + assert_param(IS_DAC_ALIGN(DAC_Align)); + assert_param(IS_DAC_DATA(Data)); + + tmp = (uint32_t)DACx; + tmp += DHR12R2_OFFSET + DAC_Align; + + /* Set the DAC channel2 selected data holding register */ + *(__IO uint32_t *)tmp = Data; +} + +/** + * @brief Set the specified data holding register value for dual channel DAC. + * @param DACx: where x can be 1 to select the DAC peripheral. + * @note This function isn't applicable for DAC2. + * @param DAC_Align: Specifies the data alignment for dual channel DAC. + * This parameter can be one of the following values: + * @arg DAC_Align_8b_R: 8bit right data alignment selected + * @arg DAC_Align_12b_L: 12bit left data alignment selected + * @arg DAC_Align_12b_R: 12bit right data alignment selected + * @param Data2: Data for DAC Channel2 to be loaded in the selected data + * holding register. + * @param Data1: Data for DAC Channel1 to be loaded in the selected data + * holding register. + * @note In dual mode, a unique register access is required to write in both + * DAC channels at the same time. + * @retval None + */ +void DAC_SetDualChannelData(DAC_TypeDef* DACx, uint32_t DAC_Align, uint16_t Data2, uint16_t Data1) +{ + uint32_t data = 0, tmp = 0; + + /* Check the parameters */ + assert_param(IS_DAC_LIST1_PERIPH(DACx)); + assert_param(IS_DAC_ALIGN(DAC_Align)); + assert_param(IS_DAC_DATA(Data1)); + assert_param(IS_DAC_DATA(Data2)); + + /* Calculate and set dual DAC data holding register value */ + if (DAC_Align == DAC_Align_8b_R) + { + data = ((uint32_t)Data2 << 8) | Data1; + } + else + { + data = ((uint32_t)Data2 << 16) | Data1; + } + + tmp = (uint32_t)DACx; + tmp += DHR12RD_OFFSET + DAC_Align; + + /* Set the dual DAC selected data holding register */ + *(__IO uint32_t *)tmp = data; +} + +/** + * @brief Returns the last data output value of the selected DAC channel. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @retval The selected DAC channel data output value. + */ +uint16_t DAC_GetDataOutputValue(DAC_TypeDef* DACx, uint32_t DAC_Channel) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + + tmp = (uint32_t) DACx; + tmp += DOR_OFFSET + ((uint32_t)DAC_Channel >> 2); + + /* Returns the DAC channel data output register value */ + return (uint16_t) (*(__IO uint32_t*) tmp); +} + +/** + * @} + */ + +/** @defgroup DAC_Group2 DMA management functions + * @brief DMA management functions + * +@verbatim + =============================================================================== + ##### DMA management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified DAC channel DMA request. + * When enabled DMA1 is generated when an external trigger (EXTI Line9, + * TIM2, TIM4, TIM6, TIM7 or TIM9 but not a software trigger) occurs + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param NewState: new state of the selected DAC channel DMA request. + * This parameter can be: ENABLE or DISABLE. + * @note The DAC channel1 (channel2) is mapped on DMA1 channel3 (channel4) which + * must be already configured. + * @retval None + */ +void DAC_DMACmd(DAC_TypeDef* DACx, uint32_t DAC_Channel, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected DAC channel DMA request */ + DACx->CR |= (DAC_CR_DMAEN1 << DAC_Channel); + } + else + { + /* Disable the selected DAC channel DMA request */ + DACx->CR &= (~(DAC_CR_DMAEN1 << DAC_Channel)); + } +} + +/** + * @} + */ + +/** @defgroup DAC_Group3 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified DAC interrupts. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_IT: specifies the DAC interrupt sources to be enabled or disabled. + * This parameter can be: + * @arg DAC_IT_DMAUDR: DMA underrun interrupt mask + * @note The DMA underrun occurs when a second external trigger arrives before + * the acknowledgement for the first external trigger is received (first request). + * @param NewState: new state of the specified DAC interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DAC_ITConfig(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_DAC_IT(DAC_IT)); + + if (NewState != DISABLE) + { + /* Enable the selected DAC interrupts */ + DACx->CR |= (DAC_IT << DAC_Channel); + } + else + { + /* Disable the selected DAC interrupts */ + DACx->CR &= (~(uint32_t)(DAC_IT << DAC_Channel)); + } +} + +/** + * @brief Checks whether the specified DAC flag is set or not. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: thee selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_FLAG: specifies the flag to check. + * This parameter can be: + * @arg DAC_FLAG_DMAUDR: DMA underrun flag + * @note The DMA underrun occurs when a second external trigger arrives before + * the acknowledgement for the first external trigger is received (first request). + * @retval The new state of DAC_FLAG (SET or RESET). + */ +FlagStatus DAC_GetFlagStatus(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_FLAG) +{ + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_DAC_FLAG(DAC_FLAG)); + + /* Check the status of the specified DAC flag */ + if ((DACx->SR & (DAC_FLAG << DAC_Channel)) != (uint8_t)RESET) + { + /* DAC_FLAG is set */ + bitstatus = SET; + } + else + { + /* DAC_FLAG is reset */ + bitstatus = RESET; + } + /* Return the DAC_FLAG status */ + return bitstatus; +} + +/** + * @brief Clears the DAC channel's pending flags. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_FLAG: specifies the flag to clear. + * This parameter can be: + * @arg DAC_FLAG_DMAUDR: DMA underrun flag + * @retval None + */ +void DAC_ClearFlag(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_FLAG) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_DAC_FLAG(DAC_FLAG)); + + /* Clear the selected DAC flags */ + DACx->SR = (DAC_FLAG << DAC_Channel); +} + +/** + * @brief Checks whether the specified DAC interrupt has occurred or not. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_IT: specifies the DAC interrupt source to check. + * This parameter can be: + * @arg DAC_IT_DMAUDR: DMA underrun interrupt mask + * @note The DMA underrun occurs when a second external trigger arrives before + * the acknowledgement for the first external trigger is received (first request). + * @retval The new state of DAC_IT (SET or RESET). + */ +ITStatus DAC_GetITStatus(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_IT) +{ + ITStatus bitstatus = RESET; + uint32_t enablestatus = 0; + + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_DAC_IT(DAC_IT)); + + /* Get the DAC_IT enable bit status */ + enablestatus = (DACx->CR & (DAC_IT << DAC_Channel)) ; + + /* Check the status of the specified DAC interrupt */ + if (((DACx->SR & (DAC_IT << DAC_Channel)) != (uint32_t)RESET) && enablestatus) + { + /* DAC_IT is set */ + bitstatus = SET; + } + else + { + /* DAC_IT is reset */ + bitstatus = RESET; + } + /* Return the DAC_IT status */ + return bitstatus; +} + +/** + * @brief Clears the DAC channel's interrupt pending bits. + * @param DACx: where x can be 1 or 2 to select the DAC peripheral. + * @param DAC_Channel: the selected DAC channel. + * This parameter can be one of the following values: + * @arg DAC_Channel_1: DAC Channel1 selected + * @arg DAC_Channel_2: DAC Channel2 selected + * @param DAC_IT: specifies the DAC interrupt pending bit to clear. + * This parameter can be the following values: + * @arg DAC_IT_DMAUDR: DMA underrun interrupt mask + * @retval None + */ +void DAC_ClearITPendingBit(DAC_TypeDef* DACx, uint32_t DAC_Channel, uint32_t DAC_IT) +{ + /* Check the parameters */ + assert_param(IS_DAC_ALL_PERIPH(DACx)); + assert_param(IS_DAC_CHANNEL(DAC_Channel)); + assert_param(IS_DAC_IT(DAC_IT)); + + /* Clear the selected DAC interrupt pending bits */ + DACx->SR = (DAC_IT << DAC_Channel); +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dbgmcu.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dbgmcu.c new file mode 100755 index 0000000000..7887905b20 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dbgmcu.c @@ -0,0 +1,216 @@ +/** + ****************************************************************************** + * @file stm32f30x_dbgmcu.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Debug MCU (DBGMCU) peripheral: + * + Device and Revision ID management + * + Peripherals Configuration + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_dbgmcu.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup DBGMCU + * @brief DBGMCU driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define IDCODE_DEVID_MASK ((uint32_t)0x00000FFF) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup DBGMCU_Private_Functions + * @{ + */ + +/** @defgroup DBGMCU_Group1 Device and Revision ID management functions + * @brief Device and Revision ID management functions + * +@verbatim + ============================================================================== + ##### Device and Revision ID management functions ##### + ============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Returns the device revision identifier. + * @param None + * @retval Device revision identifier + */ +uint32_t DBGMCU_GetREVID(void) +{ + return(DBGMCU->IDCODE >> 16); +} + +/** + * @brief Returns the device identifier. + * @param None + * @retval Device identifier + */ +uint32_t DBGMCU_GetDEVID(void) +{ + return(DBGMCU->IDCODE & IDCODE_DEVID_MASK); +} + +/** + * @} + */ + +/** @defgroup DBGMCU_Group2 Peripherals Configuration functions + * @brief Peripherals Configuration + * +@verbatim + ============================================================================== + ##### Peripherals Configuration functions ##### + ============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures low power mode behavior when the MCU is in Debug mode. + * @param DBGMCU_Periph: specifies the low power mode. + * This parameter can be any combination of the following values: + * @arg DBGMCU_SLEEP: Keep debugger connection during SLEEP mode. + * @arg DBGMCU_STOP: Keep debugger connection during STOP mode. + * @arg DBGMCU_STANDBY: Keep debugger connection during STANDBY mode. + * @param NewState: new state of the specified low power mode in Debug mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DBGMCU_PERIPH(DBGMCU_Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + DBGMCU->CR |= DBGMCU_Periph; + } + else + { + DBGMCU->CR &= ~DBGMCU_Periph; + } +} + +/** + * @brief Configures APB1 peripheral behavior when the MCU is in Debug mode. + * @param DBGMCU_Periph: specifies the APB1 peripheral. + * This parameter can be any combination of the following values: + * @arg DBGMCU_TIM2_STOP: TIM2 counter stopped when Core is halted. + * @arg DBGMCU_TIM3_STOP: TIM3 counter stopped when Core is halted. + * @arg DBGMCU_TIM4_STOP: TIM4 counter stopped when Core is halted. + * @arg DBGMCU_TIM6_STOP: TIM6 counter stopped when Core is halted. + * @arg DBGMCU_TIM7_STOP: TIM7 counter stopped when Core is halted. + * @arg DBGMCU_RTC_STOP: RTC Calendar and Wakeup counter are stopped when + * Core is halted. + * @arg DBGMCU_WWDG_STOP: Debug WWDG stopped when Core is halted. + * @arg DBGMCU_IWDG_STOP: Debug IWDG stopped when Core is halted. + * @arg DBGMCU_I2C1_SMBUS_TIMEOUT: I2C1 SMBUS timeout mode stopped when + * Core is halted. + * @arg DBGMCU_I2C2_SMBUS_TIMEOUT: I2C2 SMBUS timeout mode stopped when + * Core is halted. + * @arg DBGMCU_CAN1_STOP: Debug CAN2 stopped when Core is halted. + * @arg DBGMCU_I2C3_SMBUS_TIMEOUT: I2C3 SMBUS timeout mode stopped when + * Core is halted. + * @param NewState: new state of the specified APB1 peripheral in Debug mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DBGMCU_APB1PERIPH(DBGMCU_Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + DBGMCU->APB1FZ |= DBGMCU_Periph; + } + else + { + DBGMCU->APB1FZ &= ~DBGMCU_Periph; + } +} + +/** + * @brief Configures APB2 peripheral behavior when the MCU is in Debug mode. + * @param DBGMCU_Periph: specifies the APB2 peripheral. + * This parameter can be any combination of the following values: + * @arg DBGMCU_TIM1_STOP: TIM1 counter stopped when Core is halted. + * @arg DBGMCU_TIM8_STOP: TIM8 counter stopped when Core is halted. + * @arg DBGMCU_TIM15_STOP: TIM15 counter stopped when Core is halted. + * @arg DBGMCU_TIM16_STOP: TIM16 counter stopped when Core is halted. + * @arg DBGMCU_TIM17_STOP: TIM17 counter stopped when Core is halted. + * @arg DBGMCU_TIM20_STOP: TIM20 counter stopped when Core is halted. + * @param NewState: new state of the specified APB2 peripheral in Debug mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DBGMCU_APB2PERIPH(DBGMCU_Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + DBGMCU->APB2FZ |= DBGMCU_Periph; + } + else + { + DBGMCU->APB2FZ &= ~DBGMCU_Periph; + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dma.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dma.c new file mode 100755 index 0000000000..11bf7252a3 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_dma.c @@ -0,0 +1,866 @@ +/** + ****************************************************************************** + * @file stm32f30x_dma.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Direct Memory Access controller (DMA): + * + Initialization and Configuration + * + Data Counter + * + Interrupts and flags management + * + @verbatim + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] + (#) Enable The DMA controller clock using + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE) function for DMA1 or + using RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE) function for DMA2. + (#) Enable and configure the peripheral to be connected to the DMA channel + (except for internal SRAM / FLASH memories: no initialization is necessary). + (#) For a given Channel, program the Source and Destination addresses, + the transfer Direction, the Buffer Size, the Peripheral and Memory + Incrementation mode and Data Size, the Circular or Normal mode, + the channel transfer Priority and the Memory-to-Memory transfer + mode (if needed) using the DMA_Init() function. + (#) Enable the NVIC and the corresponding interrupt(s) using the function + DMA_ITConfig() if you need to use DMA interrupts. + (#) Enable the DMA channel using the DMA_Cmd() function. + (#) Activate the needed channel Request using PPP_DMACmd() function for + any PPP peripheral except internal SRAM and FLASH (ie. SPI, USART ...) + The function allowing this operation is provided in each PPP peripheral + driver (ie. SPI_DMACmd for SPI peripheral). + (#) Optionally, you can configure the number of data to be transferred + when the channel is disabled (ie. after each Transfer Complete event + or when a Transfer Error occurs) using the function DMA_SetCurrDataCounter(). + And you can get the number of remaining data to be transferred using + the function DMA_GetCurrDataCounter() at run time (when the DMA channel is + enabled and running). + (#) To control DMA events you can use one of the following two methods: + (##) Check on DMA channel flags using the function DMA_GetFlagStatus(). + (##) Use DMA interrupts through the function DMA_ITConfig() at initialization + phase and DMA_GetITStatus() function into interrupt routines in + communication phase. + After checking on a flag you should clear it using DMA_ClearFlag() + function. And after checking on an interrupt event you should + clear it using DMA_ClearITPendingBit() function. + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_dma.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup DMA + * @brief DMA driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define CCR_CLEAR_MASK ((uint32_t)0xFFFF800F) /* DMA Channel config registers Masks */ +#define FLAG_Mask ((uint32_t)0x10000000) /* DMA2 FLAG mask */ + + +/* DMA1 Channelx interrupt pending bit masks */ +#define DMA1_CHANNEL1_IT_MASK ((uint32_t)(DMA_ISR_GIF1 | DMA_ISR_TCIF1 | DMA_ISR_HTIF1 | DMA_ISR_TEIF1)) +#define DMA1_CHANNEL2_IT_MASK ((uint32_t)(DMA_ISR_GIF2 | DMA_ISR_TCIF2 | DMA_ISR_HTIF2 | DMA_ISR_TEIF2)) +#define DMA1_CHANNEL3_IT_MASK ((uint32_t)(DMA_ISR_GIF3 | DMA_ISR_TCIF3 | DMA_ISR_HTIF3 | DMA_ISR_TEIF3)) +#define DMA1_CHANNEL4_IT_MASK ((uint32_t)(DMA_ISR_GIF4 | DMA_ISR_TCIF4 | DMA_ISR_HTIF4 | DMA_ISR_TEIF4)) +#define DMA1_CHANNEL5_IT_MASK ((uint32_t)(DMA_ISR_GIF5 | DMA_ISR_TCIF5 | DMA_ISR_HTIF5 | DMA_ISR_TEIF5)) +#define DMA1_CHANNEL6_IT_MASK ((uint32_t)(DMA_ISR_GIF6 | DMA_ISR_TCIF6 | DMA_ISR_HTIF6 | DMA_ISR_TEIF6)) +#define DMA1_CHANNEL7_IT_MASK ((uint32_t)(DMA_ISR_GIF7 | DMA_ISR_TCIF7 | DMA_ISR_HTIF7 | DMA_ISR_TEIF7)) + +/* DMA2 Channelx interrupt pending bit masks */ +#define DMA2_CHANNEL1_IT_MASK ((uint32_t)(DMA_ISR_GIF1 | DMA_ISR_TCIF1 | DMA_ISR_HTIF1 | DMA_ISR_TEIF1)) +#define DMA2_CHANNEL2_IT_MASK ((uint32_t)(DMA_ISR_GIF2 | DMA_ISR_TCIF2 | DMA_ISR_HTIF2 | DMA_ISR_TEIF2)) +#define DMA2_CHANNEL3_IT_MASK ((uint32_t)(DMA_ISR_GIF3 | DMA_ISR_TCIF3 | DMA_ISR_HTIF3 | DMA_ISR_TEIF3)) +#define DMA2_CHANNEL4_IT_MASK ((uint32_t)(DMA_ISR_GIF4 | DMA_ISR_TCIF4 | DMA_ISR_HTIF4 | DMA_ISR_TEIF4)) +#define DMA2_CHANNEL5_IT_MASK ((uint32_t)(DMA_ISR_GIF5 | DMA_ISR_TCIF5 | DMA_ISR_HTIF5 | DMA_ISR_TEIF5)) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup DMA_Private_Functions + * @{ + */ + +/** @defgroup DMA_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + [..] This subsection provides functions allowing to initialize the DMA channel + source and destination addresses, incrementation and data sizes, transfer + direction, buffer size, circular/normal mode selection, memory-to-memory + mode selection and channel priority value. + [..] The DMA_Init() function follows the DMA configuration procedures as described + in reference manual (RM00316). + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the DMAy Channelx registers to their default reset + * values. + * @param DMAy_Channelx: where y can be 1 or 2 to select the DMA and + * x can be 1 to 7 for DMA1 and 1 to 5 for DMA2 to select the DMA Channel. + * @retval None + */ +void DMA_DeInit(DMA_Channel_TypeDef* DMAy_Channelx) +{ + /* Check the parameters */ + assert_param(IS_DMA_ALL_PERIPH(DMAy_Channelx)); + + /* Disable the selected DMAy Channelx */ + DMAy_Channelx->CCR &= (uint16_t)(~DMA_CCR_EN); + + /* Reset DMAy Channelx control register */ + DMAy_Channelx->CCR = 0; + + /* Reset DMAy Channelx remaining bytes register */ + DMAy_Channelx->CNDTR = 0; + + /* Reset DMAy Channelx peripheral address register */ + DMAy_Channelx->CPAR = 0; + + /* Reset DMAy Channelx memory address register */ + DMAy_Channelx->CMAR = 0; + + if (DMAy_Channelx == DMA1_Channel1) + { + /* Reset interrupt pending bits for DMA1 Channel1 */ + DMA1->IFCR |= DMA1_CHANNEL1_IT_MASK; + } + else if (DMAy_Channelx == DMA1_Channel2) + { + /* Reset interrupt pending bits for DMA1 Channel2 */ + DMA1->IFCR |= DMA1_CHANNEL2_IT_MASK; + } + else if (DMAy_Channelx == DMA1_Channel3) + { + /* Reset interrupt pending bits for DMA1 Channel3 */ + DMA1->IFCR |= DMA1_CHANNEL3_IT_MASK; + } + else if (DMAy_Channelx == DMA1_Channel4) + { + /* Reset interrupt pending bits for DMA1 Channel4 */ + DMA1->IFCR |= DMA1_CHANNEL4_IT_MASK; + } + else if (DMAy_Channelx == DMA1_Channel5) + { + /* Reset interrupt pending bits for DMA1 Channel5 */ + DMA1->IFCR |= DMA1_CHANNEL5_IT_MASK; + } + else if (DMAy_Channelx == DMA1_Channel6) + { + /* Reset interrupt pending bits for DMA1 Channel6 */ + DMA1->IFCR |= DMA1_CHANNEL6_IT_MASK; + } + else if (DMAy_Channelx == DMA1_Channel7) + { + /* Reset interrupt pending bits for DMA1 Channel7 */ + DMA1->IFCR |= DMA1_CHANNEL7_IT_MASK; + } + else if (DMAy_Channelx == DMA2_Channel1) + { + /* Reset interrupt pending bits for DMA2 Channel1 */ + DMA2->IFCR |= DMA2_CHANNEL1_IT_MASK; + } + else if (DMAy_Channelx == DMA2_Channel2) + { + /* Reset interrupt pending bits for DMA2 Channel2 */ + DMA2->IFCR |= DMA2_CHANNEL2_IT_MASK; + } + else if (DMAy_Channelx == DMA2_Channel3) + { + /* Reset interrupt pending bits for DMA2 Channel3 */ + DMA2->IFCR |= DMA2_CHANNEL3_IT_MASK; + } + else if (DMAy_Channelx == DMA2_Channel4) + { + /* Reset interrupt pending bits for DMA2 Channel4 */ + DMA2->IFCR |= DMA2_CHANNEL4_IT_MASK; + } + else + { + if (DMAy_Channelx == DMA2_Channel5) + { + /* Reset interrupt pending bits for DMA2 Channel5 */ + DMA2->IFCR |= DMA2_CHANNEL5_IT_MASK; + } + } +} + +/** + * @brief Initializes the DMAy Channelx according to the specified parameters + * in the DMA_InitStruct. + * @param DMAy_Channelx: where y can be 1 or 2 to select the DMA and + * x can be 1 to 7 for DMA1 and 1 to 5 for DMA2 to select the DMA Channel. + * @param DMA_InitStruct: pointer to a DMA_InitTypeDef structure that contains + * the configuration information for the specified DMA Channel. + * @retval None + */ +void DMA_Init(DMA_Channel_TypeDef* DMAy_Channelx, DMA_InitTypeDef* DMA_InitStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_DMA_ALL_PERIPH(DMAy_Channelx)); + assert_param(IS_DMA_DIR(DMA_InitStruct->DMA_DIR)); + assert_param(IS_DMA_PERIPHERAL_INC_STATE(DMA_InitStruct->DMA_PeripheralInc)); + assert_param(IS_DMA_MEMORY_INC_STATE(DMA_InitStruct->DMA_MemoryInc)); + assert_param(IS_DMA_PERIPHERAL_DATA_SIZE(DMA_InitStruct->DMA_PeripheralDataSize)); + assert_param(IS_DMA_MEMORY_DATA_SIZE(DMA_InitStruct->DMA_MemoryDataSize)); + assert_param(IS_DMA_MODE(DMA_InitStruct->DMA_Mode)); + assert_param(IS_DMA_PRIORITY(DMA_InitStruct->DMA_Priority)); + assert_param(IS_DMA_M2M_STATE(DMA_InitStruct->DMA_M2M)); + +/*--------------------------- DMAy Channelx CCR Configuration ----------------*/ + /* Get the DMAy_Channelx CCR value */ + tmpreg = DMAy_Channelx->CCR; + + /* Clear MEM2MEM, PL, MSIZE, PSIZE, MINC, PINC, CIRC and DIR bits */ + tmpreg &= CCR_CLEAR_MASK; + + /* Configure DMAy Channelx: data transfer, data size, priority level and mode */ + /* Set DIR bit according to DMA_DIR value */ + /* Set CIRC bit according to DMA_Mode value */ + /* Set PINC bit according to DMA_PeripheralInc value */ + /* Set MINC bit according to DMA_MemoryInc value */ + /* Set PSIZE bits according to DMA_PeripheralDataSize value */ + /* Set MSIZE bits according to DMA_MemoryDataSize value */ + /* Set PL bits according to DMA_Priority value */ + /* Set the MEM2MEM bit according to DMA_M2M value */ + tmpreg |= DMA_InitStruct->DMA_DIR | DMA_InitStruct->DMA_Mode | + DMA_InitStruct->DMA_PeripheralInc | DMA_InitStruct->DMA_MemoryInc | + DMA_InitStruct->DMA_PeripheralDataSize | DMA_InitStruct->DMA_MemoryDataSize | + DMA_InitStruct->DMA_Priority | DMA_InitStruct->DMA_M2M; + + /* Write to DMAy Channelx CCR */ + DMAy_Channelx->CCR = tmpreg; + +/*--------------------------- DMAy Channelx CNDTR Configuration --------------*/ + /* Write to DMAy Channelx CNDTR */ + DMAy_Channelx->CNDTR = DMA_InitStruct->DMA_BufferSize; + +/*--------------------------- DMAy Channelx CPAR Configuration ---------------*/ + /* Write to DMAy Channelx CPAR */ + DMAy_Channelx->CPAR = DMA_InitStruct->DMA_PeripheralBaseAddr; + +/*--------------------------- DMAy Channelx CMAR Configuration ---------------*/ + /* Write to DMAy Channelx CMAR */ + DMAy_Channelx->CMAR = DMA_InitStruct->DMA_MemoryBaseAddr; +} + +/** + * @brief Fills each DMA_InitStruct member with its default value. + * @param DMA_InitStruct: pointer to a DMA_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void DMA_StructInit(DMA_InitTypeDef* DMA_InitStruct) +{ +/*-------------- Reset DMA init structure parameters values ------------------*/ + /* Initialize the DMA_PeripheralBaseAddr member */ + DMA_InitStruct->DMA_PeripheralBaseAddr = 0; + /* Initialize the DMA_MemoryBaseAddr member */ + DMA_InitStruct->DMA_MemoryBaseAddr = 0; + /* Initialize the DMA_DIR member */ + DMA_InitStruct->DMA_DIR = DMA_DIR_PeripheralSRC; + /* Initialize the DMA_BufferSize member */ + DMA_InitStruct->DMA_BufferSize = 0; + /* Initialize the DMA_PeripheralInc member */ + DMA_InitStruct->DMA_PeripheralInc = DMA_PeripheralInc_Disable; + /* Initialize the DMA_MemoryInc member */ + DMA_InitStruct->DMA_MemoryInc = DMA_MemoryInc_Disable; + /* Initialize the DMA_PeripheralDataSize member */ + DMA_InitStruct->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + /* Initialize the DMA_MemoryDataSize member */ + DMA_InitStruct->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + /* Initialize the DMA_Mode member */ + DMA_InitStruct->DMA_Mode = DMA_Mode_Normal; + /* Initialize the DMA_Priority member */ + DMA_InitStruct->DMA_Priority = DMA_Priority_Low; + /* Initialize the DMA_M2M member */ + DMA_InitStruct->DMA_M2M = DMA_M2M_Disable; +} + +/** + * @brief Enables or disables the specified DMAy Channelx. + * @param DMAy_Channelx: where y can be 1 or 2 to select the DMA and + * x can be 1 to 7 for DMA1 and 1 to 5 for DMA2 to select the DMA Channel. + * @param NewState: new state of the DMAy Channelx. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DMA_Cmd(DMA_Channel_TypeDef* DMAy_Channelx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DMA_ALL_PERIPH(DMAy_Channelx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected DMAy Channelx */ + DMAy_Channelx->CCR |= DMA_CCR_EN; + } + else + { + /* Disable the selected DMAy Channelx */ + DMAy_Channelx->CCR &= (uint16_t)(~DMA_CCR_EN); + } +} + +/** + * @} + */ + +/** @defgroup DMA_Group2 Data Counter functions + * @brief Data Counter functions + * +@verbatim + =============================================================================== + ##### Data Counter functions ##### + =============================================================================== + [..] This subsection provides function allowing to configure and read the buffer + size (number of data to be transferred).The DMA data counter can be written + only when the DMA channel is disabled (ie. after transfer complete event). + [..] The following function can be used to write the Channel data counter value: + (+) void DMA_SetCurrDataCounter(DMA_Channel_TypeDef* DMAy_Channelx, uint16_t DataNumber). + [..] + (@) It is advised to use this function rather than DMA_Init() in situations + where only the Data buffer needs to be reloaded. + [..] The DMA data counter can be read to indicate the number of remaining transfers + for the relative DMA channel. This counter is decremented at the end of each + data transfer and when the transfer is complete: + (+) If Normal mode is selected: the counter is set to 0. + (+) If Circular mode is selected: the counter is reloaded with the initial + value(configured before enabling the DMA channel). + [..] The following function can be used to read the Channel data counter value: + (+) uint16_t DMA_GetCurrDataCounter(DMA_Channel_TypeDef* DMAy_Channelx). + +@endverbatim + * @{ + */ + +/** + * @brief Sets the number of data units in the current DMAy Channelx transfer. + * @param DMAy_Channelx: where y can be 1 or 2 to select the DMA and + * x can be 1 to 7 for DMA1 and 1 to 5 for DMA2 to select the DMA Channel. + * @param DataNumber: The number of data units in the current DMAy Channelx + * transfer. + * @note This function can only be used when the DMAy_Channelx is disabled. + * @retval None. + */ +void DMA_SetCurrDataCounter(DMA_Channel_TypeDef* DMAy_Channelx, uint16_t DataNumber) +{ + /* Check the parameters */ + assert_param(IS_DMA_ALL_PERIPH(DMAy_Channelx)); + +/*--------------------------- DMAy Channelx CNDTR Configuration --------------*/ + /* Write to DMAy Channelx CNDTR */ + DMAy_Channelx->CNDTR = DataNumber; +} + +/** + * @brief Returns the number of remaining data units in the current + * DMAy Channelx transfer. + * @param DMAy_Channelx: where y can be 1 or 2 to select the DMA and + * x can be 1 to 7 for DMA1 and 1 to 5 for DMA2 to select the DMA Channel. + * @retval The number of remaining data units in the current DMAy Channelx + * transfer. + */ +uint16_t DMA_GetCurrDataCounter(DMA_Channel_TypeDef* DMAy_Channelx) +{ + /* Check the parameters */ + assert_param(IS_DMA_ALL_PERIPH(DMAy_Channelx)); + /* Return the number of remaining data units for DMAy Channelx */ + return ((uint16_t)(DMAy_Channelx->CNDTR)); +} + +/** + * @} + */ + +/** @defgroup DMA_Group3 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] This subsection provides functions allowing to configure the DMA Interrupt + sources and check or clear the flags or pending bits status. + The user should identify which mode will be used in his application to manage + the DMA controller events: Polling mode or Interrupt mode. + + *** Polling Mode *** + ==================== + [..] Each DMA channel can be managed through 4 event Flags (y : DMA Controller + number, x : DMA channel number): + (#) DMAy_FLAG_TCx : to indicate that a Transfer Complete event occurred. + (#) DMAy_FLAG_HTx : to indicate that a Half-Transfer Complete event occurred. + (#) DMAy_FLAG_TEx : to indicate that a Transfer Error occurred. + (#) DMAy_FLAG_GLx : to indicate that at least one of the events described + above occurred. + [..] + (@) Clearing DMAy_FLAG_GLx results in clearing all other pending flags of the + same channel (DMAy_FLAG_TCx, DMAy_FLAG_HTx and DMAy_FLAG_TEx). + [..] In this Mode it is advised to use the following functions: + (+) FlagStatus DMA_GetFlagStatus(uint32_t DMA_FLAG); + (+) void DMA_ClearFlag(uint32_t DMA_FLAG); + + *** Interrupt Mode *** + ====================== + [..] Each DMA channel can be managed through 4 Interrupts: + (+) Interrupt Source + (##) DMA_IT_TC: specifies the interrupt source for the Transfer Complete + event. + (##) DMA_IT_HT: specifies the interrupt source for the Half-transfer Complete + event. + (##) DMA_IT_TE: specifies the interrupt source for the transfer errors event. + (##) DMA_IT_GL: to indicate that at least one of the interrupts described + above occurred. + -@@- Clearing DMA_IT_GL interrupt results in clearing all other interrupts of + the same channel (DMA_IT_TCx, DMA_IT_HT and DMA_IT_TE). + [..] In this Mode it is advised to use the following functions: + (+) void DMA_ITConfig(DMA_Channel_TypeDef* DMAy_Channelx, uint32_t DMA_IT, FunctionalState NewState); + (+) ITStatus DMA_GetITStatus(uint32_t DMA_IT); + (+) void DMA_ClearITPendingBit(uint32_t DMA_IT); + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified DMAy Channelx interrupts. + * @param DMAy_Channelx: where y can be 1 or 2 to select the DMA and + * x can be 1 to 7 for DMA1 and 1 to 5 for DMA2 to select the DMA Channel. + * @param DMA_IT: specifies the DMA interrupts sources to be enabled + * or disabled. + * This parameter can be any combination of the following values: + * @arg DMA_IT_TC: Transfer complete interrupt mask + * @arg DMA_IT_HT: Half transfer interrupt mask + * @arg DMA_IT_TE: Transfer error interrupt mask + * @param NewState: new state of the specified DMA interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void DMA_ITConfig(DMA_Channel_TypeDef* DMAy_Channelx, uint32_t DMA_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_DMA_ALL_PERIPH(DMAy_Channelx)); + assert_param(IS_DMA_CONFIG_IT(DMA_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected DMA interrupts */ + DMAy_Channelx->CCR |= DMA_IT; + } + else + { + /* Disable the selected DMA interrupts */ + DMAy_Channelx->CCR &= ~DMA_IT; + } +} + +/** + * @brief Checks whether the specified DMAy Channelx flag is set or not. + * @param DMAy_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg DMA1_FLAG_GL1: DMA1 Channel1 global flag. + * @arg DMA1_FLAG_TC1: DMA1 Channel1 transfer complete flag. + * @arg DMA1_FLAG_HT1: DMA1 Channel1 half transfer flag. + * @arg DMA1_FLAG_TE1: DMA1 Channel1 transfer error flag. + * @arg DMA1_FLAG_GL2: DMA1 Channel2 global flag. + * @arg DMA1_FLAG_TC2: DMA1 Channel2 transfer complete flag. + * @arg DMA1_FLAG_HT2: DMA1 Channel2 half transfer flag. + * @arg DMA1_FLAG_TE2: DMA1 Channel2 transfer error flag. + * @arg DMA1_FLAG_GL3: DMA1 Channel3 global flag. + * @arg DMA1_FLAG_TC3: DMA1 Channel3 transfer complete flag. + * @arg DMA1_FLAG_HT3: DMA1 Channel3 half transfer flag. + * @arg DMA1_FLAG_TE3: DMA1 Channel3 transfer error flag. + * @arg DMA1_FLAG_GL4: DMA1 Channel4 global flag. + * @arg DMA1_FLAG_TC4: DMA1 Channel4 transfer complete flag. + * @arg DMA1_FLAG_HT4: DMA1 Channel4 half transfer flag. + * @arg DMA1_FLAG_TE4: DMA1 Channel4 transfer error flag. + * @arg DMA1_FLAG_GL5: DMA1 Channel5 global flag. + * @arg DMA1_FLAG_TC5: DMA1 Channel5 transfer complete flag. + * @arg DMA1_FLAG_HT5: DMA1 Channel5 half transfer flag. + * @arg DMA1_FLAG_TE5: DMA1 Channel5 transfer error flag. + * @arg DMA1_FLAG_GL6: DMA1 Channel6 global flag. + * @arg DMA1_FLAG_TC6: DMA1 Channel6 transfer complete flag. + * @arg DMA1_FLAG_HT6: DMA1 Channel6 half transfer flag. + * @arg DMA1_FLAG_TE6: DMA1 Channel6 transfer error flag. + * @arg DMA1_FLAG_GL7: DMA1 Channel7 global flag. + * @arg DMA1_FLAG_TC7: DMA1 Channel7 transfer complete flag. + * @arg DMA1_FLAG_HT7: DMA1 Channel7 half transfer flag. + * @arg DMA1_FLAG_TE7: DMA1 Channel7 transfer error flag. + * @arg DMA2_FLAG_GL1: DMA2 Channel1 global flag. + * @arg DMA2_FLAG_TC1: DMA2 Channel1 transfer complete flag. + * @arg DMA2_FLAG_HT1: DMA2 Channel1 half transfer flag. + * @arg DMA2_FLAG_TE1: DMA2 Channel1 transfer error flag. + * @arg DMA2_FLAG_GL2: DMA2 Channel2 global flag. + * @arg DMA2_FLAG_TC2: DMA2 Channel2 transfer complete flag. + * @arg DMA2_FLAG_HT2: DMA2 Channel2 half transfer flag. + * @arg DMA2_FLAG_TE2: DMA2 Channel2 transfer error flag. + * @arg DMA2_FLAG_GL3: DMA2 Channel3 global flag. + * @arg DMA2_FLAG_TC3: DMA2 Channel3 transfer complete flag. + * @arg DMA2_FLAG_HT3: DMA2 Channel3 half transfer flag. + * @arg DMA2_FLAG_TE3: DMA2 Channel3 transfer error flag. + * @arg DMA2_FLAG_GL4: DMA2 Channel4 global flag. + * @arg DMA2_FLAG_TC4: DMA2 Channel4 transfer complete flag. + * @arg DMA2_FLAG_HT4: DMA2 Channel4 half transfer flag. + * @arg DMA2_FLAG_TE4: DMA2 Channel4 transfer error flag. + * @arg DMA2_FLAG_GL5: DMA2 Channel5 global flag. + * @arg DMA2_FLAG_TC5: DMA2 Channel5 transfer complete flag. + * @arg DMA2_FLAG_HT5: DMA2 Channel5 half transfer flag. + * @arg DMA2_FLAG_TE5: DMA2 Channel5 transfer error flag. + * + * @note + * The Global flag (DMAy_FLAG_GLx) is set whenever any of the other flags + * relative to the same channel is set (Transfer Complete, Half-transfer + * Complete or Transfer Error flags: DMAy_FLAG_TCx, DMAy_FLAG_HTx or + * DMAy_FLAG_TEx). + * + * @retval The new state of DMAy_FLAG (SET or RESET). + */ +FlagStatus DMA_GetFlagStatus(uint32_t DMAy_FLAG) +{ + FlagStatus bitstatus = RESET; + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_DMA_GET_FLAG(DMAy_FLAG)); + + /* Calculate the used DMAy */ + if ((DMAy_FLAG & FLAG_Mask) != (uint32_t)RESET) + { + /* Get DMA2 ISR register value */ + tmpreg = DMA2->ISR ; + } + else + { + /* Get DMA1 ISR register value */ + tmpreg = DMA1->ISR ; + } + + /* Check the status of the specified DMAy flag */ + if ((tmpreg & DMAy_FLAG) != (uint32_t)RESET) + { + /* DMAy_FLAG is set */ + bitstatus = SET; + } + else + { + /* DMAy_FLAG is reset */ + bitstatus = RESET; + } + + /* Return the DMAy_FLAG status */ + return bitstatus; +} + +/** + * @brief Clears the DMAy Channelx's pending flags. + * @param DMAy_FLAG: specifies the flag to clear. + * This parameter can be any combination (for the same DMA) of the following values: + * @arg DMA1_FLAG_GL1: DMA1 Channel1 global flag. + * @arg DMA1_FLAG_TC1: DMA1 Channel1 transfer complete flag. + * @arg DMA1_FLAG_HT1: DMA1 Channel1 half transfer flag. + * @arg DMA1_FLAG_TE1: DMA1 Channel1 transfer error flag. + * @arg DMA1_FLAG_GL2: DMA1 Channel2 global flag. + * @arg DMA1_FLAG_TC2: DMA1 Channel2 transfer complete flag. + * @arg DMA1_FLAG_HT2: DMA1 Channel2 half transfer flag. + * @arg DMA1_FLAG_TE2: DMA1 Channel2 transfer error flag. + * @arg DMA1_FLAG_GL3: DMA1 Channel3 global flag. + * @arg DMA1_FLAG_TC3: DMA1 Channel3 transfer complete flag. + * @arg DMA1_FLAG_HT3: DMA1 Channel3 half transfer flag. + * @arg DMA1_FLAG_TE3: DMA1 Channel3 transfer error flag. + * @arg DMA1_FLAG_GL4: DMA1 Channel4 global flag. + * @arg DMA1_FLAG_TC4: DMA1 Channel4 transfer complete flag. + * @arg DMA1_FLAG_HT4: DMA1 Channel4 half transfer flag. + * @arg DMA1_FLAG_TE4: DMA1 Channel4 transfer error flag. + * @arg DMA1_FLAG_GL5: DMA1 Channel5 global flag. + * @arg DMA1_FLAG_TC5: DMA1 Channel5 transfer complete flag. + * @arg DMA1_FLAG_HT5: DMA1 Channel5 half transfer flag. + * @arg DMA1_FLAG_TE5: DMA1 Channel5 transfer error flag. + * @arg DMA1_FLAG_GL6: DMA1 Channel6 global flag. + * @arg DMA1_FLAG_TC6: DMA1 Channel6 transfer complete flag. + * @arg DMA1_FLAG_HT6: DMA1 Channel6 half transfer flag. + * @arg DMA1_FLAG_TE6: DMA1 Channel6 transfer error flag. + * @arg DMA1_FLAG_GL7: DMA1 Channel7 global flag. + * @arg DMA1_FLAG_TC7: DMA1 Channel7 transfer complete flag. + * @arg DMA1_FLAG_HT7: DMA1 Channel7 half transfer flag. + * @arg DMA1_FLAG_TE7: DMA1 Channel7 transfer error flag. + * @arg DMA2_FLAG_GL1: DMA2 Channel1 global flag. + * @arg DMA2_FLAG_TC1: DMA2 Channel1 transfer complete flag. + * @arg DMA2_FLAG_HT1: DMA2 Channel1 half transfer flag. + * @arg DMA2_FLAG_TE1: DMA2 Channel1 transfer error flag. + * @arg DMA2_FLAG_GL2: DMA2 Channel2 global flag. + * @arg DMA2_FLAG_TC2: DMA2 Channel2 transfer complete flag. + * @arg DMA2_FLAG_HT2: DMA2 Channel2 half transfer flag. + * @arg DMA2_FLAG_TE2: DMA2 Channel2 transfer error flag. + * @arg DMA2_FLAG_GL3: DMA2 Channel3 global flag. + * @arg DMA2_FLAG_TC3: DMA2 Channel3 transfer complete flag. + * @arg DMA2_FLAG_HT3: DMA2 Channel3 half transfer flag. + * @arg DMA2_FLAG_TE3: DMA2 Channel3 transfer error flag. + * @arg DMA2_FLAG_GL4: DMA2 Channel4 global flag. + * @arg DMA2_FLAG_TC4: DMA2 Channel4 transfer complete flag. + * @arg DMA2_FLAG_HT4: DMA2 Channel4 half transfer flag. + * @arg DMA2_FLAG_TE4: DMA2 Channel4 transfer error flag. + * @arg DMA2_FLAG_GL5: DMA2 Channel5 global flag. + * @arg DMA2_FLAG_TC5: DMA2 Channel5 transfer complete flag. + * @arg DMA2_FLAG_HT5: DMA2 Channel5 half transfer flag. + * @arg DMA2_FLAG_TE5: DMA2 Channel5 transfer error flag. + * + * @note + * Clearing the Global flag (DMAy_FLAG_GLx) results in clearing all other flags + * relative to the same channel (Transfer Complete, Half-transfer Complete and + * Transfer Error flags: DMAy_FLAG_TCx, DMAy_FLAG_HTx and DMAy_FLAG_TEx). + * + * @retval None + */ +void DMA_ClearFlag(uint32_t DMAy_FLAG) +{ + /* Check the parameters */ + assert_param(IS_DMA_CLEAR_FLAG(DMAy_FLAG)); + +/* Calculate the used DMAy */ + if ((DMAy_FLAG & FLAG_Mask) != (uint32_t)RESET) + { + /* Clear the selected DMAy flags */ + DMA2->IFCR = DMAy_FLAG; + } + else + { + /* Clear the selected DMAy flags */ + DMA1->IFCR = DMAy_FLAG; + } +} + +/** + * @brief Checks whether the specified DMAy Channelx interrupt has occurred or not. + * @param DMAy_IT: specifies the DMAy interrupt source to check. + * This parameter can be one of the following values: + * @arg DMA1_IT_GL1: DMA1 Channel1 global interrupt. + * @arg DMA1_IT_TC1: DMA1 Channel1 transfer complete interrupt. + * @arg DMA1_IT_HT1: DMA1 Channel1 half transfer interrupt. + * @arg DMA1_IT_TE1: DMA1 Channel1 transfer error interrupt. + * @arg DMA1_IT_GL2: DMA1 Channel2 global interrupt. + * @arg DMA1_IT_TC2: DMA1 Channel2 transfer complete interrupt. + * @arg DMA1_IT_HT2: DMA1 Channel2 half transfer interrupt. + * @arg DMA1_IT_TE2: DMA1 Channel2 transfer error interrupt. + * @arg DMA1_IT_GL3: DMA1 Channel3 global interrupt. + * @arg DMA1_IT_TC3: DMA1 Channel3 transfer complete interrupt. + * @arg DMA1_IT_HT3: DMA1 Channel3 half transfer interrupt. + * @arg DMA1_IT_TE3: DMA1 Channel3 transfer error interrupt. + * @arg DMA1_IT_GL4: DMA1 Channel4 global interrupt. + * @arg DMA1_IT_TC4: DMA1 Channel4 transfer complete interrupt. + * @arg DMA1_IT_HT4: DMA1 Channel4 half transfer interrupt. + * @arg DMA1_IT_TE4: DMA1 Channel4 transfer error interrupt. + * @arg DMA1_IT_GL5: DMA1 Channel5 global interrupt. + * @arg DMA1_IT_TC5: DMA1 Channel5 transfer complete interrupt. + * @arg DMA1_IT_HT5: DMA1 Channel5 half transfer interrupt. + * @arg DMA1_IT_TE5: DMA1 Channel5 transfer error interrupt. + * @arg DMA1_IT_GL6: DMA1 Channel6 global interrupt. + * @arg DMA1_IT_TC6: DMA1 Channel6 transfer complete interrupt. + * @arg DMA1_IT_HT6: DMA1 Channel6 half transfer interrupt. + * @arg DMA1_IT_TE6: DMA1 Channel6 transfer error interrupt. + * @arg DMA1_IT_GL7: DMA1 Channel7 global interrupt. + * @arg DMA1_IT_TC7: DMA1 Channel7 transfer complete interrupt. + * @arg DMA1_IT_HT7: DMA1 Channel7 half transfer interrupt. + * @arg DMA1_IT_TE7: DMA1 Channel7 transfer error interrupt. + * @arg DMA2_IT_GL1: DMA2 Channel1 global interrupt. + * @arg DMA2_IT_TC1: DMA2 Channel1 transfer complete interrupt. + * @arg DMA2_IT_HT1: DMA2 Channel1 half transfer interrupt. + * @arg DMA2_IT_TE1: DMA2 Channel1 transfer error interrupt. + * @arg DMA2_IT_GL2: DMA2 Channel2 global interrupt. + * @arg DMA2_IT_TC2: DMA2 Channel2 transfer complete interrupt. + * @arg DMA2_IT_HT2: DMA2 Channel2 half transfer interrupt. + * @arg DMA2_IT_TE2: DMA2 Channel2 transfer error interrupt. + * @arg DMA2_IT_GL3: DMA2 Channel3 global interrupt. + * @arg DMA2_IT_TC3: DMA2 Channel3 transfer complete interrupt. + * @arg DMA2_IT_HT3: DMA2 Channel3 half transfer interrupt. + * @arg DMA2_IT_TE3: DMA2 Channel3 transfer error interrupt. + * @arg DMA2_IT_GL4: DMA2 Channel4 global interrupt. + * @arg DMA2_IT_TC4: DMA2 Channel4 transfer complete interrupt. + * @arg DMA2_IT_HT4: DMA2 Channel4 half transfer interrupt. + * @arg DMA2_IT_TE4: DMA2 Channel4 transfer error interrupt. + * @arg DMA2_IT_GL5: DMA2 Channel5 global interrupt. + * @arg DMA2_IT_TC5: DMA2 Channel5 transfer complete interrupt. + * @arg DMA2_IT_HT5: DMA2 Channel5 half transfer interrupt. + * @arg DMA2_IT_TE5: DMA2 Channel5 transfer error interrupt. + * + * @note + * The Global interrupt (DMAy_FLAG_GLx) is set whenever any of the other + * interrupts relative to the same channel is set (Transfer Complete, + * Half-transfer Complete or Transfer Error interrupts: DMAy_IT_TCx, + * DMAy_IT_HTx or DMAy_IT_TEx). + * + * @retval The new state of DMAy_IT (SET or RESET). + */ +ITStatus DMA_GetITStatus(uint32_t DMAy_IT) +{ + ITStatus bitstatus = RESET; + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_DMA_GET_IT(DMAy_IT)); + + /* Calculate the used DMA */ + if ((DMAy_IT & FLAG_Mask) != (uint32_t)RESET) + { + /* Get DMA2 ISR register value */ + tmpreg = DMA2->ISR; + } + else + { + /* Get DMA1 ISR register value */ + tmpreg = DMA1->ISR; + } + + /* Check the status of the specified DMAy interrupt */ + if ((tmpreg & DMAy_IT) != (uint32_t)RESET) + { + /* DMAy_IT is set */ + bitstatus = SET; + } + else + { + /* DMAy_IT is reset */ + bitstatus = RESET; + } + /* Return the DMAy_IT status */ + return bitstatus; +} + +/** + * @brief Clears the DMAy Channelx's interrupt pending bits. + * @param DMAy_IT: specifies the DMAy interrupt pending bit to clear. + * This parameter can be any combination (for the same DMA) of the following values: + * @arg DMA1_IT_GL1: DMA1 Channel1 global interrupt. + * @arg DMA1_IT_TC1: DMA1 Channel1 transfer complete interrupt. + * @arg DMA1_IT_HT1: DMA1 Channel1 half transfer interrupt. + * @arg DMA1_IT_TE1: DMA1 Channel1 transfer error interrupt. + * @arg DMA1_IT_GL2: DMA1 Channel2 global interrupt. + * @arg DMA1_IT_TC2: DMA1 Channel2 transfer complete interrupt. + * @arg DMA1_IT_HT2: DMA1 Channel2 half transfer interrupt. + * @arg DMA1_IT_TE2: DMA1 Channel2 transfer error interrupt. + * @arg DMA1_IT_GL3: DMA1 Channel3 global interrupt. + * @arg DMA1_IT_TC3: DMA1 Channel3 transfer complete interrupt. + * @arg DMA1_IT_HT3: DMA1 Channel3 half transfer interrupt. + * @arg DMA1_IT_TE3: DMA1 Channel3 transfer error interrupt. + * @arg DMA1_IT_GL4: DMA1 Channel4 global interrupt. + * @arg DMA1_IT_TC4: DMA1 Channel4 transfer complete interrupt. + * @arg DMA1_IT_HT4: DMA1 Channel4 half transfer interrupt. + * @arg DMA1_IT_TE4: DMA1 Channel4 transfer error interrupt. + * @arg DMA1_IT_GL5: DMA1 Channel5 global interrupt. + * @arg DMA1_IT_TC5: DMA1 Channel5 transfer complete interrupt. + * @arg DMA1_IT_HT5: DMA1 Channel5 half transfer interrupt. + * @arg DMA1_IT_TE5: DMA1 Channel5 transfer error interrupt. + * @arg DMA1_IT_GL6: DMA1 Channel6 global interrupt. + * @arg DMA1_IT_TC6: DMA1 Channel6 transfer complete interrupt. + * @arg DMA1_IT_HT6: DMA1 Channel6 half transfer interrupt. + * @arg DMA1_IT_TE6: DMA1 Channel6 transfer error interrupt. + * @arg DMA1_IT_GL7: DMA1 Channel7 global interrupt. + * @arg DMA1_IT_TC7: DMA1 Channel7 transfer complete interrupt. + * @arg DMA1_IT_HT7: DMA1 Channel7 half transfer interrupt. + * @arg DMA1_IT_TE7: DMA1 Channel7 transfer error interrupt. + * @arg DMA2_IT_GL1: DMA2 Channel1 global interrupt. + * @arg DMA2_IT_TC1: DMA2 Channel1 transfer complete interrupt. + * @arg DMA2_IT_HT1: DMA2 Channel1 half transfer interrupt. + * @arg DMA2_IT_TE1: DMA2 Channel1 transfer error interrupt. + * @arg DMA2_IT_GL2: DMA2 Channel2 global interrupt. + * @arg DMA2_IT_TC2: DMA2 Channel2 transfer complete interrupt. + * @arg DMA2_IT_HT2: DMA2 Channel2 half transfer interrupt. + * @arg DMA2_IT_TE2: DMA2 Channel2 transfer error interrupt. + * @arg DMA2_IT_GL3: DMA2 Channel3 global interrupt. + * @arg DMA2_IT_TC3: DMA2 Channel3 transfer complete interrupt. + * @arg DMA2_IT_HT3: DMA2 Channel3 half transfer interrupt. + * @arg DMA2_IT_TE3: DMA2 Channel3 transfer error interrupt. + * @arg DMA2_IT_GL4: DMA2 Channel4 global interrupt. + * @arg DMA2_IT_TC4: DMA2 Channel4 transfer complete interrupt. + * @arg DMA2_IT_HT4: DMA2 Channel4 half transfer interrupt. + * @arg DMA2_IT_TE4: DMA2 Channel4 transfer error interrupt. + * @arg DMA2_IT_GL5: DMA2 Channel5 global interrupt. + * @arg DMA2_IT_TC5: DMA2 Channel5 transfer complete interrupt. + * @arg DMA2_IT_HT5: DMA2 Channel5 half transfer interrupt. + * @arg DMA2_IT_TE5: DMA2 Channel5 transfer error interrupt. + * + * @note + * Clearing the Global interrupt (DMAy_IT_GLx) results in clearing all other + * interrupts relative to the same channel (Transfer Complete, Half-transfer + * Complete and Transfer Error interrupts: DMAy_IT_TCx, DMAy_IT_HTx and + * DMAy_IT_TEx). + * + * @retval None + */ +void DMA_ClearITPendingBit(uint32_t DMAy_IT) +{ + /* Check the parameters */ + assert_param(IS_DMA_CLEAR_IT(DMAy_IT)); + + /* Calculate the used DMAy */ + if ((DMAy_IT & FLAG_Mask) != (uint32_t)RESET) + { + /* Clear the selected DMAy interrupt pending bits */ + DMA2->IFCR = DMAy_IT; + } + else + { + /* Clear the selected DMAy interrupt pending bits */ + DMA1->IFCR = DMAy_IT; + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_exti.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_exti.c new file mode 100755 index 0000000000..b9976c08d4 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_exti.c @@ -0,0 +1,349 @@ +/** + ****************************************************************************** + * @file stm32f30x_exti.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the EXTI peripheral: + * + Initialization and Configuration + * + Interrupts and flags management + * + @verbatim + =============================================================================== + ##### EXTI features ##### + =============================================================================== + [..] External interrupt/event lines are mapped as following: + (#) All available GPIO pins are connected to the 16 external + interrupt/event lines from EXTI0 to EXTI15. + (#) EXTI line 16 is connected to the PVD output + (#) EXTI line 17 is connected to the RTC Alarm event + (#) EXTI line 18 is connected to USB Device wakeup event + (#) EXTI line 19 is connected to the RTC Tamper and TimeStamp events + (#) EXTI line 20 is connected to the RTC wakeup event + (#) EXTI line 21 is connected to the Comparator 1 wakeup event + (#) EXTI line 22 is connected to the Comparator 2 wakeup event + (#) EXTI line 23 is connected to the I2C1 wakeup event + (#) EXTI line 24 is connected to the I2C2 wakeup event + (#) EXTI line 25 is connected to the USART1 wakeup event + (#) EXTI line 26 is connected to the USART2 wakeup event + (#) EXTI line 27 is reserved + (#) EXTI line 28 is connected to the USART3 wakeup event + (#) EXTI line 29 is connected to the Comparator 3 event + (#) EXTI line 30 is connected to the Comparator 4 event + (#) EXTI line 31 is connected to the Comparator 5 event + (#) EXTI line 32 is connected to the Comparator 6 event + (#) EXTI line 33 is connected to the Comparator 7 event + (#) EXTI line 34 is connected for thr UART4 wakeup event + (#) EXTI line 35 is connected for the UART5 wakeup event + + ##### How to use this driver ##### + =============================================================================== + [..] In order to use an I/O pin as an external interrupt source, + follow steps below: + (#) Configure the I/O in input mode using GPIO_Init(). + (#) Select the input source pin for the EXTI line using + SYSCFG_EXTILineConfig(). + (#) Select the mode(interrupt, event) and configure the trigger + selection (Rising, falling or both) using EXTI_Init(). For the + internal interrupt, the trigger selection is not needed + (the active edge is always the rising one). + (#) Configure NVIC IRQ channel mapped to the EXTI line using NVIC_Init(). + (#) Optionally, you can generate a software interrupt using the function + EXTI_GenerateSWInterrupt(). + [..] + (@) SYSCFG APB clock must be enabled to get write access to SYSCFG_EXTICRx + registers using RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_exti.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup EXTI + * @brief EXTI driver modules + * @{ + */ + + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define EXTI_LINENONE ((uint32_t)0x00000) /* No interrupt selected */ + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup EXTI_Private_Functions + * @{ + */ + +/** @defgroup EXTI_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the EXTI peripheral registers to their default reset + * values. + * @param None + * @retval None + */ +void EXTI_DeInit(void) +{ + EXTI->IMR = 0x1F800000; + EXTI->EMR = 0x00000000; + EXTI->RTSR = 0x00000000; + EXTI->FTSR = 0x00000000; + EXTI->SWIER = 0x00000000; + EXTI->PR = 0xE07FFFFF; + EXTI->IMR2 = 0x0000000C; + EXTI->EMR2 = 0x00000000; + EXTI->RTSR2 = 0x00000000; + EXTI->FTSR2 = 0x00000000; + EXTI->SWIER2 = 0x00000000; + EXTI->PR2 = 0x00000003; +} + +/** + * @brief Initializes the EXTI peripheral according to the specified + * parameters in the EXTI_InitStruct. + * EXTI_Line specifies the EXTI line (EXTI0....EXTI35). + * EXTI_Mode specifies which EXTI line is used as interrupt or an event. + * EXTI_Trigger selects the trigger. When the trigger occurs, interrupt + * pending bit will be set. + * EXTI_LineCmd controls (Enable/Disable) the EXTI line. + * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure that + * contains the configuration information for the EXTI peripheral. + * @retval None + */ + + +void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_EXTI_MODE(EXTI_InitStruct->EXTI_Mode)); + assert_param(IS_EXTI_TRIGGER(EXTI_InitStruct->EXTI_Trigger)); + assert_param(IS_EXTI_LINE_ALL(EXTI_InitStruct->EXTI_Line)); + assert_param(IS_FUNCTIONAL_STATE(EXTI_InitStruct->EXTI_LineCmd)); + + tmp = (uint32_t)EXTI_BASE; + + if (EXTI_InitStruct->EXTI_LineCmd != DISABLE) + { + /* Clear EXTI line configuration */ + *(__IO uint32_t *) (((uint32_t) &(EXTI->IMR)) + ((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20) &= ~(uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + *(__IO uint32_t *) (((uint32_t) &(EXTI->EMR)) + ((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20) &= ~(uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + + tmp += EXTI_InitStruct->EXTI_Mode + (((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20); + + *(__IO uint32_t *) tmp |= (uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + + tmp = (uint32_t)EXTI_BASE; + + /* Clear Rising Falling edge configuration */ + *(__IO uint32_t *) (((uint32_t) &(EXTI->RTSR)) + ((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20) &= ~(uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + *(__IO uint32_t *) (((uint32_t) &(EXTI->FTSR)) + ((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20) &= ~(uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + + /* Select the trigger for the selected interrupts */ + if (EXTI_InitStruct->EXTI_Trigger == EXTI_Trigger_Rising_Falling) + { + /* Rising Falling edge */ + *(__IO uint32_t *) (((uint32_t) &(EXTI->RTSR)) + ((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20) |= (uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + *(__IO uint32_t *) (((uint32_t) &(EXTI->FTSR)) + ((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20) |= (uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + } + else + { + tmp += EXTI_InitStruct->EXTI_Trigger + (((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20); + + *(__IO uint32_t *) tmp |= (uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + } + } + + else + { + tmp += EXTI_InitStruct->EXTI_Mode + (((EXTI_InitStruct->EXTI_Line) >> 5 ) * 0x20); + + /* Disable the selected external lines */ + *(__IO uint32_t *) tmp &= ~(uint32_t)(1 << (EXTI_InitStruct->EXTI_Line & 0x1F)); + } + +} + +/** + * @brief Fills each EXTI_InitStruct member with its reset value. + * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct) +{ + EXTI_InitStruct->EXTI_Line = EXTI_LINENONE; + EXTI_InitStruct->EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStruct->EXTI_Trigger = EXTI_Trigger_Rising_Falling; + EXTI_InitStruct->EXTI_LineCmd = DISABLE; +} + +/** + * @brief Generates a Software interrupt on selected EXTI line. + * @param EXTI_Line: specifies the EXTI line on which the software interrupt + * will be generated. + * This parameter can be any combination of EXTI_Linex where x can be (0..20). + * @retval None + */ +void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line) +{ + /* Check the parameters */ + assert_param(IS_EXTI_LINE_EXT(EXTI_Line)); + + *(__IO uint32_t *) (((uint32_t) &(EXTI->SWIER)) + ((EXTI_Line) >> 5 ) * 0x20) |= (uint32_t)(1 << (EXTI_Line & 0x1F)); + +} + +/** + * @} + */ + +/** @defgroup EXTI_Group2 Interrupts and flags management functions + * @brief EXTI Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] + This section provides functions allowing to configure the EXTI Interrupts + sources and check or clear the flags or pending bits status. + +@endverbatim + * @{ + */ + +/** + * @brief Checks whether the specified EXTI line flag is set or not. + * @param EXTI_Line: specifies the EXTI line flag to check. + * This parameter can be any combination of EXTI_Linex where x can be (0..20). + * @retval The new state of EXTI_Line (SET or RESET). + */ +FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line) +{ + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_GET_EXTI_LINE(EXTI_Line)); + + if ((*(__IO uint32_t *) (((uint32_t) &(EXTI->PR)) + ((EXTI_Line) >> 5 ) * 0x20)& (uint32_t)(1 << (EXTI_Line & 0x1F))) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the EXTI's line pending flags. + * @param EXTI_Line: specifies the EXTI lines flags to clear. + * This parameter can be any combination of EXTI_Linex where x can be (0..20). + * @retval None + */ +void EXTI_ClearFlag(uint32_t EXTI_Line) +{ + /* Check the parameters */ + assert_param(IS_EXTI_LINE_EXT(EXTI_Line)); + + *(__IO uint32_t *) (((uint32_t) &(EXTI->PR)) + ((EXTI_Line) >> 5 ) * 0x20) = (1 << (EXTI_Line & 0x1F)); +} + +/** + * @brief Checks whether the specified EXTI line is asserted or not. + * @param EXTI_Line: specifies the EXTI line to check. + * This parameter can be any combination of EXTI_Linex where x can be (0..20). + * @retval The new state of EXTI_Line (SET or RESET). + */ +ITStatus EXTI_GetITStatus(uint32_t EXTI_Line) +{ + ITStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_GET_EXTI_LINE(EXTI_Line)); + + if ((*(__IO uint32_t *) (((uint32_t) &(EXTI->PR)) + ((EXTI_Line) >> 5 ) * 0x20)& (uint32_t)(1 << (EXTI_Line & 0x1F))) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; + +} + +/** + * @brief Clears the EXTI's line pending bits. + * @param EXTI_Line: specifies the EXTI lines to clear. + * This parameter can be any combination of EXTI_Linex where x can be (0..20). + * @retval None + */ +void EXTI_ClearITPendingBit(uint32_t EXTI_Line) +{ + /* Check the parameters */ + assert_param(IS_EXTI_LINE_EXT(EXTI_Line)); + + *(__IO uint32_t *) (((uint32_t) &(EXTI->PR)) + ((EXTI_Line) >> 5 ) * 0x20) = (1 << (EXTI_Line & 0x1F)); +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_flash.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_flash.c new file mode 100755 index 0000000000..de327143a3 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_flash.c @@ -0,0 +1,1186 @@ +/** + ****************************************************************************** + * @file stm32f30x_flash.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the FLASH peripheral: + * + FLASH Interface configuration + * + FLASH Memory Programming + * + Option Bytes Programming + * + Interrupts and flags management + * + @verbatim + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] This driver provides functions to configure and program the FLASH + memory of all STM32F30x devices. These functions are split in 4 groups: + (#) FLASH Interface configuration functions: this group includes the + management of following features: + (++) Set the latency. + (++) Enable/Disable the Half Cycle Access. + (++) Enable/Disable the prefetch buffer. + (#) FLASH Memory Programming functions: this group includes all needed + functions to erase and program the main memory: + (++) Lock and Unlock the FLASH interface. + (++) Erase function: Erase page, erase all pages. + (++) Program functions: Half Word and Word write. + (#) FLASH Option Bytes Programming functions: this group includes all + needed functions to manage the Option Bytes: + (++) Lock and Unlock the Flash Option bytes. + (++) Launch the Option Bytes loader + (++) Erase the Option Bytes + (++) Set/Reset the write protection + (++) Set the Read protection Level + (++) Program the user option Bytes + (++) Set/Reset the BOOT1 bit + (++) Enable/Disable the VDDA Analog Monitoring + (++) Enable/Disable the SRAM parity + (++) Get the user option bytes + (++) Get the Write protection + (++) Get the read protection status + (#) FLASH Interrupts and flags management functions: this group includes + all needed functions to: + (++) Enable/Disable the FLASH interrupt sources. + (++) Get flags status. + (++) Clear flags. + (++) Get FLASH operation status. + (++) Wait for last FLASH operation. + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_flash.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup FLASH + * @brief FLASH driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* FLASH Mask */ +#define RDPRT_MASK ((uint32_t)0x00000002) +#define WRP01_MASK ((uint32_t)0x0000FFFF) +#define WRP23_MASK ((uint32_t)0xFFFF0000) +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup FLASH_Private_Functions + * @{ + */ + +/** @defgroup FLASH_Group1 FLASH Interface configuration functions + * @brief FLASH Interface configuration functions + * + +@verbatim + =============================================================================== + ##### FLASH Interface configuration functions ##### + =============================================================================== + [..] This group includes the following functions: + (+) void FLASH_SetLatency(uint32_t FLASH_Latency); + (+) void FLASH_HalfCycleAccessCmd(uint32_t FLASH_HalfCycleAccess); + (+) void FLASH_PrefetchBufferCmd(FunctionalState NewState); + [..] The unlock sequence is not needed for these functions. + +@endverbatim + * @{ + */ + +/** + * @brief Sets the code latency value. + * @param FLASH_Latency: specifies the FLASH Latency value. + * This parameter can be one of the following values: + * @arg FLASH_Latency_0: FLASH Zero Latency cycle + * @arg FLASH_Latency_1: FLASH One Latency cycle + * @arg FLASH_Latency_2: FLASH Two Latency cycles + * @retval None + */ +void FLASH_SetLatency(uint32_t FLASH_Latency) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_FLASH_LATENCY(FLASH_Latency)); + + /* Read the ACR register */ + tmpreg = FLASH->ACR; + + /* Sets the Latency value */ + tmpreg &= (uint32_t) (~((uint32_t)FLASH_ACR_LATENCY)); + tmpreg |= FLASH_Latency; + + /* Write the ACR register */ + FLASH->ACR = tmpreg; +} + +/** + * @brief Enables or disables the Half cycle flash access. + * @param FLASH_HalfCycleAccess: specifies the FLASH Half cycle Access mode. + * This parameter can be one of the following values: + * @arg FLASH_HalfCycleAccess_Enable: FLASH Half Cycle Enable + * @arg FLASH_HalfCycleAccess_Disable: FLASH Half Cycle Disable + * @retval None + */ +void FLASH_HalfCycleAccessCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if(NewState != DISABLE) + { + FLASH->ACR |= FLASH_ACR_HLFCYA; + } + else + { + FLASH->ACR &= (uint32_t)(~((uint32_t)FLASH_ACR_HLFCYA)); + } +} + +/** + * @brief Enables or disables the Prefetch Buffer. + * @param NewState: new state of the Prefetch Buffer. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void FLASH_PrefetchBufferCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if(NewState != DISABLE) + { + FLASH->ACR |= FLASH_ACR_PRFTBE; + } + else + { + FLASH->ACR &= (uint32_t)(~((uint32_t)FLASH_ACR_PRFTBE)); + } +} + +/** + * @} + */ + +/** @defgroup FLASH_Group2 FLASH Memory Programming functions + * @brief FLASH Memory Programming functions + * +@verbatim + =============================================================================== + ##### FLASH Memory Programming functions ##### + =============================================================================== + [..] This group includes the following functions: + (+) void FLASH_Unlock(void); + (+) void FLASH_Lock(void); + (+) FLASH_Status FLASH_ErasePage(uint32_t Page_Address); + (+) FLASH_Status FLASH_EraseAllPages(void); + (+) FLASH_Status FLASH_ProgramWord(uint32_t Address, uint32_t Data); + (+) FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data); + [..] Any operation of erase or program should follow these steps: + (#) Call the FLASH_Unlock() function to enable the FLASH control register + program memory access. + (#) Call the desired function to erase page or program data. + (#) Call the FLASH_Lock() function to disable the FLASH control register + access (recommended to protect the FLASH memory against possible + unwanted operation). + +@endverbatim + * @{ + */ + +/** + * @brief Unlocks the FLASH control register access + * @param None + * @retval None + */ +void FLASH_Unlock(void) +{ + if((FLASH->CR & FLASH_CR_LOCK) != RESET) + { + /* Authorize the FLASH Registers access */ + FLASH->KEYR = FLASH_KEY1; + FLASH->KEYR = FLASH_KEY2; + } +} + +/** + * @brief Locks the FLASH control register access + * @param None + * @retval None + */ +void FLASH_Lock(void) +{ + /* Set the LOCK Bit to lock the FLASH Registers access */ + FLASH->CR |= FLASH_CR_LOCK; +} + +/** + * @brief Erases a specified page in program memory. + * @note To correctly run this function, the FLASH_Unlock() function + * must be called before. + * @note Call the FLASH_Lock() to disable the flash memory access + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param Page_Address: The page address in program memory to be erased. + * @note A Page is erased in the Program memory only if the address to load + * is the start address of a page (multiple of 1024 bytes). + * @retval FLASH Status: The returned value can be: + * FLASH_ERROR_PROGRAM, FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_ErasePage(uint32_t Page_Address) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_FLASH_PROGRAM_ADDRESS(Page_Address)); + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the previous operation is completed, proceed to erase the page */ + FLASH->CR |= FLASH_CR_PER; + FLASH->AR = Page_Address; + FLASH->CR |= FLASH_CR_STRT; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + /* Disable the PER Bit */ + FLASH->CR &= ~FLASH_CR_PER; + } + + /* Return the Erase Status */ + return status; +} + +/** + * @brief Erases all FLASH pages. + * @note To correctly run this function, the FLASH_Unlock() function + * must be called before. + * all the FLASH_Lock() to disable the flash memory access + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param None + * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG, + * FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_EraseAllPages(void) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* if the previous operation is completed, proceed to erase all pages */ + FLASH->CR |= FLASH_CR_MER; + FLASH->CR |= FLASH_CR_STRT; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + /* Disable the MER Bit */ + FLASH->CR &= ~FLASH_CR_MER; + } + + /* Return the Erase Status */ + return status; +} + +/** + * @brief Programs a word at a specified address. + * @note To correctly run this function, the FLASH_Unlock() function + * must be called before. + * Call the FLASH_Lock() to disable the flash memory access + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param Address: specifies the address to be programmed. + * @param Data: specifies the data to be programmed. + * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG, + * FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_ProgramWord(uint32_t Address, uint32_t Data) +{ + FLASH_Status status = FLASH_COMPLETE; + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_FLASH_PROGRAM_ADDRESS(Address)); + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the previous operation is completed, proceed to program the new first + half word */ + FLASH->CR |= FLASH_CR_PG; + + *(__IO uint16_t*)Address = (uint16_t)Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the previous operation is completed, proceed to program the new second + half word */ + tmp = Address + 2; + + *(__IO uint16_t*) tmp = Data >> 16; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + /* Disable the PG Bit */ + FLASH->CR &= ~FLASH_CR_PG; + } + else + { + /* Disable the PG Bit */ + FLASH->CR &= ~FLASH_CR_PG; + } + } + + /* Return the Program Status */ + return status; +} + +/** + * @brief Programs a half word at a specified address. + * @note To correctly run this function, the FLASH_Unlock() function + * must be called before. + * Call the FLASH_Lock() to disable the flash memory access + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param Address: specifies the address to be programmed. + * @param Data: specifies the data to be programmed. + * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG, + * FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_FLASH_PROGRAM_ADDRESS(Address)); + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the previous operation is completed, proceed to program the new data */ + FLASH->CR |= FLASH_CR_PG; + + *(__IO uint16_t*)Address = Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + /* Disable the PG Bit */ + FLASH->CR &= ~FLASH_CR_PG; + } + + /* Return the Program Status */ + return status; +} + +/** + * @} + */ + +/** @defgroup FLASH_Group3 Option Bytes Programming functions + * @brief Option Bytes Programming functions + * +@verbatim + =============================================================================== + ##### Option Bytes Programming functions ##### + =============================================================================== + [..] This group includes the following functions: + (+) void FLASH_OB_Unlock(void); + (+) void FLASH_OB_Lock(void); + (+) void FLASH_OB_Erase(void); + (+) FLASH_Status FLASH_OB_WRPConfig(uint32_t OB_WRP, FunctionalState NewState); + (+) FLASH_Status FLASH_OB_RDPConfig(uint8_t OB_RDP); + (+) FLASH_Status FLASH_OB_UserConfig(uint8_t OB_IWDG, uint8_t OB_STOP, uint8_t OB_STDBY); + (+) FLASH_Status FLASH_OB_BOOTConfig(uint8_t OB_BOOT1); + (+) FLASH_Status FLASH_OB_VDDAConfig(uint8_t OB_VDDA_ANALOG); + (+) FLASH_Status FLASH_OB_SRMParityConfig(uint8_t OB_SRAM_Parity); + (+) FLASH_Status FLASH_OB_WriteUser(uint8_t OB_USER); + (+) FLASH_Status FLASH_OB_Launch(void); + (+) uint32_t FLASH_OB_GetUser(void); + (+) uint8_t FLASH_OB_GetWRP(void); + (+) uint8_t FLASH_OB_GetRDP(void); + [..] Any operation of erase or program should follow these steps: + (#) Call the FLASH_OB_Unlock() function to enable the FLASH option control + register access. + (#) Call one or several functions to program the desired Option Bytes: + (++) void FLASH_OB_WRPConfig(uint32_t OB_WRP, FunctionalState NewState); + => to Enable/Disable the desired sector write protection. + (++) FLASH_Status FLASH_OB_RDPConfig(uint8_t OB_RDP) => to set the + desired read Protection Level. + (++) FLASH_Status FLASH_OB_UserConfig(uint8_t OB_IWDG, uint8_t OB_STOP, uint8_t OB_STDBY); + => to configure the user Option Bytes. + (++) FLASH_Status FLASH_OB_BOOTConfig(uint8_t OB_BOOT1); + => to set the boot1 mode + (++) FLASH_Status FLASH_OB_VDDAConfig(uint8_t OB_VDDA_ANALOG); + => to Enable/Disable the VDDA monitoring. + (++) FLASH_Status FLASH_OB_SRMParityConfig(uint8_t OB_SRAM_Parity); + => to Enable/Disable the SRAM Parity check. + (++) FLASH_Status FLASH_OB_WriteUser(uint8_t OB_USER); + => to write all user option bytes: OB_IWDG, OB_STOP, OB_STDBY, + OB_BOOT1, OB_VDDA_ANALOG and OB_VDD_SD12. + (#) Once all needed Option Bytes to be programmed are correctly written, + call the FLASH_OB_Launch() function to launch the Option Bytes + programming process. + (#@) When changing the IWDG mode from HW to SW or from SW to HW, a system + reset is needed to make the change effective. + (#) Call the FLASH_OB_Lock() function to disable the FLASH option control + register access (recommended to protect the Option Bytes against + possible unwanted operations). + +@endverbatim + * @{ + */ + +/** + * @brief Unlocks the option bytes block access. + * @param None + * @retval None + */ +void FLASH_OB_Unlock(void) +{ + if((FLASH->CR & FLASH_CR_OPTWRE) == RESET) + { + /* Unlocking the option bytes block access */ + FLASH->OPTKEYR = FLASH_OPTKEY1; + FLASH->OPTKEYR = FLASH_OPTKEY2; + } +} + +/** + * @brief Locks the option bytes block access. + * @param None + * @retval None + */ +void FLASH_OB_Lock(void) +{ + /* Set the OPTWREN Bit to lock the option bytes block access */ + FLASH->CR &= ~FLASH_CR_OPTWRE; +} + +/** + * @brief Launch the option byte loading. + * @param None + * @retval None + */ +void FLASH_OB_Launch(void) +{ + /* Set the OBL_Launch bit to launch the option byte loading */ + FLASH->CR |= FLASH_CR_OBL_LAUNCH; +} + +/** + * @brief Erases the FLASH option bytes. + * @note This functions erases all option bytes except the Read protection (RDP). + * @param None + * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG, + * FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_OB_Erase(void) +{ + uint16_t rdptmp = OB_RDP_Level_0; + + FLASH_Status status = FLASH_COMPLETE; + + /* Get the actual read protection Option Byte value */ + if(FLASH_OB_GetRDP() != RESET) + { + rdptmp = 0x00; + } + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the previous operation is completed, proceed to erase the option bytes */ + FLASH->CR |= FLASH_CR_OPTER; + FLASH->CR |= FLASH_CR_STRT; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the erase operation is completed, disable the OPTER Bit */ + FLASH->CR &= ~FLASH_CR_OPTER; + + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + /* Restore the last read protection Option Byte value */ + OB->RDP = (uint16_t)rdptmp; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + else + { + if (status != FLASH_TIMEOUT) + { + /* Disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + } + /* Return the erase status */ + return status; +} + +/** + * @brief Write protects the desired pages + * @note To correctly run this function, the FLASH_OB_Unlock() function + * must be called before. + * @note Call the FLASH_OB_Lock() to disable the flash control register access and the option bytes + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param OB_WRP: specifies the address of the pages to be write protected. + * This parameter can be: + * @arg value between OB_WRP_Pages0to35 and OB_WRP_Pages60to63 + * @arg OB_WRP_AllPages + * @retval FLASH Status: The returned value can be: + * FLASH_ERROR_PROGRAM, FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_OB_EnableWRP(uint32_t OB_WRP) +{ + uint16_t WRP0_Data = 0xFFFF, WRP1_Data = 0xFFFF, WRP2_Data = 0xFFFF, WRP3_Data = 0xFFFF; + + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_OB_WRP(OB_WRP)); + + OB_WRP = (uint32_t)(~OB_WRP); + WRP0_Data = (uint16_t)(OB_WRP & OB_WRP0_WRP0); + WRP1_Data = (uint16_t)((OB_WRP >> 8) & OB_WRP0_WRP0); + WRP2_Data = (uint16_t)((OB_WRP >> 16) & OB_WRP0_WRP0) ; + WRP3_Data = (uint16_t)((OB_WRP >> 24) & OB_WRP0_WRP0) ; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + FLASH->CR |= FLASH_CR_OPTPG; + + if(WRP0_Data != 0xFF) + { + OB->WRP0 = WRP0_Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + } + if((status == FLASH_COMPLETE) && (WRP1_Data != 0xFF)) + { + OB->WRP1 = WRP1_Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + } + if((status == FLASH_COMPLETE) && (WRP2_Data != 0xFF)) + { + OB->WRP2 = WRP2_Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + } + if((status == FLASH_COMPLETE) && (WRP3_Data != 0xFF)) + { + OB->WRP3 = WRP3_Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + } + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the write protection operation Status */ + return status; +} + +/** + * @brief Enables or disables the read out protection. + * @note To correctly run this function, the FLASH_OB_Unlock() function + * must be called before. + * @note Call the FLASH_OB_Lock() to disable the flash control register access and the option bytes + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param FLASH_ReadProtection_Level: specifies the read protection level. + * This parameter can be: + * @arg OB_RDP_Level_0: No protection + * @arg OB_RDP_Level_1: Read protection of the memory + * @arg OB_RDP_Level_2: Chip protection + * @retval FLASH Status: The returned value can be: + * FLASH_ERROR_PROGRAM, FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_OB_RDPConfig(uint8_t OB_RDP) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_OB_RDP(OB_RDP)); + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + FLASH->CR |= FLASH_CR_OPTER; + FLASH->CR |= FLASH_CR_STRT; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* If the erase operation is completed, disable the OPTER Bit */ + FLASH->CR &= ~FLASH_CR_OPTER; + + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + OB->RDP = OB_RDP; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + else + { + if(status != FLASH_TIMEOUT) + { + /* Disable the OPTER Bit */ + FLASH->CR &= ~FLASH_CR_OPTER; + } + } + } + /* Return the protection operation Status */ + return status; +} + +/** + * @brief Programs the FLASH User Option Byte: IWDG_SW / RST_STOP / RST_STDBY. + * @param OB_IWDG: Selects the IWDG mode + * This parameter can be one of the following values: + * @arg OB_IWDG_SW: Software IWDG selected + * @arg OB_IWDG_HW: Hardware IWDG selected + * @param OB_STOP: Reset event when entering STOP mode. + * This parameter can be one of the following values: + * @arg OB_STOP_NoRST: No reset generated when entering in STOP + * @arg OB_STOP_RST: Reset generated when entering in STOP + * @param OB_STDBY: Reset event when entering Standby mode. + * This parameter can be one of the following values: + * @arg OB_STDBY_NoRST: No reset generated when entering in STANDBY + * @arg OB_STDBY_RST: Reset generated when entering in STANDBY + * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG, + * FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_OB_UserConfig(uint8_t OB_IWDG, uint8_t OB_STOP, uint8_t OB_STDBY) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_OB_IWDG_SOURCE(OB_IWDG)); + assert_param(IS_OB_STOP_SOURCE(OB_STOP)); + assert_param(IS_OB_STDBY_SOURCE(OB_STDBY)); + + /* Authorize the small information block programming */ + FLASH->OPTKEYR = FLASH_KEY1; + FLASH->OPTKEYR = FLASH_KEY2; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + OB->USER = (uint8_t)((uint8_t)(OB_IWDG | OB_STOP) | (uint8_t)(OB_STDBY |0xF8)); + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the Option Byte program Status */ + return status; +} + +/** + * @brief Sets or resets the BOOT1. + * @param OB_BOOT1: Set or Reset the BOOT1. + * This parameter can be one of the following values: + * @arg OB_BOOT1_RESET: BOOT1 Reset + * @arg OB_BOOT1_SET: BOOT1 Set + * @retval None + */ +FLASH_Status FLASH_OB_BOOTConfig(uint8_t OB_BOOT1) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_OB_BOOT1(OB_BOOT1)); + + /* Authorize the small information block programming */ + FLASH->OPTKEYR = FLASH_KEY1; + FLASH->OPTKEYR = FLASH_KEY2; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + OB->USER = OB_BOOT1|0xEF; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the Option Byte program Status */ + return status; +} + +/** + * @brief Sets or resets the analogue monitoring on VDDA Power source. + * @param OB_VDDA_ANALOG: Selects the analog monitoring on VDDA Power source. + * This parameter can be one of the following values: + * @arg OB_VDDA_ANALOG_ON: Analog monitoring on VDDA Power source ON + * @arg OB_VDDA_ANALOG_OFF: Analog monitoring on VDDA Power source OFF + * @retval None + */ +FLASH_Status FLASH_OB_VDDAConfig(uint8_t OB_VDDA_ANALOG) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_OB_VDDA_ANALOG(OB_VDDA_ANALOG)); + + /* Authorize the small information block programming */ + FLASH->OPTKEYR = FLASH_KEY1; + FLASH->OPTKEYR = FLASH_KEY2; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + OB->USER = OB_VDDA_ANALOG |0xDF; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the Option Byte program Status */ + return status; +} + +/** + * @brief Sets or resets the SRAM parity. + * @param OB_SRAM_Parity: Set or Reset the SRAM parity enable bit. + * This parameter can be one of the following values: + * @arg OB_SRAM_PARITY_SET: Set SRAM parity. + * @arg OB_SRAM_PARITY_RESET: Reset SRAM parity. + * @retval None + */ +FLASH_Status FLASH_OB_SRAMParityConfig(uint8_t OB_SRAM_Parity) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check the parameters */ + assert_param(IS_OB_SRAM_PARITY(OB_SRAM_Parity)); + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + OB->USER = OB_SRAM_Parity | 0xBF; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the Option Byte program Status */ + return status; +} + +/** + * @brief Programs the FLASH User Option Byte: IWDG_SW / RST_STOP / RST_STDBY/ BOOT1 and OB_VDDA_ANALOG. + * @note To correctly run this function, the FLASH_OB_Unlock() function + * must be called before. + * @note Call the FLASH_OB_Lock() to disable the flash control register access and the option bytes + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param OB_USER: Selects all user option bytes + * This parameter is a combination of the following values: + * @arg OB_IWDG_SW / OB_IWDG_HW: Software / Hardware WDG selected + * @arg OB_STOP_NoRST / OB_STOP_RST: No reset / Reset generated when entering in STOP + * @arg OB_STDBY_NoRST / OB_STDBY_RST: No reset / Reset generated when entering in STANDBY + * @arg OB_BOOT1_RESET / OB_BOOT1_SET: BOOT1 Reset / Set + * @arg OB_VDDA_ANALOG_ON / OB_VDDA_ANALOG_OFF: Analog monitoring on VDDA Power source ON / OFF + * @retval FLASH Status: The returned value can be: + * FLASH_ERROR_PROGRAM, FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_OB_WriteUser(uint8_t OB_USER) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Authorize the small information block programming */ + FLASH->OPTKEYR = FLASH_KEY1; + FLASH->OPTKEYR = FLASH_KEY2; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* Enable the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + + OB->USER = OB_USER | 0x88; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* if the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the Option Byte program Status */ + return status; + +} + +/** + * @brief Programs a half word at a specified Option Byte Data address. + * @note To correctly run this function, the FLASH_OB_Unlock() function + * must be called before. + * Call the FLASH_OB_Lock() to disable the flash control register access and the option bytes + * (recommended to protect the FLASH memory against possible unwanted operation) + * @param Address: specifies the address to be programmed. + * This parameter can be 0x1FFFF804 or 0x1FFFF806. + * @param Data: specifies the data to be programmed. + * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG, + * FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_ProgramOptionByteData(uint32_t Address, uint8_t Data) +{ + FLASH_Status status = FLASH_COMPLETE; + /* Check the parameters */ + assert_param(IS_OB_DATA_ADDRESS(Address)); + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status == FLASH_COMPLETE) + { + /* Enables the Option Bytes Programming operation */ + FLASH->CR |= FLASH_CR_OPTPG; + *(__IO uint16_t*)Address = Data; + + /* Wait for last operation to be completed */ + status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT); + + if(status != FLASH_TIMEOUT) + { + /* If the program operation is completed, disable the OPTPG Bit */ + FLASH->CR &= ~FLASH_CR_OPTPG; + } + } + /* Return the Option Byte Data Program Status */ + return status; +} + +/** + * @brief Returns the FLASH User Option Bytes values. + * @param None + * @retval The FLASH User Option Bytes . + */ +uint8_t FLASH_OB_GetUser(void) +{ + /* Return the User Option Byte */ + return (uint8_t)(FLASH->OBR >> 8); +} + +/** + * @brief Returns the FLASH Write Protection Option Bytes value. + * @param None + * @retval The FLASH Write Protection Option Bytes value + */ +uint32_t FLASH_OB_GetWRP(void) +{ + /* Return the FLASH write protection Register value */ + return (uint32_t)(FLASH->WRPR); +} + +/** + * @brief Checks whether the FLASH Read out Protection Status is set or not. + * @param None + * @retval FLASH ReadOut Protection Status(SET or RESET) + */ +FlagStatus FLASH_OB_GetRDP(void) +{ + FlagStatus readstatus = RESET; + + if ((uint8_t)(FLASH->OBR & (FLASH_OBR_RDPRT1 | FLASH_OBR_RDPRT2)) != RESET) + { + readstatus = SET; + } + else + { + readstatus = RESET; + } + return readstatus; +} + +/** + * @} + */ + +/** @defgroup FLASH_Group4 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified FLASH interrupts. + * @param FLASH_IT: specifies the FLASH interrupt sources to be enabled or + * disabled. + * This parameter can be any combination of the following values: + * @arg FLASH_IT_EOP: FLASH end of programming Interrupt + * @arg FLASH_IT_ERR: FLASH Error Interrupt + * @retval None + */ +void FLASH_ITConfig(uint32_t FLASH_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FLASH_IT(FLASH_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if(NewState != DISABLE) + { + /* Enable the interrupt sources */ + FLASH->CR |= FLASH_IT; + } + else + { + /* Disable the interrupt sources */ + FLASH->CR &= ~(uint32_t)FLASH_IT; + } +} + +/** + * @brief Checks whether the specified FLASH flag is set or not. + * @param FLASH_FLAG: specifies the FLASH flag to check. + * This parameter can be one of the following values: + * @arg FLASH_FLAG_BSY: FLASH write/erase operations in progress flag + * @arg FLASH_FLAG_PGERR: FLASH Programming error flag + * @arg FLASH_FLAG_WRPERR: FLASH Write protected error flag + * @arg FLASH_FLAG_EOP: FLASH End of Programming flag + * @retval The new state of FLASH_FLAG (SET or RESET). + */ +FlagStatus FLASH_GetFlagStatus(uint32_t FLASH_FLAG) +{ + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_FLASH_GET_FLAG(FLASH_FLAG)); + + if((FLASH->SR & FLASH_FLAG) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + /* Return the new state of FLASH_FLAG (SET or RESET) */ + return bitstatus; +} + +/** + * @brief Clears the FLASH's pending flags. + * @param FLASH_FLAG: specifies the FLASH flags to clear. + * This parameter can be any combination of the following values: + * @arg FLASH_FLAG_PGERR: FLASH Programming error flag + * @arg FLASH_FLAG_WRPERR: FLASH Write protected error flag + * @arg FLASH_FLAG_EOP: FLASH End of Programming flag + * @retval None + */ +void FLASH_ClearFlag(uint32_t FLASH_FLAG) +{ + /* Check the parameters */ + assert_param(IS_FLASH_CLEAR_FLAG(FLASH_FLAG)); + + /* Clear the flags */ + FLASH->SR = FLASH_FLAG; +} + +/** + * @brief Returns the FLASH Status. + * @param None + * @retval FLASH Status: The returned value can be: + * FLASH_BUSY, FLASH_ERROR_PROGRAM, FLASH_ERROR_WRP or FLASH_COMPLETE. + */ +FLASH_Status FLASH_GetStatus(void) +{ + FLASH_Status FLASHstatus = FLASH_COMPLETE; + + if((FLASH->SR & FLASH_FLAG_BSY) == FLASH_FLAG_BSY) + { + FLASHstatus = FLASH_BUSY; + } + else + { + if((FLASH->SR & (uint32_t)FLASH_FLAG_WRPERR)!= (uint32_t)0x00) + { + FLASHstatus = FLASH_ERROR_WRP; + } + else + { + if((FLASH->SR & (uint32_t)(FLASH_SR_PGERR)) != (uint32_t)0x00) + { + FLASHstatus = FLASH_ERROR_PROGRAM; + } + else + { + FLASHstatus = FLASH_COMPLETE; + } + } + } + /* Return the FLASH Status */ + return FLASHstatus; +} + +/** + * @brief Waits for a FLASH operation to complete or a TIMEOUT to occur. + * @param Timeout: FLASH programming Timeout + * @retval FLASH Status: The returned value can be: FLASH_BUSY, + * FLASH_ERROR_PROGRAM, FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. + */ +FLASH_Status FLASH_WaitForLastOperation(uint32_t Timeout) +{ + FLASH_Status status = FLASH_COMPLETE; + + /* Check for the FLASH Status */ + status = FLASH_GetStatus(); + + /* Wait for a FLASH operation to complete or a TIMEOUT to occur */ + while((status == FLASH_BUSY) && (Timeout != 0x00)) + { + status = FLASH_GetStatus(); + Timeout--; + } + + if(Timeout == 0x00 ) + { + status = FLASH_TIMEOUT; + } + /* Return the operation status */ + return status; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_fmc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_fmc.c new file mode 100755 index 0000000000..4c55198f64 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_fmc.c @@ -0,0 +1,1001 @@ +/** + ****************************************************************************** + * @file stm32f30x_fmc.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the FMC peripheral: + * + Interface with SRAM, PSRAM, NOR and OneNAND memories + * + Interface with NAND memories + * + Interface with 16-bit PC Card compatible memories + * + Interrupts and flags management + * + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_fmc.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup FMC + * @brief FMC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* --------------------- FMC registers bit mask ---------------------------- */ +/* FMC BCRx Mask */ +#define BCR_MBKEN_SET ((uint32_t)0x00000001) +#define BCR_MBKEN_RESET ((uint32_t)0x000FFFFE) +#define BCR_FACCEN_SET ((uint32_t)0x00000040) + +/* FMC PCRx Mask */ +#define PCR_PBKEN_SET ((uint32_t)0x00000004) +#define PCR_PBKEN_RESET ((uint32_t)0x000FFFFB) +#define PCR_ECCEN_SET ((uint32_t)0x00000040) +#define PCR_ECCEN_RESET ((uint32_t)0x000FFFBF) +#define PCR_MEMORYTYPE_NAND ((uint32_t)0x00000008) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup FMC_Private_Functions + * @{ + */ + +/** @defgroup FMC_Group1 NOR/SRAM Controller functions + * @brief NOR/SRAM Controller functions + * +@verbatim + =============================================================================== + ##### NOR and SRAM Controller functions ##### + =============================================================================== + + [..] The following sequence should be followed to configure the FMC to interface + with SRAM, PSRAM, NOR or OneNAND memory connected to the NOR/SRAM Bank: + + (#) Enable the clock for the FMC and associated GPIOs using the following functions: + RCC_AHB3PeriphClockCmd(RCC_AHB3Periph_FMC, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOx, ENABLE); + + (#) FMC pins configuration + (++) Connect the involved FMC pins to AF12 using the following function + GPIO_PinAFConfig(GPIOx, GPIO_PinSourcex, GPIO_AF_FMC); + (++) Configure these FMC pins in alternate function mode by calling the function + GPIO_Init(); + + (#) Declare a FMC_NORSRAMInitTypeDef structure, for example: + FMC_NORSRAMInitTypeDef FMC_NORSRAMInitStructure; + and fill the FMC_NORSRAMInitStructure variable with the allowed values of + the structure member. + + (#) Initialize the NOR/SRAM Controller by calling the function + FMC_NORSRAMInit(&FMC_NORSRAMInitStructure); + + (#) Then enable the NOR/SRAM Bank, for example: + FMC_NORSRAMCmd(FMC_Bank1_NORSRAM2, ENABLE); + + (#) At this stage you can read/write from/to the memory connected to the NOR/SRAM Bank. + +@endverbatim + * @{ + */ + +/** + * @brief De-initializes the FMC NOR/SRAM Banks registers to their default + * reset values. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank1_NORSRAM1: FMC Bank1 NOR/SRAM1 + * @arg FMC_Bank1_NORSRAM2: FMC Bank1 NOR/SRAM2 + * @arg FMC_Bank1_NORSRAM3: FMC Bank1 NOR/SRAM3 + * @arg FMC_Bank1_NORSRAM4: FMC Bank1 NOR/SRAM4 + * @retval None + */ +void FMC_NORSRAMDeInit(uint32_t FMC_Bank) +{ + /* Check the parameter */ + assert_param(IS_FMC_NORSRAM_BANK(FMC_Bank)); + + /* FMC_Bank1_NORSRAM1 */ + if(FMC_Bank == FMC_Bank1_NORSRAM1) + { + FMC_Bank1->BTCR[FMC_Bank] = 0x000030DB; + } + /* FMC_Bank1_NORSRAM2, FMC_Bank1_NORSRAM3 or FMC_Bank1_NORSRAM4 */ + else + { + FMC_Bank1->BTCR[FMC_Bank] = 0x000030D2; + } + FMC_Bank1->BTCR[FMC_Bank + 1] = 0x0FFFFFFF; + FMC_Bank1E->BWTR[FMC_Bank] = 0x0FFFFFFF; +} + +/** + * @brief Initializes the FMC NOR/SRAM Banks according to the specified + * parameters in the FMC_NORSRAMInitStruct. + * @param FMC_NORSRAMInitStruct : pointer to a FMC_NORSRAMInitTypeDef structure + * that contains the configuration information for the FMC NOR/SRAM + * specified Banks. + * @retval None + */ +void FMC_NORSRAMInit(FMC_NORSRAMInitTypeDef* FMC_NORSRAMInitStruct) +{ + /* Check the parameters */ + assert_param(IS_FMC_NORSRAM_BANK(FMC_NORSRAMInitStruct->FMC_Bank)); + assert_param(IS_FMC_MUX(FMC_NORSRAMInitStruct->FMC_DataAddressMux)); + assert_param(IS_FMC_MEMORY(FMC_NORSRAMInitStruct->FMC_MemoryType)); + assert_param(IS_FMC_NORSRAM_MEMORY_WIDTH(FMC_NORSRAMInitStruct->FMC_MemoryDataWidth)); + assert_param(IS_FMC_BURSTMODE(FMC_NORSRAMInitStruct->FMC_BurstAccessMode)); + assert_param(IS_FMC_WAIT_POLARITY(FMC_NORSRAMInitStruct->FMC_WaitSignalPolarity)); + assert_param(IS_FMC_WRAP_MODE(FMC_NORSRAMInitStruct->FMC_WrapMode)); + assert_param(IS_FMC_WAIT_SIGNAL_ACTIVE(FMC_NORSRAMInitStruct->FMC_WaitSignalActive)); + assert_param(IS_FMC_WRITE_OPERATION(FMC_NORSRAMInitStruct->FMC_WriteOperation)); + assert_param(IS_FMC_WAITE_SIGNAL(FMC_NORSRAMInitStruct->FMC_WaitSignal)); + assert_param(IS_FMC_EXTENDED_MODE(FMC_NORSRAMInitStruct->FMC_ExtendedMode)); + assert_param(IS_FMC_ASYNWAIT(FMC_NORSRAMInitStruct->FMC_AsynchronousWait)); + assert_param(IS_FMC_WRITE_BURST(FMC_NORSRAMInitStruct->FMC_WriteBurst)); + assert_param(IS_FMC_ADDRESS_SETUP_TIME(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AddressSetupTime)); + assert_param(IS_FMC_ADDRESS_HOLD_TIME(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AddressHoldTime)); + assert_param(IS_FMC_DATASETUP_TIME(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_DataSetupTime)); + assert_param(IS_FMC_TURNAROUND_TIME(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_BusTurnAroundDuration)); + assert_param(IS_FMC_CLK_DIV(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_CLKDivision)); + assert_param(IS_FMC_DATA_LATENCY(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_DataLatency)); + assert_param(IS_FMC_ACCESS_MODE(FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AccessMode)); + + /* NOR/SRAM Bank control register configuration */ + FMC_Bank1->BTCR[FMC_NORSRAMInitStruct->FMC_Bank] = + (uint32_t)FMC_NORSRAMInitStruct->FMC_DataAddressMux | + FMC_NORSRAMInitStruct->FMC_MemoryType | + FMC_NORSRAMInitStruct->FMC_MemoryDataWidth | + FMC_NORSRAMInitStruct->FMC_BurstAccessMode | + FMC_NORSRAMInitStruct->FMC_WaitSignalPolarity | + FMC_NORSRAMInitStruct->FMC_WrapMode | + FMC_NORSRAMInitStruct->FMC_WaitSignalActive | + FMC_NORSRAMInitStruct->FMC_WriteOperation | + FMC_NORSRAMInitStruct->FMC_WaitSignal | + FMC_NORSRAMInitStruct->FMC_ExtendedMode | + FMC_NORSRAMInitStruct->FMC_AsynchronousWait | + FMC_NORSRAMInitStruct->FMC_WriteBurst; + + + if(FMC_NORSRAMInitStruct->FMC_MemoryType == FMC_MemoryType_NOR) + { + FMC_Bank1->BTCR[FMC_NORSRAMInitStruct->FMC_Bank] |= (uint32_t)BCR_FACCEN_SET; + } + + /* NOR/SRAM Bank timing register configuration */ + FMC_Bank1->BTCR[FMC_NORSRAMInitStruct->FMC_Bank+1] = + (uint32_t)FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AddressSetupTime | + (FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AddressHoldTime << 4) | + (FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_DataSetupTime << 8) | + (FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_BusTurnAroundDuration << 16) | + (((FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_CLKDivision)-1) << 20) | + (((FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_DataLatency)-2) << 24) | + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AccessMode; + + /* NOR/SRAM Bank timing register for write configuration, if extended mode is used */ + if(FMC_NORSRAMInitStruct->FMC_ExtendedMode == FMC_ExtendedMode_Enable) + { + assert_param(IS_FMC_ADDRESS_SETUP_TIME(FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AddressSetupTime)); + assert_param(IS_FMC_ADDRESS_HOLD_TIME(FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AddressHoldTime)); + assert_param(IS_FMC_DATASETUP_TIME(FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_DataSetupTime)); + assert_param(IS_FMC_CLK_DIV(FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_CLKDivision)); + assert_param(IS_FMC_DATA_LATENCY(FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_DataLatency)); + assert_param(IS_FMC_ACCESS_MODE(FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AccessMode)); + + FMC_Bank1E->BWTR[FMC_NORSRAMInitStruct->FMC_Bank] = + (uint32_t)FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AddressSetupTime | + (FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AddressHoldTime << 4 )| + (FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_DataSetupTime << 8) | + (((FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_CLKDivision)-1) << 20) | + (((FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_DataLatency)-2) << 24) | + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AccessMode; + } + else + { + FMC_Bank1E->BWTR[FMC_NORSRAMInitStruct->FMC_Bank] = 0x0FFFFFFF; + } + +} + +/** + * @brief Fills each FMC_NORSRAMInitStruct member with its default value. + * @param FMC_NORSRAMInitStruct: pointer to a FMC_NORSRAMInitTypeDef structure + * which will be initialized. + * @retval None + */ +void FMC_NORSRAMStructInit(FMC_NORSRAMInitTypeDef* FMC_NORSRAMInitStruct) +{ + /* Reset NOR/SRAM Init structure parameters values */ + FMC_NORSRAMInitStruct->FMC_Bank = FMC_Bank1_NORSRAM1; + FMC_NORSRAMInitStruct->FMC_DataAddressMux = FMC_DataAddressMux_Enable; + FMC_NORSRAMInitStruct->FMC_MemoryType = FMC_MemoryType_SRAM; + FMC_NORSRAMInitStruct->FMC_MemoryDataWidth = FMC_NORSRAM_MemoryDataWidth_16b; + FMC_NORSRAMInitStruct->FMC_BurstAccessMode = FMC_BurstAccessMode_Disable; + FMC_NORSRAMInitStruct->FMC_AsynchronousWait = FMC_AsynchronousWait_Disable; + FMC_NORSRAMInitStruct->FMC_WaitSignalPolarity = FMC_WaitSignalPolarity_Low; + FMC_NORSRAMInitStruct->FMC_WrapMode = FMC_WrapMode_Disable; + FMC_NORSRAMInitStruct->FMC_WaitSignalActive = FMC_WaitSignalActive_BeforeWaitState; + FMC_NORSRAMInitStruct->FMC_WriteOperation = FMC_WriteOperation_Enable; + FMC_NORSRAMInitStruct->FMC_WaitSignal = FMC_WaitSignal_Enable; + FMC_NORSRAMInitStruct->FMC_ExtendedMode = FMC_ExtendedMode_Disable; + FMC_NORSRAMInitStruct->FMC_WriteBurst = FMC_WriteBurst_Disable; + + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AddressSetupTime = 15; + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AddressHoldTime = 15; + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_DataSetupTime = 255; + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_BusTurnAroundDuration = 15; + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_CLKDivision = 15; + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_DataLatency = 15; + FMC_NORSRAMInitStruct->FMC_ReadWriteTimingStruct->FMC_AccessMode = FMC_AccessMode_A; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AddressSetupTime = 15; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AddressHoldTime = 15; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_DataSetupTime = 255; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_BusTurnAroundDuration = 15; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_CLKDivision = 16; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_DataLatency = 17; + FMC_NORSRAMInitStruct->FMC_WriteTimingStruct->FMC_AccessMode = FMC_AccessMode_A; +} + +/** + * @brief Enables or disables the specified NOR/SRAM Memory Bank. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank1_NORSRAM1: FMC Bank1 NOR/SRAM1 + * @arg FMC_Bank1_NORSRAM2: FMC Bank1 NOR/SRAM2 + * @arg FMC_Bank1_NORSRAM3: FMC Bank1 NOR/SRAM3 + * @arg FMC_Bank1_NORSRAM4: FMC Bank1 NOR/SRAM4 + * @param NewState: new state of the FMC_Bank. This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void FMC_NORSRAMCmd(uint32_t FMC_Bank, FunctionalState NewState) +{ + assert_param(IS_FMC_NORSRAM_BANK(FMC_Bank)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected NOR/SRAM Bank by setting the PBKEN bit in the BCRx register */ + FMC_Bank1->BTCR[FMC_Bank] |= BCR_MBKEN_SET; + } + else + { + /* Disable the selected NOR/SRAM Bank by clearing the PBKEN bit in the BCRx register */ + FMC_Bank1->BTCR[FMC_Bank] &= BCR_MBKEN_RESET; + } +} +/** + * @} + */ + +/** @defgroup FMC_Group2 NAND Controller functions + * @brief NAND Controller functions + * +@verbatim + =============================================================================== + ##### NAND Controller functions ##### + =============================================================================== + + [..] The following sequence should be followed to configure the FMC to interface + with 8-bit or 16-bit NAND memory connected to the NAND Bank: + + (#) Enable the clock for the FMC and associated GPIOs using the following functions: + (++) RCC_AHB3PeriphClockCmd(RCC_AHB3Periph_FMC, ENABLE); + (++) RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOx, ENABLE); + + (#) FMC pins configuration + (++) Connect the involved FMC pins to AF12 using the following function + GPIO_PinAFConfig(GPIOx, GPIO_PinSourcex, GPIO_AF_FMC); + (++) Configure these FMC pins in alternate function mode by calling the function + GPIO_Init(); + + (#) Declare a FMC_NANDInitTypeDef structure, for example: + FMC_NANDInitTypeDef FMC_NANDInitStructure; + and fill the FMC_NANDInitStructure variable with the allowed values of + the structure member. + + (#) Initialize the NAND Controller by calling the function + FMC_NANDInit(&FMC_NANDInitStructure); + + (#) Then enable the NAND Bank, for example: + FMC_NANDCmd(FMC_Bank3_NAND, ENABLE); + + (#) At this stage you can read/write from/to the memory connected to the NAND Bank. + + [..] + (@) To enable the Error Correction Code (ECC), you have to use the function + FMC_NANDECCCmd(FMC_Bank3_NAND, ENABLE); + [..] + (@) and to get the current ECC value you have to use the function + ECCval = FMC_GetECC(FMC_Bank3_NAND); + +@endverbatim + * @{ + */ + +/** + * @brief De-initializes the FMC NAND Banks registers to their default reset values. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @retval None + */ +void FMC_NANDDeInit(uint32_t FMC_Bank) +{ + /* Check the parameter */ + assert_param(IS_FMC_NAND_BANK(FMC_Bank)); + + if(FMC_Bank == FMC_Bank2_NAND) + { + /* Set the FMC_Bank2 registers to their reset values */ + FMC_Bank2->PCR2 = 0x00000018; + FMC_Bank2->SR2 = 0x00000040; + FMC_Bank2->PMEM2 = 0xFCFCFCFC; + FMC_Bank2->PATT2 = 0xFCFCFCFC; + } + /* FMC_Bank3_NAND */ + else + { + /* Set the FMC_Bank3 registers to their reset values */ + FMC_Bank3->PCR3 = 0x00000018; + FMC_Bank3->SR3 = 0x00000040; + FMC_Bank3->PMEM3 = 0xFCFCFCFC; + FMC_Bank3->PATT3 = 0xFCFCFCFC; + } +} + +/** + * @brief Initializes the FMC NAND Banks according to the specified parameters + * in the FMC_NANDInitStruct. + * @param FMC_NANDInitStruct : pointer to a FMC_NANDInitTypeDef structure that + * contains the configuration information for the FMC NAND specified Banks. + * @retval None + */ +void FMC_NANDInit(FMC_NANDInitTypeDef* FMC_NANDInitStruct) +{ + uint32_t tmppcr = 0x00000000, tmppmem = 0x00000000, tmppatt = 0x00000000; + + /* Check the parameters */ + assert_param(IS_FMC_NAND_BANK(FMC_NANDInitStruct->FMC_Bank)); + assert_param(IS_FMC_WAIT_FEATURE(FMC_NANDInitStruct->FMC_Waitfeature)); + assert_param(IS_FMC_NAND_MEMORY_WIDTH(FMC_NANDInitStruct->FMC_MemoryDataWidth)); + assert_param(IS_FMC_ECC_STATE(FMC_NANDInitStruct->FMC_ECC)); + assert_param(IS_FMC_ECCPAGE_SIZE(FMC_NANDInitStruct->FMC_ECCPageSize)); + assert_param(IS_FMC_TCLR_TIME(FMC_NANDInitStruct->FMC_TCLRSetupTime)); + assert_param(IS_FMC_TAR_TIME(FMC_NANDInitStruct->FMC_TARSetupTime)); + assert_param(IS_FMC_SETUP_TIME(FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_SetupTime)); + assert_param(IS_FMC_WAIT_TIME(FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_WaitSetupTime)); + assert_param(IS_FMC_HOLD_TIME(FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HoldSetupTime)); + assert_param(IS_FMC_HIZ_TIME(FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HiZSetupTime)); + assert_param(IS_FMC_SETUP_TIME(FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_SetupTime)); + assert_param(IS_FMC_WAIT_TIME(FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_WaitSetupTime)); + assert_param(IS_FMC_HOLD_TIME(FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HoldSetupTime)); + assert_param(IS_FMC_HIZ_TIME(FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HiZSetupTime)); + + /* Set the tmppcr value according to FMC_NANDInitStruct parameters */ + tmppcr = (uint32_t)FMC_NANDInitStruct->FMC_Waitfeature | + PCR_MEMORYTYPE_NAND | + FMC_NANDInitStruct->FMC_MemoryDataWidth | + FMC_NANDInitStruct->FMC_ECC | + FMC_NANDInitStruct->FMC_ECCPageSize | + (FMC_NANDInitStruct->FMC_TCLRSetupTime << 9 )| + (FMC_NANDInitStruct->FMC_TARSetupTime << 13); + + /* Set tmppmem value according to FMC_CommonSpaceTimingStructure parameters */ + tmppmem = (uint32_t)FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_SetupTime | + (FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_WaitSetupTime << 8) | + (FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HoldSetupTime << 16)| + (FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HiZSetupTime << 24); + + /* Set tmppatt value according to FMC_AttributeSpaceTimingStructure parameters */ + tmppatt = (uint32_t)FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_SetupTime | + (FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_WaitSetupTime << 8) | + (FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HoldSetupTime << 16)| + (FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HiZSetupTime << 24); + + if(FMC_NANDInitStruct->FMC_Bank == FMC_Bank2_NAND) + { + /* FMC_Bank2_NAND registers configuration */ + FMC_Bank2->PCR2 = tmppcr; + FMC_Bank2->PMEM2 = tmppmem; + FMC_Bank2->PATT2 = tmppatt; + } + else + { + /* FMC_Bank3_NAND registers configuration */ + FMC_Bank3->PCR3 = tmppcr; + FMC_Bank3->PMEM3 = tmppmem; + FMC_Bank3->PATT3 = tmppatt; + } +} + + +/** + * @brief Fills each FMC_NANDInitStruct member with its default value. + * @param FMC_NANDInitStruct: pointer to a FMC_NANDInitTypeDef structure which + * will be initialized. + * @retval None + */ +void FMC_NANDStructInit(FMC_NANDInitTypeDef* FMC_NANDInitStruct) +{ + /* Reset NAND Init structure parameters values */ + FMC_NANDInitStruct->FMC_Bank = FMC_Bank2_NAND; + FMC_NANDInitStruct->FMC_Waitfeature = FMC_Waitfeature_Disable; + FMC_NANDInitStruct->FMC_MemoryDataWidth = FMC_NAND_MemoryDataWidth_16b; + FMC_NANDInitStruct->FMC_ECC = FMC_ECC_Disable; + FMC_NANDInitStruct->FMC_ECCPageSize = FMC_ECCPageSize_256Bytes; + FMC_NANDInitStruct->FMC_TCLRSetupTime = 0x0; + FMC_NANDInitStruct->FMC_TARSetupTime = 0x0; + FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_SetupTime = 252; + FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_WaitSetupTime = 252; + FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HoldSetupTime = 252; + FMC_NANDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HiZSetupTime = 252; + FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_SetupTime = 252; + FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_WaitSetupTime = 252; + FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HoldSetupTime = 252; + FMC_NANDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HiZSetupTime = 252; +} + +/** + * @brief Enables or disables the specified NAND Memory Bank. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @param NewState: new state of the FMC_Bank. This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void FMC_NANDCmd(uint32_t FMC_Bank, FunctionalState NewState) +{ + assert_param(IS_FMC_NAND_BANK(FMC_Bank)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected NAND Bank by setting the PBKEN bit in the PCRx register */ + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->PCR2 |= PCR_PBKEN_SET; + } + else + { + FMC_Bank3->PCR3 |= PCR_PBKEN_SET; + } + } + else + { + /* Disable the selected NAND Bank by clearing the PBKEN bit in the PCRx register */ + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->PCR2 &= PCR_PBKEN_RESET; + } + else + { + FMC_Bank3->PCR3 &= PCR_PBKEN_RESET; + } + } +} +/** + * @brief Enables or disables the FMC NAND ECC feature. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @param NewState: new state of the FMC NAND ECC feature. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void FMC_NANDECCCmd(uint32_t FMC_Bank, FunctionalState NewState) +{ + assert_param(IS_FMC_NAND_BANK(FMC_Bank)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected NAND Bank ECC function by setting the ECCEN bit in the PCRx register */ + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->PCR2 |= PCR_ECCEN_SET; + } + else + { + FMC_Bank3->PCR3 |= PCR_ECCEN_SET; + } + } + else + { + /* Disable the selected NAND Bank ECC function by clearing the ECCEN bit in the PCRx register */ + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->PCR2 &= PCR_ECCEN_RESET; + } + else + { + FMC_Bank3->PCR3 &= PCR_ECCEN_RESET; + } + } +} + +/** + * @brief Returns the error correction code register value. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @retval The Error Correction Code (ECC) value. + */ +uint32_t FMC_GetECC(uint32_t FMC_Bank) +{ + uint32_t eccval = 0x00000000; + + if(FMC_Bank == FMC_Bank2_NAND) + { + /* Get the ECCR2 register value */ + eccval = FMC_Bank2->ECCR2; + } + else + { + /* Get the ECCR3 register value */ + eccval = FMC_Bank3->ECCR3; + } + /* Return the error correction code value */ + return(eccval); +} +/** + * @} + */ + +/** @defgroup FMC_Group3 PCCARD Controller functions + * @brief PCCARD Controller functions + * +@verbatim + =============================================================================== + ##### PCCARD Controller functions ##### + =============================================================================== + + [..] he following sequence should be followed to configure the FMC to interface + with 16-bit PC Card compatible memory connected to the PCCARD Bank: + + (#) Enable the clock for the FMC and associated GPIOs using the following functions: + (++) RCC_AHB3PeriphClockCmd(RCC_AHB3Periph_FMC, ENABLE); + (++) RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOx, ENABLE); + + (#) FMC pins configuration + (++) Connect the involved FMC pins to AF12 using the following function + GPIO_PinAFConfig(GPIOx, GPIO_PinSourcex, GPIO_AF_FMC); + (++) Configure these FMC pins in alternate function mode by calling the function + GPIO_Init(); + + (#) Declare a FMC_PCCARDInitTypeDef structure, for example: + FMC_PCCARDInitTypeDef FMC_PCCARDInitStructure; + and fill the FMC_PCCARDInitStructure variable with the allowed values of + the structure member. + + (#) Initialize the PCCARD Controller by calling the function + FMC_PCCARDInit(&FMC_PCCARDInitStructure); + + (#) Then enable the PCCARD Bank: + FMC_PCCARDCmd(ENABLE); + + (#) At this stage you can read/write from/to the memory connected to the PCCARD Bank. + +@endverbatim + * @{ + */ + +/** + * @brief De-initializes the FMC PCCARD Bank registers to their default reset values. + * @param None + * @retval None + */ +void FMC_PCCARDDeInit(void) +{ + /* Set the FMC_Bank4 registers to their reset values */ + FMC_Bank4->PCR4 = 0x00000018; + FMC_Bank4->SR4 = 0x00000000; + FMC_Bank4->PMEM4 = 0xFCFCFCFC; + FMC_Bank4->PATT4 = 0xFCFCFCFC; + FMC_Bank4->PIO4 = 0xFCFCFCFC; +} + +/** + * @brief Initializes the FMC PCCARD Bank according to the specified parameters + * in the FMC_PCCARDInitStruct. + * @param FMC_PCCARDInitStruct : pointer to a FMC_PCCARDInitTypeDef structure + * that contains the configuration information for the FMC PCCARD Bank. + * @retval None + */ +void FMC_PCCARDInit(FMC_PCCARDInitTypeDef* FMC_PCCARDInitStruct) +{ + /* Check the parameters */ + assert_param(IS_FMC_WAIT_FEATURE(FMC_PCCARDInitStruct->FMC_Waitfeature)); + assert_param(IS_FMC_TCLR_TIME(FMC_PCCARDInitStruct->FMC_TCLRSetupTime)); + assert_param(IS_FMC_TAR_TIME(FMC_PCCARDInitStruct->FMC_TARSetupTime)); + + assert_param(IS_FMC_SETUP_TIME(FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_SetupTime)); + assert_param(IS_FMC_WAIT_TIME(FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_WaitSetupTime)); + assert_param(IS_FMC_HOLD_TIME(FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HoldSetupTime)); + assert_param(IS_FMC_HIZ_TIME(FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HiZSetupTime)); + + assert_param(IS_FMC_SETUP_TIME(FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_SetupTime)); + assert_param(IS_FMC_WAIT_TIME(FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_WaitSetupTime)); + assert_param(IS_FMC_HOLD_TIME(FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HoldSetupTime)); + assert_param(IS_FMC_HIZ_TIME(FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HiZSetupTime)); + assert_param(IS_FMC_SETUP_TIME(FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_SetupTime)); + assert_param(IS_FMC_WAIT_TIME(FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_WaitSetupTime)); + assert_param(IS_FMC_HOLD_TIME(FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_HoldSetupTime)); + assert_param(IS_FMC_HIZ_TIME(FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_HiZSetupTime)); + + /* Set the PCR4 register value according to FMC_PCCARDInitStruct parameters */ + FMC_Bank4->PCR4 = (uint32_t)FMC_PCCARDInitStruct->FMC_Waitfeature | + FMC_NAND_MemoryDataWidth_16b | + (FMC_PCCARDInitStruct->FMC_TCLRSetupTime << 9) | + (FMC_PCCARDInitStruct->FMC_TARSetupTime << 13); + + /* Set PMEM4 register value according to FMC_CommonSpaceTimingStructure parameters */ + FMC_Bank4->PMEM4 = (uint32_t)FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_SetupTime | + (FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_WaitSetupTime << 8) | + (FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HoldSetupTime << 16)| + (FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HiZSetupTime << 24); + + /* Set PATT4 register value according to FMC_AttributeSpaceTimingStructure parameters */ + FMC_Bank4->PATT4 = (uint32_t)FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_SetupTime | + (FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_WaitSetupTime << 8) | + (FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HoldSetupTime << 16)| + (FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HiZSetupTime << 24); + + /* Set PIO4 register value according to FMC_IOSpaceTimingStructure parameters */ + FMC_Bank4->PIO4 = (uint32_t)FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_SetupTime | + (FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_WaitSetupTime << 8) | + (FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_HoldSetupTime << 16)| + (FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_HiZSetupTime << 24); +} + +/** + * @brief Fills each FMC_PCCARDInitStruct member with its default value. + * @param FMC_PCCARDInitStruct: pointer to a FMC_PCCARDInitTypeDef structure + * which will be initialized. + * @retval None + */ +void FMC_PCCARDStructInit(FMC_PCCARDInitTypeDef* FMC_PCCARDInitStruct) +{ + /* Reset PCCARD Init structure parameters values */ + FMC_PCCARDInitStruct->FMC_Waitfeature = FMC_Waitfeature_Disable; + FMC_PCCARDInitStruct->FMC_TCLRSetupTime = 0; + FMC_PCCARDInitStruct->FMC_TARSetupTime = 0; + FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_SetupTime = 252; + FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_WaitSetupTime = 252; + FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HoldSetupTime = 252; + FMC_PCCARDInitStruct->FMC_CommonSpaceTimingStruct->FMC_HiZSetupTime = 252; + FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_SetupTime = 252; + FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_WaitSetupTime = 252; + FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HoldSetupTime = 252; + FMC_PCCARDInitStruct->FMC_AttributeSpaceTimingStruct->FMC_HiZSetupTime = 252; + FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_SetupTime = 252; + FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_WaitSetupTime = 252; + FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_HoldSetupTime = 252; + FMC_PCCARDInitStruct->FMC_IOSpaceTimingStruct->FMC_HiZSetupTime = 252; +} + +/** + * @brief Enables or disables the PCCARD Memory Bank. + * @param NewState: new state of the PCCARD Memory Bank. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void FMC_PCCARDCmd(FunctionalState NewState) +{ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the PCCARD Bank by setting the PBKEN bit in the PCR4 register */ + FMC_Bank4->PCR4 |= PCR_PBKEN_SET; + } + else + { + /* Disable the PCCARD Bank by clearing the PBKEN bit in the PCR4 register */ + FMC_Bank4->PCR4 &= PCR_PBKEN_RESET; + } +} + +/** + * @} + */ + +/** @defgroup FMC_Group4 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified FMC interrupts. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @arg FMC_Bank4_PCCARD: FMC Bank4 PCCARD + * @arg FMC_Bank1_SDRAM: FMC Bank1 SDRAM + * @arg FMC_Bank2_SDRAM: FMC Bank2 SDRAM + * @param FMC_IT: specifies the FMC interrupt sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg FMC_IT_RisingEdge: Rising edge detection interrupt. + * @arg FMC_IT_Level: Level edge detection interrupt. + * @arg FMC_IT_FallingEdge: Falling edge detection interrupt. + * @param NewState: new state of the specified FMC interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void FMC_ITConfig(uint32_t FMC_Bank, uint32_t FMC_IT, FunctionalState NewState) +{ + assert_param(IS_FMC_IT_BANK(FMC_Bank)); + assert_param(IS_FMC_IT(FMC_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected FMC_Bank2 interrupts */ + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->SR2 |= FMC_IT; + } + /* Enable the selected FMC_Bank3 interrupts */ + else if (FMC_Bank == FMC_Bank3_NAND) + { + FMC_Bank3->SR3 |= FMC_IT; + } + /* Enable the selected FMC_Bank4 interrupts */ + else + { + FMC_Bank4->SR4 |= FMC_IT; + } + } + else + { + /* Disable the selected FMC_Bank2 interrupts */ + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->SR2 &= (uint32_t)~FMC_IT; + } + /* Disable the selected FMC_Bank3 interrupts */ + else if (FMC_Bank == FMC_Bank3_NAND) + { + FMC_Bank3->SR3 &= (uint32_t)~FMC_IT; + } + /* Disable the selected FMC_Bank4 interrupts */ + else + { + FMC_Bank4->SR4 &= (uint32_t)~FMC_IT; + } + } +} + +/** + * @brief Checks whether the specified FMC flag is set or not. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @arg FMC_Bank4_PCCARD: FMC Bank4 PCCARD + * @param FMC_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg FMC_FLAG_RisingEdge: Rising edge detection Flag. + * @arg FMC_FLAG_Level: Level detection Flag. + * @arg FMC_FLAG_FallingEdge: Falling edge detection Flag. + * @arg FMC_FLAG_FEMPT: Fifo empty Flag. + * @retval The new state of FMC_FLAG (SET or RESET). + */ +FlagStatus FMC_GetFlagStatus(uint32_t FMC_Bank, uint32_t FMC_FLAG) +{ + FlagStatus bitstatus = RESET; + uint32_t tmpsr = 0x00000000; + + /* Check the parameters */ + assert_param(IS_FMC_GETFLAG_BANK(FMC_Bank)); + assert_param(IS_FMC_GET_FLAG(FMC_FLAG)); + + if(FMC_Bank == FMC_Bank2_NAND) + { + tmpsr = FMC_Bank2->SR2; + } + else if(FMC_Bank == FMC_Bank3_NAND) + { + tmpsr = FMC_Bank3->SR3; + } + else + { + tmpsr = FMC_Bank4->SR4; + } + + /* Get the flag status */ + if ((tmpsr & FMC_FLAG) != FMC_FLAG ) + { + bitstatus = RESET; + } + else + { + bitstatus = SET; + } + /* Return the flag status */ + return bitstatus; +} + +/** + * @brief Clears the FMC's pending flags. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @arg FMC_Bank4_PCCARD: FMC Bank4 PCCARD + * @param FMC_FLAG: specifies the flag to clear. + * This parameter can be any combination of the following values: + * @arg FMC_FLAG_RisingEdge: Rising edge detection Flag. + * @arg FMC_FLAG_Level: Level detection Flag. + * @arg FMC_FLAG_FallingEdge: Falling edge detection Flag. + * @retval None + */ +void FMC_ClearFlag(uint32_t FMC_Bank, uint32_t FMC_FLAG) +{ + /* Check the parameters */ + assert_param(IS_FMC_GETFLAG_BANK(FMC_Bank)); + assert_param(IS_FMC_CLEAR_FLAG(FMC_FLAG)) ; + + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->SR2 &= (~FMC_FLAG); + } + else if(FMC_Bank == FMC_Bank3_NAND) + { + FMC_Bank3->SR3 &= (~FMC_FLAG); + } + /* FMC_Bank4 PCCARD */ + else + { + FMC_Bank4->SR4 &= (~FMC_FLAG); + } + +} + +/** + * @brief Checks whether the specified FMC interrupt has occurred or not. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @arg FMC_Bank4_PCCARD: FMC Bank4 PCCARD + * @param FMC_IT: specifies the FMC interrupt source to check. + * This parameter can be one of the following values: + * @arg FMC_IT_RisingEdge: Rising edge detection interrupt. + * @arg FMC_IT_Level: Level edge detection interrupt. + * @arg FMC_IT_FallingEdge: Falling edge detection interrupt. + * @retval The new state of FMC_IT (SET or RESET). + */ +ITStatus FMC_GetITStatus(uint32_t FMC_Bank, uint32_t FMC_IT) +{ + ITStatus bitstatus = RESET; + uint32_t tmpsr = 0; + uint32_t itstatus = 0; + uint32_t itenable = 0; + + /* Check the parameters */ + assert_param(IS_FMC_IT_BANK(FMC_Bank)); + assert_param(IS_FMC_GET_IT(FMC_IT)); + + if(FMC_Bank == FMC_Bank2_NAND) + { + tmpsr = FMC_Bank2->SR2; + } + else if(FMC_Bank == FMC_Bank3_NAND) + { + tmpsr = FMC_Bank3->SR3; + } + /* FMC_Bank4 PCCARD */ + else + { + tmpsr = FMC_Bank4->SR4; + } + + /* get the IT enable bit status*/ + itenable = tmpsr & FMC_IT; + + /* get the corresponding IT Flag status*/ + itstatus = tmpsr & (FMC_IT >> 3); + + if ((itstatus != (uint32_t)RESET) && (itenable != (uint32_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the FMC's interrupt pending bits. + * @param FMC_Bank: specifies the FMC Bank to be used + * This parameter can be one of the following values: + * @arg FMC_Bank2_NAND: FMC Bank2 NAND + * @arg FMC_Bank3_NAND: FMC Bank3 NAND + * @arg FMC_Bank4_PCCARD: FMC Bank4 PCCARD + * @param FMC_IT: specifies the interrupt pending bit to clear. + * This parameter can be any combination of the following values: + * @arg FMC_IT_RisingEdge: Rising edge detection interrupt. + * @arg FMC_IT_Level: Level edge detection interrupt. + * @arg FMC_IT_FallingEdge: Falling edge detection interrupt. + * @retval None + */ +void FMC_ClearITPendingBit(uint32_t FMC_Bank, uint32_t FMC_IT) +{ + /* Check the parameters */ + assert_param(IS_FMC_IT_BANK(FMC_Bank)); + assert_param(IS_FMC_IT(FMC_IT)); + + if(FMC_Bank == FMC_Bank2_NAND) + { + FMC_Bank2->SR2 &= ~(FMC_IT >> 3); + } + else if(FMC_Bank == FMC_Bank3_NAND) + { + FMC_Bank3->SR3 &= ~(FMC_IT >> 3); + } + /* FMC_Bank4 PCCARD */ + else + { + FMC_Bank4->SR4 &= ~(FMC_IT >> 3); + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_gpio.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_gpio.c new file mode 100755 index 0000000000..c8d8eabb21 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_gpio.c @@ -0,0 +1,545 @@ +/** + ****************************************************************************** + * @file stm32f30x_gpio.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the GPIO peripheral: + * + Initialization and Configuration functions + * + GPIO Read and Write functions + * + GPIO Alternate functions configuration functions + * + * @verbatim + + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] + (#) Enable the GPIO AHB clock using RCC_AHBPeriphClockCmd() + (#) Configure the GPIO pin(s) using GPIO_Init() + Four possible configuration are available for each pin: + (++) Input: Floating, Pull-up, Pull-down. + (++) Output: Push-Pull (Pull-up, Pull-down or no Pull), + Open Drain (Pull-up, Pull-down or no Pull). + In output mode, the speed is configurable: Low, Medium, Fast or High. + (++) Alternate Function: Push-Pull (Pull-up, Pull-down or no Pull), + Open Drain (Pull-up, Pull-down or no Pull). + (++) Analog: required mode when a pin is to be used as ADC channel, + DAC output or comparator input. + (#) Peripherals alternate function: + (++) For ADC, DAC and comparators, configure the desired pin in + analog mode using GPIO_InitStruct->GPIO_Mode = GPIO_Mode_AN + (++) For other peripherals (TIM, USART...): + (+++) Connect the pin to the desired peripherals' Alternate + Function (AF) using GPIO_PinAFConfig() function. + (+++) Configure the desired pin in alternate function mode using + GPIO_InitStruct->GPIO_Mode = GPIO_Mode_AF + (+++) Select the type, pull-up/pull-down and output speed via + GPIO_PuPd, GPIO_OType and GPIO_Speed members. + (+++) Call GPIO_Init() function. + (#) To get the level of a pin configured in input mode use GPIO_ReadInputDataBit() + (#) To set/reset the level of a pin configured in output mode use + GPIO_SetBits()/GPIO_ResetBits() + (#) During and just after reset, the alternate functions are not active + and the GPIO pins are configured in input floating mode (except JTAG pins). + (#) The LSE oscillator pins OSC32_IN and OSC32_OUT can be used as + general-purpose (PC14 and PC15, respectively) when the LSE + oscillator is off. The LSE has priority over the GPIO function. + (#) The HSE oscillator pins OSC_IN/OSC_OUT can be used as general-purpose + (PF0 and PF1 respectively) when the HSE oscillator is off. The HSE has + the priority over the GPIO function. + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_gpio.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup GPIO + * @brief GPIO driver modules + * @{ + */ + + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup GPIO_Private_Functions + * @{ + */ + +/** @defgroup GPIO_Group1 Initialization and Configuration + * @brief Initialization and Configuration + * +@verbatim + =============================================================================== + ##### Initialization and Configuration ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the GPIOx peripheral registers to their default reset + * values. + * @param GPIOx: where x can be (A, B, C, D, E or F) to select the GPIO peripheral. + * @retval None + */ +void GPIO_DeInit(GPIO_TypeDef* GPIOx) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + + if(GPIOx == GPIOA) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOA, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOA, DISABLE); + } + else if(GPIOx == GPIOB) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOB, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOB, DISABLE); + } + else if(GPIOx == GPIOC) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOC, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOC, DISABLE); + } + else if(GPIOx == GPIOD) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOD, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOD, DISABLE); + } + else if(GPIOx == GPIOE) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOE, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOE, DISABLE); + } + else if(GPIOx == GPIOF) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOF, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOF, DISABLE); + } + else if(GPIOx == GPIOG) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOG, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOG, DISABLE); + } + else + { + if(GPIOx == GPIOH) + { + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOH, ENABLE); + RCC_AHBPeriphResetCmd(RCC_AHBPeriph_GPIOH, DISABLE); + } + } +} + +/** + * @brief Initializes the GPIOx peripheral according to the specified + * parameters in the GPIO_InitStruct. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_InitStruct: pointer to a GPIO_InitTypeDef structure that + * contains the configuration information for the specified GPIO + * peripheral. + * @note GPIO_Pin: selects the pin to be configured: + * GPIO_Pin_0->GPIO_Pin_15 for GPIOA, GPIOB, GPIOC, GPIOD and GPIOE; + * GPIO_Pin_0->GPIO_Pin_2, GPIO_Pin_4, GPIO_Pin_6, GPIO_Pin_9 + * and GPIO_Pin_10 for GPIOF. + * @retval None + */ +void GPIO_Init(GPIO_TypeDef* GPIOx, GPIO_InitTypeDef* GPIO_InitStruct) +{ + uint32_t pinpos = 0x00, pos = 0x00 , currentpin = 0x00; + uint32_t tmpreg = 0x00; + + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GPIO_PIN(GPIO_InitStruct->GPIO_Pin)); + assert_param(IS_GPIO_MODE(GPIO_InitStruct->GPIO_Mode)); + assert_param(IS_GPIO_PUPD(GPIO_InitStruct->GPIO_PuPd)); + + /*-------------------------- Configure the port pins -----------------------*/ + /*-- GPIO Mode Configuration --*/ + for (pinpos = 0x00; pinpos < 0x10; pinpos++) + { + pos = ((uint32_t)0x01) << pinpos; + + /* Get the port pins position */ + currentpin = (GPIO_InitStruct->GPIO_Pin) & pos; + + if (currentpin == pos) + { + if ((GPIO_InitStruct->GPIO_Mode == GPIO_Mode_OUT) || (GPIO_InitStruct->GPIO_Mode == GPIO_Mode_AF)) + { + /* Check Speed mode parameters */ + assert_param(IS_GPIO_SPEED(GPIO_InitStruct->GPIO_Speed)); + + /* Speed mode configuration */ + GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2)); + GPIOx->OSPEEDR |= ((uint32_t)(GPIO_InitStruct->GPIO_Speed) << (pinpos * 2)); + + /* Check Output mode parameters */ + assert_param(IS_GPIO_OTYPE(GPIO_InitStruct->GPIO_OType)); + + /* Output mode configuration */ + GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos)); + GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_InitStruct->GPIO_OType) << ((uint16_t)pinpos)); + } + + GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2)); + + GPIOx->MODER |= (((uint32_t)GPIO_InitStruct->GPIO_Mode) << (pinpos * 2)); + + /* Use temporary variable to update PUPDR register configuration, to avoid + unexpected transition in the GPIO pin configuration. */ + tmpreg = GPIOx->PUPDR; + tmpreg &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2)); + tmpreg |= (((uint32_t)GPIO_InitStruct->GPIO_PuPd) << (pinpos * 2)); + GPIOx->PUPDR = tmpreg; + } + } +} + +/** + * @brief Fills each GPIO_InitStruct member with its default value. + * @param GPIO_InitStruct: pointer to a GPIO_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void GPIO_StructInit(GPIO_InitTypeDef* GPIO_InitStruct) +{ + /* Reset GPIO init structure parameters values */ + GPIO_InitStruct->GPIO_Pin = GPIO_Pin_All; + GPIO_InitStruct->GPIO_Mode = GPIO_Mode_IN; + GPIO_InitStruct->GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStruct->GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct->GPIO_PuPd = GPIO_PuPd_NOPULL; +} + +/** + * @brief Locks GPIO Pins configuration registers. + * The locked registers are GPIOx_MODER, GPIOx_OTYPER, GPIOx_OSPEEDR, + * GPIOx_PUPDR, GPIOx_AFRL and GPIOx_AFRH. + * @note The configuration of the locked GPIO pins can no longer be modified + * until the next reset. + * @param GPIOx: where x can be (A or B or D) to select the GPIO peripheral. + * @param GPIO_Pin: specifies the port bit to be written. + * This parameter can be any combination of GPIO_Pin_x where x can be (0..15). + * @retval None + */ +void GPIO_PinLockConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + uint32_t tmp = 0x00010000; + + /* Check the parameters */ + assert_param(IS_GPIO_LIST_PERIPH(GPIOx)); + assert_param(IS_GPIO_PIN(GPIO_Pin)); + + tmp |= GPIO_Pin; + /* Set LCKK bit */ + GPIOx->LCKR = tmp; + /* Reset LCKK bit */ + GPIOx->LCKR = GPIO_Pin; + /* Set LCKK bit */ + GPIOx->LCKR = tmp; + /* Read LCKK bit */ + tmp = GPIOx->LCKR; + /* Read LCKK bit */ + tmp = GPIOx->LCKR; +} + +/** + * @} + */ + +/** @defgroup GPIO_Group2 GPIO Read and Write + * @brief GPIO Read and Write + * +@verbatim + =============================================================================== + ##### GPIO Read and Write ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Reads the specified input port pin. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_Pin: specifies the port bit to read. + * @note This parameter can be GPIO_Pin_x where x can be : + * (0..15) for GPIOA, GPIOB, GPIOC, GPIOD or GPIOE; + * (0..2, 4, 6, 9..10) for GPIOF. + * @retval The input port pin value. + */ +uint8_t GPIO_ReadInputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + uint8_t bitstatus = 0x00; + + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GET_GPIO_PIN(GPIO_Pin)); + + if ((GPIOx->IDR & GPIO_Pin) != (uint32_t)Bit_RESET) + { + bitstatus = (uint8_t)Bit_SET; + } + else + { + bitstatus = (uint8_t)Bit_RESET; + } + return bitstatus; +} + +/** + * @brief Reads the specified input port pin. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @retval The input port pin value. + */ +uint16_t GPIO_ReadInputData(GPIO_TypeDef* GPIOx) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + + return ((uint16_t)GPIOx->IDR); +} + +/** + * @brief Reads the specified output data port bit. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_Pin: Specifies the port bit to read. + * @note This parameter can be GPIO_Pin_x where x can be : + * (0..15) for GPIOA, GPIOB, GPIOC, GPIOD or GPIOE; + * (0..2, 4, 6, 9..10) for GPIOF. + * @retval The output port pin value. + */ +uint8_t GPIO_ReadOutputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + uint8_t bitstatus = 0x00; + + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GET_GPIO_PIN(GPIO_Pin)); + + if ((GPIOx->ODR & GPIO_Pin) != (uint32_t)Bit_RESET) + { + bitstatus = (uint8_t)Bit_SET; + } + else + { + bitstatus = (uint8_t)Bit_RESET; + } + return bitstatus; +} + +/** + * @brief Reads the specified GPIO output data port. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @retval GPIO output data port value. + */ +uint16_t GPIO_ReadOutputData(GPIO_TypeDef* GPIOx) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + + return ((uint16_t)GPIOx->ODR); +} + +/** + * @brief Sets the selected data port bits. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_Pin: specifies the port bits to be written. + * @note This parameter can be GPIO_Pin_x where x can be : + * (0..15) for GPIOA, GPIOB, GPIOC, GPIOD or GPIOE; + * (0..2, 4, 6, 9..10) for GPIOF. + * @retval None + */ +void GPIO_SetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GPIO_PIN(GPIO_Pin)); + + GPIOx->BSRR = GPIO_Pin; +} + +/** + * @brief Clears the selected data port bits. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_Pin: specifies the port bits to be written. + * @note This parameter can be GPIO_Pin_x where x can be : + * (0..15) for GPIOA, GPIOB, GPIOC, GPIOD or GPIOE; + * (0..2, 4, 6, 9..10) for GPIOF. + * @retval None + */ +void GPIO_ResetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GPIO_PIN(GPIO_Pin)); + + GPIOx->BRR = GPIO_Pin; +} + +/** + * @brief Sets or clears the selected data port bit. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_Pin: specifies the port bit to be written. + * @note This parameter can be GPIO_Pin_x where x can be : + * (0..15) for GPIOA, GPIOB, GPIOC, GPIOD or GPIOE; + * (0..2, 4, 6, 9..10) for GPIOF. + * @param BitVal: specifies the value to be written to the selected bit. + * This parameter can be one of the BitAction enumeration values: + * @arg Bit_RESET: to clear the port pin + * @arg Bit_SET: to set the port pin + * @retval None + */ +void GPIO_WriteBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, BitAction BitVal) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GET_GPIO_PIN(GPIO_Pin)); + assert_param(IS_GPIO_BIT_ACTION(BitVal)); + + if (BitVal != Bit_RESET) + { + GPIOx->BSRR = GPIO_Pin; + } + else + { + GPIOx->BRR = GPIO_Pin ; + } +} + +/** + * @brief Writes data to the specified GPIO data port. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param PortVal: specifies the value to be written to the port output data + * register. + * @retval None + */ +void GPIO_Write(GPIO_TypeDef* GPIOx, uint16_t PortVal) +{ + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + + GPIOx->ODR = PortVal; +} + +/** + * @} + */ + +/** @defgroup GPIO_Group3 GPIO Alternate functions configuration functions + * @brief GPIO Alternate functions configuration functions + * +@verbatim + =============================================================================== + ##### GPIO Alternate functions configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Writes data to the specified GPIO data port. + * @param GPIOx: where x can be (A, B, C, D, E, F, G or H) to select the GPIO peripheral. + * @param GPIO_PinSource: specifies the pin for the Alternate function. + * This parameter can be GPIO_PinSourcex where x can be (0..15). + * @param GPIO_AF: selects the pin to be used as Alternate function. + * This parameter can be one of the following value: + * @arg GPIO_AF_0: JTCK-SWCLK, JTDI, JTDO/TRACESW0, JTMS-SWDAT, MCO, NJTRST, + * TRACED, TRACECK. + * @arg GPIO_AF_1: OUT, TIM2, TIM15, TIM16, TIM17. + * @arg GPIO_AF_2: COMP1_OUT, TIM1, TIM2, TIM3, TIM4, TIM8, TIM15, TIM16. + * @arg GPIO_AF_3: COMP7_OUT, TIM8, TIM15, Touch, HRTIM. + * @arg GPIO_AF_4: I2C1, I2C2, TIM1, TIM8, TIM16, TIM17. + * @arg GPIO_AF_5: IR_OUT, I2S2, I2S3, SPI1, SPI2, TIM8, USART4, USART5 + * @arg GPIO_AF_6: IR_OUT, I2S2, I2S3, SPI2, SPI3, TIM1, TIM8 + * @arg GPIO_AF_7: AOP2_OUT, CAN, COMP3_OUT, COMP5_OUT, COMP6_OUT, USART1, + * USART2, USART3. + * @arg GPIO_AF_8: COMP1_OUT, COMP2_OUT, COMP3_OUT, COMP4_OUT, COMP5_OUT, + * COMP6_OUT. + * @arg GPIO_AF_9: AOP4_OUT, CAN, TIM1, TIM8, TIM15. + * @arg GPIO_AF_10: AOP1_OUT, AOP3_OUT, TIM2, TIM3, TIM4, TIM8, TIM17. + * @arg GPIO_AF_11: TIM1, TIM8. + * @arg GPIO_AF_12: TIM1, HRTIM. + * @arg GPIO_AF_13: HRTIM, AOP2_OUT. + * @arg GPIO_AF_14: USBDM, USBDP. + * @arg GPIO_AF_15: OUT. + * @note The pin should already been configured in Alternate Function mode(AF) + * using GPIO_InitStruct->GPIO_Mode = GPIO_Mode_AF + * @note Refer to the Alternate function mapping table in the device datasheet + * for the detailed mapping of the system and peripherals alternate + * function I/O pins. + * @retval None + */ +void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF) +{ + uint32_t temp = 0x00; + uint32_t temp_2 = 0x00; + + /* Check the parameters */ + assert_param(IS_GPIO_ALL_PERIPH(GPIOx)); + assert_param(IS_GPIO_PIN_SOURCE(GPIO_PinSource)); + assert_param(IS_GPIO_AF(GPIO_AF)); + + temp = ((uint32_t)(GPIO_AF) << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4)); + GPIOx->AFR[GPIO_PinSource >> 0x03] &= ~((uint32_t)0xF << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4)); + temp_2 = GPIOx->AFR[GPIO_PinSource >> 0x03] | temp; + GPIOx->AFR[GPIO_PinSource >> 0x03] = temp_2; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c new file mode 100755 index 0000000000..40e3bd7a6f --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c @@ -0,0 +1,4103 @@ +/** + ****************************************************************************** + * @file stm32f30x_hrtim.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief HRTIMx module driver. + * + * This file provides firmware functions to manage the following + * functionalities of the HRTIMx peripheral: + * + Initialization/de-initialization methods + * + I/O operation methods + * + Peripheral Control methods + * + @verbatim +================================================================================ + ##### ##### +================================================================================ + + [..] < HRTIM introduction: + (#) The high-resolution timer can generate up to 10 digital signals with + highly accurate timings. + It is primarily intended to drive power conversion systems such as + switch mode power supplies or lighting systems, + but can be of general purpose usage, whenever a very fine timing + resolution is expected. + + (#) Its modular architecture allows to generate either independent or + coupled waveforms. + The wave-shape is defined by self-contained timings + (using counters and compare units) and a broad range of external events, + such as analog or digital feedbacks and synchronisation signals. + This allows to produce a large variety of control signal (PWM, phase-shifted, + constant Ton,...) and address most of conversion topologies. + + (#) For control and monitoring purposes, the timer has also timing measure + capabilities and links to built-in ADC and DAC converters. + Last, it features light-load management mode and is able to handle + various fault schemes for safe shut-down purposes. + + + ##### How to use this driver ##### +================================================================================ + [..] This driver provides functions to configure and program the HRTIM + of all stm32f33x devices. + These functions are split in 9 groups: + + (#) HRTIM Simple TimeBase management: this group includes all needed functions + to configure the HRTIM Timebase unit: + (++) Initializes the HRTIMx timer in simple time base mode + (++) Start/Stop the time base generation + (++) Deinitialize the HRTIM peripheral + + + (#) HRTIM simple Output Compare management: this group includes all needed + functions to configure the Compare unit used in Output compare mode: + (++) Initializes the HRTIMx timer time base unit + (++) Configure the compare unit in simple Output Compare mode + (++) Start/Stop the Output compare generation + + (#) HRTIM simple PWM management: this group includes all needed + functions to configure the Compare unit used in PWM mode: + (++) Initializes the HRTIMx timer time base unit + (++) Configure the compare unit in simple PWM mode + (++) Start/Stop the PWM generation + + (#) HRTIM simple Capture management: this group includes all needed + functions to configure the Capture unit used in Capture mode: + (++) Initializes the HRTIMx timer time base unit + (++) Configure the compare unit in simple Capture mode + (++) Start/Stop the Capture mode + + (#) HRTIM simple One Pulse management: this group includes all needed + functions to configure the Capture unit and Compare unit used in One Pulse mode: + (++) Initializes the HRTIMx timer time base unit + (++) Configure the compare unit and the capture unit in simple One Pulse mode + (++) Start/Stop the One Pulse mode generation + + (#) HRTIM Waveform management: this group includes all needed + functions to configure the HRTIM possible waveform mode: + (++) Initializes the HRTIMx timer Master time base unit + (++) Initializes the HRTIMx timer Slaves time base unit + (++) Configures the HRTIMx timer Compare unit + (++) Configures the HRTIMx Slave timer Capture unit + (++) Configures the HRTIMx timer Output unit + (++) Configures the HRTIMx timer DeadTime / Chopper / Burst features + (++) Configures the HRTIMx timer Fault / External event features + (++) Configures the HRTIMx timer Synchronization features: Internal/External connection, DACs,... + (++) Configures the HRTIMx timer Synchronization features: ADCs Triggers + (++) HRTIMx timer Outputs Start/Stop + (++) Start/Stop the HRTIMx Timer counters + + (#) HRTIM interrupts, DMA and flags management + (++) Enable/Disable interrupt sources + (++) Get flags status + (++) Clear flags/ Pending bits + (++) Enable/Disable DMA requests + (++) Configure DMA burst mode + + (#) TIM specific interface management, this group includes all + needed functions to use the specific TIM interface: + (++) HRTIMx timer DLL calibration + + @endverbatim + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_hrtim.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup HRTIM + * @brief HRTIM driver module + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define HRTIM_FLTR_FLTxEN (HRTIM_FLTR_FLT1EN |\ + HRTIM_FLTR_FLT2EN |\ + HRTIM_FLTR_FLT3EN |\ + HRTIM_FLTR_FLT4EN | \ + HRTIM_FLTR_FLT5EN) + +#define HRTIM_TIMCR_TIMUPDATETRIGGER (HRTIM_TIMUPDATETRIGGER_MASTER |\ + HRTIM_TIMUPDATETRIGGER_TIMER_A |\ + HRTIM_TIMUPDATETRIGGER_TIMER_B |\ + HRTIM_TIMUPDATETRIGGER_TIMER_C |\ + HRTIM_TIMUPDATETRIGGER_TIMER_D |\ + HRTIM_TIMUPDATETRIGGER_TIMER_E) + +#define HRTIM_TIM_OFFSET (uint32_t)0x00000080 +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +static uint32_t TimerIdxToTimerId[] = +{ + HRTIM_TIMERID_TIMER_A, + HRTIM_TIMERID_TIMER_B, + HRTIM_TIMERID_TIMER_C, + HRTIM_TIMERID_TIMER_D, + HRTIM_TIMERID_TIMER_E, + HRTIM_TIMERID_MASTER, +}; + +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ +static void HRTIM_MasterBase_Config(HRTIM_TypeDef* HRTIMx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruc); +static void HRTIM_TimingUnitBase_Config(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct); +static void HRTIM_MasterWaveform_Config(HRTIM_TypeDef * HRTIMx, HRTIM_TimerInitTypeDef * TimerInit); +static void HRTIM_TimingUnitWaveform_Config(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_TimerInitTypeDef * TimerInit); +static void HRTIM_CompareUnitConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CompareUnit, + HRTIM_CompareCfgTypeDef * CompareCfg); +static void HRTIM_CaptureUnitConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit, + uint32_t Event); +static void HRTIM_OutputConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output, + HRTIM_OutputCfgTypeDef * OutputCfg); +static void HRTIM_ExternalEventConfig(HRTIM_TypeDef * HRTIMx, + uint32_t Event, + HRTIM_EventCfgTypeDef * EventCfg); +static void HRTIM_TIM_ResetConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Event); + /** @defgroup HRTIM_Private_Functions + * @{ + */ + +/** @defgroup HRTIM_Group1 Initialization/de-initialization methods + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization/de-initialization methods ##### + =============================================================================== + [..] This section provides functions allowing to: + (+)Initializes timer in basic time base mode + (+)Initializes timer in basic OC mode + (+)Initializes timer in basic PWM mode + (+)Initializes timer in basic Capture mode + (+)Initializes timer in One Pulse mode + (+)Initializes a timer operating in waveform mode + (+)De-initializes the HRTIMx timer + +@endverbatim + * @{ + */ + +/** + * @brief Initializes the HRTIMx timer in basic time base mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 for master timer + * @arg 0x1 to 0x5 for timers A to E + * @note The time-base unit initialization parameters specify: + * The timer counter operating mode (continuous, one shot) + * The timer clock prescaler + * The timer period + * The timer repetition counter. + * @retval None + */ +void HRTIM_SimpleBase_Init(HRTIM_TypeDef* HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + assert_param(IS_HRTIM_MODE(HRTIM_BaseInitStruct->Mode)); + + if (TimerIdx == HRTIM_TIMERINDEX_MASTER) + { + /* Configure master timer */ + HRTIM_MasterBase_Config(HRTIMx, HRTIM_BaseInitStruct); + } + else + { + /* Configure timing unit */ + HRTIM_TimingUnitBase_Config(HRTIMx, TimerIdx, HRTIM_BaseInitStruct); + } +} + +/** + * @brief De-initializes a timer operating in all mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @retval None + */ +void HRTIM_DeInit(__attribute__((unused)) HRTIM_TypeDef* HRTIMx) +{ + /* Check the parameters */ + RCC_APB2PeriphResetCmd(RCC_APB2Periph_HRTIM1, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_HRTIM1, DISABLE); + } + +/** + * @brief Initializes the HRTIMx timer in basic output compare mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x1 to 0x5 for timers A to E + * @note Initializes the time-base unit of the timer and prepare it to + * operate in output compare mode + * @retval None + */ +void HRTIM_SimpleOC_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + assert_param(IS_HRTIM_MODE(HRTIM_BaseInitStruct->Mode)); + + /* Configure timing unit */ + HRTIM_TimingUnitBase_Config(HRTIMx, TimerIdx, HRTIM_BaseInitStruct); +} + +/** + * @brief Initializes the HRTIMx timer in basic PWM mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x1 to 0x5 for timers A to E + * @note Initializes the time-base unit of the timer and prepare it to + * operate in capture mode + * @retval None + */ +void HRTIM_SimplePWM_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + assert_param(IS_HRTIM_MODE(HRTIM_BaseInitStruct->Mode)); + + /* Configure timing unit */ + HRTIM_TimingUnitBase_Config(HRTIMx, TimerIdx, HRTIM_BaseInitStruct); +} + +/** + * @brief Initializes a timer operating in basic capture mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x1 to 0x5 for timers A to E + * @retval None + */ +void HRTIM_SimpleCapture_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + assert_param(IS_HRTIM_MODE(HRTIM_BaseInitStruct->Mode)); + + /* Configure timing unit */ + HRTIM_TimingUnitBase_Config(HRTIMx, TimerIdx, HRTIM_BaseInitStruct); +} + +/** + * @brief Initializes the HRTIMx timer in basic one pulse mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x1 to 0x5 for timers A to E + * @note Initializes the time-base unit of the timer and prepare it to + * operate in one pulse mode. In this mode the counter operates + * in single shot mode (retriggerable or not) + * @retval None + */ +void HRTIM_SimpleOnePulse_Init(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + assert_param(IS_HRTIM_MODE(HRTIM_BaseInitStruct->Mode)); + + /* Configure timing unit */ + HRTIM_TimingUnitBase_Config(HRTIMx, TimerIdx, HRTIM_BaseInitStruct); +} + +/** + * @brief Initializes a timer operating in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 for master timer + * @arg 0x1 to 0x5 for timers A to E + * @param pTimerInit: pointer to the timer initialization data structure + * @retval None + */ +void HRTIM_Waveform_Init(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct, + HRTIM_TimerInitTypeDef* HRTIM_TimerInitStruct) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_HALFMODE(HRTIM_TimerInitStruct->HalfModeEnable)); + assert_param(IS_HRTIM_SYNCSTART(HRTIM_TimerInitStruct->StartOnSync)); + assert_param(IS_HRTIM_SYNCRESET(HRTIM_TimerInitStruct->ResetOnSync)); + assert_param(IS_HRTIM_DACSYNC(HRTIM_TimerInitStruct->DACSynchro)); + assert_param(IS_HRTIM_PRELOAD(HRTIM_TimerInitStruct->PreloadEnable)); + assert_param(IS_HRTIM_TIMERBURSTMODE(HRTIM_TimerInitStruct->BurstMode)); + assert_param(IS_HRTIM_UPDATEONREPETITION(HRTIM_TimerInitStruct->RepetitionUpdate)); + + if (TimerIdx == HRTIM_TIMERINDEX_MASTER) + { + /* Check parameters */ + assert_param(IS_HRTIM_UPDATEGATING_MASTER(HRTIM_TimerInitStruct->UpdateGating)); + + /* Configure master timer */ + HRTIM_MasterBase_Config(HRTIMx, HRTIM_BaseInitStruct); + HRTIM_MasterWaveform_Config(HRTIMx, HRTIM_TimerInitStruct); + } + else + { + /* Check parameters */ + assert_param(IS_HRTIM_UPDATEGATING_TIM(HRTIM_TimerInitStruct->UpdateGating)); + + /* Configure timing unit */ + HRTIM_TimingUnitBase_Config(HRTIMx, TimerIdx, HRTIM_BaseInitStruct); + HRTIM_TimingUnitWaveform_Config(HRTIMx, TimerIdx, HRTIM_TimerInitStruct); + } +} + +/** + * @} + */ + +/** @defgroup HRTIM_Group2 I/O operation methods + * @brief Data transfers functions + * +@verbatim + =============================================================================== + ##### IO operation methods ##### + =============================================================================== + [..] + This subsection provides a set of functions allowing to manage the HRTIMx data + transfers. + (+) Starts the DLL calibration. + (+) Starts / stops the counter of a timer operating in basic time base mode + (+) Starts / stops the output compare signal generation on the designed timer output + (+) Starts / stops the PWM output signal generation on the designed timer output + (+) Enables / disables a basic capture on the designed capture unit + +@endverbatim + * @{ + */ + +/** + * @brief Starts the DLL calibration + * @param HRTIMx: pointer to HRTIMx peripheral + * @param CalibrationRate: DLL calibration period + * This parameter can be one of the following values: + * @arg HRTIM_SINGLE_CALIBRATION: One shot DLL calibration + * @arg HRTIM_CALIBRATIONRATE_7300: 7.3 ms + * @arg HRTIM_CALIBRATIONRATE_910: 910 us + * @arg HRTIM_CALIBRATIONRATE_114: 114 us + * @arg HRTIM_CALIBRATIONRATE_14: 14 us + * @retval None + */ +void HRTIM_DLLCalibrationStart(HRTIM_TypeDef * HRTIMx, uint32_t CalibrationRate) +{ + uint32_t HRTIM_dllcr; + + /* Check the parameters */ + assert_param(IS_HRTIM_CALIBRATIONRATE(CalibrationRate)); + + /* Configure DLL Calibration */ + HRTIM_dllcr = (HRTIMx->HRTIM_COMMON).DLLCR; + + if (CalibrationRate == HRTIM_SINGLE_CALIBRATION) + { + /* One shot DLL calibration */ + HRTIM_dllcr &= ~(HRTIM_DLLCR_CALEN); + HRTIM_dllcr |= HRTIM_DLLCR_CAL; + } + else + { + /* Periodic DLL calibration */ + HRTIM_dllcr &= ~(HRTIM_DLLCR_CALRTE | HRTIM_DLLCR_CAL); + HRTIM_dllcr |= (CalibrationRate | HRTIM_DLLCR_CALEN); + } + + /* Update HRTIMx register */ + HRTIMx->HRTIM_COMMON.DLLCR = HRTIM_dllcr; + +} +/** + * @brief Starts the counter of a timer operating in basic time base mode + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x5 for master timer + * @arg 0x0 to 0x4 for timers A to E + * @retval None + */ +void HRTIM_SimpleBaseStart(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + /* Enable the timer counter */ + __HRTIM_ENABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Stops the counter of a timer operating in basic time base mode + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x5 for master timer + * @arg 0x0 to 0x4 for timers A to E + * @retval None + */ +void HRTIM_SimpleBaseStop(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + /* Disable the timer counter */ + __HRTIM_DISABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Starts the output compare signal generation on the designed timer output + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param OCChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_SimpleOCStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OCChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, OCChannel)); + + /* Enable the timer output */ + (HRTIMx->HRTIM_COMMON).OENR |= OCChannel; + + /* Enable the timer counter */ + __HRTIM_ENABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); + +} + +/** + * @brief Stops the output compare signal generation on the designed timer output + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param OCChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_SimpleOCStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OCChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, OCChannel)); + + /* Disable the timer output */ + HRTIMx->HRTIM_COMMON.DISR |= OCChannel; + + /* Disable the timer counter */ + __HRTIM_DISABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Starts the PWM output signal generation on the designed timer output + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param PWMChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_SimplePWMStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t PWMChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, PWMChannel)); + + /* Enable the timer output */ + HRTIMx->HRTIM_COMMON.OENR |= PWMChannel; + + /* Enable the timer counter */ + __HRTIM_ENABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Stops the PWM output signal generation on the designed timer output + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param PWMChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_SimplePWMStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t PWMChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, PWMChannel)); + + /* Disable the timer output */ + HRTIMx->HRTIM_COMMON.DISR |= PWMChannel; + + /* Disable the timer counter */ + __HRTIM_DISABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Enables a basic capture on the designed capture unit + * @param HRTIMx: pointer to HRTIM peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CaptureChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_CAPTUREUNIT_1: Capture unit 1 + * @arg HRTIM_CAPTUREUNIT_2: Capture unit 2 + * @retval None + * @note The external event triggering the capture is available for all timing + * units. It can be used directly and is active as soon as the timing + * unit counter is enabled. + */ +void HRTIM_SimpleCaptureStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + __attribute__((unused)) uint32_t CaptureChannel) +{ + /* Enable the timer counter */ + __HRTIM_ENABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); + +} + +/** + * @brief Disables a basic capture on the designed capture unit + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CaptureChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_CAPTUREUNIT_1: Capture unit 1 + * @arg HRTIM_CAPTUREUNIT_2: Capture unit 2 + * @retval None + */ +void HRTIM_SimpleCaptureStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_CAPTUREUNIT(CaptureChannel)); + + /* Set the capture unit trigger */ + switch (CaptureChannel) + { + case HRTIM_CAPTUREUNIT_1: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT1xCR = HRTIM_CAPTURETRIGGER_NONE; + } + break; + case HRTIM_CAPTUREUNIT_2: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT2xCR = HRTIM_CAPTURETRIGGER_NONE; + } + break; + default: + break; + } + + /* Disable the timer counter */ + if ((HRTIMx->HRTIM_TIMERx[TimerIdx].CPT1xCR == HRTIM_CAPTURETRIGGER_NONE) && + (HRTIMx->HRTIM_TIMERx[TimerIdx].CPT2xCR == HRTIM_CAPTURETRIGGER_NONE)) + { + __HRTIM_DISABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); + } + +} + +/** + * @brief Enables the basic one pulse signal generation on the designed output + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param OnePulseChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_SimpleOnePulseStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OnePulseChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, OnePulseChannel)); + + /* Enable the timer output */ + HRTIMx->HRTIM_COMMON.OENR |= OnePulseChannel; + + /* Enable the timer counter */ + __HRTIM_ENABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Disables the basic one pulse signal generation on the designed output + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param OnePulseChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_SimpleOnePulseStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OnePulseChannel) +{ + /* Check the parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, OnePulseChannel)); + + /* Disable the timer output */ + HRTIMx->HRTIM_COMMON.DISR |= OnePulseChannel; + + /* Disable the timer counter */ + __HRTIM_DISABLE(HRTIMx, TimerIdxToTimerId[TimerIdx]); +} + +/** + * @brief Starts the counter of the designated timer(s) operating in waveform mode + * Timers can be combined (ORed) to allow for simultaneous counter start + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimersToStart: Timer counter(s) to start + * This parameter can be any combination of the following values: + * @arg HRTIM_TIMERID_MASTER + * @arg HRTIM_TIMERID_TIMER_A + * @arg HRTIM_TIMERID_TIMER_B + * @arg HRTIM_TIMERID_TIMER_C + * @arg HRTIM_TIMERID_TIMER_D + * @arg HRTIM_TIMERID_TIMER_E + * @retval None + */ +void HRTIM_WaveformCounterStart(HRTIM_TypeDef * HRTIMx, + uint32_t TimersToStart) +{ + /* Enable timer(s) counter */ + HRTIMx->HRTIM_MASTER.MCR |= TimersToStart; +} + +/** + * @brief Stops the counter of the designated timer(s) operating in waveform mode + * Timers can be combined (ORed) to allow for simultaneous counter stop + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimersToStop: Timer counter(s) to stop + * This parameter can be any combination of the following values: + * @arg HRTIM_TIMERID_MASTER + * @arg HRTIM_TIMERID_TIMER_A + * @arg HRTIM_TIMERID_TIMER_B + * @arg HRTIM_TIMERID_TIMER_C + * @arg HRTIM_TIMERID_TIMER_D + * @arg HRTIM_TIMERID_TIMER_E + * @retval None + */ +void HRTIM_WaveformCounterStop(HRTIM_TypeDef * HRTIMx, + uint32_t TimersToStop) +{ + /* Disable timer(s) counter */ + HRTIMx->HRTIM_MASTER.MCR &= ~TimersToStop; +} + +/** + * @brief Enables the generation of the waveform signal on the designated output(s) + * Outputs can be combined (ORed) to allow for simultaneous output enabling + * @param HRTIMx: pointer to HRTIMx peripheral + * @param OutputsToStart: Timer output(s) to enable + * This parameter can be any combination of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_WaveformOutputStart(HRTIM_TypeDef * HRTIMx, + uint32_t OutputsToStart) +{ + /* Enable the HRTIM outputs */ + HRTIMx->HRTIM_COMMON.OENR = OutputsToStart; +} + +/** + * @brief Disables the generation of the waveform signal on the designated output(s) + * Outputs can be combined (ORed) to allow for simultaneous output disabling + * @param HRTIMx: pointer to HRTIMx peripheral + * @param OutputsToStop: Timer output(s) to disable + * This parameter can be any combination of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval None + */ +void HRTIM_WaveformOutputStop(HRTIM_TypeDef * HRTIMx, + uint32_t OutputsToStop) +{ + /* Disable the HRTIM outputs */ + HRTIMx->HRTIM_COMMON.DISR = OutputsToStop; +} + +/** + * @brief Enables or disables the Master and slaves interrupt request + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param HRTIM_IT: specifies the HRTIM interrupts sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg HRTIM_MASTER_IT_MCMP1: Master compare 1 interrupt source + * @arg HRTIM_MASTER_IT_MCMP2: Master compare 2 interrupt source + * @arg HRTIM_MASTER_IT_MCMP3: Master compare 3 interrupt Interrupt source + * @arg HRTIM_MASTER_IT_MCMP4: Master compare 4 Interrupt source + * @arg HRTIM_MASTER_IT_MREP: Master Repetition Interrupt source + * @arg HRTIM_MASTER_IT_SYNC: Synchronization input Interrupt source + * @arg HRTIM_MASTER_IT_MUPD: Master update Interrupt source + * @arg HRTIM_TIM_IT_CMP1: Timer compare 1 Interrupt source + * @arg HRTIM_TIM_IT_CMP2: Timer compare 2 Interrupt source + * @arg HRTIM_TIM_IT_CMP3: Timer compare 3 Interrupt source + * @arg HRTIM_TIM_IT_CMP4: Timer compare 4 Interrupt source + * @arg HRTIM_TIM_IT_REP: Timer repetition Interrupt source + * @arg HRTIM_TIM_IT_UPD: Timer update Interrupt source + * @arg HRTIM_TIM_IT_CPT1: Timer capture 1 Interrupt source + * @arg HRTIM_TIM_IT_CPT2: Timer capture 2 Interrupt source + * @arg HRTIM_TIM_IT_SET1: Timer output 1 set Interrupt source + * @arg HRTIM_TIM_IT_RST1: Timer output 1 reset Interrupt source + * @arg HRTIM_TIM_IT_SET2: Timer output 2 set Interrupt source + * @arg HRTIM_TIM_IT_RST2: Timer output 2 reset Interrupt source + * @arg HRTIM_TIM_IT_RST: Timer reset Interrupt source + * @arg HRTIM_TIM_IT_DLYPRT1: Timer delay protection Interrupt source + * @param NewState: new state of the TIM interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void HRTIM_ITConfig(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_IT, FunctionalState NewState) +{ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + switch(TimerIdx) + { + case HRTIM_TIMERINDEX_MASTER: + { + if(NewState != DISABLE) + { + HRTIMx->HRTIM_MASTER.MDIER |= HRTIM_IT; + } + else + { + HRTIMx->HRTIM_MASTER.MDIER &= ~HRTIM_IT; + } + } + break; + case HRTIM_TIMERINDEX_TIMER_A: + case HRTIM_TIMERINDEX_TIMER_B: + case HRTIM_TIMERINDEX_TIMER_C: + case HRTIM_TIMERINDEX_TIMER_D: + case HRTIM_TIMERINDEX_TIMER_E: + { + if(NewState != DISABLE) + { + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxDIER |= HRTIM_IT; + } + else + { + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxDIER &= ~HRTIM_IT; + } + } + break; + + default: + break; + } +} + +/** + * @brief Enables or disables the common interrupt request + * @param HRTIMx: pointer to HRTIMx peripheral + * @param HRTIM_IT: specifies the HRTIM interrupts sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg HRTIM_IT_FLT1: Fault 1 interrupt source + * @arg HRTIM_IT_FLT2: Fault 2 interrupt source + * @arg HRTIM_IT_FLT3: Fault 3 interrupt Interrupt source + * @arg HRTIM_IT_FLT4: Fault 4 Interrupt source + * @arg HRTIM_IT_FLT5: Fault 5 Interrupt source + * @arg HRTIM_IT_SYSFLT: System Fault Interrupt source + * @arg HRTIM_IT_DLLRDY: DLL ready Interrupt source + * @arg HRTIM_IT_BMPER: Burst mode period Interrupt source + * @param NewState: new state of the TIM interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void HRTIM_ITCommonConfig(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonIT, FunctionalState NewState) +{ + if(NewState != DISABLE) + { + HRTIMx->HRTIM_COMMON.IER |= HRTIM_CommonIT; + } + else + { + HRTIMx->HRTIM_COMMON.IER &= ~HRTIM_CommonIT; + } +} + +/** + * @brief Clears the Master and slaves interrupt flags + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param HRTIM_FLAG: specifies the HRTIM flags sources to be cleared. + * This parameter can be any combination of the following values: + * @arg HRTIM_MASTER_FLAG_MCMP1: Master compare 1 interrupt flag + * @arg HRTIM_MASTER_FLAG_MCMP2: Master compare 2 interrupt flag + * @arg HRTIM_MASTER_FLAG_MCMP3: Master compare 3 interrupt Interrupt flag + * @arg HRTIM_MASTER_FLAG_MCMP4: Master compare 4 Interrupt flag + * @arg HRTIM_MASTER_FLAG_MREP: Master Repetition Interrupt flag + * @arg HRTIM_MASTER_FLAG_SYNC: Synchronization input Interrupt flag + * @arg HRTIM_MASTER_FLAG_MUPD: Master update Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP1: Timer compare 1 Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP2: Timer compare 2 Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP3: Timer compare 3 Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP4: Timer compare 4 Interrupt flag + * @arg HRTIM_TIM_FLAG_REP: Timer repetition Interrupt flag + * @arg HRTIM_TIM_FLAG_UPD: Timer update Interrupt flag + * @arg HRTIM_TIM_FLAG_CPT1: Timer capture 1 Interrupt flag + * @arg HRTIM_TIM_FLAG_CPT2: Timer capture 2 Interrupt flag + * @arg HRTIM_TIM_FLAG_SET1: Timer output 1 set Interrupt flag + * @arg HRTIM_TIM_FLAG_RST1: Timer output 1 reset Interrupt flag + * @arg HRTIM_TIM_FLAG_SET2: Timer output 2 set Interrupt flag + * @arg HRTIM_TIM_FLAG_RST2: Timer output 2 reset Interrupt flag + * @arg HRTIM_TIM_FLAG_RST: Timer reset Interrupt flag + * @arg HRTIM_TIM_FLAG_DLYPRT1: Timer delay protection Interrupt flag + * @retval None + */ +void HRTIM_ClearFlag(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_FLAG) +{ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + switch(TimerIdx) + { + case HRTIM_TIMERINDEX_MASTER: + { + HRTIMx->HRTIM_MASTER.MICR |= HRTIM_FLAG; + } + break; + case HRTIM_TIMERINDEX_TIMER_A: + case HRTIM_TIMERINDEX_TIMER_B: + case HRTIM_TIMERINDEX_TIMER_C: + case HRTIM_TIMERINDEX_TIMER_D: + case HRTIM_TIMERINDEX_TIMER_E: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxICR |= HRTIM_FLAG; + } + break; + + default: + break; + } +} + +/** + * @brief Clears the common interrupt flags + * @param HRTIMx: pointer to HRTIMx peripheral + * @param HRTIM_FLAG: specifies the HRTIM flags to be cleared. + * This parameter can be any combination of the following values: + * @arg HRTIM_FLAG_FLT1: Fault 1 interrupt flag + * @arg HRTIM_FLAG_FLT2: Fault 2 interrupt flag + * @arg HRTIM_FLAG_FLT3: Fault 3 interrupt Interrupt flag + * @arg HRTIM_FLAG_FLT4: Fault 4 Interrupt flag + * @arg HRTIM_FLAG_FLT5: Fault 5 Interrupt flag + * @arg HRTIM_FLAG_SYSFLT: System Fault Interrupt flag + * @arg HRTIM_FLAG_DLLRDY: DLL ready Interrupt flag + * @arg HRTIM_FLAG_BMPER: Burst mode period Interrupt flag + * @retval None + */ +void HRTIM_ClearCommonFlag(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonFLAG) +{ + HRTIMx->HRTIM_COMMON.ICR |= HRTIM_CommonFLAG; +} + +/** + * @brief Clears the Master and slaves interrupt request pending bits + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param HRTIM_IT: specifies the HRTIM interrupts sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg HRTIM_MASTER_IT_MCMP1: Master compare 1 interrupt source + * @arg HRTIM_MASTER_IT_MCMP2: Master compare 2 interrupt source + * @arg HRTIM_MASTER_IT_MCMP3: Master compare 3 interrupt Interrupt source + * @arg HRTIM_MASTER_IT_MCMP4: Master compare 4 Interrupt source + * @arg HRTIM_MASTER_IT_MREP: Master Repetition Interrupt source + * @arg HRTIM_MASTER_IT_SYNC: Synchronization input Interrupt source + * @arg HRTIM_MASTER_IT_MUPD: Master update Interrupt source + * @arg HRTIM_TIM_IT_CMP1: Timer compare 1 Interrupt source + * @arg HRTIM_TIM_IT_CMP2: Timer compare 2 Interrupt source + * @arg HRTIM_TIM_IT_CMP3: Timer compare 3 Interrupt source + * @arg HRTIM_TIM_IT_CMP4: Timer compare 4 Interrupt source + * @arg HRTIM_TIM_IT_REP: Timer repetition Interrupt source + * @arg HRTIM_TIM_IT_UPD: Timer update Interrupt source + * @arg HRTIM_TIM_IT_CPT1: Timer capture 1 Interrupt source + * @arg HRTIM_TIM_IT_CPT2: Timer capture 2 Interrupt source + * @arg HRTIM_TIM_IT_SET1: Timer output 1 set Interrupt source + * @arg HRTIM_TIM_IT_RST1: Timer output 1 reset Interrupt source + * @arg HRTIM_TIM_IT_SET2: Timer output 2 set Interrupt source + * @arg HRTIM_TIM_IT_RST2: Timer output 2 reset Interrupt source + * @arg HRTIM_TIM_IT_RST: Timer reset Interrupt source + * @arg HRTIM_TIM_IT_DLYPRT: Timer delay protection Interrupt source + * @retval None + */ +void HRTIM_ClearITPendingBit(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_IT) +{ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + switch(TimerIdx) + { + case HRTIM_TIMERINDEX_MASTER: + { + HRTIMx->HRTIM_MASTER.MICR |= HRTIM_IT; + } + break; + case HRTIM_TIMERINDEX_TIMER_A: + case HRTIM_TIMERINDEX_TIMER_B: + case HRTIM_TIMERINDEX_TIMER_C: + case HRTIM_TIMERINDEX_TIMER_D: + case HRTIM_TIMERINDEX_TIMER_E: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxICR |= HRTIM_IT; + } + break; + + default: + break; + } +} + +/** + * @brief Clears the common interrupt pending bits + * @param HRTIMx: pointer to HRTIMx peripheral + * @param HRTIM_IT: specifies the HRTIM interrupts sources to be cleared. + * This parameter can be any combination of the following values: + * @arg HRTIM_IT_FLT1: Fault 1 interrupt source + * @arg HRTIM_IT_FLT2: Fault 2 interrupt source + * @arg HRTIM_IT_FLT3: Fault 3 interrupt Interrupt source + * @arg HRTIM_IT_FLT4: Fault 4 Interrupt source + * @arg HRTIM_IT_FLT5: Fault 5 Interrupt source + * @arg HRTIM_IT_SYSFLT: System Fault Interrupt source + * @arg HRTIM_IT_DLLRDY: DLL ready Interrupt source + * @arg HRTIM_IT_BMPER: Burst mode period Interrupt source + * @retval None + */ +void HRTIM_ClearCommonITPendingBit(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonIT) +{ + HRTIMx->HRTIM_COMMON.ICR |= HRTIM_CommonIT; +} + + +/** + * @brief Checks whether the specified HRTIM flag is set or not. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param HRTIM_FLAG: specifies the HRTIM flags to check. + * This parameter can be any combination of the following values: + * @arg HRTIM_MASTER_FLAG_MCMP1: Master compare 1 interrupt flag + * @arg HRTIM_MASTER_FLAG_MCMP2: Master compare 2 interrupt flag + * @arg HRTIM_MASTER_FLAG_MCMP3: Master compare 3 interrupt Interrupt flag + * @arg HRTIM_MASTER_FLAG_MCMP4: Master compare 4 Interrupt flag + * @arg HRTIM_MASTER_FLAG_MREP: Master Repetition Interrupt flag + * @arg HRTIM_MASTER_FLAG_SYNC: Synchronization input Interrupt flag + * @arg HRTIM_MASTER_FLAG_MUPD: Master update Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP1: Timer compare 1 Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP2: Timer compare 2 Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP3: Timer compare 3 Interrupt flag + * @arg HRTIM_TIM_FLAG_CMP4: Timer compare 4 Interrupt flag + * @arg HRTIM_TIM_FLAG_REP: Timer repetition Interrupt flag + * @arg HRTIM_TIM_FLAG_UPD: Timer update Interrupt flag + * @arg HRTIM_TIM_FLAG_CPT1: Timer capture 1 Interrupt flag + * @arg HRTIM_TIM_FLAG_CPT2: Timer capture 2 Interrupt flag + * @arg HRTIM_TIM_FLAG_SET1: Timer output 1 set Interrupt flag + * @arg HRTIM_TIM_FLAG_RST1: Timer output 1 reset Interrupt flag + * @arg HRTIM_TIM_FLAG_SET2: Timer output 2 set Interrupt flag + * @arg HRTIM_TIM_FLAG_RST2: Timer output 2 reset Interrupt flag + * @arg HRTIM_TIM_FLAG_RST: Timer reset Interrupt flag + * @arg HRTIM_TIM_FLAG_DLYPRT: Timer delay protection Interrupt flag + * @retval The new state of HRTIM_FLAG (SET or RESET). + */ +FlagStatus HRTIM_GetFlagStatus(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_FLAG) +{ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + FlagStatus bitstatus = RESET; + + switch(TimerIdx) + { + case HRTIM_TIMERINDEX_MASTER: + { + if ((HRTIMx->HRTIM_MASTER.MISR & HRTIM_FLAG) != RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + } + break; + + case HRTIM_TIMERINDEX_TIMER_A: + case HRTIM_TIMERINDEX_TIMER_B: + case HRTIM_TIMERINDEX_TIMER_C: + case HRTIM_TIMERINDEX_TIMER_D: + case HRTIM_TIMERINDEX_TIMER_E: + { + if ((HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_FLAG) != RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + } + break; + + default: + break; + } + return bitstatus; +} + +/** + * @brief Checks whether the specified HRTIM common flag is set or not. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param HRTIM_FLAG: specifies the HRTIM flags to check. + * This parameter can be any combination of the following values: + * @arg HRTIM_FLAG_FLT1: Fault 1 interrupt flag + * @arg HRTIM_FLAG_FLT2: Fault 2 interrupt flag + * @arg HRTIM_FLAG_FLT3: Fault 3 interrupt Interrupt flag + * @arg HRTIM_FLAG_FLT4: Fault 4 Interrupt flag + * @arg HRTIM_FLAG_FLT5: Fault 5 Interrupt flag + * @arg HRTIM_FLAG_SYSFLT: System Fault Interrupt flag + * @arg HRTIM_FLAG_DLLRDY: DLL ready Interrupt flag + * @arg HRTIM_FLAG_BMPER: Burst mode period Interrupt flag + * @retval The new state of HRTIM_FLAG (SET or RESET). + */ +FlagStatus HRTIM_GetCommonFlagStatus(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonFLAG) +{ + FlagStatus bitstatus = RESET; + + if((HRTIMx->HRTIM_COMMON.ISR & HRTIM_CommonFLAG) != RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Checks whether the specified HRTIM interrupt has occurred or not. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param HRTIM_IT: specifies the HRTIM flags sources to be cleared. + * This parameter can be any combination of the following values: + * @arg HRTIM_MASTER_IT_MCMP1: Master compare 1 interrupt + * @arg HRTIM_MASTER_IT_MCMP2: Master compare 2 interrupt + * @arg HRTIM_MASTER_IT_MCMP3: Master compare 3 interrupt Interrupt + * @arg HRTIM_MASTER_IT_MCMP4: Master compare 4 Interrupt + * @arg HRTIM_MASTER_IT_MREP: Master Repetition Interrupt + * @arg HRTIM_MASTER_IT_SYNC: Synchronization input Interrupt + * @arg HRTIM_MASTER_IT_MUPD: Master update Interrupt + * @arg HRTIM_TIM_IT_CMP1: Timer compare 1 Interrupt + * @arg HRTIM_TIM_IT_CMP2: Timer compare 2 Interrupt + * @arg HRTIM_TIM_IT_CMP3: Timer compare 3 Interrupt + * @arg HRTIM_TIM_IT_CMP4: Timer compare 4 Interrupt + * @arg HRTIM_TIM_IT_REP: Timer repetition Interrupt + * @arg HRTIM_TIM_IT_UPD: Timer update Interrupt + * @arg HRTIM_TIM_IT_CPT1: Timer capture 1 Interrupt + * @arg HRTIM_TIM_IT_CPT2: Timer capture 2 Interrupt + * @arg HRTIM_TIM_IT_SET1: Timer output 1 set Interrupt + * @arg HRTIM_TIM_IT_RST1: Timer output 1 reset Interrupt + * @arg HRTIM_TIM_IT_SET2: Timer output 2 set Interrupt + * @arg HRTIM_TIM_IT_RST2: Timer output 2 reset Interrupt + * @arg HRTIM_TIM_IT_RST: Timer reset Interrupt + * @arg HRTIM_TIM_IT_DLYPRT: Timer delay protection Interrupt + * @retval The new state of the HRTIM_IT(SET or RESET). + */ +ITStatus HRTIM_GetITStatus(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_IT) +{ + ITStatus bitstatus = RESET; + uint16_t itstatus = 0x0, itenable = 0x0; + + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + switch(TimerIdx) + { + case HRTIM_TIMERINDEX_MASTER: + { + itstatus = HRTIMx->HRTIM_MASTER.MISR & HRTIM_IT; + + itenable = HRTIMx->HRTIM_MASTER.MDIER & HRTIM_IT; + if ((itstatus != (uint16_t)RESET) && (itenable != (uint16_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + } + break; + + case HRTIM_TIMERINDEX_TIMER_A: + case HRTIM_TIMERINDEX_TIMER_B: + case HRTIM_TIMERINDEX_TIMER_C: + case HRTIM_TIMERINDEX_TIMER_D: + case HRTIM_TIMERINDEX_TIMER_E: + { + itstatus = HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_IT; + + itenable = HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxDIER & HRTIM_IT; + if ((itstatus != (uint16_t)RESET) && (itenable != (uint16_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + } + break; + + default: + break; + } + return bitstatus; +} + +/** + * @brief Checks whether the specified HRTIM common interrupt has occurred or not. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param HRTIM_IT: specifies the HRTIM interrupt source to check. + * This parameter can be any combination of the following values: + * @arg HRTIM_IT_FLT1: Fault 1 interrupt + * @arg HRTIM_IT_FLT2: Fault 2 interrupt + * @arg HRTIM_IT_FLT3: Fault 3 interrupt Interrupt + * @arg HRTIM_IT_FLT4: Fault 4 Interrupt + * @arg HRTIM_IT_FLT5: Fault 5 Interrupt + * @arg HRTIM_IT_SYSFLT: System Fault Interrupt + * @arg HRTIM_IT_DLLRDY: DLL ready Interrupt flag + * @arg HRTIM_IT_BMPER: Burst mode period Interrupt + * @retval The new state of HRTIM_FLAG (SET or RESET). + */ +ITStatus HRTIM_GetCommonITStatus(HRTIM_TypeDef * HRTIMx, uint32_t HRTIM_CommonIT) +{ + ITStatus bitstatus = RESET; + uint16_t itstatus = 0x0, itenable = 0x0; + + itstatus = HRTIMx->HRTIM_COMMON.ISR & HRTIM_CommonIT; + itenable = HRTIMx->HRTIM_COMMON.IER & HRTIM_CommonIT; + + if ((itstatus != (uint16_t)RESET) && (itenable != (uint16_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + + return bitstatus; +} + +/** + * @brief Enables or disables the HRTIMx's DMA Requests. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param HRTIM_DMA: specifies the DMA Request sources. + * This parameter can be any combination of the following values: + * @arg HRTIM_MASTER_DMA_MCMP1: Master compare 1 DMA request source + * @arg HRTIM_MASTER_DMA_MCMP2: Master compare 2 DMA request source + * @arg HRTIM_MASTER_DMA_MCMP3: Master compare 3 DMA request source + * @arg HRTIM_MASTER_DMA_MCMP4: Master compare 4 DMA request source + * @arg HRTIM_MASTER_DMA_MREP: Master Repetition DMA request source + * @arg HRTIM_MASTER_DMA_SYNC: Synchronization input DMA request source + * @arg HRTIM_MASTER_DMA_MUPD:Master update DMA request source + * @arg HRTIM_TIM_DMA_CMP1: Timer compare 1 DMA request source + * @arg HRTIM_TIM_DMA_CMP2: Timer compare 2 DMA request source + * @arg HRTIM_TIM_DMA_CMP3: Timer compare 3 DMA request source + * @arg HRTIM_TIM_DMA_CMP4: Timer compare 4 DMA request source + * @arg HRTIM_TIM_DMA_REP: Timer repetition DMA request source + * @arg HRTIM_TIM_DMA_UPD: Timer update DMA request source + * @arg HRTIM_TIM_DMA_CPT1: Timer capture 1 DMA request source + * @arg HRTIM_TIM_DMA_CPT2: Timer capture 2 DMA request source + * @arg HRTIM_TIM_DMA_SET1: Timer output 1 set DMA request source + * @arg HRTIM_TIM_DMA_RST1: Timer output 1 reset DMA request source + * @arg HRTIM_TIM_DMA_SET2: Timer output 2 set DMA request source + * @arg HRTIM_TIM_DMA_RST2: Timer output 2 reset DMA request source + * @arg HRTIM_TIM_DMA_RST: Timer reset DMA request source + * @arg HRTIM_TIM_DMA_DLYPRT: Timer delay protection DMA request source + * @param NewState: new state of the DMA Request sources. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void HRTIM_DMACmd(HRTIM_TypeDef* HRTIMx, uint32_t TimerIdx, uint32_t HRTIM_DMA, FunctionalState NewState) +{ + assert_param(IS_HRTIM_TIMERINDEX(TimerIdx)); + + switch(TimerIdx) + { + case HRTIM_TIMERINDEX_MASTER: + { + if(NewState != DISABLE) + { + HRTIMx->HRTIM_MASTER.MDIER |= HRTIM_DMA; + } + else + { + HRTIMx->HRTIM_MASTER.MDIER &= ~HRTIM_DMA; + } + } + break; + case HRTIM_TIMERINDEX_TIMER_A: + case HRTIM_TIMERINDEX_TIMER_B: + case HRTIM_TIMERINDEX_TIMER_C: + case HRTIM_TIMERINDEX_TIMER_D: + case HRTIM_TIMERINDEX_TIMER_E: + { + if(NewState != DISABLE) + { + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxDIER |= HRTIM_DMA; + } + else + { + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxDIER &= ~HRTIM_DMA; + } + } + break; + + default: + break; + } +} + +/** + * @} + */ + +/** @defgroup HRTIM_Group3 Peripheral Control methods + * @brief management functions + * +@verbatim + =============================================================================== + ##### Peripheral Control methods ##### + =============================================================================== + [..] + This subsection provides a set of functions allowing to control the HRTIMx data + transfers. + +@endverbatim + * @{ + */ + +/** + * @brief Configures an output in basic output compare mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param OCChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @param pBasicOCChannelCfg: pointer to the basic output compare output configuration structure + * @note When the timer operates in basic output compare mode: + * Output 1 is implicitly controlled by the compare unit 1 + * Output 2 is implicitly controlled by the compare unit 2 + * Output Set/Reset crossbar is set according to the selected output compare mode: + * Toggle: SETxyR = RSTxyR = CMPy + * Active: SETxyR = CMPy, RSTxyR = 0 + * Inactive: SETxy =0, RSTxy = CMPy + * @retval None + */ +void HRTIM_SimpleOCChannelConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OCChannel, + HRTIM_BasicOCChannelCfgTypeDef* pBasicOCChannelCfg) +{ + uint32_t CompareUnit = HRTIM_COMPAREUNIT_1; + HRTIM_CompareCfgTypeDef CompareCfg; + HRTIM_OutputCfgTypeDef OutputCfg; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, OCChannel)); + assert_param(IS_HRTIM_BASICOCMODE(pBasicOCChannelCfg->Mode)); + assert_param(IS_HRTIM_OUTPUTPOLARITY(pBasicOCChannelCfg->Polarity)); + assert_param(IS_HRTIM_OUTPUTIDLESTATE(pBasicOCChannelCfg->IdleState)); + + /* Configure timer compare unit */ + switch (OCChannel) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + CompareUnit = HRTIM_COMPAREUNIT_1; + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + CompareUnit = HRTIM_COMPAREUNIT_2; + } + break; + default: + break; + } + + CompareCfg.CompareValue = pBasicOCChannelCfg->Pulse; + CompareCfg.AutoDelayedMode = HRTIM_AUTODELAYEDMODE_REGULAR; + CompareCfg.AutoDelayedTimeout = 0; + + HRTIM_CompareUnitConfig(HRTIMx, + TimerIdx, + CompareUnit, + &CompareCfg); + + /* Configure timer output */ + OutputCfg.Polarity = pBasicOCChannelCfg->Polarity; + OutputCfg.IdleState = pBasicOCChannelCfg->IdleState; + OutputCfg.FaultState = HRTIM_OUTPUTFAULTSTATE_NONE; + OutputCfg.IdleMode = HRTIM_OUTPUTIDLEMODE_NONE; + OutputCfg.ChopperModeEnable = HRTIM_OUTPUTCHOPPERMODE_DISABLED; + OutputCfg.BurstModeEntryDelayed = HRTIM_OUTPUTBURSTMODEENTRY_REGULAR; + + switch (pBasicOCChannelCfg->Mode) + { + case HRTIM_BASICOCMODE_TOGGLE: + { + if (CompareUnit == HRTIM_COMPAREUNIT_1) + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP1; + } + else + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP2; + } + OutputCfg.ResetSource = OutputCfg.SetSource; + } + break; + case HRTIM_BASICOCMODE_ACTIVE: + { + if (CompareUnit == HRTIM_COMPAREUNIT_1) + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP1; + } + else + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP2; + } + OutputCfg.ResetSource = HRTIM_OUTPUTRESET_NONE; + } + break; + case HRTIM_BASICOCMODE_INACTIVE: + { + if (CompareUnit == HRTIM_COMPAREUNIT_1) + { + OutputCfg.ResetSource = HRTIM_OUTPUTRESET_TIMCMP1; + } + else + { + OutputCfg.ResetSource = HRTIM_OUTPUTRESET_TIMCMP2; + } + OutputCfg.SetSource = HRTIM_OUTPUTSET_NONE; + } + break; + default: + break; + } + + HRTIM_OutputConfig(HRTIMx, TimerIdx, OCChannel, &OutputCfg); +} + +/** + * @brief Configures an output in basic PWM mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param PWMChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @param pBasicPWMChannelCfg: pointer to the basic PWM output configuration structure + * @note When the timer operates in basic PWM output mode: + * Output 1 is implicitly controlled by the compare unit 1 + * Output 2 is implicitly controlled by the compare unit 2 + * Output Set/Reset crossbar is set as follows: + * Output 1: SETx1R = CMP1, RSTx1R = PER + * Output 2: SETx2R = CMP2, RST2R = PER + * @retval None + */ +void HRTIM_SimplePWMChannelConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t PWMChannel, + HRTIM_BasicPWMChannelCfgTypeDef* pBasicPWMChannelCfg) +{ + uint32_t CompareUnit = HRTIM_COMPAREUNIT_1; + HRTIM_CompareCfgTypeDef CompareCfg; + HRTIM_OutputCfgTypeDef OutputCfg; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, PWMChannel)); + assert_param(IS_HRTIM_OUTPUTPOLARITY(pBasicPWMChannelCfg->Polarity)); + assert_param(IS_HRTIM_OUTPUTIDLESTATE(pBasicPWMChannelCfg->IdleState)); + + /* Configure timer compare unit */ + switch (PWMChannel) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + CompareUnit = HRTIM_COMPAREUNIT_1; + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + CompareUnit = HRTIM_COMPAREUNIT_2; + } + break; + default: + break; + } + + CompareCfg.CompareValue = pBasicPWMChannelCfg->Pulse; + CompareCfg.AutoDelayedMode = HRTIM_AUTODELAYEDMODE_REGULAR; + CompareCfg.AutoDelayedTimeout = 0; + + HRTIM_CompareUnitConfig(HRTIMx, + TimerIdx, + CompareUnit, + &CompareCfg); + + /* Configure timer output */ + OutputCfg.Polarity = pBasicPWMChannelCfg->Polarity; + OutputCfg.IdleState = pBasicPWMChannelCfg->IdleState; + OutputCfg.FaultState = HRTIM_OUTPUTFAULTSTATE_NONE; + OutputCfg.IdleMode = HRTIM_OUTPUTIDLEMODE_NONE; + OutputCfg.ChopperModeEnable = HRTIM_OUTPUTCHOPPERMODE_DISABLED; + OutputCfg.BurstModeEntryDelayed = HRTIM_OUTPUTBURSTMODEENTRY_REGULAR; + + if (CompareUnit == HRTIM_COMPAREUNIT_1) + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP1; + } + else + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP2; + } + OutputCfg.ResetSource = HRTIM_OUTPUTSET_TIMPER; + + HRTIM_OutputConfig(HRTIMx, TimerIdx, PWMChannel, &OutputCfg); +} + +/** + * @brief Configures a basic capture + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CaptureChannel: Capture unit + * This parameter can be one of the following values: + * @arg HRTIM_CAPTUREUNIT_1: Capture unit 1 + * @arg HRTIM_CAPTUREUNIT_2: Capture unit 2 + * @param pBasicCaptureChannelCfg: pointer to the basic capture configuration structure + * @note When the timer operates in basic capture mode the capture is triggered + * by the designated external event and GPIO input is implicitly used as event source. + * The capture can be triggered by a rising edge, a falling edge or both + * edges on event channel. + * @retval None + */ +void HRTIM_SimpleCaptureChannelConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureChannel, + HRTIM_BasicCaptureChannelCfgTypeDef* pBasicCaptureChannelCfg) +{ + HRTIM_EventCfgTypeDef EventCfg; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_CAPTUREUNIT(CaptureChannel)); + assert_param(IS_HRTIM_EVENT(pBasicCaptureChannelCfg->Event)); + assert_param(IS_HRTIM_EVENTPOLARITY(pBasicCaptureChannelCfg->EventPolarity)); + assert_param(IS_HRTIM_EVENTSENSITIVITY(pBasicCaptureChannelCfg->EventSensitivity)); + assert_param(IS_HRTIM_EVENTFILTER(pBasicCaptureChannelCfg->EventFilter)); + + /* Configure external event channel */ + EventCfg.FastMode = HRTIM_EVENTFASTMODE_DISABLE; + EventCfg.Filter = pBasicCaptureChannelCfg->EventFilter; + EventCfg.Polarity = pBasicCaptureChannelCfg->EventPolarity; + EventCfg.Sensitivity = pBasicCaptureChannelCfg->EventSensitivity; + EventCfg.Source = HRTIM_EVENTSRC_1; + + HRTIM_ExternalEventConfig(HRTIMx, + pBasicCaptureChannelCfg->Event, + &EventCfg); + + /* Memorize capture trigger (will be configured when the capture is started */ + HRTIM_CaptureUnitConfig(HRTIMx, + TimerIdx, + CaptureChannel, + pBasicCaptureChannelCfg->Event); +} + +/** + * @brief Configures an output basic one pulse mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param OnePulseChannel: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @param pBasicOnePulseChannelCfg: pointer to the basic one pulse output configuration structure + * @note When the timer operates in basic one pulse mode: + * the timer counter is implicitly started by the reset event, + * the reset of the timer counter is triggered by the designated external event + * GPIO input is implicitly used as event source, + * Output 1 is implicitly controlled by the compare unit 1, + * Output 2 is implicitly controlled by the compare unit 2. + * Output Set/Reset crossbar is set as follows: + * Output 1: SETx1R = CMP1, RSTx1R = PER + * Output 2: SETx2R = CMP2, RST2R = PER + * The counter mode should be HRTIM_MODE_SINGLESHOT_RETRIGGERABLE + * @retval None + */ +void HRTIM_SimpleOnePulseChannelConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t OnePulseChannel, + HRTIM_BasicOnePulseChannelCfgTypeDef* pBasicOnePulseChannelCfg) +{ + uint32_t CompareUnit = HRTIM_COMPAREUNIT_1; + HRTIM_CompareCfgTypeDef CompareCfg; + HRTIM_OutputCfgTypeDef OutputCfg; + HRTIM_EventCfgTypeDef EventCfg; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, OnePulseChannel)); + assert_param(IS_HRTIM_OUTPUTPOLARITY(pBasicOnePulseChannelCfg->OutputPolarity)); + assert_param(IS_HRTIM_OUTPUTIDLESTATE(pBasicOnePulseChannelCfg->OutputIdleState)); + assert_param(IS_HRTIM_EVENT(pBasicOnePulseChannelCfg->Event)); + assert_param(IS_HRTIM_EVENTPOLARITY(pBasicOnePulseChannelCfg->EventPolarity)); + assert_param(IS_HRTIM_EVENTSENSITIVITY(pBasicOnePulseChannelCfg->EventSensitivity)); + assert_param(IS_HRTIM_EVENTFILTER(pBasicOnePulseChannelCfg->EventFilter)); + + /* Configure timer compare unit */ + switch (OnePulseChannel) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + CompareUnit = HRTIM_COMPAREUNIT_1; + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + CompareUnit = HRTIM_COMPAREUNIT_2; + } + break; + default: + break; + } + + CompareCfg.CompareValue = pBasicOnePulseChannelCfg->Pulse; + CompareCfg.AutoDelayedMode = HRTIM_AUTODELAYEDMODE_REGULAR; + CompareCfg.AutoDelayedTimeout = 0; + + HRTIM_CompareUnitConfig(HRTIMx, + TimerIdx, + CompareUnit, + &CompareCfg); + + /* Configure timer output */ + OutputCfg.Polarity = pBasicOnePulseChannelCfg->OutputPolarity; + OutputCfg.IdleState = pBasicOnePulseChannelCfg->OutputIdleState; + OutputCfg.FaultState = HRTIM_OUTPUTFAULTSTATE_NONE; + OutputCfg.IdleMode = HRTIM_OUTPUTIDLEMODE_NONE; + OutputCfg.ChopperModeEnable = HRTIM_OUTPUTCHOPPERMODE_DISABLED; + OutputCfg.BurstModeEntryDelayed = HRTIM_OUTPUTBURSTMODEENTRY_REGULAR; + + if (CompareUnit == HRTIM_COMPAREUNIT_1) + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP1; + } + else + { + OutputCfg.SetSource = HRTIM_OUTPUTSET_TIMCMP2; + } + OutputCfg.ResetSource = HRTIM_OUTPUTSET_TIMPER; + + HRTIM_OutputConfig(HRTIMx, + TimerIdx, + OnePulseChannel, + &OutputCfg); + + /* Configure external event channel */ + EventCfg.FastMode = HRTIM_EVENTFASTMODE_DISABLE; + EventCfg.Filter = pBasicOnePulseChannelCfg->EventFilter; + EventCfg.Polarity = pBasicOnePulseChannelCfg->EventPolarity; + EventCfg.Sensitivity = pBasicOnePulseChannelCfg->EventSensitivity; + EventCfg.Source = HRTIM_EVENTSRC_1; + + HRTIM_ExternalEventConfig(HRTIMx, + pBasicOnePulseChannelCfg->Event, + &EventCfg); + + /* Configure the timer reset register */ + HRTIM_TIM_ResetConfig(HRTIMx, + TimerIdx, + pBasicOnePulseChannelCfg->Event); +} + +/** + * @brief Configures the general behavior of a timer operating in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param pTimerCfg: pointer to the timer configuration structure + * @note When the timer operates in waveform mode, all the features supported by + * the HRTIMx are available without any limitation. + * @retval None + */ +void HRTIM_WaveformTimerConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_TimerCfgTypeDef * pTimerCfg) +{ + uint32_t HRTIM_timcr; + uint32_t HRTIM_timfltr; + uint32_t HRTIM_timoutr; + uint32_t HRTIM_timrstr; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_TIMPUSHPULLMODE(pTimerCfg->PushPull)); + assert_param(IS_HRTIM_TIMFAULTENABLE(pTimerCfg->FaultEnable)); + assert_param(IS_HRTIM_TIMFAULTLOCK(pTimerCfg->FaultLock)); + assert_param(IS_HRTIM_TIMDEADTIMEINSERTION(pTimerCfg->DeadTimeInsertion)); + assert_param(IS_HRTIM_TIMDELAYEDPROTECTION(pTimerCfg->DelayedProtectionMode)); + assert_param(IS_HRTIM_TIMUPDATETRIGGER(pTimerCfg->UpdateTrigger)); + assert_param(IS_HRTIM_TIMRESETTRIGGER(pTimerCfg->ResetTrigger)); + assert_param(IS_HRTIM_TIMUPDATEONRESET(pTimerCfg->ResetUpdate)); + + /* Configure timing unit (Timer A to Timer E) */ + HRTIM_timcr = HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR; + HRTIM_timfltr = HRTIMx->HRTIM_TIMERx[TimerIdx].FLTxR; + HRTIM_timoutr = HRTIMx->HRTIM_TIMERx[TimerIdx].OUTxR; + HRTIM_timrstr = HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR; + + /* Set the push-pull mode */ + HRTIM_timcr &= ~(HRTIM_TIMCR_PSHPLL); + HRTIM_timcr |= pTimerCfg->PushPull; + + /* Enable/Disable registers update on timer counter reset */ + HRTIM_timcr &= ~(HRTIM_TIMCR_TRSTU); + HRTIM_timcr |= pTimerCfg->ResetUpdate; + + /* Set the timer update trigger */ + HRTIM_timcr &= ~(HRTIM_TIMCR_TIMUPDATETRIGGER); + HRTIM_timcr |= pTimerCfg->UpdateTrigger; + + /* Enable/Disable the fault channel at timer level */ + HRTIM_timfltr &= ~(HRTIM_FLTR_FLTxEN); + HRTIM_timfltr |= (pTimerCfg->FaultEnable & HRTIM_FLTR_FLTxEN); + + /* Lock/Unlock fault sources at timer level */ + HRTIM_timfltr &= ~(HRTIM_FLTR_FLTCLK); + HRTIM_timfltr |= pTimerCfg->FaultLock; + + /* Enable/Disable dead time insertion at timer level */ + HRTIM_timoutr &= ~(HRTIM_OUTR_DTEN); + HRTIM_timoutr |= pTimerCfg->DeadTimeInsertion; + + /* Enable/Disable delayed protection at timer level */ + HRTIM_timoutr &= ~(HRTIM_OUTR_DLYPRT| HRTIM_OUTR_DLYPRTEN); + HRTIM_timoutr |= pTimerCfg->DelayedProtectionMode; + + /* Set the timer counter reset trigger */ + HRTIM_timrstr = pTimerCfg->ResetTrigger; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR = HRTIM_timcr; + HRTIMx->HRTIM_TIMERx[TimerIdx].FLTxR = HRTIM_timfltr; + HRTIMx->HRTIM_TIMERx[TimerIdx].OUTxR = HRTIM_timoutr; + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_timrstr; + } + +/** + * @brief Configures the compare unit of a timer operating in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * 0xFF for master timer + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CompareUnit: Compare unit to configure + * This parameter can be one of the following values: + * @arg HRTIM_COMPAREUNIT_1: Compare unit 1 + * @arg HRTIM_COMPAREUNIT_2: Compare unit 2 + * @arg HRTIM_COMPAREUNIT_3: Compare unit 3 + * @arg HRTIM_COMPAREUNIT_4: Compare unit 4 + * @param pCompareCfg: pointer to the compare unit configuration structure + * @note When auto delayed mode is required for compare unit 2 or compare unit 4, + * application has to configure separately the capture unit. Capture unit + * to configure in that case depends on the compare unit auto delayed mode + * is applied to (see below): + * Auto delayed on output compare 2: capture unit 1 must be configured + * Auto delayed on output compare 4: capture unit 2 must be configured + * @retval None + */ + void HRTIM_WaveformCompareConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CompareUnit, + HRTIM_CompareCfgTypeDef* pCompareCfg) +{ + uint32_t HRTIM_timcr; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_COMPAREUNIT_AUTODELAYEDMODE(CompareUnit, pCompareCfg->AutoDelayedMode)); + + /* Configure the compare unit */ + switch (CompareUnit) + { + case HRTIM_COMPAREUNIT_1: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP1xR = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_2: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP2xR = pCompareCfg->CompareValue; + + if (pCompareCfg->AutoDelayedMode != HRTIM_AUTODELAYEDMODE_REGULAR) + { + /* Configure auto-delayed mode */ + HRTIM_timcr = HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR; + HRTIM_timcr &= ~HRTIM_TIMCR_DELCMP2; + HRTIM_timcr |= pCompareCfg->AutoDelayedMode; + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR = HRTIM_timcr; + + /* Set the compare value for timeout compare unit (if any) */ + if (pCompareCfg->AutoDelayedMode == HRTIM_AUTODELAYEDMODE_AUTODELAYED_TIMEOUTCMP1) + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP1xR = pCompareCfg->AutoDelayedTimeout; + } + else if (pCompareCfg->AutoDelayedMode == HRTIM_AUTODELAYEDMODE_AUTODELAYED_TIMEOUTCMP3) + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP3xR = pCompareCfg->AutoDelayedTimeout; + } + } + } + break; + case HRTIM_COMPAREUNIT_3: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP3xR = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_4: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP4xR = pCompareCfg->CompareValue; + + if (pCompareCfg->AutoDelayedMode != HRTIM_AUTODELAYEDMODE_REGULAR) + { + /* Configure auto-delayed mode */ + HRTIM_timcr = HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR; + HRTIM_timcr &= ~HRTIM_TIMCR_DELCMP4; + HRTIM_timcr |= (pCompareCfg->AutoDelayedMode << 2); + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR = HRTIM_timcr; + + /* Set the compare value for timeout compare unit (if any) */ + if (pCompareCfg->AutoDelayedMode == HRTIM_AUTODELAYEDMODE_AUTODELAYED_TIMEOUTCMP1) + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP1xR = pCompareCfg->AutoDelayedTimeout; + } + else if (pCompareCfg->AutoDelayedMode == HRTIM_AUTODELAYEDMODE_AUTODELAYED_TIMEOUTCMP3) + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP3xR = pCompareCfg->AutoDelayedTimeout; + } + } + } + break; + default: + break; + } +} + +/** + * @brief Sets the HRTIMx Master Comparex Register value + * @param HRTIMx: pointer to HRTIMx peripheral + * @param CompareUnit: Compare unit to configure + * This parameter can be one of the following values: + * @arg HRTIM_COMPAREUNIT_1: Compare unit 1 + * @arg HRTIM_COMPAREUNIT_2: Compare unit 2 + * @arg HRTIM_COMPAREUNIT_3: Compare unit 3 + * @arg HRTIM_COMPAREUNIT_4: Compare unit 4 + * @param Compare: specifies the Comparex register new value + * @retval None + */ +void HRTIM_MasterSetCompare(HRTIM_TypeDef * HRTIMx, + uint32_t CompareUnit, + uint32_t Compare) +{ + /* Check parameters */ + assert_param(IS_HRTIM_COMPAREUNIT(CompareUnit)); + + /* Configure the compare unit */ + switch (CompareUnit) + { + case HRTIM_COMPAREUNIT_1: + { + /* Set the compare value */ + HRTIMx->HRTIM_MASTER.MCMP1R = Compare; + } + break; + case HRTIM_COMPAREUNIT_2: + { + /* Set the compare value */ + HRTIMx->HRTIM_MASTER.MCMP2R = Compare; + } + break; + case HRTIM_COMPAREUNIT_3: + { + /* Set the compare value */ + HRTIMx->HRTIM_MASTER.MCMP3R = Compare; + } + break; + case HRTIM_COMPAREUNIT_4: + { + /* Set the compare value */ + HRTIMx->HRTIM_MASTER.MCMP4R = Compare; + } + break; + default: + break; + } +} + +/** + * @brief Sets the HRTIMx Slave Comparex Register value + * @param HRTIMx: pointer to HRTIMx peripheral + * @param CompareUnit: Compare unit to configure + * This parameter can be one of the following values: + * @arg HRTIM_COMPAREUNIT_1: Compare unit 1 + * @arg HRTIM_COMPAREUNIT_2: Compare unit 2 + * @arg HRTIM_COMPAREUNIT_3: Compare unit 3 + * @arg HRTIM_COMPAREUNIT_4: Compare unit 4 + * @param Compare: specifies the Comparex register new value + * @retval None + */ +void HRTIM_SlaveSetCompare(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CompareUnit, + uint32_t Compare) +{ + /* Check parameters */ + assert_param(IS_HRTIM_COMPAREUNIT(CompareUnit)); + + /* Configure the compare unit */ + switch (CompareUnit) + { + case HRTIM_COMPAREUNIT_1: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP1xR = Compare; + } + break; + case HRTIM_COMPAREUNIT_2: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP2xR = Compare; + } + break; + case HRTIM_COMPAREUNIT_3: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP3xR = Compare; + } + break; + case HRTIM_COMPAREUNIT_4: + { + /* Set the compare value */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP4xR = Compare; + } + break; + default: + break; + } +} +/** + * @brief Configures the capture unit of a timer operating in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CaptureChannel: Capture unit to configure + * This parameter can be one of the following values: + * @arg HRTIM_CAPTUREUNIT_1: Capture unit 1 + * @arg HRTIM_CAPTUREUNIT_2: Capture unit 2 + * @param pCaptureCfg: pointer to the compare unit configuration structure + * @retval None + */ +void HRTIM_WaveformCaptureConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit, + HRTIM_CaptureCfgTypeDef* pCaptureCfg) +{ + /* Configure the capture unit */ + switch (CaptureUnit) + { + case HRTIM_CAPTUREUNIT_1: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT1xCR = pCaptureCfg->Trigger; + } + break; + case HRTIM_CAPTUREUNIT_2: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT2xCR = pCaptureCfg->Trigger; + } + break; + default: + break; + } +} + +/** + * @brief Configures the output of a timer operating in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param Output: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @param pOutputCfg: pointer to the timer output configuration structure + * @retval None + */ +void HRTIM_WaveformOutputConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output, + HRTIM_OutputCfgTypeDef * pOutputCfg) +{ + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, Output)); + assert_param(IS_HRTIM_OUTPUTPOLARITY(pOutputCfg->Polarity)); + assert_param(IS_HRTIM_OUTPUTIDLESTATE(pOutputCfg->IdleState)); + assert_param(IS_HRTIM_OUTPUTIDLEMODE(pOutputCfg->IdleMode)); + assert_param(IS_HRTIM_OUTPUTFAULTSTATE(pOutputCfg->FaultState)); + assert_param(IS_HRTIM_OUTPUTCHOPPERMODE(pOutputCfg->ChopperModeEnable)); + assert_param(IS_HRTIM_OUTPUTBURSTMODEENTRY(pOutputCfg->BurstModeEntryDelayed)); + + /* Configure the timer output */ + HRTIM_OutputConfig(HRTIMx, TimerIdx, Output, pOutputCfg); +} + +/** + * @brief Configures the event filtering capabilities of a timer (blanking, windowing) + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param Event: external event for which timer event filtering must be configured + * This parameter can be one of the following values: + * @arg HRTIM_EVENT_1: External event 1 + * @arg HRTIM_EVENT_2: External event 2 + * @arg HRTIM_EVENT_3: External event 3 + * @arg HRTIM_EVENT_4: External event 4 + * @arg HRTIM_EVENT_5: External event 5 + * @arg HRTIM_EVENT_6: External event 6 + * @arg HRTIM_EVENT_7: External event 7 + * @arg HRTIM_EVENT_8: External event 8 + * @arg HRTIM_EVENT_9: External event 9 + * @arg HRTIM_EVENT_10: External event 10 + * @param pTimerEventFilteringCfg: pointer to the timer event filtering configuration structure + * @retval None + */ +void HRTIM_TimerEventFilteringConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Event, + HRTIM_TimerEventFilteringCfgTypeDef* pTimerEventFilteringCfg) +{ + uint32_t HRTIM_eefr; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_EVENT(Event)); + assert_param(IS_HRTIM_TIMEVENTFILTER(pTimerEventFilteringCfg->Filter)); + assert_param(IS_HRTIM_TIMEVENTLATCH(pTimerEventFilteringCfg->Latch)); + + /* Configure timer event filtering capabilities */ + switch (Event) + { + case HRTIM_TIMEVENTFILTER_NONE: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1 = 0; + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2 = 0; + } + break; + case HRTIM_EVENT_1: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1; + HRTIM_eefr &= ~(HRTIM_EEFR1_EE1FLTR | HRTIM_EEFR1_EE1LTCH); + HRTIM_eefr |= (pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_2: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1; + HRTIM_eefr &= ~(HRTIM_EEFR1_EE2FLTR | HRTIM_EEFR1_EE2LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 6); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_3: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1; + HRTIM_eefr &= ~(HRTIM_EEFR1_EE3FLTR | HRTIM_EEFR1_EE3LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 12); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_4: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1; + HRTIM_eefr &= ~(HRTIM_EEFR1_EE4FLTR | HRTIM_EEFR1_EE4LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 18); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_5: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1; + HRTIM_eefr &= ~(HRTIM_EEFR1_EE5FLTR | HRTIM_EEFR1_EE5LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 24); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR1 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_6: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2; + HRTIM_eefr &= ~(HRTIM_EEFR2_EE6FLTR | HRTIM_EEFR2_EE6LTCH); + HRTIM_eefr |= (pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_7: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2; + HRTIM_eefr &= ~(HRTIM_EEFR2_EE7FLTR | HRTIM_EEFR2_EE7LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 6); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_8: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2; + HRTIM_eefr &= ~(HRTIM_EEFR2_EE8FLTR | HRTIM_EEFR2_EE8LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 12); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_9: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2; + HRTIM_eefr &= ~(HRTIM_EEFR2_EE9FLTR | HRTIM_EEFR2_EE9LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 18); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2 = HRTIM_eefr; + } + break; + case HRTIM_EVENT_10: + { + HRTIM_eefr = HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2; + HRTIM_eefr &= ~(HRTIM_EEFR2_EE10FLTR | HRTIM_EEFR2_EE10LTCH); + HRTIM_eefr |= ((pTimerEventFilteringCfg->Filter | pTimerEventFilteringCfg->Latch) << 24); + HRTIMx->HRTIM_TIMERx[TimerIdx].EEFxR2 = HRTIM_eefr; + } + break; + default: + break; + } +} + +/** + * @brief Configures the dead time insertion feature for a timer + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param pDeadTimeCfg: pointer to the dead time insertion configuration structure + * @retval None + */ +void HRTIM_DeadTimeConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_DeadTimeCfgTypeDef* pDeadTimeCfg) +{ + uint32_t HRTIM_dtr; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_TIMDEADTIME_RISINGSIGN(pDeadTimeCfg->RisingSign)); + assert_param(IS_HRTIM_TIMDEADTIME_RISINGLOCK(pDeadTimeCfg->RisingLock)); + assert_param(IS_HRTIM_TIMDEADTIME_RISINGSIGNLOCK(pDeadTimeCfg->RisingSignLock)); + assert_param(IS_HRTIM_TIMDEADTIME_FALLINGSIGN(pDeadTimeCfg->FallingSign)); + assert_param(IS_HRTIM_TIMDEADTIME_FALLINGLOCK(pDeadTimeCfg->FallingLock)); + assert_param(IS_HRTIM_TIMDEADTIME_FALLINGSIGNLOCK(pDeadTimeCfg->FallingSignLock)); + + HRTIM_dtr = HRTIMx->HRTIM_TIMERx[TimerIdx].DTxR; + + /* Clear timer dead times configuration */ + HRTIM_dtr &= ~(HRTIM_DTR_DTR | HRTIM_DTR_SDTR | HRTIM_DTR_DTPRSC | + HRTIM_DTR_DTRSLK | HRTIM_DTR_DTRLK | HRTIM_DTR_SDTF | + HRTIM_DTR_SDTR | HRTIM_DTR_DTFSLK | HRTIM_DTR_DTFLK); + + /* Set timer dead times configuration */ + HRTIM_dtr |= (pDeadTimeCfg->Prescaler << 10); + HRTIM_dtr |= pDeadTimeCfg->RisingValue; + HRTIM_dtr |= pDeadTimeCfg->RisingSign; + HRTIM_dtr |= pDeadTimeCfg->RisingSignLock; + HRTIM_dtr |= pDeadTimeCfg->RisingLock; + HRTIM_dtr |= (pDeadTimeCfg->FallingValue << 16); + HRTIM_dtr |= pDeadTimeCfg->FallingSign; + HRTIM_dtr |= pDeadTimeCfg->FallingSignLock; + HRTIM_dtr |= pDeadTimeCfg->FallingLock; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_TIMERx[TimerIdx].DTxR = HRTIM_dtr; +} + +/** + * @brief Configures the chopper mode feature for a timer + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param pChopperModeCfg: pointer to the chopper mode configuration structure + * @retval None + */ +void HRTIM_ChopperModeConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_ChopperModeCfgTypeDef* pChopperModeCfg) +{ + uint32_t HRTIM_chpr; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + + HRTIM_chpr = HRTIMx->HRTIM_TIMERx[TimerIdx].CHPxR; + + /* Clear timer chopper mode configuration */ + HRTIM_chpr &= ~(HRTIM_CHPR_CARFRQ | HRTIM_CHPR_CARDTY | HRTIM_CHPR_STRPW); + + /* Set timer chopper mode configuration */ + HRTIM_chpr |= pChopperModeCfg->CarrierFreq; + HRTIM_chpr |= (pChopperModeCfg->DutyCycle << 4); + HRTIM_chpr |= (pChopperModeCfg->StartPulse << 7); + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_TIMERx[TimerIdx].CHPxR = HRTIM_chpr; +} + +/** + * @brief Configures the burst DMA controller for a timer + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x5 for master timer + * @arg 0x0 to 0x4 for timers A to E + * @param RegistersToUpdate: registers to be written by DMA + * This parameter can be any combination of the following values: + * @arg HRTIM_BURSTDMA_CR: HRTIM_MCR or HRTIM_TIMxCR + * @arg HRTIM_BURSTDMA_ICR: HRTIM_MICR or HRTIM_TIMxICR + * @arg HRTIM_BURSTDMA_DIER: HRTIM_MDIER or HRTIM_TIMxDIER + * @arg HRTIM_BURSTDMA_CNT: HRTIM_MCNT or HRTIM_TIMxCNT + * @arg HRTIM_BURSTDMA_PER: HRTIM_MPER or HRTIM_TIMxPER + * @arg HRTIM_BURSTDMA_REP: HRTIM_MREP or HRTIM_TIMxREP + * @arg HRTIM_BURSTDMA_CMP1: HRTIM_MCMP1 or HRTIM_TIMxCMP1 + * @arg HRTIM_BURSTDMA_CMP2: HRTIM_MCMP2 or HRTIM_TIMxCMP2 + * @arg HRTIM_BURSTDMA_CMP3: HRTIM_MCMP3 or HRTIM_TIMxCMP3 + * @arg HRTIM_BURSTDMA_CMP4: HRTIM_MCMP4 or HRTIM_TIMxCMP4 + * @arg HRTIM_BURSTDMA_DTR: HRTIM_TIMxDTR + * @arg HRTIM_BURSTDMA_SET1R: HRTIM_TIMxSET1R + * @arg HRTIM_BURSTDMA_RST1R: HRTIM_TIMxRST1R + * @arg HRTIM_BURSTDMA_SET2R: HRTIM_TIMxSET2R + * @arg HRTIM_BURSTDMA_RST2R: HRTIM_TIMxRST2R + * @arg HRTIM_BURSTDMA_EEFR1: HRTIM_TIMxEEFR1 + * @arg HRTIM_BURSTDMA_EEFR2: HRTIM_TIMxEEFR2 + * @arg HRTIM_BURSTDMA_RSTR: HRTIM_TIMxRSTR + * @arg HRTIM_BURSTDMA_CHPR: HRTIM_TIMxCHPR + * @arg HRTIM_BURSTDMA_OUTR: HRTIM_TIMxOUTR + * @arg HRTIM_BURSTDMA_FLTR: HRTIM_TIMxFLTR + * @retval None + */ +void HRTIM_BurstDMAConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t RegistersToUpdate) +{ + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_BURSTDMA(TimerIdx, RegistersToUpdate)); + + /* Set the burst DMA timer update register */ + switch (TimerIdx) + { + case HRTIM_TIMERINDEX_TIMER_A: + { + HRTIMx->HRTIM_COMMON.BDTAUPR = RegistersToUpdate; + } + break; + case HRTIM_TIMERINDEX_TIMER_B: + { + HRTIMx->HRTIM_COMMON.BDTBUPR = RegistersToUpdate; + } + break; + case HRTIM_TIMERINDEX_TIMER_C: + { + HRTIMx->HRTIM_COMMON.BDTCUPR = RegistersToUpdate; + } + break; + case HRTIM_TIMERINDEX_TIMER_D: + { + HRTIMx->HRTIM_COMMON.BDTDUPR = RegistersToUpdate; + } + break; + case HRTIM_TIMERINDEX_TIMER_E: + { + HRTIMx->HRTIM_COMMON.BDTEUPR = RegistersToUpdate; + } + break; + case HRTIM_TIMERINDEX_MASTER: + { + HRTIMx->HRTIM_COMMON.BDMUPDR = RegistersToUpdate; + } + break; + default: + break; + } +} + +/** + * @brief Configures the external input/output synchronization of the HRTIMx + * @param HRTIMx: pointer to HRTIMx peripheral + * @param pSynchroCfg: pointer to the input/output synchronization configuration structure + * @retval None + */ +void HRTIM_SynchronizationConfig(HRTIM_TypeDef *HRTIMx, HRTIM_SynchroCfgTypeDef * pSynchroCfg) +{ + uint32_t HRTIM_mcr; + + /* Check parameters */ + assert_param(IS_HRTIM_SYNCINPUTSOURCE(pSynchroCfg->SyncInputSource)); + assert_param(IS_HRTIM_SYNCOUTPUTSOURCE(pSynchroCfg->SyncOutputSource)); + assert_param(IS_HRTIM_SYNCOUTPUTPOLARITY(pSynchroCfg->SyncOutputPolarity)); + + HRTIM_mcr = HRTIMx->HRTIM_MASTER.MCR; + + /* Set the synchronization input source */ + HRTIM_mcr &= ~(HRTIM_MCR_SYNC_IN); + HRTIM_mcr |= pSynchroCfg->SyncInputSource; + + /* Set the event to be sent on the synchronization output */ + HRTIM_mcr &= ~(HRTIM_MCR_SYNC_SRC); + HRTIM_mcr |= pSynchroCfg->SyncOutputSource; + + /* Set the polarity of the synchronization output */ + HRTIM_mcr &= ~(HRTIM_MCR_SYNC_OUT); + HRTIM_mcr |= pSynchroCfg->SyncOutputPolarity; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_MASTER.MCR = HRTIM_mcr; +} + +/** + * @brief Configures the burst mode feature of the HRTIMx + * @param HRTIMx: pointer to HRTIMx peripheral + * @param pBurstModeCfg: pointer to the burst mode configuration structure + * @retval None + */ +void HRTIM_BurstModeConfig(HRTIM_TypeDef * HRTIMx, + HRTIM_BurstModeCfgTypeDef* pBurstModeCfg) +{ + uint32_t HRTIM_bmcr; + + /* Check parameters */ + assert_param(IS_HRTIM_BURSTMODE(pBurstModeCfg->Mode)); + assert_param(IS_HRTIM_BURSTMODECLOCKSOURCE(pBurstModeCfg->ClockSource)); + assert_param(IS_HRTIM_HRTIM_BURSTMODEPRESCALER(pBurstModeCfg->Prescaler)); + assert_param(IS_HRTIM_BURSTMODEPRELOAD(pBurstModeCfg->PreloadEnable)); + + HRTIM_bmcr = HRTIMx->HRTIM_COMMON.BMCR; + + /* Set the burst mode operating mode */ + HRTIM_bmcr &= ~(HRTIM_BMCR_BMOM); + HRTIM_bmcr |= pBurstModeCfg->Mode; + + /* Set the burst mode clock source */ + HRTIM_bmcr &= ~(HRTIM_BMCR_BMCLK); + HRTIM_bmcr |= pBurstModeCfg->ClockSource; + + /* Set the burst mode prescaler */ + HRTIM_bmcr &= ~(HRTIM_BMCR_BMPSC); + HRTIM_bmcr |= pBurstModeCfg->Prescaler; + + /* Enable/disable burst mode registers preload */ + HRTIM_bmcr &= ~(HRTIM_BMCR_BMPREN); + HRTIM_bmcr |= pBurstModeCfg->PreloadEnable; + + /* Set the burst mode trigger */ + HRTIMx->HRTIM_COMMON.BMTRGR = pBurstModeCfg->Trigger; + + /* Set the burst mode compare value */ + HRTIMx->HRTIM_COMMON.BMCMPR = pBurstModeCfg->IdleDuration; + + /* Set the burst mode period */ + HRTIMx->HRTIM_COMMON.BMPER = pBurstModeCfg->Period; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.BMCR = HRTIM_bmcr; +} + +/** + * @brief Configures the conditioning of an external event + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Event: external event to configure + * This parameter can be one of the following values: + * @arg HRTIM_EVENT_1: External event 1 + * @arg HRTIM_EVENT_2: External event 2 + * @arg HRTIM_EVENT_3: External event 3 + * @arg HRTIM_EVENT_4: External event 4 + * @arg HRTIM_EVENT_5: External event 5 + * @arg HRTIM_EVENT_6: External event 6 + * @arg HRTIM_EVENT_7: External event 7 + * @arg HRTIM_EVENT_8: External event 8 + * @arg HRTIM_EVENT_9: External event 9 + * @arg HRTIM_EVENT_10: External event 10 + * @param pEventCfg: pointer to the event conditioning configuration structure + * @retval None + */ +void HRTIM_EventConfig(HRTIM_TypeDef * HRTIMx, + uint32_t Event, + HRTIM_EventCfgTypeDef* pEventCfg) +{ + /* Check parameters */ + assert_param(IS_HRTIM_EVENTSRC(pEventCfg->Source)); + assert_param(IS_HRTIM_EVENTPOLARITY(pEventCfg->Polarity)); + assert_param(IS_HRTIM_EVENTSENSITIVITY(pEventCfg->Sensitivity)); + assert_param(IS_HRTIM_EVENTFASTMODE(pEventCfg->FastMode)); + assert_param(IS_HRTIM_EVENTFILTER(pEventCfg->Filter)); + + /* Configure the event channel */ + HRTIM_ExternalEventConfig(HRTIMx, Event, pEventCfg); + +} + +/** + * @brief Configures the external event conditioning block prescaler + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Prescaler: Prescaler value + * This parameter can be one of the following values: + * @arg HRTIM_EVENTPRESCALER_DIV1: fEEVS=fHRTIMx + * @arg HRTIM_EVENTPRESCALER_DIV2: fEEVS=fHRTIMx / 2 + * @arg HRTIM_EVENTPRESCALER_DIV4: fEEVS=fHRTIMx / 4 + * @arg HRTIM_EVENTPRESCALER_DIV8: fEEVS=fHRTIMx / 8 + * @retval None + */ +void HRTIM_EventPrescalerConfig(HRTIM_TypeDef * HRTIMx, + uint32_t Prescaler) +{ + uint32_t HRTIM_eecr3; + + /* Check parameters */ + assert_param(IS_HRTIM_EVENTPRESCALER(Prescaler)); + + /* Set the external event prescaler */ + HRTIM_eecr3 = HRTIMx->HRTIM_COMMON.EECR3; + HRTIM_eecr3 &= ~(HRTIM_EECR3_EEVSD); + HRTIM_eecr3 |= Prescaler; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.EECR3 = HRTIM_eecr3; +} + +/** + * @brief Configures the conditioning of fault input + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Fault: fault input to configure + * This parameter can be one of the following values: + * @arg HRTIM_FAULT_1: Fault input 1 + * @arg HRTIM_FAULT_2: Fault input 2 + * @arg HRTIM_FAULT_3: Fault input 3 + * @arg HRTIM_FAULT_4: Fault input 4 + * @arg HRTIM_FAULT_5: Fault input 5 + * @param pFaultCfg: pointer to the fault conditioning configuration structure + * @retval None + */ +void HRTIM_FaultConfig(HRTIM_TypeDef * HRTIMx, + HRTIM_FaultCfgTypeDef* pFaultCfg, + uint32_t Fault) +{ + uint32_t HRTIM_fltinr1; + uint32_t HRTIM_fltinr2; + + /* Check parameters */ + assert_param(IS_HRTIM_FAULT(Fault)); + assert_param(IS_HRTIM_FAULTSOURCE(pFaultCfg->Source)); + assert_param(IS_HRTIM_FAULTPOLARITY(pFaultCfg->Polarity)); + assert_param(IS_HRTIM_FAULTFILTER(pFaultCfg->Filter)); + assert_param(IS_HRTIM_FAULTLOCK(pFaultCfg->Lock)); + + /* Configure fault channel */ + HRTIM_fltinr1 = HRTIMx->HRTIM_COMMON.FLTINxR1; + HRTIM_fltinr2 = HRTIMx->HRTIM_COMMON.FLTINxR2; + + switch (Fault) + { + case HRTIM_FAULT_1: + { + HRTIM_fltinr1 &= ~(HRTIM_FLTINR1_FLT1P | HRTIM_FLTINR1_FLT1SRC | HRTIM_FLTINR1_FLT1F | HRTIM_FLTINR1_FLT1LCK); + HRTIM_fltinr1 |= pFaultCfg->Polarity; + HRTIM_fltinr1 |= pFaultCfg->Source; + HRTIM_fltinr1 |= pFaultCfg->Filter; + HRTIM_fltinr1 |= pFaultCfg->Lock; + } + break; + case HRTIM_FAULT_2: + { + HRTIM_fltinr1 &= ~(HRTIM_FLTINR1_FLT2P | HRTIM_FLTINR1_FLT2SRC | HRTIM_FLTINR1_FLT2F | HRTIM_FLTINR1_FLT2LCK); + HRTIM_fltinr1 |= (pFaultCfg->Polarity << 8); + HRTIM_fltinr1 |= (pFaultCfg->Source << 8); + HRTIM_fltinr1 |= (pFaultCfg->Filter << 8); + HRTIM_fltinr1 |= (pFaultCfg->Lock << 8); + } + break; + case HRTIM_FAULT_3: + { + HRTIM_fltinr1 &= ~(HRTIM_FLTINR1_FLT3P | HRTIM_FLTINR1_FLT3SRC | HRTIM_FLTINR1_FLT3F | HRTIM_FLTINR1_FLT3LCK); + HRTIM_fltinr1 |= (pFaultCfg->Polarity << 16); + HRTIM_fltinr1 |= (pFaultCfg->Source << 16); + HRTIM_fltinr1 |= (pFaultCfg->Filter << 16); + HRTIM_fltinr1 |= (pFaultCfg->Lock << 16); + } + break; + case HRTIM_FAULT_4: + { + HRTIM_fltinr1 &= ~(HRTIM_FLTINR1_FLT4P | HRTIM_FLTINR1_FLT4SRC | HRTIM_FLTINR1_FLT4F | HRTIM_FLTINR1_FLT4LCK); + HRTIM_fltinr1 |= (pFaultCfg->Polarity << 24); + HRTIM_fltinr1 |= (pFaultCfg->Source << 24); + HRTIM_fltinr1 |= (pFaultCfg->Filter << 24); + HRTIM_fltinr1 |= (pFaultCfg->Lock << 24); + } + break; + case HRTIM_FAULT_5: + { + HRTIM_fltinr2 &= ~(HRTIM_FLTINR2_FLT5P | HRTIM_FLTINR2_FLT5SRC | HRTIM_FLTINR2_FLT5F | HRTIM_FLTINR2_FLT5LCK); + HRTIM_fltinr2 |= pFaultCfg->Polarity; + HRTIM_fltinr2 |= pFaultCfg->Source; + HRTIM_fltinr2 |= pFaultCfg->Filter; + HRTIM_fltinr2 |= pFaultCfg->Lock; + } + break; + default: + break; + } + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.FLTINxR1 = HRTIM_fltinr1; + HRTIMx->HRTIM_COMMON.FLTINxR2 = HRTIM_fltinr2; +} + +/** + * @brief Configures the fault conditioning block prescaler + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Prescaler: Prescaler value + * This parameter can be one of the following values: + * @arg HRTIM_FAULTPRESCALER_DIV1: fFLTS=fHRTIMx + * @arg HRTIM_FAULTPRESCALER_DIV2: fFLTS=fHRTIMx / 2 + * @arg HRTIM_FAULTPRESCALER_DIV4: fFLTS=fHRTIMx / 4 + * @arg HRTIM_FAULTPRESCALER_DIV8: fFLTS=fHRTIMx / 8 + * @retval None + */ +void HRTIM_FaultPrescalerConfig(HRTIM_TypeDef * HRTIMx, + uint32_t Prescaler) +{ + uint32_t HRTIM_fltinr2; + + /* Check parameters */ + assert_param(IS_HRTIM_FAULTPRESCALER(Prescaler)); + + /* Set the external event prescaler */ + HRTIM_fltinr2 = HRTIMx->HRTIM_COMMON.FLTINxR2; + HRTIM_fltinr2 &= ~(HRTIM_FLTINR2_FLTSD); + HRTIM_fltinr2 |= Prescaler; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.FLTINxR2 = HRTIM_fltinr2; +} + +/** + * @brief Enables or disables the HRTIMx Fault mode. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Fault: fault input to configure + * This parameter can be one of the following values: + * @arg HRTIM_FAULT_1: Fault input 1 + * @arg HRTIM_FAULT_2: Fault input 2 + * @arg HRTIM_FAULT_3: Fault input 3 + * @arg HRTIM_FAULT_4: Fault input 4 + * @arg HRTIM_FAULT_5: Fault input 5 + * @param Enable: Fault mode controller enabling + * This parameter can be one of the following values: + * @arg HRTIM_FAULT_ENABLED: Fault mode enabled + * @arg HRTIM_FAULT_DISABLED: Fault mode disabled + * @retval None + */ +void HRTIM_FaultModeCtl(HRTIM_TypeDef * HRTIMx, uint32_t Fault, uint32_t Enable) +{ + uint32_t HRTIM_fltinr1; + uint32_t HRTIM_fltinr2; + + /* Check parameters */ + assert_param(IS_HRTIM_FAULT(Fault)); + assert_param(IS_HRTIM_FAULTCTL(Enable)); + + /* Configure fault channel */ + HRTIM_fltinr1 = HRTIMx->HRTIM_COMMON.FLTINxR1; + HRTIM_fltinr2 = HRTIMx->HRTIM_COMMON.FLTINxR2; + + switch (Fault) + { + case HRTIM_FAULT_1: + { + HRTIM_fltinr1 &= ~HRTIM_FLTINR1_FLT1E; + HRTIM_fltinr1 |= Enable; + } + break; + case HRTIM_FAULT_2: + { + HRTIM_fltinr1 &= ~HRTIM_FLTINR1_FLT2E; + HRTIM_fltinr1 |= (Enable<< 8); + } + break; + case HRTIM_FAULT_3: + { + HRTIM_fltinr1 &= ~HRTIM_FLTINR1_FLT3E; + HRTIM_fltinr1 |= (Enable << 16); + } + break; + case HRTIM_FAULT_4: + { + HRTIM_fltinr1 &= ~HRTIM_FLTINR1_FLT4E; + HRTIM_fltinr1 |= (Enable << 24); + } + break; + case HRTIM_FAULT_5: + { + HRTIM_fltinr2 &= ~HRTIM_FLTINR2_FLT5E; + HRTIM_fltinr2 |= Enable; + } + break; + default: + break; + } + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.FLTINxR1 = HRTIM_fltinr1; + HRTIMx->HRTIM_COMMON.FLTINxR2 = HRTIM_fltinr2; +} + +/** + * @brief Configures both the ADC trigger register update source and the ADC + * trigger source. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param ADC trigger: ADC trigger to configure + * This parameter can be one of the following values: + * @arg HRTIM_ADCTRIGGER_1: ADC trigger 1 + * @arg HRTIM_ADCTRIGGER_2: ADC trigger 2 + * @arg HRTIM_ADCTRIGGER_3: ADC trigger 3 + * @arg HRTIM_ADCTRIGGER_4: ADC trigger 4 + * @param pADCTriggerCfg: pointer to the ADC trigger configuration structure + * @retval None + */ +void HRTIM_ADCTriggerConfig(HRTIM_TypeDef * HRTIMx, + uint32_t ADCTrigger, + HRTIM_ADCTriggerCfgTypeDef* pADCTriggerCfg) +{ + uint32_t HRTIM_cr1; + + /* Check parameters */ + assert_param(IS_HRTIM_ADCTRIGGER(ADCTrigger)); + assert_param(IS_HRTIM_ADCTRIGGERUPDATE(pADCTriggerCfg->UpdateSource)); + + /* Set the ADC trigger update source */ + HRTIM_cr1 = HRTIMx->HRTIM_COMMON.CR1; + + switch (ADCTrigger) + { + case HRTIM_ADCTRIGGER_1: + { + HRTIM_cr1 &= ~(HRTIM_CR1_ADC1USRC); + HRTIM_cr1 |= pADCTriggerCfg->UpdateSource; + + /* Set the ADC trigger 1 source */ + HRTIMx->HRTIM_COMMON.ADC1R = pADCTriggerCfg->Trigger; + } + break; + case HRTIM_ADCTRIGGER_2: + { + HRTIM_cr1 &= ~(HRTIM_CR1_ADC2USRC); + HRTIM_cr1 |= (pADCTriggerCfg->UpdateSource << 3); + + /* Set the ADC trigger 2 source */ + HRTIMx->HRTIM_COMMON.ADC2R = pADCTriggerCfg->Trigger; + } + break; + case HRTIM_ADCTRIGGER_3: + { + HRTIM_cr1 &= ~(HRTIM_CR1_ADC3USRC); + HRTIM_cr1 |= (pADCTriggerCfg->UpdateSource << 6); + + /* Set the ADC trigger 3 source */ + HRTIMx->HRTIM_COMMON.ADC3R = pADCTriggerCfg->Trigger; + } + case HRTIM_ADCTRIGGER_4: + { + HRTIM_cr1 &= ~(HRTIM_CR1_ADC4USRC); + HRTIM_cr1 |= (pADCTriggerCfg->UpdateSource << 9); + + /* Set the ADC trigger 4 source */ + HRTIMx->HRTIM_COMMON.ADC4R = pADCTriggerCfg->Trigger; + } + break; + default: + break; + } + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.CR1 = HRTIM_cr1; +} + + +/** + * @brief Enables or disables the HRTIMx burst mode controller. + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Enable: Burst mode controller enabling + * This parameter can be one of the following values: + * @arg HRTIM_BURSTMODECTL_ENABLED: Burst mode enabled + * @arg HRTIM_BURSTMODECTL_DISABLED: Burst mode disabled + * @retval None + */ +void HRTIM_BurstModeCtl(HRTIM_TypeDef * HRTIMx, uint32_t Enable) +{ + uint32_t HRTIM_bmcr; + + /* Check parameters */ + assert_param(IS_HRTIM_BURSTMODECTL(Enable)); + + /* Enable/Disable the burst mode controller */ + HRTIM_bmcr = HRTIMx->HRTIM_COMMON.BMCR; + HRTIM_bmcr &= ~(HRTIM_BMCR_BME); + HRTIM_bmcr |= Enable; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_COMMON.BMCR = HRTIM_bmcr; +} + +/** + * @brief Triggers a software capture on the designed capture unit + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CaptureUnit: Capture unit to trig + * This parameter can be one of the following values: + * @arg HRTIM_CAPTUREUNIT_1: Capture unit 1 + * @arg HRTIM_CAPTUREUNIT_2: Capture unit 2 + * @retval None + * @note The 'software capture' bit in the capture configuration register is + * automatically reset by hardware + */ +void HRTIM_SoftwareCapture(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit) +{ + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_CAPTUREUNIT(CaptureUnit)); + + /* Force a software capture on concerned capture unit */ + switch (CaptureUnit) + { + case HRTIM_CAPTUREUNIT_1: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT1xCR |= HRTIM_CPT1CR_SWCPT; + } + break; + case HRTIM_CAPTUREUNIT_2: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT2xCR |= HRTIM_CPT2CR_SWCPT; + } + break; + default: + break; + } +} + +/** + * @brief Triggers the update of the registers of one or several timers + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimersToUpdate: timers concerned with the software register update + * This parameter can be any combination of the following values: + * @arg HRTIM_TIMERUPDATE_MASTER + * @arg HRTIM_TIMERUPDATE_A + * @arg HRTIM_TIMERUPDATE_B + * @arg HRTIM_TIMERUPDATE_C + * @arg HRTIM_TIMERUPDATE_D + * @arg HRTIM_TIMERUPDATE_E + * @retval None + * @note The 'software update' bits in the HRTIMx control register 2 register are + * automatically reset by hardware + */ +void HRTIM_SoftwareUpdate(HRTIM_TypeDef * HRTIMx, + uint32_t TimersToUpdate) +{ + /* Check parameters */ + assert_param(IS_HRTIM_TIMERUPDATE(TimersToUpdate)); + + /* Force timer(s) registers update */ + HRTIMx->HRTIM_COMMON.CR2 |= TimersToUpdate; + +} + +/** + * @brief Triggers the reset of one or several timers + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimersToUpdate: timers concerned with the software counter reset + * This parameter can be any combination of the following values: + * @arg HRTIM_TIMER_MASTER + * @arg HRTIM_TIMER_A + * @arg HRTIM_TIMER_B + * @arg HRTIM_TIMER_C + * @arg HRTIM_TIMER_D + * @arg HRTIM_TIMER_E + * @retval None + * @note The 'software reset' bits in the HRTIMx control register 2 are + * automatically reset by hardware + */ +void HRTIM_SoftwareReset(HRTIM_TypeDef * HRTIMx, + uint32_t TimersToReset) +{ + /* Check parameters */ + assert_param(IS_HRTIM_TIMERRESET(TimersToReset)); + + /* Force timer(s) registers update */ + HRTIMx->HRTIM_COMMON.CR2 |= TimersToReset; + +} + +/** + * @brief Forces the timer output to its active or inactive state + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param Output: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @param OutputLevel: indicates whether the output is forced to its active or inactive state + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUTLEVEL_ACTIVE: output is forced to its active state + * @arg HRTIM_OUTPUTLEVEL_INACTIVE: output is forced to its inactive state + * @retval None + * @note The 'software set/reset trigger' bit in the output set/reset registers + * is automatically reset by hardware + */ +void HRTIM_WaveformSetOutputLevel(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output, + uint32_t OutputLevel) +{ + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, Output)); + assert_param(IS_HRTIM_OUTPUTLEVEL(OutputLevel)); + + /* Force timer output level */ + switch (Output) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + if (OutputLevel == HRTIM_OUTPUTLEVEL_ACTIVE) + { + /* Force output to its active state */ + HRTIMx->HRTIM_TIMERx[TimerIdx].SETx1R |= HRTIM_SET1R_SST; + } + else + { + /* Force output to its inactive state */ + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTx1R |= HRTIM_RST1R_SRT; + } + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + if (OutputLevel == HRTIM_OUTPUTLEVEL_ACTIVE) + { + /* Force output to its active state */ + HRTIMx->HRTIM_TIMERx[TimerIdx].SETx2R |= HRTIM_SET2R_SST; + } + else + { + /* Force output to its inactive state */ + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTx2R |= HRTIM_RST2R_SRT; + } + } + break; + default: + break; + } +} + + +/** + * @} + */ + +/** @defgroup HRTIM_Group4 Peripheral State methods + * @brief Peripheral State functions + * +@verbatim + =============================================================================== + ##### Peripheral State methods ##### + =============================================================================== + [..] + This subsection permit to get in run-time the status of the peripheral + and the data flow. + +@endverbatim + * @{ + */ + +/** + * @brief Returns actual value of the capture register of the designated capture unit + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param CaptureUnit: Capture unit to trig + * This parameter can be one of the following values: + * @arg HRTIM_CAPTUREUNIT_1: Capture unit 1 + * @arg HRTIM_CAPTUREUNIT_2: Capture unit 2 + * @retval Captured value + */ +uint32_t HRTIM_GetCapturedValue(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit) +{ + uint32_t captured_value = 0; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + assert_param(IS_HRTIM_CAPTUREUNIT(CaptureUnit)); + + /* Read captured value */ + switch (CaptureUnit) + { + case HRTIM_CAPTUREUNIT_1: + { + captured_value = HRTIMx->HRTIM_TIMERx[TimerIdx].CPT1xR; + } + break; + case HRTIM_CAPTUREUNIT_2: + { + captured_value = HRTIMx->HRTIM_TIMERx[TimerIdx].CPT2xR; + } + break; + default: + break; + } + + return captured_value; +} + +/** + * @brief Returns actual level (active or inactive) of the designated output + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param Output: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval Output level + * @note Returned output level is taken before the output stage (chopper, + * polarity). + */ +uint32_t HRTIM_WaveformGetOutputLevel(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output) +{ + uint32_t output_level = HRTIM_OUTPUTLEVEL_INACTIVE; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, Output)); + + /* Read the output level */ + switch (Output) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + if ((HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_TIMISR_O1CPY) != RESET) + { + output_level = HRTIM_OUTPUTLEVEL_ACTIVE; + } + else + { + output_level = HRTIM_OUTPUTLEVEL_INACTIVE; + } + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + if ((HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_TIMISR_O2CPY) != RESET) + { + output_level = HRTIM_OUTPUTLEVEL_ACTIVE; + } + else + { + output_level = HRTIM_OUTPUTLEVEL_INACTIVE; + } + } + break; + default: + break; + } + + return output_level; +} + +/** + * @brief Returns actual state (RUN, IDLE, FAULT) of the designated output + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param Output: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TE1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TE2: Timer E - Output 2 + * @retval Output state + */ +uint32_t HRTIM_WaveformGetOutputState(HRTIM_TypeDef * HRTIMx, + __attribute__((unused)) uint32_t TimerIdx, + uint32_t Output) +{ + uint32_t output_bit = 0; + uint32_t output_state = HRTIM_OUTPUTSTATE_IDLE; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, Output)); + + /* Set output state according to output control status and output disable status */ + switch (Output) + { + case HRTIM_OUTPUT_TA1: + { + output_bit = HRTIM_OENR_TA1OEN; + } + break; + case HRTIM_OUTPUT_TA2: + { + output_bit = HRTIM_OENR_TA2OEN; + } + break; + case HRTIM_OUTPUT_TB1: + { + output_bit = HRTIM_OENR_TB1OEN; + } + break; + case HRTIM_OUTPUT_TB2: + { + output_bit = HRTIM_OENR_TB2OEN; + } + break; + case HRTIM_OUTPUT_TC1: + { + output_bit = HRTIM_OENR_TC1OEN; + } + break; + case HRTIM_OUTPUT_TC2: + { + output_bit = HRTIM_OENR_TC2OEN; + } + break; + case HRTIM_OUTPUT_TD1: + { + output_bit = HRTIM_OENR_TD1OEN; + } + break; + case HRTIM_OUTPUT_TD2: + { + output_bit = HRTIM_OENR_TD2OEN; + } + break; + case HRTIM_OUTPUT_TE1: + { + output_bit = HRTIM_OENR_TE1OEN; + } + break; + case HRTIM_OUTPUT_TE2: + { + output_bit = HRTIM_OENR_TE2OEN; + } + break; + default: + break; + } + + if ((HRTIMx->HRTIM_COMMON.OENR & output_bit) != RESET) + { + /* Output is enabled: output in RUN state (whatever output disable status is)*/ + output_state = HRTIM_OUTPUTSTATE_RUN; + } + else + { + if ((HRTIMx->HRTIM_COMMON.ODSR & output_bit) != RESET) + { + /* Output is disabled: output in FAULT state */ + output_state = HRTIM_OUTPUTSTATE_FAULT; + } + else + { + /* Output is disabled: output in IDLE state */ + output_state = HRTIM_OUTPUTSTATE_IDLE; + } + } + + return(output_state); +} + +/** + * @brief Returns the level (active or inactive) of the designated output + * when the delayed protection was triggered + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @param Output: Timer output + * This parameter can be one of the following values: + * @arg HRTIM_OUTPUT_TA1: Timer A - Output 1 + * @arg HRTIM_OUTPUT_TA2: Timer A - Output 2 + * @arg HRTIM_OUTPUT_TB1: Timer B - Output 1 + * @arg HRTIM_OUTPUT_TB2: Timer B - Output 2 + * @arg HRTIM_OUTPUT_TC1: Timer C - Output 1 + * @arg HRTIM_OUTPUT_TC2: Timer C - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer D - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer D - Output 2 + * @arg HRTIM_OUTPUT_TD1: Timer E - Output 1 + * @arg HRTIM_OUTPUT_TD2: Timer E - Output 2 + * @retval Delayed protection status + */ +uint32_t HRTIM_GetDelayedProtectionStatus(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output) +{ + uint32_t delayed_protection_status = HRTIM_OUTPUTLEVEL_INACTIVE; + + /* Check parameters */ + assert_param(IS_HRTIM_TIMER_OUTPUT(TimerIdx, Output)); + + /* Read the delayed protection status */ + switch (Output) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + if ((HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_TIMISR_O1STAT) != RESET) + { + /* Output 1 was active when the delayed idle protection was triggered */ + delayed_protection_status = HRTIM_OUTPUTLEVEL_ACTIVE; + } + else + { + /* Output 1 was inactive when the delayed idle protection was triggered */ + delayed_protection_status = HRTIM_OUTPUTLEVEL_INACTIVE; + } + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + if ((HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_TIMISR_O2STAT) != RESET) + { + /* Output 2 was active when the delayed idle protection was triggered */ + delayed_protection_status = HRTIM_OUTPUTLEVEL_ACTIVE; + } + else + { + /* Output 2 was inactive when the delayed idle protection was triggered */ + delayed_protection_status = HRTIM_OUTPUTLEVEL_INACTIVE; + } + } + break; + default: + break; + } + + return delayed_protection_status; +} + +/** + * @brief Returns the actual status (active or inactive) of the burst mode controller + * @param HRTIMx: pointer to HRTIMx peripheral + * @retval Burst mode controller status + */ +uint32_t HRTIM_GetBurstStatus(HRTIM_TypeDef * HRTIMx) +{ + uint32_t burst_mode_status; + + /* Read burst mode status */ + burst_mode_status = (HRTIMx->HRTIM_COMMON.BMCR & HRTIM_BMCR_BMSTAT); + + return burst_mode_status; +} + +/** + * @brief Indicates on which output the signal is currently active (when the + * push pull mode is enabled) + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @retval Burst mode controller status + */ +uint32_t HRTIM_GetCurrentPushPullStatus(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx) +{ + uint32_t current_pushpull_status; + + /* Check the parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + + /* Read current push pull status */ + current_pushpull_status = (HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_TIMISR_CPPSTAT); + + return current_pushpull_status; +} + + +/** + * @brief Indicates on which output the signal was applied, in push-pull mode + balanced fault mode or delayed idle mode, when the protection was triggered + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * This parameter can be one of the following values: + * @arg 0x0 to 0x4 for timers A to E + * @retval Idle Push Pull Status + */ +uint32_t HRTIM_GetIdlePushPullStatus(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx) +{ + uint32_t idle_pushpull_status; + + /* Check the parameters */ + assert_param(IS_HRTIM_TIMING_UNIT(TimerIdx)); + + /* Read current push pull status */ + idle_pushpull_status = (HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxISR & HRTIM_TIMISR_IPPSTAT); + + return idle_pushpull_status; +} + +/** + * @brief Configures the master timer time base + * @param HRTIMx: pointer to HRTIMx peripheral + * @retval None + */ +void HRTIM_MasterBase_Config(HRTIM_TypeDef * HRTIMx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Set the prescaler ratio */ + HRTIMx->HRTIM_MASTER.MCR &= (uint32_t) ~(HRTIM_MCR_CK_PSC); + HRTIMx->HRTIM_MASTER.MCR |= (uint32_t)HRTIM_BaseInitStruct->PrescalerRatio; + + /* Set the operating mode */ + HRTIMx->HRTIM_MASTER.MCR &= (uint32_t) ~(HRTIM_MCR_CONT | HRTIM_MCR_RETRIG); + HRTIMx->HRTIM_MASTER.MCR |= (uint32_t)HRTIM_BaseInitStruct->Mode; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_MASTER.MPER = HRTIM_BaseInitStruct->Period; + HRTIMx->HRTIM_MASTER.MREP = HRTIM_BaseInitStruct->RepetitionCounter; +} + +/** + * @brief Configures timing unit (timer A to timer E) time base + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @retval None + */ +void HRTIM_TimingUnitBase_Config(HRTIM_TypeDef * HRTIMx, uint32_t TimerIdx, HRTIM_BaseInitTypeDef* HRTIM_BaseInitStruct) +{ + /* Set the prescaler ratio */ + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR &= (uint32_t) ~(HRTIM_TIMCR_CK_PSC); + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR |= (uint32_t)HRTIM_BaseInitStruct->PrescalerRatio; + + /* Set the operating mode */ + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR &= (uint32_t) ~(HRTIM_TIMCR_CONT | HRTIM_TIMCR_RETRIG); + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR |= (uint32_t)HRTIM_BaseInitStruct->Mode; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_TIMERx[TimerIdx].PERxR = HRTIM_BaseInitStruct->Period; + HRTIMx->HRTIM_TIMERx[TimerIdx].REPxR = HRTIM_BaseInitStruct->RepetitionCounter; +} + +/** + * @brief Configures the master timer in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @param pTimerInit: pointer to the timer initialization data structure + * @retval None + */ +void HRTIM_MasterWaveform_Config(HRTIM_TypeDef * HRTIMx, + HRTIM_TimerInitTypeDef * pTimerInit) +{ + uint32_t HRTIM_mcr; + uint32_t HRTIM_bmcr; + + /* Configure master timer */ + HRTIM_mcr = HRTIMx->HRTIM_MASTER.MCR; + HRTIM_bmcr = HRTIMx->HRTIM_COMMON.BMCR; + + /* Enable/Disable the half mode */ + HRTIM_mcr &= ~(HRTIM_MCR_HALF); + HRTIM_mcr |= pTimerInit->HalfModeEnable; + + /* Enable/Disable the timer start upon synchronization event reception */ + HRTIM_mcr &= ~(HRTIM_MCR_SYNCSTRTM); + HRTIM_mcr |= pTimerInit->StartOnSync; + + /* Enable/Disable the timer reset upon synchronization event reception */ + HRTIM_mcr &= ~(HRTIM_MCR_SYNCRSTM); + HRTIM_mcr |= pTimerInit->ResetOnSync; + + /* Enable/Disable the DAC synchronization event generation */ + HRTIM_mcr &= ~(HRTIM_MCR_DACSYNC); + HRTIM_mcr |= pTimerInit->DACSynchro; + + /* Enable/Disable preload mechanism for timer registers */ + HRTIM_mcr &= ~(HRTIM_MCR_PREEN); + HRTIM_mcr |= pTimerInit->PreloadEnable; + + /* Master timer registers update handling */ + HRTIM_mcr &= ~(HRTIM_MCR_BRSTDMA); + HRTIM_mcr |= (pTimerInit->UpdateGating << 2); + + /* Enable/Disable registers update on repetition */ + HRTIM_mcr &= ~(HRTIM_MCR_MREPU); + HRTIM_mcr |= pTimerInit->RepetitionUpdate; + + /* Set the timer burst mode */ + HRTIM_bmcr &= ~(HRTIM_BMCR_MTBM); + HRTIM_bmcr |= pTimerInit->BurstMode; + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_MASTER.MCR = HRTIM_mcr; + HRTIMx->HRTIM_COMMON.BMCR = HRTIM_bmcr; + +} + +/** + * @brief Configures timing unit (timer A to timer E) in waveform mode + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @param pTimerInit: pointer to the timer initialization data structure + * @retval None + */ +void HRTIM_TimingUnitWaveform_Config(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + HRTIM_TimerInitTypeDef * pTimerInit) +{ + uint32_t HRTIM_timcr; + uint32_t HRTIM_bmcr; + + /* Configure timing unit */ + HRTIM_timcr = HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR; + HRTIM_bmcr = HRTIMx->HRTIM_COMMON.BMCR; + + /* Enable/Disable the half mode */ + HRTIM_timcr &= ~(HRTIM_TIMCR_HALF); + HRTIM_timcr |= pTimerInit->HalfModeEnable; + + /* Enable/Disable the timer start upon synchronization event reception */ + HRTIM_timcr &= ~(HRTIM_TIMCR_SYNCSTRT); + HRTIM_timcr |= pTimerInit->StartOnSync; + + /* Enable/Disable the timer reset upon synchronization event reception */ + HRTIM_timcr &= ~(HRTIM_TIMCR_SYNCRST); + HRTIM_timcr |= pTimerInit->ResetOnSync; + + /* Enable/Disable the DAC synchronization event generation */ + HRTIM_timcr &= ~(HRTIM_TIMCR_DACSYNC); + HRTIM_timcr |= pTimerInit->DACSynchro; + + /* Enable/Disable preload mechanism for timer registers */ + HRTIM_timcr &= ~(HRTIM_TIMCR_PREEN); + HRTIM_timcr |= pTimerInit->PreloadEnable; + + /* Timing unit registers update handling */ + HRTIM_timcr &= ~(HRTIM_TIMCR_UPDGAT); + HRTIM_timcr |= pTimerInit->UpdateGating; + + /* Enable/Disable registers update on repetition */ + HRTIM_timcr &= ~(HRTIM_TIMCR_TREPU); + if (pTimerInit->RepetitionUpdate == HRTIM_UPDATEONREPETITION_ENABLED) + { + HRTIM_timcr |= HRTIM_TIMCR_TREPU; + } + + /* Set the timer burst mode */ + switch (TimerIdx) + { + case HRTIM_TIMERINDEX_TIMER_A: + { + HRTIM_bmcr &= ~(HRTIM_BMCR_TABM); + HRTIM_bmcr |= ( pTimerInit->BurstMode << 1); + } + break; + case HRTIM_TIMERINDEX_TIMER_B: + { + HRTIM_bmcr &= ~(HRTIM_BMCR_TBBM); + HRTIM_bmcr |= ( pTimerInit->BurstMode << 2); + } + break; + case HRTIM_TIMERINDEX_TIMER_C: + { + HRTIM_bmcr &= ~(HRTIM_BMCR_TCBM); + HRTIM_bmcr |= ( pTimerInit->BurstMode << 3); + } + break; + case HRTIM_TIMERINDEX_TIMER_D: + { + HRTIM_bmcr &= ~(HRTIM_BMCR_TDBM); + HRTIM_bmcr |= ( pTimerInit->BurstMode << 4); + } + break; + case HRTIM_TIMERINDEX_TIMER_E: + { + HRTIM_bmcr &= ~(HRTIM_BMCR_TEBM); + HRTIM_bmcr |= ( pTimerInit->BurstMode << 5); + } + break; + default: + break; + } + + /* Update the HRTIMx registers */ + HRTIMx->HRTIM_TIMERx[TimerIdx].TIMxCR = HRTIM_timcr; + HRTIMx->HRTIM_COMMON.BMCR = HRTIM_bmcr; +} + +/** + * @brief Configures a compare unit + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @param CompareUnit: Compare unit identifier + * @param pCompareCfg: pointer to the compare unit configuration data structure + * @retval None + */ +void HRTIM_CompareUnitConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CompareUnit, + HRTIM_CompareCfgTypeDef * pCompareCfg) +{ + if (TimerIdx == HRTIM_TIMERINDEX_MASTER) + { + /* Configure the compare unit of the master timer */ + switch (CompareUnit) + { + case HRTIM_COMPAREUNIT_1: + { + HRTIMx->HRTIM_MASTER.MCMP1R = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_2: + { + HRTIMx->HRTIM_MASTER.MCMP2R = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_3: + { + HRTIMx->HRTIM_MASTER.MCMP3R = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_4: + { + HRTIMx->HRTIM_MASTER.MCMP4R = pCompareCfg->CompareValue; + } + break; + default: + break; + } + } + else + { + /* Configure the compare unit of the timing unit */ + switch (CompareUnit) + { + case HRTIM_COMPAREUNIT_1: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP1xR = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_2: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP2xR = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_3: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP3xR = pCompareCfg->CompareValue; + } + break; + case HRTIM_COMPAREUNIT_4: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CMP4xR = pCompareCfg->CompareValue; + } + break; + default: + break; + } + } +} + +/** + * @brief Configures a capture unit + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @param CaptureUnit: Capture unit identifier + * @param pCaptureCfg: pointer to the compare unit configuration data structure + * @retval None + */ +void HRTIM_CaptureUnitConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t CaptureUnit, + uint32_t Event) +{ + uint32_t CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_1; + + switch (Event) + { + case HRTIM_EVENT_1: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_1; + } + break; + case HRTIM_EVENT_2: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_2; + } + break; + case HRTIM_EVENT_3: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_3; + } + break; + case HRTIM_EVENT_4: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_4; + } + break; + case HRTIM_EVENT_5: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_5; + } + break; + case HRTIM_EVENT_6: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_6; + } + break; + case HRTIM_EVENT_7: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_7; + } + break; + case HRTIM_EVENT_8: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_8; + } + break; + case HRTIM_EVENT_9: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_9; + } + break; + case HRTIM_EVENT_10: + { + CaptureTrigger = HRTIM_CAPTURETRIGGER_EEV_10; + } + break; + default: + break; + + } + switch (CaptureUnit) + { + case HRTIM_CAPTUREUNIT_1: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT1xCR = CaptureTrigger; + } + break; + case HRTIM_CAPTUREUNIT_2: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].CPT2xCR = CaptureTrigger; + } + break; + default: + break; + } +} + +/** + * @brief Configures the output of a timing unit + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @param Output: timing unit output identifier + * @param pOutputCfg: pointer to the output configuration data structure + * @retval None + */ +void HRTIM_OutputConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Output, + HRTIM_OutputCfgTypeDef * pOutputCfg) +{ + uint32_t HRTIM_outr; + uint32_t shift = 0; + + HRTIM_outr = HRTIMx->HRTIM_TIMERx[TimerIdx].OUTxR; + + switch (Output) + { + case HRTIM_OUTPUT_TA1: + case HRTIM_OUTPUT_TB1: + case HRTIM_OUTPUT_TC1: + case HRTIM_OUTPUT_TD1: + case HRTIM_OUTPUT_TE1: + { + /* Set the output set/reset crossbar */ + HRTIMx->HRTIM_TIMERx[TimerIdx].SETx1R = pOutputCfg->SetSource; + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTx1R = pOutputCfg->ResetSource; + + shift = 0; + } + break; + case HRTIM_OUTPUT_TA2: + case HRTIM_OUTPUT_TB2: + case HRTIM_OUTPUT_TC2: + case HRTIM_OUTPUT_TD2: + case HRTIM_OUTPUT_TE2: + { + /* Set the output set/reset crossbar */ + HRTIMx->HRTIM_TIMERx[TimerIdx].SETx2R = pOutputCfg->SetSource; + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTx2R = pOutputCfg->ResetSource; + + shift = 16; + } + break; + default: + break; + } + + /* Clear output config */ + HRTIM_outr &= ~((HRTIM_OUTR_POL1 | + HRTIM_OUTR_IDLM1 | + HRTIM_OUTR_IDLES1| + HRTIM_OUTR_FAULT1| + HRTIM_OUTR_CHP1 | + HRTIM_OUTR_DIDL1) << shift); + + /* Set the polarity */ + HRTIM_outr |= (pOutputCfg->Polarity << shift); + + /* Set the IDLE mode */ + HRTIM_outr |= (pOutputCfg->IdleMode << shift); + + /* Set the IDLE state */ + HRTIM_outr |= (pOutputCfg->IdleState << shift); + + /* Set the FAULT state */ + HRTIM_outr |= (pOutputCfg->FaultState << shift); + + /* Set the chopper mode */ + HRTIM_outr |= (pOutputCfg->ChopperModeEnable << shift); + + /* Set the burst mode entry mode */ + HRTIM_outr |= (pOutputCfg->BurstModeEntryDelayed << shift); + + /* Update HRTIMx register */ + HRTIMx->HRTIM_TIMERx[TimerIdx].OUTxR = HRTIM_outr; +} + +/** + * @brief Configures an external event channel + * @param HRTIMx: pointer to HRTIMx peripheral + * @param Event: Event channel identifier + * @param pEventCfg: pointer to the event channel configuration data structure + * @retval None + */ +static void HRTIM_ExternalEventConfig(HRTIM_TypeDef * HRTIMx, + uint32_t Event, + HRTIM_EventCfgTypeDef *pEventCfg) +{ + uint32_t hrtim_eecr1; + uint32_t hrtim_eecr2; + uint32_t hrtim_eecr3; + + /* Configure external event channel */ + hrtim_eecr1 = HRTIMx->HRTIM_COMMON.EECR1; + hrtim_eecr2 = HRTIMx->HRTIM_COMMON.EECR2; + hrtim_eecr3 = HRTIMx->HRTIM_COMMON.EECR3; + + switch (Event) + { + case HRTIM_EVENT_1: + { + hrtim_eecr1 &= ~(HRTIM_EECR1_EE1SRC | HRTIM_EECR1_EE1POL | HRTIM_EECR1_EE1SNS | HRTIM_EECR1_EE1FAST); + hrtim_eecr1 |= pEventCfg->Source; + hrtim_eecr1 |= pEventCfg->Polarity; + hrtim_eecr1 |= pEventCfg->Sensitivity; + /* Update the HRTIM registers (all bit fields but EE1FAST bit) */ + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + /* Update the HRTIM registers (EE1FAST bit) */ + hrtim_eecr1 |= pEventCfg->FastMode; + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + } + break; + case HRTIM_EVENT_2: + { + hrtim_eecr1 &= ~(HRTIM_EECR1_EE2SRC | HRTIM_EECR1_EE2POL | HRTIM_EECR1_EE2SNS | HRTIM_EECR1_EE2FAST); + hrtim_eecr1 |= (pEventCfg->Source << 6); + hrtim_eecr1 |= (pEventCfg->Polarity << 6); + hrtim_eecr1 |= (pEventCfg->Sensitivity << 6); + /* Update the HRTIM registers (all bit fields but EE2FAST bit) */ + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + /* Update the HRTIM registers (EE2FAST bit) */ + hrtim_eecr1 |= (pEventCfg->FastMode << 6); + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + } + break; + case HRTIM_EVENT_3: + { + hrtim_eecr1 &= ~(HRTIM_EECR1_EE3SRC | HRTIM_EECR1_EE3POL | HRTIM_EECR1_EE3SNS | HRTIM_EECR1_EE3FAST); + hrtim_eecr1 |= (pEventCfg->Source << 12); + hrtim_eecr1 |= (pEventCfg->Polarity << 12); + hrtim_eecr1 |= (pEventCfg->Sensitivity << 12); + /* Update the HRTIM registers (all bit fields but EE3FAST bit) */ + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + /* Update the HRTIM registers (EE3FAST bit) */ + hrtim_eecr1 |= (pEventCfg->FastMode << 12); + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + } + break; + case HRTIM_EVENT_4: + { + hrtim_eecr1 &= ~(HRTIM_EECR1_EE4SRC | HRTIM_EECR1_EE4POL | HRTIM_EECR1_EE4SNS | HRTIM_EECR1_EE4FAST); + hrtim_eecr1 |= (pEventCfg->Source << 18); + hrtim_eecr1 |= (pEventCfg->Polarity << 18); + hrtim_eecr1 |= (pEventCfg->Sensitivity << 18); + /* Update the HRTIM registers (all bit fields but EE4FAST bit) */ + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + /* Update the HRTIM registers (EE4FAST bit) */ + hrtim_eecr1 |= (pEventCfg->FastMode << 18); + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + } + break; + case HRTIM_EVENT_5: + { + hrtim_eecr1 &= ~(HRTIM_EECR1_EE5SRC | HRTIM_EECR1_EE5POL | HRTIM_EECR1_EE5SNS | HRTIM_EECR1_EE5FAST); + hrtim_eecr1 |= (pEventCfg->Source << 24); + hrtim_eecr1 |= (pEventCfg->Polarity << 24); + hrtim_eecr1 |= (pEventCfg->Sensitivity << 24); + /* Update the HRTIM registers (all bit fields but EE5FAST bit) */ + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + /* Update the HRTIM registers (EE5FAST bit) */ + hrtim_eecr1 |= (pEventCfg->FastMode << 24); + HRTIMx->HRTIM_COMMON.EECR1 = hrtim_eecr1; + } + break; + case HRTIM_EVENT_6: + { + hrtim_eecr2 &= ~(HRTIM_EECR2_EE6SRC | HRTIM_EECR2_EE6POL | HRTIM_EECR2_EE6SNS); + hrtim_eecr2 |= pEventCfg->Source; + hrtim_eecr2 |= pEventCfg->Polarity; + hrtim_eecr2 |= pEventCfg->Sensitivity; + hrtim_eecr3 &= ~(HRTIM_EECR3_EE6F); + hrtim_eecr3 |= pEventCfg->Filter; + /* Update the HRTIM registers */ + HRTIMx->HRTIM_COMMON.EECR2 = hrtim_eecr2; + HRTIMx->HRTIM_COMMON.EECR3 = hrtim_eecr3; + } + break; + case HRTIM_EVENT_7: + { + hrtim_eecr2 &= ~(HRTIM_EECR2_EE7SRC | HRTIM_EECR2_EE7POL | HRTIM_EECR2_EE7SNS); + hrtim_eecr2 |= (pEventCfg->Source << 6); + hrtim_eecr2 |= (pEventCfg->Polarity << 6); + hrtim_eecr2 |= (pEventCfg->Sensitivity << 6); + hrtim_eecr3 &= ~(HRTIM_EECR3_EE7F); + hrtim_eecr3 |= (pEventCfg->Filter << 6); + /* Update the HRTIM registers */ + HRTIMx->HRTIM_COMMON.EECR2 = hrtim_eecr2; + HRTIMx->HRTIM_COMMON.EECR3 = hrtim_eecr3; + } + break; + case HRTIM_EVENT_8: + { + hrtim_eecr2 &= ~(HRTIM_EECR2_EE8SRC | HRTIM_EECR2_EE8POL | HRTIM_EECR2_EE8SNS); + hrtim_eecr2 |= (pEventCfg->Source << 12); + hrtim_eecr2 |= (pEventCfg->Polarity << 12); + hrtim_eecr2 |= (pEventCfg->Sensitivity << 12); + hrtim_eecr3 &= ~(HRTIM_EECR3_EE8F); + hrtim_eecr3 |= (pEventCfg->Filter << 12); + /* Update the HRTIM registers */ + HRTIMx->HRTIM_COMMON.EECR2 = hrtim_eecr2; + HRTIMx->HRTIM_COMMON.EECR3 = hrtim_eecr3; + } + break; + case HRTIM_EVENT_9: + { + hrtim_eecr2 &= ~(HRTIM_EECR2_EE9SRC | HRTIM_EECR2_EE9POL | HRTIM_EECR2_EE9SNS); + hrtim_eecr2 |= (pEventCfg->Source << 18); + hrtim_eecr2 |= (pEventCfg->Polarity << 18); + hrtim_eecr2 |= (pEventCfg->Sensitivity << 18); + hrtim_eecr3 &= ~(HRTIM_EECR3_EE9F); + hrtim_eecr3 |= (pEventCfg->Filter << 18); + /* Update the HRTIM registers */ + HRTIMx->HRTIM_COMMON.EECR2 = hrtim_eecr2; + HRTIMx->HRTIM_COMMON.EECR3 = hrtim_eecr3; + } + break; + case HRTIM_EVENT_10: + { + hrtim_eecr2 &= ~(HRTIM_EECR2_EE10SRC | HRTIM_EECR2_EE10POL | HRTIM_EECR2_EE10SNS); + hrtim_eecr2 |= (pEventCfg->Source << 24); + hrtim_eecr2 |= (pEventCfg->Polarity << 24); + hrtim_eecr2 |= (pEventCfg->Sensitivity << 24); + hrtim_eecr3 &= ~(HRTIM_EECR3_EE10F); + hrtim_eecr3 |= (pEventCfg->Filter << 24); + /* Update the HRTIM registers */ + HRTIMx->HRTIM_COMMON.EECR2 = hrtim_eecr2; + HRTIMx->HRTIM_COMMON.EECR3 = hrtim_eecr3; + } + break; + default: + break; + } +} + +/** + * @brief Configures the timer counter reset + * @param HRTIMx: pointer to HRTIMx peripheral + * @param TimerIdx: Timer index + * @param Event: Event channel identifier + * @retval None + */ +void HRTIM_TIM_ResetConfig(HRTIM_TypeDef * HRTIMx, + uint32_t TimerIdx, + uint32_t Event) +{ + switch (Event) + { + case HRTIM_EVENT_1: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_1; + } + break; + case HRTIM_EVENT_2: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_2; + } + break; + case HRTIM_EVENT_3: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_3; + } + break; + case HRTIM_EVENT_4: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_4; + } + break; + case HRTIM_EVENT_5: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_5; + } + break; + case HRTIM_EVENT_6: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_6; + } + break; + case HRTIM_EVENT_7: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_7; + } + break; + case HRTIM_EVENT_8: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_8; + } + break; + case HRTIM_EVENT_9: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_9; + } + break; + case HRTIM_EVENT_10: + { + HRTIMx->HRTIM_TIMERx[TimerIdx].RSTxR = HRTIM_TIMRESETTRIGGER_EEV_10; + } + break; + default: + break; + } +} +/** + * @} + */ +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + + + diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_i2c.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_i2c.c new file mode 100755 index 0000000000..f2f30eb7a4 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_i2c.c @@ -0,0 +1,1585 @@ +/** + ****************************************************************************** + * @file stm32f30x_i2c.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Inter-Integrated circuit (I2C): + * + Initialization and Configuration + * + Communications handling + * + SMBUS management + * + I2C registers management + * + Data transfers management + * + DMA transfers management + * + Interrupts and flags management + * + * @verbatim + ============================================================================ + ##### How to use this driver ##### + ============================================================================ + [..] + (#) Enable peripheral clock using RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2Cx, ENABLE) + function for I2C1 or I2C2. + (#) Enable SDA, SCL and SMBA (when used) GPIO clocks using + RCC_AHBPeriphClockCmd() function. + (#) Peripherals alternate function: + (++) Connect the pin to the desired peripherals' Alternate + Function (AF) using GPIO_PinAFConfig() function. + (++) Configure the desired pin in alternate function by: + GPIO_InitStruct->GPIO_Mode = GPIO_Mode_AF + (++) Select the type, OpenDrain and speed via + GPIO_PuPd, GPIO_OType and GPIO_Speed members + (++) Call GPIO_Init() function. + (#) Program the Mode, Timing , Own address, Ack and Acknowledged Address + using the I2C_Init() function. + (#) Optionally you can enable/configure the following parameters without + re-initialization (i.e there is no need to call again I2C_Init() function): + (++) Enable the acknowledge feature using I2C_AcknowledgeConfig() function. + (++) Enable the dual addressing mode using I2C_DualAddressCmd() function. + (++) Enable the general call using the I2C_GeneralCallCmd() function. + (++) Enable the clock stretching using I2C_StretchClockCmd() function. + (++) Enable the PEC Calculation using I2C_CalculatePEC() function. + (++) For SMBus Mode: + (+++) Enable the SMBusAlert pin using I2C_SMBusAlertCmd() function. + (#) Enable the NVIC and the corresponding interrupt using the function + I2C_ITConfig() if you need to use interrupt mode. + (#) When using the DMA mode + (++) Configure the DMA using DMA_Init() function. + (++) Active the needed channel Request using I2C_DMACmd() function. + (#) Enable the I2C using the I2C_Cmd() function. + (#) Enable the DMA using the DMA_Cmd() function when using DMA mode in the + transfers. + [..] + (@) When using I2C in Fast Mode Plus, SCL and SDA pin 20mA current drive capability + must be enabled by setting the driving capability control bit in SYSCFG. + + @endverbatim + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_i2c.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup I2C + * @brief I2C driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +#define CR1_CLEAR_MASK ((uint32_t)0x00CFE0FF) /*I2C_AnalogFilter)); + assert_param(IS_I2C_DIGITAL_FILTER(I2C_InitStruct->I2C_DigitalFilter)); + assert_param(IS_I2C_MODE(I2C_InitStruct->I2C_Mode)); + assert_param(IS_I2C_OWN_ADDRESS1(I2C_InitStruct->I2C_OwnAddress1)); + assert_param(IS_I2C_ACK(I2C_InitStruct->I2C_Ack)); + assert_param(IS_I2C_ACKNOWLEDGE_ADDRESS(I2C_InitStruct->I2C_AcknowledgedAddress)); + + /* Disable I2Cx Peripheral */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_PE); + + /*---------------------------- I2Cx FILTERS Configuration ------------------*/ + /* Get the I2Cx CR1 value */ + tmpreg = I2Cx->CR1; + /* Clear I2Cx CR1 register */ + tmpreg &= CR1_CLEAR_MASK; + /* Configure I2Cx: analog and digital filter */ + /* Set ANFOFF bit according to I2C_AnalogFilter value */ + /* Set DFN bits according to I2C_DigitalFilter value */ + tmpreg |= (uint32_t)I2C_InitStruct->I2C_AnalogFilter |(I2C_InitStruct->I2C_DigitalFilter << 8); + + /* Write to I2Cx CR1 */ + I2Cx->CR1 = tmpreg; + + /*---------------------------- I2Cx TIMING Configuration -------------------*/ + /* Configure I2Cx: Timing */ + /* Set TIMINGR bits according to I2C_Timing */ + /* Write to I2Cx TIMING */ + I2Cx->TIMINGR = I2C_InitStruct->I2C_Timing & TIMING_CLEAR_MASK; + + /* Enable I2Cx Peripheral */ + I2Cx->CR1 |= I2C_CR1_PE; + + /*---------------------------- I2Cx OAR1 Configuration ---------------------*/ + /* Clear tmpreg local variable */ + tmpreg = 0; + /* Clear OAR1 register */ + I2Cx->OAR1 = (uint32_t)tmpreg; + /* Clear OAR2 register */ + I2Cx->OAR2 = (uint32_t)tmpreg; + /* Configure I2Cx: Own Address1 and acknowledged address */ + /* Set OA1MODE bit according to I2C_AcknowledgedAddress value */ + /* Set OA1 bits according to I2C_OwnAddress1 value */ + tmpreg = (uint32_t)((uint32_t)I2C_InitStruct->I2C_AcknowledgedAddress | \ + (uint32_t)I2C_InitStruct->I2C_OwnAddress1); + /* Write to I2Cx OAR1 */ + I2Cx->OAR1 = tmpreg; + /* Enable Own Address1 acknowledgement */ + I2Cx->OAR1 |= I2C_OAR1_OA1EN; + + /*---------------------------- I2Cx MODE Configuration ---------------------*/ + /* Configure I2Cx: mode */ + /* Set SMBDEN and SMBHEN bits according to I2C_Mode value */ + tmpreg = I2C_InitStruct->I2C_Mode; + /* Write to I2Cx CR1 */ + I2Cx->CR1 |= tmpreg; + + /*---------------------------- I2Cx ACK Configuration ----------------------*/ + /* Get the I2Cx CR2 value */ + tmpreg = I2Cx->CR2; + /* Clear I2Cx CR2 register */ + tmpreg &= CR2_CLEAR_MASK; + /* Configure I2Cx: acknowledgement */ + /* Set NACK bit according to I2C_Ack value */ + tmpreg |= I2C_InitStruct->I2C_Ack; + /* Write to I2Cx CR2 */ + I2Cx->CR2 = tmpreg; +} + +/** + * @brief Fills each I2C_InitStruct member with its default value. + * @param I2C_InitStruct: pointer to an I2C_InitTypeDef structure which will be initialized. + * @retval None + */ +void I2C_StructInit(I2C_InitTypeDef* I2C_InitStruct) +{ + /*---------------- Reset I2C init structure parameters values --------------*/ + /* Initialize the I2C_Timing member */ + I2C_InitStruct->I2C_Timing = 0; + /* Initialize the I2C_AnalogFilter member */ + I2C_InitStruct->I2C_AnalogFilter = I2C_AnalogFilter_Enable; + /* Initialize the I2C_DigitalFilter member */ + I2C_InitStruct->I2C_DigitalFilter = 0; + /* Initialize the I2C_Mode member */ + I2C_InitStruct->I2C_Mode = I2C_Mode_I2C; + /* Initialize the I2C_OwnAddress1 member */ + I2C_InitStruct->I2C_OwnAddress1 = 0; + /* Initialize the I2C_Ack member */ + I2C_InitStruct->I2C_Ack = I2C_Ack_Disable; + /* Initialize the I2C_AcknowledgedAddress member */ + I2C_InitStruct->I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; +} + +/** + * @brief Enables or disables the specified I2C peripheral. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_Cmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected I2C peripheral */ + I2Cx->CR1 |= I2C_CR1_PE; + } + else + { + /* Disable the selected I2C peripheral */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_PE); + } +} + + +/** + * @brief Enables or disables the specified I2C software reset. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @retval None + */ +void I2C_SoftwareResetCmd(I2C_TypeDef* I2Cx) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Disable peripheral */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_PE); + + /* Perform a dummy read to delay the disable of peripheral for minimum + 3 APB clock cycles to perform the software reset functionality */ + *(__IO uint32_t *)(uint32_t)I2Cx; + + /* Enable peripheral */ + I2Cx->CR1 |= I2C_CR1_PE; +} + +/** + * @brief Enables or disables the specified I2C interrupts. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_IT: specifies the I2C interrupts sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg I2C_IT_ERRI: Error interrupt mask + * @arg I2C_IT_TCI: Transfer Complete interrupt mask + * @arg I2C_IT_STOPI: Stop Detection interrupt mask + * @arg I2C_IT_NACKI: Not Acknowledge received interrupt mask + * @arg I2C_IT_ADDRI: Address Match interrupt mask + * @arg I2C_IT_RXI: RX interrupt mask + * @arg I2C_IT_TXI: TX interrupt mask + * @param NewState: new state of the specified I2C interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_ITConfig(I2C_TypeDef* I2Cx, uint32_t I2C_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_I2C_CONFIG_IT(I2C_IT)); + + if (NewState != DISABLE) + { + /* Enable the selected I2C interrupts */ + I2Cx->CR1 |= I2C_IT; + } + else + { + /* Disable the selected I2C interrupts */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_IT); + } +} + +/** + * @brief Enables or disables the I2C Clock stretching. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx Clock stretching. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_StretchClockCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable clock stretching */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_NOSTRETCH); + } + else + { + /* Disable clock stretching */ + I2Cx->CR1 |= I2C_CR1_NOSTRETCH; + } +} + +/** + * @brief Enables or disables I2C wakeup from stop mode. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx stop mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_StopModeCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable wakeup from stop mode */ + I2Cx->CR1 |= I2C_CR1_WUPEN; + } + else + { + /* Disable wakeup from stop mode */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_WUPEN); + } +} + +/** + * @brief Enables or disables the I2C own address 2. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C own address 2. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_DualAddressCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable own address 2 */ + I2Cx->OAR2 |= I2C_OAR2_OA2EN; + } + else + { + /* Disable own address 2 */ + I2Cx->OAR2 &= (uint32_t)~((uint32_t)I2C_OAR2_OA2EN); + } +} + +/** + * @brief Configures the I2C slave own address 2 and mask. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Address: specifies the slave address to be programmed. + * @param Mask: specifies own address 2 mask to be programmed. + * This parameter can be one of the following values: + * @arg I2C_OA2_NoMask: no mask. + * @arg I2C_OA2_Mask01: OA2[1] is masked and don't care. + * @arg I2C_OA2_Mask02: OA2[2:1] are masked and don't care. + * @arg I2C_OA2_Mask03: OA2[3:1] are masked and don't care. + * @arg I2C_OA2_Mask04: OA2[4:1] are masked and don't care. + * @arg I2C_OA2_Mask05: OA2[5:1] are masked and don't care. + * @arg I2C_OA2_Mask06: OA2[6:1] are masked and don't care. + * @arg I2C_OA2_Mask07: OA2[7:1] are masked and don't care. + * @retval None + */ +void I2C_OwnAddress2Config(I2C_TypeDef* I2Cx, uint16_t Address, uint8_t Mask) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_OWN_ADDRESS2(Address)); + assert_param(IS_I2C_OWN_ADDRESS2_MASK(Mask)); + + /* Get the old register value */ + tmpreg = I2Cx->OAR2; + + /* Reset I2Cx OA2 bit [7:1] and OA2MSK bit [1:0] */ + tmpreg &= (uint32_t)~((uint32_t)(I2C_OAR2_OA2 | I2C_OAR2_OA2MSK)); + + /* Set I2Cx SADD */ + tmpreg |= (uint32_t)(((uint32_t)Address & I2C_OAR2_OA2) | \ + (((uint32_t)Mask << 8) & I2C_OAR2_OA2MSK)) ; + + /* Store the new register value */ + I2Cx->OAR2 = tmpreg; +} + +/** + * @brief Enables or disables the I2C general call mode. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C general call mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_GeneralCallCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable general call mode */ + I2Cx->CR1 |= I2C_CR1_GCEN; + } + else + { + /* Disable general call mode */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_GCEN); + } +} + +/** + * @brief Enables or disables the I2C slave byte control. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C slave byte control. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_SlaveByteControlCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable slave byte control */ + I2Cx->CR1 |= I2C_CR1_SBC; + } + else + { + /* Disable slave byte control */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_SBC); + } +} + +/** + * @brief Configures the slave address to be transmitted after start generation. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Address: specifies the slave address to be programmed. + * @note This function should be called before generating start condition. + * @retval None + */ +void I2C_SlaveAddressConfig(I2C_TypeDef* I2Cx, uint16_t Address) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_SLAVE_ADDRESS(Address)); + + /* Get the old register value */ + tmpreg = I2Cx->CR2; + + /* Reset I2Cx SADD bit [9:0] */ + tmpreg &= (uint32_t)~((uint32_t)I2C_CR2_SADD); + + /* Set I2Cx SADD */ + tmpreg |= (uint32_t)((uint32_t)Address & I2C_CR2_SADD); + + /* Store the new register value */ + I2Cx->CR2 = tmpreg; +} + +/** + * @brief Enables or disables the I2C 10-bit addressing mode for the master. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C 10-bit addressing mode. + * This parameter can be: ENABLE or DISABLE. + * @note This function should be called before generating start condition. + * @retval None + */ +void I2C_10BitAddressingModeCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable 10-bit addressing mode */ + I2Cx->CR2 |= I2C_CR2_ADD10; + } + else + { + /* Disable 10-bit addressing mode */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_ADD10); + } +} + +/** + * @} + */ + + +/** @defgroup I2C_Group2 Communications handling functions + * @brief Communications handling functions + * +@verbatim + =============================================================================== + ##### Communications handling functions ##### + =============================================================================== + [..] This section provides a set of functions that handles I2C communication. + + [..] Automatic End mode is enabled using I2C_AutoEndCmd() function. When Reload + mode is enabled via I2C_ReloadCmd() AutoEnd bit has no effect. + + [..] I2C_NumberOfBytesConfig() function set the number of bytes to be transferred, + this configuration should be done before generating start condition in master + mode. + + [..] When switching from master write operation to read operation in 10Bit addressing + mode, master can only sends the 1st 7 bits of the 10 bit address, followed by + Read direction by enabling HEADR bit using I2C_10BitAddressHeader() function. + + [..] In master mode, when transferring more than 255 bytes Reload mode should be used + to handle communication. In the first phase of transfer, Nbytes should be set to + 255. After transferring these bytes TCR flag is set and I2C_TransferHandling() + function should be called to handle remaining communication. + + [..] In master mode, when software end mode is selected when all data is transferred + TC flag is set I2C_TransferHandling() function should be called to generate STOP + or generate ReStart. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the I2C automatic end mode (stop condition is + * automatically sent when nbytes data are transferred). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C automatic end mode. + * This parameter can be: ENABLE or DISABLE. + * @note This function has effect if Reload mode is disabled. + * @retval None + */ +void I2C_AutoEndCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable Auto end mode */ + I2Cx->CR2 |= I2C_CR2_AUTOEND; + } + else + { + /* Disable Auto end mode */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_AUTOEND); + } +} + +/** + * @brief Enables or disables the I2C nbytes reload mode. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the nbytes reload mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_ReloadCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable Auto Reload mode */ + I2Cx->CR2 |= I2C_CR2_RELOAD; + } + else + { + /* Disable Auto Reload mode */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_RELOAD); + } +} + +/** + * @brief Configures the number of bytes to be transmitted/received. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Number_Bytes: specifies the number of bytes to be programmed. + * @retval None + */ +void I2C_NumberOfBytesConfig(I2C_TypeDef* I2Cx, uint8_t Number_Bytes) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Get the old register value */ + tmpreg = I2Cx->CR2; + + /* Reset I2Cx Nbytes bit [7:0] */ + tmpreg &= (uint32_t)~((uint32_t)I2C_CR2_NBYTES); + + /* Set I2Cx Nbytes */ + tmpreg |= (uint32_t)(((uint32_t)Number_Bytes << 16 ) & I2C_CR2_NBYTES); + + /* Store the new register value */ + I2Cx->CR2 = tmpreg; +} + +/** + * @brief Configures the type of transfer request for the master. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_Direction: specifies the transfer request direction to be programmed. + * This parameter can be one of the following values: + * @arg I2C_Direction_Transmitter: Master request a write transfer + * @arg I2C_Direction_Receiver: Master request a read transfer + * @retval None + */ +void I2C_MasterRequestConfig(I2C_TypeDef* I2Cx, uint16_t I2C_Direction) +{ +/* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_DIRECTION(I2C_Direction)); + + /* Test on the direction to set/reset the read/write bit */ + if (I2C_Direction == I2C_Direction_Transmitter) + { + /* Request a write Transfer */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_RD_WRN); + } + else + { + /* Request a read Transfer */ + I2Cx->CR2 |= I2C_CR2_RD_WRN; + } +} + +/** + * @brief Generates I2Cx communication START condition. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C START condition generation. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_GenerateSTART(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Generate a START condition */ + I2Cx->CR2 |= I2C_CR2_START; + } + else + { + /* Disable the START condition generation */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_START); + } +} + +/** + * @brief Generates I2Cx communication STOP condition. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C STOP condition generation. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_GenerateSTOP(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Generate a STOP condition */ + I2Cx->CR2 |= I2C_CR2_STOP; + } + else + { + /* Disable the STOP condition generation */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_STOP); + } +} + +/** + * @brief Enables or disables the I2C 10-bit header only mode with read direction. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2C 10-bit header only mode. + * This parameter can be: ENABLE or DISABLE. + * @note This mode can be used only when switching from master transmitter mode + * to master receiver mode. + * @retval None + */ +void I2C_10BitAddressHeaderCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable 10-bit header only mode */ + I2Cx->CR2 |= I2C_CR2_HEAD10R; + } + else + { + /* Disable 10-bit header only mode */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_HEAD10R); + } +} + +/** + * @brief Generates I2C communication Acknowledge. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the Acknowledge. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_AcknowledgeConfig(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable ACK generation */ + I2Cx->CR2 &= (uint32_t)~((uint32_t)I2C_CR2_NACK); + } + else + { + /* Enable NACK generation */ + I2Cx->CR2 |= I2C_CR2_NACK; + } +} + +/** + * @brief Returns the I2C slave matched address . + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @retval The value of the slave matched address . + */ +uint8_t I2C_GetAddressMatched(I2C_TypeDef* I2Cx) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Return the slave matched address in the SR1 register */ + return (uint8_t)(((uint32_t)I2Cx->ISR & I2C_ISR_ADDCODE) >> 16) ; +} + +/** + * @brief Returns the I2C slave received request. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @retval The value of the received request. + */ +uint16_t I2C_GetTransferDirection(I2C_TypeDef* I2Cx) +{ + uint32_t tmpreg = 0; + uint16_t direction = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Return the slave matched address in the SR1 register */ + tmpreg = (uint32_t)(I2Cx->ISR & I2C_ISR_DIR); + + /* If write transfer is requested */ + if (tmpreg == 0) + { + /* write transfer is requested */ + direction = I2C_Direction_Transmitter; + } + else + { + /* Read transfer is requested */ + direction = I2C_Direction_Receiver; + } + return direction; +} + +/** + * @brief Handles I2Cx communication when starting transfer or during transfer (TC or TCR flag are set). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Address: specifies the slave address to be programmed. + * @param Number_Bytes: specifies the number of bytes to be programmed. + * This parameter must be a value between 0 and 255. + * @param ReloadEndMode: new state of the I2C START condition generation. + * This parameter can be one of the following values: + * @arg I2C_Reload_Mode: Enable Reload mode . + * @arg I2C_AutoEnd_Mode: Enable Automatic end mode. + * @arg I2C_SoftEnd_Mode: Enable Software end mode. + * @param StartStopMode: new state of the I2C START condition generation. + * This parameter can be one of the following values: + * @arg I2C_No_StartStop: Don't Generate stop and start condition. + * @arg I2C_Generate_Stop: Generate stop condition (Number_Bytes should be set to 0). + * @arg I2C_Generate_Start_Read: Generate Restart for read request. + * @arg I2C_Generate_Start_Write: Generate Restart for write request. + * @retval None + */ +void I2C_TransferHandling(I2C_TypeDef* I2Cx, uint16_t Address, uint8_t Number_Bytes, uint32_t ReloadEndMode, uint32_t StartStopMode) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_SLAVE_ADDRESS(Address)); + assert_param(IS_RELOAD_END_MODE(ReloadEndMode)); + assert_param(IS_START_STOP_MODE(StartStopMode)); + + /* Get the CR2 register value */ + tmpreg = I2Cx->CR2; + + /* clear tmpreg specific bits */ + tmpreg &= (uint32_t)~((uint32_t)(I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD | I2C_CR2_AUTOEND | I2C_CR2_RD_WRN | I2C_CR2_START | I2C_CR2_STOP)); + + /* update tmpreg */ + tmpreg |= (uint32_t)(((uint32_t)Address & I2C_CR2_SADD) | (((uint32_t)Number_Bytes << 16 ) & I2C_CR2_NBYTES) | \ + (uint32_t)ReloadEndMode | (uint32_t)StartStopMode); + + /* update CR2 register */ + I2Cx->CR2 = tmpreg; +} + +/** + * @} + */ + + +/** @defgroup I2C_Group3 SMBUS management functions + * @brief SMBUS management functions + * +@verbatim + =============================================================================== + ##### SMBUS management functions ##### + =============================================================================== + [..] This section provides a set of functions that handles SMBus communication + and timeouts detection. + + [..] The SMBus Device default address (0b1100 001) is enabled by calling I2C_Init() + function and setting I2C_Mode member of I2C_InitTypeDef() structure to + I2C_Mode_SMBusDevice. + + [..] The SMBus Host address (0b0001 000) is enabled by calling I2C_Init() + function and setting I2C_Mode member of I2C_InitTypeDef() structure to + I2C_Mode_SMBusHost. + + [..] The Alert Response Address (0b0001 100) is enabled using I2C_SMBusAlertCmd() + function. + + [..] To detect cumulative SCL stretch in master and slave mode, TIMEOUTB should be + configured (in accordance to SMBus specification) using I2C_TimeoutBConfig() + function then I2C_ExtendedClockTimeoutCmd() function should be called to enable + the detection. + + [..] SCL low timeout is detected by configuring TIMEOUTB using I2C_TimeoutBConfig() + function followed by the call of I2C_ClockTimeoutCmd(). When adding to this + procedure the call of I2C_IdleClockTimeoutCmd() function, Bus Idle condition + (both SCL and SDA high) is detected also. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables I2C SMBus alert. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx SMBus alert. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_SMBusAlertCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable SMBus alert */ + I2Cx->CR1 |= I2C_CR1_ALERTEN; + } + else + { + /* Disable SMBus alert */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_ALERTEN); + } +} + +/** + * @brief Enables or disables I2C Clock Timeout (SCL Timeout detection). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx clock Timeout. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_ClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable Clock Timeout */ + I2Cx->TIMEOUTR |= I2C_TIMEOUTR_TIMOUTEN; + } + else + { + /* Disable Clock Timeout */ + I2Cx->TIMEOUTR &= (uint32_t)~((uint32_t)I2C_TIMEOUTR_TIMOUTEN); + } +} + +/** + * @brief Enables or disables I2C Extended Clock Timeout (SCL cumulative Timeout detection). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx Extended clock Timeout. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_ExtendedClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable Clock Timeout */ + I2Cx->TIMEOUTR |= I2C_TIMEOUTR_TEXTEN; + } + else + { + /* Disable Clock Timeout */ + I2Cx->TIMEOUTR &= (uint32_t)~((uint32_t)I2C_TIMEOUTR_TEXTEN); + } +} + +/** + * @brief Enables or disables I2C Idle Clock Timeout (Bus idle SCL and SDA + * high detection). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx Idle clock Timeout. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_IdleClockTimeoutCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable Clock Timeout */ + I2Cx->TIMEOUTR |= I2C_TIMEOUTR_TIDLE; + } + else + { + /* Disable Clock Timeout */ + I2Cx->TIMEOUTR &= (uint32_t)~((uint32_t)I2C_TIMEOUTR_TIDLE); + } +} + +/** + * @brief Configures the I2C Bus Timeout A (SCL Timeout when TIDLE = 0 or Bus + * idle SCL and SDA high when TIDLE = 1). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Timeout: specifies the TimeoutA to be programmed. + * @retval None + */ +void I2C_TimeoutAConfig(I2C_TypeDef* I2Cx, uint16_t Timeout) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_TIMEOUT(Timeout)); + + /* Get the old register value */ + tmpreg = I2Cx->TIMEOUTR; + + /* Reset I2Cx TIMEOUTA bit [11:0] */ + tmpreg &= (uint32_t)~((uint32_t)I2C_TIMEOUTR_TIMEOUTA); + + /* Set I2Cx TIMEOUTA */ + tmpreg |= (uint32_t)((uint32_t)Timeout & I2C_TIMEOUTR_TIMEOUTA) ; + + /* Store the new register value */ + I2Cx->TIMEOUTR = tmpreg; +} + +/** + * @brief Configures the I2C Bus Timeout B (SCL cumulative Timeout). + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Timeout: specifies the TimeoutB to be programmed. + * @retval None + */ +void I2C_TimeoutBConfig(I2C_TypeDef* I2Cx, uint16_t Timeout) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_TIMEOUT(Timeout)); + + /* Get the old register value */ + tmpreg = I2Cx->TIMEOUTR; + + /* Reset I2Cx TIMEOUTB bit [11:0] */ + tmpreg &= (uint32_t)~((uint32_t)I2C_TIMEOUTR_TIMEOUTB); + + /* Set I2Cx TIMEOUTB */ + tmpreg |= (uint32_t)(((uint32_t)Timeout << 16) & I2C_TIMEOUTR_TIMEOUTB) ; + + /* Store the new register value */ + I2Cx->TIMEOUTR = tmpreg; +} + +/** + * @brief Enables or disables I2C PEC calculation. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx PEC calculation. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_CalculatePEC(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable PEC calculation */ + I2Cx->CR1 |= I2C_CR1_PECEN; + } + else + { + /* Disable PEC calculation */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR1_PECEN); + } +} + +/** + * @brief Enables or disables I2C PEC transmission/reception request. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param NewState: new state of the I2Cx PEC request. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_PECRequestCmd(I2C_TypeDef* I2Cx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable PEC transmission/reception request */ + I2Cx->CR1 |= I2C_CR2_PECBYTE; + } + else + { + /* Disable PEC transmission/reception request */ + I2Cx->CR1 &= (uint32_t)~((uint32_t)I2C_CR2_PECBYTE); + } +} + +/** + * @brief Returns the I2C PEC. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @retval The value of the PEC . + */ +uint8_t I2C_GetPEC(I2C_TypeDef* I2Cx) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Return the slave matched address in the SR1 register */ + return (uint8_t)((uint32_t)I2Cx->PECR & I2C_PECR_PEC); +} + +/** + * @} + */ + + +/** @defgroup I2C_Group4 I2C registers management functions + * @brief I2C registers management functions + * +@verbatim + =============================================================================== + ##### I2C registers management functions ##### + =============================================================================== + [..] This section provides a functions that allow user the management of + I2C registers. + +@endverbatim + * @{ + */ + + /** + * @brief Reads the specified I2C register and returns its value. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_Register: specifies the register to read. + * This parameter can be one of the following values: + * @arg I2C_Register_CR1: CR1 register. + * @arg I2C_Register_CR2: CR2 register. + * @arg I2C_Register_OAR1: OAR1 register. + * @arg I2C_Register_OAR2: OAR2 register. + * @arg I2C_Register_TIMINGR: TIMING register. + * @arg I2C_Register_TIMEOUTR: TIMEOUTR register. + * @arg I2C_Register_ISR: ISR register. + * @arg I2C_Register_ICR: ICR register. + * @arg I2C_Register_PECR: PECR register. + * @arg I2C_Register_RXDR: RXDR register. + * @arg I2C_Register_TXDR: TXDR register. + * @retval The value of the read register. + */ +uint32_t I2C_ReadRegister(I2C_TypeDef* I2Cx, uint8_t I2C_Register) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_REGISTER(I2C_Register)); + + tmp = (uint32_t)I2Cx; + tmp += I2C_Register; + + /* Return the selected register value */ + return (*(__IO uint32_t *) tmp); +} + +/** + * @} + */ + +/** @defgroup I2C_Group5 Data transfers management functions + * @brief Data transfers management functions + * +@verbatim + =============================================================================== + ##### Data transfers management functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage + the I2C data transfers. + + [..] The read access of the I2C_RXDR register can be done using + the I2C_ReceiveData() function and returns the received value. + Whereas a write access to the I2C_TXDR can be done using I2C_SendData() + function and stores the written data into TXDR. +@endverbatim + * @{ + */ + +/** + * @brief Sends a data byte through the I2Cx peripheral. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param Data: Byte to be transmitted.. + * @retval None + */ +void I2C_SendData(I2C_TypeDef* I2Cx, uint8_t Data) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Write in the DR register the data to be sent */ + I2Cx->TXDR = (uint8_t)Data; +} + +/** + * @brief Returns the most recent received data by the I2Cx peripheral. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @retval The value of the received data. + */ +uint8_t I2C_ReceiveData(I2C_TypeDef* I2Cx) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + + /* Return the data in the DR register */ + return (uint8_t)I2Cx->RXDR; +} + +/** + * @} + */ + + +/** @defgroup I2C_Group6 DMA transfers management functions + * @brief DMA transfers management functions + * +@verbatim + =============================================================================== + ##### DMA transfers management functions ##### + =============================================================================== + [..] This section provides two functions that can be used only in DMA mode. + [..] In DMA Mode, the I2C communication can be managed by 2 DMA Channel + requests: + (#) I2C_DMAReq_Tx: specifies the Tx buffer DMA transfer request. + (#) I2C_DMAReq_Rx: specifies the Rx buffer DMA transfer request. + [..] In this Mode it is advised to use the following function: + (+) I2C_DMACmd(I2C_TypeDef* I2Cx, uint32_t I2C_DMAReq, FunctionalState NewState); +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the I2C DMA interface. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_DMAReq: specifies the I2C DMA transfer request to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg I2C_DMAReq_Tx: Tx DMA transfer request + * @arg I2C_DMAReq_Rx: Rx DMA transfer request + * @param NewState: new state of the selected I2C DMA transfer request. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2C_DMACmd(I2C_TypeDef* I2Cx, uint32_t I2C_DMAReq, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_I2C_DMA_REQ(I2C_DMAReq)); + + if (NewState != DISABLE) + { + /* Enable the selected I2C DMA requests */ + I2Cx->CR1 |= I2C_DMAReq; + } + else + { + /* Disable the selected I2C DMA requests */ + I2Cx->CR1 &= (uint32_t)~I2C_DMAReq; + } +} +/** + * @} + */ + + +/** @defgroup I2C_Group7 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] This section provides functions allowing to configure the I2C Interrupts + sources and check or clear the flags or pending bits status. + The user should identify which mode will be used in his application to manage + the communication: Polling mode, Interrupt mode or DMA mode(refer I2C_Group6) . + + *** Polling Mode *** + ==================== + [..] In Polling Mode, the I2C communication can be managed by 15 flags: + (#) I2C_FLAG_TXE: to indicate the status of Transmit data register empty flag. + (#) I2C_FLAG_TXIS: to indicate the status of Transmit interrupt status flag . + (#) I2C_FLAG_RXNE: to indicate the status of Receive data register not empty flag. + (#) I2C_FLAG_ADDR: to indicate the status of Address matched flag (slave mode). + (#) I2C_FLAG_NACKF: to indicate the status of NACK received flag. + (#) I2C_FLAG_STOPF: to indicate the status of STOP detection flag. + (#) I2C_FLAG_TC: to indicate the status of Transfer complete flag(master mode). + (#) I2C_FLAG_TCR: to indicate the status of Transfer complete reload flag. + (#) I2C_FLAG_BERR: to indicate the status of Bus error flag. + (#) I2C_FLAG_ARLO: to indicate the status of Arbitration lost flag. + (#) I2C_FLAG_OVR: to indicate the status of Overrun/Underrun flag. + (#) I2C_FLAG_PECERR: to indicate the status of PEC error in reception flag. + (#) I2C_FLAG_TIMEOUT: to indicate the status of Timeout or Tlow detection flag. + (#) I2C_FLAG_ALERT: to indicate the status of SMBus Alert flag. + (#) I2C_FLAG_BUSY: to indicate the status of Bus busy flag. + + [..] In this Mode it is advised to use the following functions: + (+) FlagStatus I2C_GetFlagStatus(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG); + (+) void I2C_ClearFlag(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG); + + [..] + (@)Do not use the BUSY flag to handle each data transmission or reception.It is + better to use the TXIS and RXNE flags instead. + + *** Interrupt Mode *** + ====================== + [..] In Interrupt Mode, the I2C communication can be managed by 7 interrupt sources + and 15 pending bits: + [..] Interrupt Source: + (#) I2C_IT_ERRI: specifies the interrupt source for the Error interrupt. + (#) I2C_IT_TCI: specifies the interrupt source for the Transfer Complete interrupt. + (#) I2C_IT_STOPI: specifies the interrupt source for the Stop Detection interrupt. + (#) I2C_IT_NACKI: specifies the interrupt source for the Not Acknowledge received interrupt. + (#) I2C_IT_ADDRI: specifies the interrupt source for the Address Match interrupt. + (#) I2C_IT_RXI: specifies the interrupt source for the RX interrupt. + (#) I2C_IT_TXI: specifies the interrupt source for the TX interrupt. + + [..] Pending Bits: + (#) I2C_IT_TXIS: to indicate the status of Transmit interrupt status flag. + (#) I2C_IT_RXNE: to indicate the status of Receive data register not empty flag. + (#) I2C_IT_ADDR: to indicate the status of Address matched flag (slave mode). + (#) I2C_IT_NACKF: to indicate the status of NACK received flag. + (#) I2C_IT_STOPF: to indicate the status of STOP detection flag. + (#) I2C_IT_TC: to indicate the status of Transfer complete flag (master mode). + (#) I2C_IT_TCR: to indicate the status of Transfer complete reload flag. + (#) I2C_IT_BERR: to indicate the status of Bus error flag. + (#) I2C_IT_ARLO: to indicate the status of Arbitration lost flag. + (#) I2C_IT_OVR: to indicate the status of Overrun/Underrun flag. + (#) I2C_IT_PECERR: to indicate the status of PEC error in reception flag. + (#) I2C_IT_TIMEOUT: to indicate the status of Timeout or Tlow detection flag. + (#) I2C_IT_ALERT: to indicate the status of SMBus Alert flag. + + [..] In this Mode it is advised to use the following functions: + (+) void I2C_ClearITPendingBit(I2C_TypeDef* I2Cx, uint32_t I2C_IT); + (+) ITStatus I2C_GetITStatus(I2C_TypeDef* I2Cx, uint32_t I2C_IT); + +@endverbatim + * @{ + */ + +/** + * @brief Checks whether the specified I2C flag is set or not. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg I2C_FLAG_TXE: Transmit data register empty + * @arg I2C_FLAG_TXIS: Transmit interrupt status + * @arg I2C_FLAG_RXNE: Receive data register not empty + * @arg I2C_FLAG_ADDR: Address matched (slave mode) + * @arg I2C_FLAG_NACKF: NACK received flag + * @arg I2C_FLAG_STOPF: STOP detection flag + * @arg I2C_FLAG_TC: Transfer complete (master mode) + * @arg I2C_FLAG_TCR: Transfer complete reload + * @arg I2C_FLAG_BERR: Bus error + * @arg I2C_FLAG_ARLO: Arbitration lost + * @arg I2C_FLAG_OVR: Overrun/Underrun + * @arg I2C_FLAG_PECERR: PEC error in reception + * @arg I2C_FLAG_TIMEOUT: Timeout or Tlow detection flag + * @arg I2C_FLAG_ALERT: SMBus Alert + * @arg I2C_FLAG_BUSY: Bus busy + * @retval The new state of I2C_FLAG (SET or RESET). + */ +FlagStatus I2C_GetFlagStatus(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG) +{ + uint32_t tmpreg = 0; + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_GET_FLAG(I2C_FLAG)); + + /* Get the ISR register value */ + tmpreg = I2Cx->ISR; + + /* Get flag status */ + tmpreg &= I2C_FLAG; + + if(tmpreg != 0) + { + /* I2C_FLAG is set */ + bitstatus = SET; + } + else + { + /* I2C_FLAG is reset */ + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the I2Cx's pending flags. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_FLAG: specifies the flag to clear. + * This parameter can be any combination of the following values: + * @arg I2C_FLAG_ADDR: Address matched (slave mode) + * @arg I2C_FLAG_NACKF: NACK received flag + * @arg I2C_FLAG_STOPF: STOP detection flag + * @arg I2C_FLAG_BERR: Bus error + * @arg I2C_FLAG_ARLO: Arbitration lost + * @arg I2C_FLAG_OVR: Overrun/Underrun + * @arg I2C_FLAG_PECERR: PEC error in reception + * @arg I2C_FLAG_TIMEOUT: Timeout or Tlow detection flag + * @arg I2C_FLAG_ALERT: SMBus Alert + * @retval The new state of I2C_FLAG (SET or RESET). + */ +void I2C_ClearFlag(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_CLEAR_FLAG(I2C_FLAG)); + + /* Clear the selected flag */ + I2Cx->ICR = I2C_FLAG; + } + +/** + * @brief Checks whether the specified I2C interrupt has occurred or not. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_IT: specifies the interrupt source to check. + * This parameter can be one of the following values: + * @arg I2C_IT_TXIS: Transmit interrupt status + * @arg I2C_IT_RXNE: Receive data register not empty + * @arg I2C_IT_ADDR: Address matched (slave mode) + * @arg I2C_IT_NACKF: NACK received flag + * @arg I2C_IT_STOPF: STOP detection flag + * @arg I2C_IT_TC: Transfer complete (master mode) + * @arg I2C_IT_TCR: Transfer complete reload + * @arg I2C_IT_BERR: Bus error + * @arg I2C_IT_ARLO: Arbitration lost + * @arg I2C_IT_OVR: Overrun/Underrun + * @arg I2C_IT_PECERR: PEC error in reception + * @arg I2C_IT_TIMEOUT: Timeout or Tlow detection flag + * @arg I2C_IT_ALERT: SMBus Alert + * @retval The new state of I2C_IT (SET or RESET). + */ +ITStatus I2C_GetITStatus(I2C_TypeDef* I2Cx, uint32_t I2C_IT) +{ + uint32_t tmpreg = 0; + ITStatus bitstatus = RESET; + uint32_t enablestatus = 0; + + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_GET_IT(I2C_IT)); + + /* Check if the interrupt source is enabled or not */ + /* If Error interrupt */ + if((uint32_t)(I2C_IT & ERROR_IT_MASK)) + { + enablestatus = (uint32_t)((I2C_CR1_ERRIE) & (I2Cx->CR1)); + } + /* If TC interrupt */ + else if((uint32_t)(I2C_IT & TC_IT_MASK)) + { + enablestatus = (uint32_t)((I2C_CR1_TCIE) & (I2Cx->CR1)); + } + else + { + enablestatus = (uint32_t)((I2C_IT) & (I2Cx->CR1)); + } + + /* Get the ISR register value */ + tmpreg = I2Cx->ISR; + + /* Get flag status */ + tmpreg &= I2C_IT; + + /* Check the status of the specified I2C flag */ + if((tmpreg != RESET) && enablestatus) + { + /* I2C_IT is set */ + bitstatus = SET; + } + else + { + /* I2C_IT is reset */ + bitstatus = RESET; + } + + /* Return the I2C_IT status */ + return bitstatus; +} + +/** + * @brief Clears the I2Cx's interrupt pending bits. + * @param I2Cx: where x can be 1 or 2 to select the I2C peripheral. + * @param I2C_IT: specifies the interrupt pending bit to clear. + * This parameter can be any combination of the following values: + * @arg I2C_IT_ADDR: Address matched (slave mode) + * @arg I2C_IT_NACKF: NACK received flag + * @arg I2C_IT_STOPF: STOP detection flag + * @arg I2C_IT_BERR: Bus error + * @arg I2C_IT_ARLO: Arbitration lost + * @arg I2C_IT_OVR: Overrun/Underrun + * @arg I2C_IT_PECERR: PEC error in reception + * @arg I2C_IT_TIMEOUT: Timeout or Tlow detection flag + * @arg I2C_IT_ALERT: SMBus Alert + * @retval The new state of I2C_IT (SET or RESET). + */ +void I2C_ClearITPendingBit(I2C_TypeDef* I2Cx, uint32_t I2C_IT) +{ + /* Check the parameters */ + assert_param(IS_I2C_ALL_PERIPH(I2Cx)); + assert_param(IS_I2C_CLEAR_IT(I2C_IT)); + + /* Clear the selected flag */ + I2Cx->ICR = I2C_IT; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_iwdg.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_iwdg.c new file mode 100755 index 0000000000..885b2f98d2 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_iwdg.c @@ -0,0 +1,288 @@ +/** + ****************************************************************************** + * @file stm32f30x_iwdg.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Independent watchdog (IWDG) peripheral: + * + Prescaler and Counter configuration + * + IWDG activation + * + Flag management + * + @verbatim + + =============================================================================== + ##### IWDG features ##### + =============================================================================== + [..] The IWDG can be started by either software or hardware (configurable + through option byte). + [..] The IWDG is clocked by its own dedicated low-speed clock (LSI) and + thus stays active even if the main clock fails. + Once the IWDG is started, the LSI is forced ON and cannot be disabled + (LSI cannot be disabled too), and the counter starts counting down from + the reset value of 0xFFF. When it reaches the end of count value (0x000) + a system reset is generated. + The IWDG counter should be reloaded at regular intervals to prevent + an MCU reset. + [..] The IWDG is implemented in the VDD voltage domain that is still functional + in STOP and STANDBY mode (IWDG reset can wake-up from STANDBY). + [..] IWDGRST flag in RCC_CSR register can be used to inform when a IWDG + reset occurs. + [..] Min-max timeout value @41KHz (LSI): ~0.1ms / ~25.5s + The IWDG timeout may vary due to LSI frequency dispersion. STM32F30x + devices provide the capability to measure the LSI frequency (LSI clock + connected internally to TIM16 CH1 input capture). The measured value + can be used to have an IWDG timeout with an acceptable accuracy. + For more information, please refer to the STM32F30x Reference manual. + + ##### How to use this driver ##### + =============================================================================== + [..] This driver allows to use IWDG peripheral with either window option enabled + or disabled. To do so follow one of the two procedures below. + (#) Window option is enabled: + (++) Start the IWDG using IWDG_Enable() function, when the IWDG is used + in software mode (no need to enable the LSI, it will be enabled + by hardware). + (++) Enable write access to IWDG_PR and IWDG_RLR registers using + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable) function. + (++) Configure the IWDG prescaler using IWDG_SetPrescaler() function. + (++) Configure the IWDG counter value using IWDG_SetReload() function. + This value will be loaded in the IWDG counter each time the counter + is reloaded, then the IWDG will start counting down from this value. + (++) Wait for the IWDG registers to be updated using IWDG_GetFlagStatus() function. + (++) Configure the IWDG refresh window using IWDG_SetWindowValue() function. + + (#) Window option is disabled: + (++) Enable write access to IWDG_PR and IWDG_RLR registers using + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable) function. + (++) Configure the IWDG prescaler using IWDG_SetPrescaler() function. + (++) Configure the IWDG counter value using IWDG_SetReload() function. + This value will be loaded in the IWDG counter each time the counter + is reloaded, then the IWDG will start counting down from this value. + (++) Wait for the IWDG registers to be updated using IWDG_GetFlagStatus() function. + (++) reload the IWDG counter at regular intervals during normal operation + to prevent an MCU reset, using IWDG_ReloadCounter() function. + (++) Start the IWDG using IWDG_Enable() function, when the IWDG is used + in software mode (no need to enable the LSI, it will be enabled + by hardware). + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_iwdg.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup IWDG + * @brief IWDG driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* ---------------------- IWDG registers bit mask ----------------------------*/ +/* KR register bit mask */ +#define KR_KEY_RELOAD ((uint16_t)0xAAAA) +#define KR_KEY_ENABLE ((uint16_t)0xCCCC) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup IWDG_Private_Functions + * @{ + */ + +/** @defgroup IWDG_Group1 Prescaler and Counter configuration functions + * @brief Prescaler and Counter configuration functions + * +@verbatim + =============================================================================== + ##### Prescaler and Counter configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables write access to IWDG_PR and IWDG_RLR registers. + * @param IWDG_WriteAccess: new state of write access to IWDG_PR and IWDG_RLR registers. + * This parameter can be one of the following values: + * @arg IWDG_WriteAccess_Enable: Enable write access to IWDG_PR and IWDG_RLR registers + * @arg IWDG_WriteAccess_Disable: Disable write access to IWDG_PR and IWDG_RLR registers + * @retval None + */ +void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess) +{ + /* Check the parameters */ + assert_param(IS_IWDG_WRITE_ACCESS(IWDG_WriteAccess)); + IWDG->KR = IWDG_WriteAccess; +} + +/** + * @brief Sets IWDG Prescaler value. + * @param IWDG_Prescaler: specifies the IWDG Prescaler value. + * This parameter can be one of the following values: + * @arg IWDG_Prescaler_4: IWDG prescaler set to 4 + * @arg IWDG_Prescaler_8: IWDG prescaler set to 8 + * @arg IWDG_Prescaler_16: IWDG prescaler set to 16 + * @arg IWDG_Prescaler_32: IWDG prescaler set to 32 + * @arg IWDG_Prescaler_64: IWDG prescaler set to 64 + * @arg IWDG_Prescaler_128: IWDG prescaler set to 128 + * @arg IWDG_Prescaler_256: IWDG prescaler set to 256 + * @retval None + */ +void IWDG_SetPrescaler(uint8_t IWDG_Prescaler) +{ + /* Check the parameters */ + assert_param(IS_IWDG_PRESCALER(IWDG_Prescaler)); + IWDG->PR = IWDG_Prescaler; +} + +/** + * @brief Sets IWDG Reload value. + * @param Reload: specifies the IWDG Reload value. + * This parameter must be a number between 0 and 0x0FFF. + * @retval None + */ +void IWDG_SetReload(uint16_t Reload) +{ + /* Check the parameters */ + assert_param(IS_IWDG_RELOAD(Reload)); + IWDG->RLR = Reload; +} + +/** + * @brief Reloads IWDG counter with value defined in the reload register + * (write access to IWDG_PR and IWDG_RLR registers disabled). + * @param None + * @retval None + */ +void IWDG_ReloadCounter(void) +{ + IWDG->KR = KR_KEY_RELOAD; +} + + +/** + * @brief Sets the IWDG window value. + * @param WindowValue: specifies the window value to be compared to the downcounter. + * @retval None + */ +void IWDG_SetWindowValue(uint16_t WindowValue) +{ + /* Check the parameters */ + assert_param(IS_IWDG_WINDOW_VALUE(WindowValue)); + IWDG->WINR = WindowValue; +} + +/** + * @} + */ + +/** @defgroup IWDG_Group2 IWDG activation function + * @brief IWDG activation function + * +@verbatim + =============================================================================== + ##### IWDG activation function ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables IWDG (write access to IWDG_PR and IWDG_RLR registers disabled). + * @param None + * @retval None + */ +void IWDG_Enable(void) +{ + IWDG->KR = KR_KEY_ENABLE; +} + +/** + * @} + */ + +/** @defgroup IWDG_Group3 Flag management function + * @brief Flag management function + * +@verbatim + =============================================================================== + ##### Flag management function ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Checks whether the specified IWDG flag is set or not. + * @param IWDG_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg IWDG_FLAG_PVU: Prescaler Value Update on going + * @arg IWDG_FLAG_RVU: Reload Value Update on going + * @arg IWDG_FLAG_WVU: Counter Window Value Update on going + * @retval The new state of IWDG_FLAG (SET or RESET). + */ +FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_IWDG_FLAG(IWDG_FLAG)); + if ((IWDG->SR & IWDG_FLAG) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + /* Return the flag status */ + return bitstatus; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_misc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_misc.c new file mode 100755 index 0000000000..43e647dea1 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_misc.c @@ -0,0 +1,230 @@ +/** + ****************************************************************************** + * @file stm32f30x_misc.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides all the miscellaneous firmware functions (add-on + * to CMSIS functions). + * + @verbatim + + =============================================================================== + ##### How to configure Interrupts using driver ##### + =============================================================================== + [..] This section provide functions allowing to configure the NVIC interrupts + (IRQ). The Cortex-M4 exceptions are managed by CMSIS functions. + (#) Configure the NVIC Priority Grouping using NVIC_PriorityGroupConfig() + function according to the following table. + The table below gives the allowed values of the pre-emption priority + and subpriority according to the Priority Grouping configuration + performed by NVIC_PriorityGroupConfig function. + + (#) Enable and Configure the priority of the selected IRQ Channels. + [..] + (@) When the NVIC_PriorityGroup_0 is selected, it will no any nested interrupt, + the IRQ priority will be managed only by subpriority. + The sub-priority is only used to sort pending exception priorities, + and does not affect active exceptions. + (@) Lower priority values gives higher priority. + (@) Priority Order: + (#@) Lowest Preemption priority. + (#@) Lowest Subpriority. + (#@) Lowest hardware priority (IRQn position). + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_misc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup MISC + * @brief MISC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup MISC_Private_Functions + * @{ + */ + +/** + * @brief Configures the priority grouping: pre-emption priority and subpriority. + * @param NVIC_PriorityGroup: specifies the priority grouping bits length. + * This parameter can be one of the following values: + * @arg NVIC_PriorityGroup_0: 0 bits for pre-emption priority. + * 4 bits for subpriority. + * @arg NVIC_PriorityGroup_1: 1 bits for pre-emption priority. + * 3 bits for subpriority. + * @arg NVIC_PriorityGroup_2: 2 bits for pre-emption priority. + * 2 bits for subpriority. + * @arg NVIC_PriorityGroup_3: 3 bits for pre-emption priority. + * 1 bits for subpriority. + * @arg NVIC_PriorityGroup_4: 4 bits for pre-emption priority. + * 0 bits for subpriority. + * @note When NVIC_PriorityGroup_0 is selected, it will no be any nested + * interrupt. This interrupts priority is managed only with subpriority. + * @retval None + */ +void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup) +{ + /* Check the parameters */ + assert_param(IS_NVIC_PRIORITY_GROUP(NVIC_PriorityGroup)); + + /* Set the PRIGROUP[10:8] bits according to NVIC_PriorityGroup value */ + SCB->AIRCR = AIRCR_VECTKEY_MASK | NVIC_PriorityGroup; +} + +/** + * @brief Initializes the NVIC peripheral according to the specified + * parameters in the NVIC_InitStruct. + * @note To configure interrupts priority correctly, the NVIC_PriorityGroupConfig() + * function should be called before. + * @param NVIC_InitStruct: pointer to a NVIC_InitTypeDef structure that contains + * the configuration information for the specified NVIC peripheral. + * @retval None + */ +void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct) +{ + uint32_t tmppriority = 0x00, tmppre = 0x00, tmpsub = 0x0F; + + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NVIC_InitStruct->NVIC_IRQChannelCmd)); + assert_param(IS_NVIC_PREEMPTION_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelPreemptionPriority)); + assert_param(IS_NVIC_SUB_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelSubPriority)); + + if (NVIC_InitStruct->NVIC_IRQChannelCmd != DISABLE) + { + /* Compute the Corresponding IRQ Priority --------------------------------*/ + tmppriority = (0x700 - ((SCB->AIRCR) & (uint32_t)0x700))>> 0x08; + tmppre = (0x4 - tmppriority); + tmpsub = tmpsub >> tmppriority; + + tmppriority = (uint32_t)NVIC_InitStruct->NVIC_IRQChannelPreemptionPriority << tmppre; + tmppriority |= NVIC_InitStruct->NVIC_IRQChannelSubPriority & tmpsub; + tmppriority = tmppriority << 0x04; + + NVIC->IP[NVIC_InitStruct->NVIC_IRQChannel] = tmppriority; + + /* Enable the Selected IRQ Channels --------------------------------------*/ + NVIC->ISER[NVIC_InitStruct->NVIC_IRQChannel >> 0x05] = + (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F); + } + else + { + /* Disable the Selected IRQ Channels -------------------------------------*/ + NVIC->ICER[NVIC_InitStruct->NVIC_IRQChannel >> 0x05] = + (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F); + } +} + +/** + * @brief Sets the vector table location and Offset. + * @param NVIC_VectTab: specifies if the vector table is in RAM or FLASH memory. + * This parameter can be one of the following values: + * @arg NVIC_VectTab_RAM + * @arg NVIC_VectTab_FLASH + * @param Offset: Vector Table base offset field. This value must be a multiple of 0x200. + * @retval None + */ +void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset) +{ + /* Check the parameters */ + assert_param(IS_NVIC_VECTTAB(NVIC_VectTab)); + assert_param(IS_NVIC_OFFSET(Offset)); + + SCB->VTOR = NVIC_VectTab | (Offset & (uint32_t)0x1FFFFF80); +} + +/** + * @brief Selects the condition for the system to enter low power mode. + * @param LowPowerMode: Specifies the new mode for the system to enter low power mode. + * This parameter can be one of the following values: + * @arg NVIC_LP_SEVONPEND + * @arg NVIC_LP_SLEEPDEEP + * @arg NVIC_LP_SLEEPONEXIT + * @param NewState: new state of LP condition. This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_NVIC_LP(LowPowerMode)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + SCB->SCR |= LowPowerMode; + } + else + { + SCB->SCR &= (uint32_t)(~(uint32_t)LowPowerMode); + } +} + +/** + * @brief Configures the SysTick clock source. + * @param SysTick_CLKSource: specifies the SysTick clock source. + * This parameter can be one of the following values: + * @arg SysTick_CLKSource_HCLK_Div8: AHB clock divided by 8 selected as SysTick clock source. + * @arg SysTick_CLKSource_HCLK: AHB clock selected as SysTick clock source. + * @retval None + */ +void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource) +{ + /* Check the parameters */ + assert_param(IS_SYSTICK_CLK_SOURCE(SysTick_CLKSource)); + if (SysTick_CLKSource == SysTick_CLKSource_HCLK) + { + SysTick->CTRL |= SysTick_CLKSource_HCLK; + } + else + { + SysTick->CTRL &= SysTick_CLKSource_HCLK_Div8; + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_opamp.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_opamp.c new file mode 100755 index 0000000000..f215d58889 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_opamp.c @@ -0,0 +1,575 @@ +/** + ****************************************************************************** + * @file stm32f30x_opamp.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the operational amplifiers (OPAMP1,...OPAMP4) peripheral: + * + OPAMP Configuration + * + OPAMP calibration + * + @verbatim + + ============================================================================== + ##### OPAMP Peripheral Features ##### + ============================================================================== + + [..] + The device integrates 4 operational amplifiers OPAMP1, OPAMP2, OPAMP3 and OPAMP4: + + (+) The OPAMPs non inverting input can be selected among the list shown by + table below. + + (+) The OPAMPs inverting input can be selected among the list shown by + table below. + + (+) The OPAMPs outputs can be internally connected to the inverting input + (follower mode) + (+) The OPAMPs outputs can be internally connected to resistor feedback + output (Programmable Gain Amplifier mode) + + (+) The OPAMPs outputs can be internally connected to ADC + + (+) The OPAMPs can be calibrated to compensate the offset compensation + + (+) Timer-controlled Mux for automatic switch of inverting and + non-inverting input + + OPAMPs inverting/non-inverting inputs: + +--------------------------------------------------------------+ + | | | OPAMP1 | OPAMP2 | OPAMP3 | OPAMP4 | + |-----------------|--------|--------|--------|--------|--------| + | | PGA | OK | OK | OK | OK | + | Inverting Input | Vout | OK | OK | OK | OK | + | | IO1 | PC5 | PC5 | PB10 | PB10 | + | | IO2 | PA3 | PA5 | PB2 | PD8 | + |-----------------|--------|--------|--------|--------|--------| + | | IO1 | PA7 | PD14 | PB13 | PD11 | + | Non Inverting | IO2 | PA5 | PB14 | PA5 | PB11 | + | Input | IO3 | PA3 | PB0 | PA1 | PA4 | + | | IO4 | PA1 | PA7 | PB0 | PB13 | + +--------------------------------------------------------------+ + + ##### How to use this driver ##### + ============================================================================== + [..] + This driver provides functions to configure and program the OPAMP + of all STM32F30x devices. + + To use the OPAMP, perform the following steps: + + (#) Enable the SYSCFG APB clock to get write access to OPAMP + register using RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + + (#) Configure the OPAMP input in analog mode using GPIO_Init() + + (#) Configure the OPAMP using OPAMP_Init() function: + (++) Select the inverting input + (++) Select the non-inverting inverting input + + (#) Enable the OPAMP using OPAMP_Cmd() function + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_opamp.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup OPAMP + * @brief OPAMP driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define OPAMP_CSR_DEFAULT_MASK ((uint32_t)0xFFFFFF93) +#define OPAMP_CSR_TIMERMUX_MASK ((uint32_t)0xFFFFF8FF) +#define OPAMP_CSR_TRIMMING_MASK ((uint32_t)0x0000001F) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup OPAMP_Private_Functions + * @{ + */ + +/** @defgroup OPAMP_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes OPAMP peripheral registers to their default reset values. + * @note Deinitialization can't be performed if the OPAMP configuration is locked. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param None + * @retval None + */ +void OPAMP_DeInit(uint32_t OPAMP_Selection) +{ + /*!< Set OPAMP_CSR register to reset value */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) = ((uint32_t)0x00000000); +} + +/** + * @brief Initializes the OPAMP peripheral according to the specified parameters + * in OPAMP_InitStruct + * @note If the selected OPAMP is locked, initialization can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param OPAMP_InitStruct: pointer to an OPAMP_InitTypeDef structure that contains + * the configuration information for the specified OPAMP peripheral. + * - OPAMP_InvertingInput specifies the inverting input of OPAMP + * - OPAMP_NonInvertingInput specifies the non inverting input of OPAMP + * @retval None + */ +void OPAMP_Init(uint32_t OPAMP_Selection, OPAMP_InitTypeDef* OPAMP_InitStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_OPAMP_INVERTING_INPUT(OPAMP_InitStruct->OPAMP_InvertingInput)); + assert_param(IS_OPAMP_NONINVERTING_INPUT(OPAMP_InitStruct->OPAMP_NonInvertingInput)); + + /*!< Get the OPAMPx_CSR register value */ + tmpreg = *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection); + + /*!< Clear the inverting and non inverting bits selection bits */ + tmpreg &= (uint32_t) (OPAMP_CSR_DEFAULT_MASK); + + /*!< Configure OPAMP: inverting and non inverting inputs */ + tmpreg |= (uint32_t)(OPAMP_InitStruct->OPAMP_InvertingInput | OPAMP_InitStruct->OPAMP_NonInvertingInput); + + /*!< Write to OPAMPx_CSR register */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) = tmpreg; +} + +/** + * @brief Fills each OPAMP_InitStruct member with its default value. + * @param OPAMP_InitStruct: pointer to an OPAMP_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void OPAMP_StructInit(OPAMP_InitTypeDef* OPAMP_InitStruct) +{ + OPAMP_InitStruct->OPAMP_NonInvertingInput = OPAMP_NonInvertingInput_IO1; + OPAMP_InitStruct->OPAMP_InvertingInput = OPAMP_InvertingInput_IO1; +} + +/** + * @brief Configure the feedback resistor gain. + * @note If the selected OPAMP is locked, gain configuration can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param NewState: new state of the OPAMP peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void OPAMP_PGAConfig(uint32_t OPAMP_Selection, uint32_t OPAMP_PGAGain, uint32_t OPAMP_PGAConnect) +{ + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_OPAMP_PGAGAIN(OPAMP_PGAGain)); + assert_param(IS_OPAMP_PGACONNECT(OPAMP_PGAConnect)); + + /* Reset the configuration bits */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) &= (uint32_t)(~OPAMP_CSR_PGGAIN); + + /* Set the new configuration */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) |= (uint32_t) (OPAMP_PGAGain | OPAMP_PGAConnect); +} + +/** + * @brief Configure the OPAMP's internal reference. + * @note This feature is used when calibration enabled or OPAMP's reference + * connected to the non inverting input. + * @note If the selected OPAMP is locked, Vref configuration can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param OPAMP_Vref: This parameter can be: + * OPAMP_Vref_3VDDA: OPMAP Vref = 3.3% VDDA + * OPAMP_Vref_10VDDA: OPMAP Vref = 10% VDDA + * OPAMP_Vref_50VDDA: OPMAP Vref = 50% VDDA + * OPAMP_Vref_90VDDA: OPMAP Vref = 90% VDDA + * @retval None + */ +void OPAMP_VrefConfig(uint32_t OPAMP_Selection, uint32_t OPAMP_Vref) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_OPAMP_VREF(OPAMP_Vref)); + + /*!< Get the OPAMPx_CSR register value */ + tmpreg = *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection); + + /*!< Clear the CALSEL bits */ + tmpreg &= (uint32_t) (~OPAMP_CSR_CALSEL); + + /*!< Configure OPAMP reference */ + tmpreg |= (uint32_t)(OPAMP_Vref); + + /*!< Write to OPAMPx_CSR register */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) = tmpreg; +} + +/** + * @brief Connect the internal reference to the OPAMP's non inverting input. + * @note If the selected OPAMP is locked, Vref configuration can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param NewState: new state of the OPAMP peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void OPAMP_VrefConnectNonInvertingInput(uint32_t OPAMP_Selection, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Connect the internal reference to the OPAMP's non inverting input */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) |= (uint32_t) (OPAMP_CSR_FORCEVP); + } + else + { + /* Disconnect the internal reference to the OPAMP's non inverting input */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) &= (uint32_t)(~OPAMP_CSR_FORCEVP); + } +} + +/** + * @brief Enables or disables connecting the OPAMP's internal reference to ADC. + * @note If the selected OPAMP is locked, Vref connection can't be performed. + * To unlock the configuration, perform a system reset. + * @param NewState: new state of the Vrefint output. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void OPAMP_VrefConnectADCCmd(uint32_t OPAMP_Selection, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable output internal reference */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) |= (uint32_t) (OPAMP_CSR_TSTREF); + } + else + { + /* Disable output internal reference */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) &= (uint32_t)(~OPAMP_CSR_TSTREF); + } +} + +/** + * @brief Configure the OPAMP peripheral (secondary inputs) for timer-controlled + * mux mode according to the specified parameters in OPAMP_InitStruct. + * @note If the selected OPAMP is locked, timer-controlled mux configuration + * can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param OPAMP_InitStruct: pointer to an OPAMP_InitTypeDef structure that contains + * the configuration information for the specified OPAMP peripheral. + * - OPAMP_InvertingInput specifies the inverting input of OPAMP + * - OPAMP_NonInvertingInput specifies the non inverting input of OPAMP + * @note PGA and Vout can't be selected as secondary inverting input. + * @retval None + */ +void OPAMP_TimerControlledMuxConfig(uint32_t OPAMP_Selection, OPAMP_InitTypeDef* OPAMP_InitStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_OPAMP_SECONDARY_INVINPUT(OPAMP_InitStruct->OPAMP_InvertingInput)); + assert_param(IS_OPAMP_NONINVERTING_INPUT(OPAMP_InitStruct->OPAMP_NonInvertingInput)); + + /*!< Get the OPAMPx_CSR register value */ + tmpreg = *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection); + + /*!< Clear the secondary inverting bit, secondary non inverting bit and TCMEN bits */ + tmpreg &= (uint32_t) (OPAMP_CSR_TIMERMUX_MASK); + + /*!< Configure OPAMP: secondary inverting and non inverting inputs */ + tmpreg |= (uint32_t)((uint32_t)(OPAMP_InitStruct->OPAMP_InvertingInput<<3) | (uint32_t)(OPAMP_InitStruct->OPAMP_NonInvertingInput<<7)); + + /*!< Write to OPAMPx_CSR register */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) = tmpreg; +} + +/** + * @brief Enable or disable the timer-controlled mux mode. + * @note If the selected OPAMP is locked, enable/disable can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param NewState: new state of the OPAMP peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void OPAMP_TimerControlledMuxCmd(uint32_t OPAMP_Selection, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the timer-controlled Mux mode */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) |= (uint32_t) (OPAMP_CSR_TCMEN); + } + else + { + /* Disable the timer-controlled Mux mode */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) &= (uint32_t)(~OPAMP_CSR_TCMEN); + } +} + +/** + * @brief Enable or disable the OPAMP peripheral. + * @note If the selected OPAMP is locked, enable/disable can't be performed. + * To unlock the configuration, perform a system reset. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param NewState: new state of the OPAMP peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void OPAMP_Cmd(uint32_t OPAMP_Selection, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected OPAMPx peripheral */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) |= (uint32_t) (OPAMP_CSR_OPAMPxEN); + } + else + { + /* Disable the selected OPAMPx peripheral */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) &= (uint32_t)(~OPAMP_CSR_OPAMPxEN); + } +} + +/** + * @brief Return the output level (high or low) during calibration of the selected OPAMP. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * - OPAMP output is low when the non-inverting input is at a lower + * voltage than the inverting input + * - OPAMP output is high when the non-inverting input is at a higher + * voltage than the inverting input + * @note OPAMP output level is provided only during calibration phase. + * @retval Returns the selected OPAMP output level: low or high. + * + */ +uint32_t OPAMP_GetOutputLevel(uint32_t OPAMP_Selection) +{ + uint32_t opampout = 0x0; + + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + + /* Check if selected OPAMP output is high */ + if ((*(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) & (OPAMP_CSR_OUTCAL)) != 0) + { + opampout = OPAMP_OutputLevel_High; + } + else + { + opampout = OPAMP_OutputLevel_Low; + } + + /* Return the OPAMP output level */ + return (uint32_t)(opampout); +} + +/** + * @brief Select the trimming mode. + * @param OffsetTrimming: the selected offset trimming mode. + * This parameter can be one of the following values: + * @arg OPAMP_Trimming_Factory: factory trimming values are used for offset + * calibration + * @arg OPAMP_Trimming_User: user trimming values are used for offset + * calibration + * @note When OffsetTrimming_User is selected, use OPAMP_OffsetTrimConfig() + * function or OPAMP_OffsetTrimLowPowerConfig() function to adjust + * trimming value. + * @retval None + */ +void OPAMP_OffsetTrimModeSelect(uint32_t OPAMP_Selection, uint32_t OPAMP_Trimming) +{ + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_OPAMP_TRIMMING(OPAMP_Trimming)); + + /* Reset USERTRIM bit */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) &= (~(uint32_t) (OPAMP_CSR_USERTRIM)); + + /* Select trimming mode */ + *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection) |= OPAMP_Trimming; +} + +/** + * @brief Configure the trimming value of the OPAMP. + * @param OPAMP_Selection: the selected OPAMP. + * This parameter can be OPAMP_Selection_OPAMPx where x can be 1 to 4 + * to select the OPAMP peripheral. + * @param OPAMP_Input: the selected OPAMP input. + * This parameter can be one of the following values: + * @arg OPAMP_Input_Inverting: Inverting input is selected to configure the trimming value + * @arg OPAMP_Input_NonInverting: Non inverting input is selected to configure the trimming value + * @param OPAMP_TrimValue: the trimming value. This parameter can be any value lower + * or equal to 0x0000001F. + * @retval None + */ +void OPAMP_OffsetTrimConfig(uint32_t OPAMP_Selection, uint32_t OPAMP_Input, uint32_t OPAMP_TrimValue) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_OPAMP_ALL_PERIPH(OPAMP_Selection)); + assert_param(IS_OPAMP_INPUT(OPAMP_Input)); + assert_param(IS_OPAMP_TRIMMINGVALUE(OPAMP_TrimValue)); + + /*!< Get the OPAMPx_CSR register value */ + tmpreg = *(__IO uint32_t *) (OPAMP_BASE + OPAMP_Selection); + + /*!< Clear the trimming bits */ + tmpreg &= ((uint32_t)~(OPAMP_CSR_TRIMMING_MASK<
© COPYRIGHT 2015 STMicroelectronics
+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_pwr.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup PWR + * @brief PWR driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* --------- PWR registers bit address in the alias region ---------- */ +#define PWR_OFFSET (PWR_BASE - PERIPH_BASE) + +/* --- CR Register ---*/ + +/* Alias word address of DBP bit */ +#define CR_OFFSET (PWR_OFFSET + 0x00) +#define DBP_BitNumber 0x08 +#define CR_DBP_BB (PERIPH_BB_BASE + (CR_OFFSET * 32) + (DBP_BitNumber * 4)) + +/* Alias word address of PVDE bit */ +#define PVDE_BitNumber 0x04 +#define CR_PVDE_BB (PERIPH_BB_BASE + (CR_OFFSET * 32) + (PVDE_BitNumber * 4)) + +/* ------------------ PWR registers bit mask ------------------------ */ + +/* CR register bit mask */ +#define CR_DS_MASK ((uint32_t)0xFFFFFFFC) +#define CR_PLS_MASK ((uint32_t)0xFFFFFF1F) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup PWR_Private_Functions + * @{ + */ + +/** @defgroup PWR_Group1 Backup Domain Access function + * @brief Backup Domain Access function + * +@verbatim + ============================================================================== + ##### Backup Domain Access function ##### + ============================================================================== + + [..] After reset, the Backup Domain Registers (RCC BDCR Register, RTC registers + and RTC backup registers) are protected against possible stray write accesses. + [..] To enable access to Backup domain use the PWR_BackupAccessCmd(ENABLE) function. + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the PWR peripheral registers to their default reset values. + * @param None + * @retval None + */ +void PWR_DeInit(void) +{ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_PWR, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_PWR, DISABLE); +} + +/** + * @brief Enables or disables access to the RTC and backup registers. + * @note If the HSE divided by 32 is used as the RTC clock, the + * Backup Domain Access should be kept enabled. + * @param NewState: new state of the access to the RTC and backup registers. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void PWR_BackupAccessCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + *(__IO uint32_t *) CR_DBP_BB = (uint32_t)NewState; +} + +/** + * @} + */ + +/** @defgroup PWR_Group2 PVD configuration functions + * @brief PVD configuration functions + * +@verbatim + =============================================================================== + ##### PVD configuration functions ##### + ============================================================================== + [..] + (+) The PVD is used to monitor the VDD power supply by comparing it to a threshold + selected by the PVD Level (PLS[2:0] bits in the PWR_CR). + (+) A PVDO flag is available to indicate if VDD/VDDA is higher or lower than the + PVD threshold. This event is internally connected to the EXTI line16 + and can generate an interrupt if enabled through the EXTI registers. + (+) The PVD is stopped in Standby mode. + +@endverbatim + * @{ + */ + +/** + * @brief Configures the voltage threshold detected by the Power Voltage Detector(PVD). + * @param PWR_PVDLevel: specifies the PVD detection level + * This parameter can be one of the following values: + * @arg PWR_PVDLevel_0: PVD detection level set to 2.18V + * @arg PWR_PVDLevel_1: PVD detection level set to 2.28V + * @arg PWR_PVDLevel_2: PVD detection level set to 2.38V + * @arg PWR_PVDLevel_3: PVD detection level set to 2.48V + * @arg PWR_PVDLevel_4: PVD detection level set to 2.58V + * @arg PWR_PVDLevel_5: PVD detection level set to 2.68V + * @arg PWR_PVDLevel_6: PVD detection level set to 2.78V + * @arg PWR_PVDLevel_7: PVD detection level set to 2.88V + * @retval None + */ +void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_PWR_PVD_LEVEL(PWR_PVDLevel)); + + tmpreg = PWR->CR; + + /* Clear PLS[7:5] bits */ + tmpreg &= CR_PLS_MASK; + + /* Set PLS[7:5] bits according to PWR_PVDLevel value */ + tmpreg |= PWR_PVDLevel; + + /* Store the new value */ + PWR->CR = tmpreg; +} + +/** + * @brief Enables or disables the Power Voltage Detector(PVD). + * @param NewState: new state of the PVD. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void PWR_PVDCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + *(__IO uint32_t *) CR_PVDE_BB = (uint32_t)NewState; +} + +/** + * @} + */ + +/** @defgroup PWR_Group3 WakeUp pins configuration functions + * @brief WakeUp pins configuration functions + * +@verbatim + =============================================================================== + ##### WakeUp pins configuration functions ##### + =============================================================================== + [..] + (+) WakeUp pins are used to wakeup the system from Standby mode. These pins are + forced in input pull down configuration and are active on rising edges. + (+) There are three WakeUp pins: WakeUp Pin 1 on PA.00, WakeUp Pin 2 on PC.13 and + WakeUp Pin 3 on PE.06. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the WakeUp Pin functionality. + * @param PWR_WakeUpPin: specifies the WakeUpPin. + * This parameter can be: PWR_WakeUpPin_1, PWR_WakeUpPin_2 or PWR_WakeUpPin_3. + * @param NewState: new state of the WakeUp Pin functionality. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void PWR_WakeUpPinCmd(uint32_t PWR_WakeUpPin, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_PWR_WAKEUP_PIN(PWR_WakeUpPin)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the EWUPx pin */ + PWR->CSR |= PWR_WakeUpPin; + } + else + { + /* Disable the EWUPx pin */ + PWR->CSR &= ~PWR_WakeUpPin; + } +} + +/** + * @} + */ + + +/** @defgroup PWR_Group4 Low Power modes configuration functions + * @brief Low Power modes configuration functions + * +@verbatim + =============================================================================== + ##### Low Power modes configuration functions ##### + ============================================================================== + + [..] The devices feature three low-power modes: + (+) Sleep mode: Cortex-M4 core stopped, peripherals kept running. + (+) Stop mode: all clocks are stopped, regulator running, regulator in low power mode + (+) Standby mode: VCORE domain powered off + + *** Sleep mode *** + ================== + [..] + (+) Entry: + (++) The Sleep mode is entered by executing the WFE() or WFI() instructions. + (+) Exit: + (++) Any peripheral interrupt acknowledged by the nested vectored interrupt + controller (NVIC) can wake up the device from Sleep mode. + + *** Stop mode *** + ================= + [..] In Stop mode, all clocks in the VCORE domain are stopped, the PLL, the HSI, + and the HSE RC oscillators are disabled. Internal SRAM and register + contents are preserved. + The voltage regulator can be configured either in normal or low-power mode. + + (+) Entry: + (++) The Stop mode is entered using the PWR_EnterSTOPMode(PWR_Regulator_LowPower,) + function with regulator in LowPower or with Regulator ON. + (+) Exit: + (++) Any EXTI Line (Internal or External) configured in Interrupt/Event mode + or any internal IPs (I2C or UASRT) wakeup event. + + *** Standby mode *** + ==================== + [..] The Standby mode allows to achieve the lowest power consumption. It is based + on the Cortex-M4 deepsleep mode, with the voltage regulator disabled. + The VCORE domain is consequently powered off. The PLL, the HSI, and the HSE + oscillator are also switched off. SRAM and register + contents are lost except for the Backup domain (RTC registers, RTC backup + registers and Standby circuitry). + + [..] The voltage regulator is OFF. + + (+) Entry: + (++) The Standby mode is entered using the PWR_EnterSTANDBYMode() function. + (+) Exit: + (++) WKUP pin rising edge, RTC alarm (Alarm A and Alarm B), RTC wakeup, + tamper event, time-stamp event, external reset in NRST pin, IWDG reset. + + *** Auto-wakeup (AWU) from low-power mode *** + ============================================= + [..] The MCU can be woken up from low-power mode by an RTC Alarm event, a tamper + event, a time-stamp event, or a comparator event, without depending on an + external interrupt (Auto-wakeup mode). + + (+) RTC auto-wakeup (AWU) from the Stop mode + (++) To wake up from the Stop mode with an RTC alarm event, it is necessary to: + (+++) Configure the EXTI Line 17 to be sensitive to rising edges (Interrupt + or Event modes) using the EXTI_Init() function. + (+++) Enable the RTC Alarm Interrupt using the RTC_ITConfig() function + (+++) Configure the RTC to generate the RTC alarm using the RTC_SetAlarm() + and RTC_AlarmCmd() functions. + (++) To wake up from the Stop mode with an RTC Tamper or time stamp event, it + is necessary to: + (+++) Configure the EXTI Line 19 to be sensitive to rising edges (Interrupt + or Event modes) using the EXTI_Init() function. + (+++) Enable the RTC Tamper or time stamp Interrupt using the RTC_ITConfig() + function. + (+++) Configure the RTC to detect the tamper or time stamp event using the + RTC_TimeStampConfig(), RTC_TamperTriggerConfig() and RTC_TamperCmd() + functions. + + (+) RTC auto-wakeup (AWU) from the Standby mode + (++) To wake up from the Standby mode with an RTC alarm event, it is necessary to: + (+++) Enable the RTC Alarm Interrupt using the RTC_ITConfig() function. + (+++) Configure the RTC to generate the RTC alarm using the RTC_SetAlarm() + and RTC_AlarmCmd() functions. + (++) To wake up from the Standby mode with an RTC Tamper or time stamp event, it + is necessary to: + (+++) Enable the RTC Tamper or time stamp Interrupt using the RTC_ITConfig() + function. + (+++) Configure the RTC to detect the tamper or time stamp event using the + RTC_TimeStampConfig(), RTC_TamperTriggerConfig() and RTC_TamperCmd() + functions. + + (+) Comparator auto-wakeup (AWU) from the Stop mode + (++) To wake up from the Stop mode with a comparator wakeup event, it is necessary to: + (+++) Configure the correspondent comparator EXTI Line to be sensitive to + the selected edges (falling, rising or falling and rising) + (Interrupt or Event modes) using the EXTI_Init() function. + (+++) Configure the comparator to generate the event. + +@endverbatim + * @{ + */ + +/** + * @brief Enters Sleep mode. + * @note In Sleep mode, all I/O pins keep the same state as in Run mode. + * @param PWR_SLEEPEntry: specifies if SLEEP mode in entered with WFI or WFE instruction. + * This parameter can be one of the following values: + * @arg PWR_SLEEPEntry_WFI: enter SLEEP mode with WFI instruction + * @arg PWR_SLEEPEntry_WFE: enter SLEEP mode with WFE instruction + * @retval None + */ +void PWR_EnterSleepMode(uint8_t PWR_SLEEPEntry) +{ + /* Check the parameters */ + assert_param(IS_PWR_SLEEP_ENTRY(PWR_SLEEPEntry)); + + /* Clear SLEEPDEEP bit of Cortex System Control Register */ + SCB->SCR &= (uint32_t)~((uint32_t)SCB_SCR_SLEEPDEEP_Msk); + + /* Select SLEEP mode entry -------------------------------------------------*/ + if(PWR_SLEEPEntry == PWR_SLEEPEntry_WFI) + { + /* Request Wait For Interrupt */ + __WFI(); + } + else + { + /* Request Wait For Event */ + __SEV(); + __WFE(); + __WFE(); + } +} + +/** + * @brief Enters STOP mode. + * @note In Stop mode, all I/O pins keep the same state as in Run mode. + * @note When exiting Stop mode by issuing an interrupt or a wakeup event, + * the HSI RC oscillator is selected as system clock. + * @note When the voltage regulator operates in low power mode, an additional + * startup delay is incurred when waking up from Stop mode. + * By keeping the internal regulator ON during Stop mode, the consumption + * is higher although the startup time is reduced. + * @param PWR_Regulator: specifies the regulator state in STOP mode. + * This parameter can be one of the following values: + * @arg PWR_Regulator_ON: STOP mode with regulator ON + * @arg PWR_Regulator_LowPower: STOP mode with regulator in low power mode + * @param PWR_STOPEntry: specifies if STOP mode in entered with WFI or WFE instruction. + * This parameter can be one of the following values: + * @arg PWR_STOPEntry_WFI: enter STOP mode with WFI instruction + * @arg PWR_STOPEntry_WFE: enter STOP mode with WFE instruction + * @retval None + */ +void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_PWR_REGULATOR(PWR_Regulator)); + assert_param(IS_PWR_STOP_ENTRY(PWR_STOPEntry)); + + /* Select the regulator state in STOP mode ---------------------------------*/ + tmpreg = PWR->CR; + /* Clear PDDS and LPDSR bits */ + tmpreg &= CR_DS_MASK; + + /* Set LPDSR bit according to PWR_Regulator value */ + tmpreg |= PWR_Regulator; + + /* Store the new value */ + PWR->CR = tmpreg; + + /* Set SLEEPDEEP bit of Cortex System Control Register */ + SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk; + + /* Select STOP mode entry --------------------------------------------------*/ + if(PWR_STOPEntry == PWR_STOPEntry_WFI) + { + /* Request Wait For Interrupt */ + __WFI(); + } + else + { + /* Request Wait For Event */ + __WFE(); + } + /* Reset SLEEPDEEP bit of Cortex System Control Register */ + SCB->SCR &= (uint32_t)~((uint32_t)SCB_SCR_SLEEPDEEP_Msk); +} + +/** + * @brief Enters STANDBY mode. + * @note In Standby mode, all I/O pins are high impedance except for: + * @note Reset pad (still available) + * @note RTC_AF1 pin (PC13) if configured for Wakeup pin 2 (WKUP2), tamper, + * time-stamp, RTC Alarm out, or RTC clock calibration out. + * @note WKUP pin 1 (PA0) and WKUP pin 3 (PE6), if enabled. + * @note The Wakeup flag (WUF) need to be cleared at application level before to call this function. + * @param None + * @retval None + */ +void PWR_EnterSTANDBYMode(void) +{ + /* Select STANDBY mode */ + PWR->CR |= PWR_CR_PDDS; + + /* Set SLEEPDEEP bit of Cortex System Control Register */ + SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk; + +/* This option is used to ensure that store operations are completed */ +#if defined ( __CC_ARM ) + __force_stores(); +#endif + /* Request Wait For Interrupt */ + __WFI(); +} + +/** + * @} + */ + +/** @defgroup PWR_Group5 Flags management functions + * @brief Flags management functions + * +@verbatim + =============================================================================== + ##### Flags management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Checks whether the specified PWR flag is set or not. + * @param PWR_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg PWR_FLAG_WU: Wake Up flag. This flag indicates that a wakeup event + * was received from the WKUP pin or from the RTC alarm (Alarm A or Alarm B), + * RTC Tamper event, RTC TimeStamp event or RTC Wakeup. + * @arg PWR_FLAG_SB: StandBy flag. This flag indicates that the system was + * resumed from StandBy mode. + * @arg PWR_FLAG_PVDO: PVD Output. This flag is valid only if PVD is enabled + * by the PWR_PVDCmd() function. + * @arg PWR_FLAG_VREFINTRDY: Internal Voltage Reference Ready flag. This + * flag indicates the state of the internal voltage reference, VREFINT. + * @retval The new state of PWR_FLAG (SET or RESET). + */ +FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_PWR_GET_FLAG(PWR_FLAG)); + + if ((PWR->CSR & PWR_FLAG) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + /* Return the flag status */ + return bitstatus; +} + +/** + * @brief Clears the PWR's pending flags. + * @param PWR_FLAG: specifies the flag to clear. + * This parameter can be one of the following values: + * @arg PWR_FLAG_WU: Wake Up flag + * @arg PWR_FLAG_SB: StandBy flag + * @retval None + */ +void PWR_ClearFlag(uint32_t PWR_FLAG) +{ + /* Check the parameters */ + assert_param(IS_PWR_CLEAR_FLAG(PWR_FLAG)); + + PWR->CR |= PWR_FLAG << 2; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c new file mode 100755 index 0000000000..f8568fa301 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c @@ -0,0 +1,2017 @@ +/** + ****************************************************************************** + * @file stm32f30x_rcc.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Reset and clock control (RCC) peripheral: + * + Internal/external clocks, PLL, CSS and MCO configuration + * + System, AHB and APB busses clocks configuration + * + Peripheral clocks configuration + * + Interrupts and flags management + * + @verbatim + + =============================================================================== + ##### RCC specific features ##### + =============================================================================== + [..] After reset the device is running from HSI (8 MHz) with Flash 0 WS, + all peripherals are off except internal SRAM, Flash and SWD. + (+) There is no prescaler on High speed (AHB) and Low speed (APB) busses; + all peripherals mapped on these busses are running at HSI speed. + (+) The clock for all peripherals is switched off, except the SRAM and FLASH. + (+) All GPIOs are in input floating state, except the SWD pins which + are assigned to be used for debug purpose. + [..] Once the device starts from reset, the user application has to: + (+) Configure the clock source to be used to drive the System clock + (if the application needs higher frequency/performance). + (+) Configure the System clock frequency and Flash settings. + (+) Configure the AHB and APB busses prescalers. + (+) Enable the clock for the peripheral(s) to be used. + (+) Configure the clock source(s) for peripherals which clocks are not + derived from the System clock (ADC, TIM, I2C, USART, RTC and IWDG). + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup RCC + * @brief RCC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* ------------ RCC registers bit address in the alias region ----------- */ +#define RCC_OFFSET (RCC_BASE - PERIPH_BASE) + +/* --- CR Register ---*/ + +/* Alias word address of HSION bit */ +#define CR_OFFSET (RCC_OFFSET + 0x00) +#define HSION_BitNumber 0x00 +#define CR_HSION_BB (PERIPH_BB_BASE + (CR_OFFSET * 32) + (HSION_BitNumber * 4)) + +/* Alias word address of PLLON bit */ +#define PLLON_BitNumber 0x18 +#define CR_PLLON_BB (PERIPH_BB_BASE + (CR_OFFSET * 32) + (PLLON_BitNumber * 4)) + +/* Alias word address of CSSON bit */ +#define CSSON_BitNumber 0x13 +#define CR_CSSON_BB (PERIPH_BB_BASE + (CR_OFFSET * 32) + (CSSON_BitNumber * 4)) + +/* --- CFGR Register ---*/ +/* Alias word address of USBPRE bit */ +#define CFGR_OFFSET (RCC_OFFSET + 0x04) +#define USBPRE_BitNumber 0x16 +#define CFGR_USBPRE_BB (PERIPH_BB_BASE + (CFGR_OFFSET * 32) + (USBPRE_BitNumber * 4)) +/* Alias word address of I2SSRC bit */ +#define I2SSRC_BitNumber 0x17 +#define CFGR_I2SSRC_BB (PERIPH_BB_BASE + (CFGR_OFFSET * 32) + (I2SSRC_BitNumber * 4)) + +/* --- BDCR Register ---*/ + +/* Alias word address of RTCEN bit */ +#define BDCR_OFFSET (RCC_OFFSET + 0x20) +#define RTCEN_BitNumber 0x0F +#define BDCR_RTCEN_BB (PERIPH_BB_BASE + (BDCR_OFFSET * 32) + (RTCEN_BitNumber * 4)) + +/* Alias word address of BDRST bit */ +#define BDRST_BitNumber 0x10 +#define BDCR_BDRST_BB (PERIPH_BB_BASE + (BDCR_OFFSET * 32) + (BDRST_BitNumber * 4)) + +/* --- CSR Register ---*/ + +/* Alias word address of LSION bit */ +#define CSR_OFFSET (RCC_OFFSET + 0x24) +#define LSION_BitNumber 0x00 +#define CSR_LSION_BB (PERIPH_BB_BASE + (CSR_OFFSET * 32) + (LSION_BitNumber * 4)) + +/* ---------------------- RCC registers bit mask ------------------------ */ +/* RCC Flag Mask */ +#define FLAG_MASK ((uint8_t)0x1F) + +/* CFGR register byte 3 (Bits[31:23]) base address */ +#define CFGR_BYTE3_ADDRESS ((uint32_t)0x40021007) + +/* CIR register byte 2 (Bits[15:8]) base address */ +#define CIR_BYTE2_ADDRESS ((uint32_t)0x40021009) + +/* CIR register byte 3 (Bits[23:16]) base address */ +#define CIR_BYTE3_ADDRESS ((uint32_t)0x4002100A) + +/* CR register byte 2 (Bits[23:16]) base address */ +#define CR_BYTE2_ADDRESS ((uint32_t)0x40021002) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +static __I uint8_t APBAHBPrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9}; +static __I uint16_t ADCPrescTable[16] = {1, 2, 4, 6, 8, 10, 12, 16, 32, 64, 128, 256, 0, 0, 0, 0 }; + +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup RCC_Private_Functions + * @{ + */ + +/** @defgroup RCC_Group1 Internal and external clocks, PLL, CSS and MCO configuration functions + * @brief Internal and external clocks, PLL, CSS and MCO configuration functions + * +@verbatim + =============================================================================== + ##### Internal-external clocks, PLL, CSS and MCO configuration functions ##### + =============================================================================== + [..] This section provides functions allowing to configure the internal/external + clocks, PLL, CSS and MCO. + (#) HSI (high-speed internal), 8 MHz factory-trimmed RC used directly + or through the PLL as System clock source. + The HSI clock can be used also to clock the USART and I2C peripherals. + (#) LSI (low-speed internal), 40 KHz low consumption RC used as IWDG and/or RTC + clock source. + (#) HSE (high-speed external), 4 to 32 MHz crystal oscillator used directly or + through the PLL as System clock source. Can be used also as RTC clock source. + (#) LSE (low-speed external), 32 KHz oscillator used as RTC clock source. + LSE can be used also to clock the USART peripherals. + (#) PLL (clocked by HSI or HSE), for System clock. + (#) CSS (Clock security system), once enabled and if a HSE clock failure occurs + (HSE used directly or through PLL as System clock source), the System clock + is automatically switched to HSI and an interrupt is generated if enabled. + The interrupt is linked to the Cortex-M4 NMI (Non-Maskable Interrupt) + exception vector. + (#) MCO (microcontroller clock output), used to output SYSCLK, HSI, HSE, LSI, LSE, + PLL clock on PA8 pin. + +@endverbatim + * @{ + */ + +/** + * @brief Resets the RCC clock configuration to the default reset state. + * @note The default reset state of the clock configuration is given below: + * - HSI ON and used as system clock source + * - HSE, PLL and PLLI2S OFF + * - AHB, APB1 and APB2 prescaler set to 1. + * - CSS and MCO OFF + * - All interrupts disabled + * @note However, This function doesn't modify the configuration of the + * - Peripheral clocks + * - LSI, LSE and RTC clocks + * @param None + * @retval None + */ +void RCC_DeInit(void) +{ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset SW[1:0], HPRE[3:0], PPRE[2:0] and MCOSEL[2:0] bits */ + RCC->CFGR &= (uint32_t)0xF8FFC000; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] and ADCPRE[13:4] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFC000; + + /* Reset USARTSW[1:0], I2CSW and TIMSW bits */ + RCC->CFGR3 &= (uint32_t)0xF00ECCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; +} + +/** + * @brief Configures the External High Speed oscillator (HSE). + * @note After enabling the HSE (RCC_HSE_ON or RCC_HSE_Bypass), the application + * software should wait on HSERDY flag to be set indicating that HSE clock + * is stable and can be used to clock the PLL and/or system clock. + * @note HSE state can not be changed if it is used directly or through the + * PLL as system clock. In this case, you have to select another source + * of the system clock then change the HSE state (ex. disable it). + * @note The HSE is stopped by hardware when entering STOP and STANDBY modes. + * @note This function resets the CSSON bit, so if the Clock security system(CSS) + * was previously enabled you have to enable it again after calling this + * function. + * @param RCC_HSE: specifies the new state of the HSE. + * This parameter can be one of the following values: + * @arg RCC_HSE_OFF: turn OFF the HSE oscillator, HSERDY flag goes low after + * 6 HSE oscillator clock cycles. + * @arg RCC_HSE_ON: turn ON the HSE oscillator + * @arg RCC_HSE_Bypass: HSE oscillator bypassed with external clock + * @retval None + */ +void RCC_HSEConfig(uint8_t RCC_HSE) +{ + /* Check the parameters */ + assert_param(IS_RCC_HSE(RCC_HSE)); + + /* Reset HSEON and HSEBYP bits before configuring the HSE ------------------*/ + *(__IO uint8_t *) CR_BYTE2_ADDRESS = RCC_HSE_OFF; + + /* Set the new HSE configuration -------------------------------------------*/ + *(__IO uint8_t *) CR_BYTE2_ADDRESS = RCC_HSE; + +} + +/** + * @brief Waits for HSE start-up. + * @note This function waits on HSERDY flag to be set and return SUCCESS if + * this flag is set, otherwise returns ERROR if the timeout is reached + * and this flag is not set. The timeout value is defined by the constant + * HSE_STARTUP_TIMEOUT in stm32f30x.h file. You can tailor it depending + * on the HSE crystal used in your application. + * @param None + * @retval An ErrorStatus enumeration value: + * - SUCCESS: HSE oscillator is stable and ready to use + * - ERROR: HSE oscillator not yet ready + */ +ErrorStatus RCC_WaitForHSEStartUp(void) +{ + __IO uint32_t StartUpCounter = 0; + ErrorStatus status = ERROR; + FlagStatus HSEStatus = RESET; + + /* Wait till HSE is ready and if timeout is reached exit */ + do + { + HSEStatus = RCC_GetFlagStatus(RCC_FLAG_HSERDY); + StartUpCounter++; + } while((StartUpCounter != HSE_STARTUP_TIMEOUT) && (HSEStatus == RESET)); + + if (RCC_GetFlagStatus(RCC_FLAG_HSERDY) != RESET) + { + status = SUCCESS; + } + else + { + status = ERROR; + } + return (status); +} + +/** + * @brief Adjusts the Internal High Speed oscillator (HSI) calibration value. + * @note The calibration is used to compensate for the variations in voltage + * and temperature that influence the frequency of the internal HSI RC. + * Refer to the Application Note AN3300 for more details on how to + * calibrate the HSI. + * @param HSICalibrationValue: specifies the HSI calibration trimming value. + * This parameter must be a number between 0 and 0x1F. + * @retval None + */ +void RCC_AdjustHSICalibrationValue(uint8_t HSICalibrationValue) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_HSI_CALIBRATION_VALUE(HSICalibrationValue)); + + tmpreg = RCC->CR; + + /* Clear HSITRIM[4:0] bits */ + tmpreg &= ~RCC_CR_HSITRIM; + + /* Set the HSITRIM[4:0] bits according to HSICalibrationValue value */ + tmpreg |= (uint32_t)HSICalibrationValue << 3; + + /* Store the new value */ + RCC->CR = tmpreg; +} + +/** + * @brief Enables or disables the Internal High Speed oscillator (HSI). + * @note After enabling the HSI, the application software should wait on + * HSIRDY flag to be set indicating that HSI clock is stable and can + * be used to clock the PLL and/or system clock. + * @note HSI can not be stopped if it is used directly or through the PLL + * as system clock. In this case, you have to select another source + * of the system clock then stop the HSI. + * @note The HSI is stopped by hardware when entering STOP and STANDBY modes. + * @note When the HSI is stopped, HSIRDY flag goes low after 6 HSI oscillator + * clock cycles. + * @param NewState: new state of the HSI. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_HSICmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + *(__IO uint32_t *) CR_HSION_BB = (uint32_t)NewState; +} + +/** + * @brief Configures the External Low Speed oscillator (LSE). + * @note As the LSE is in the Backup domain and write access is denied to this + * domain after reset, you have to enable write access using + * PWR_BackupAccessCmd(ENABLE) function before to configure the LSE + * (to be done once after reset). + * @note Care must be taken when using this function to configure LSE mode + * as it clears systematically the LSEON bit before any new configuration. + * @note After enabling the LSE (RCC_LSE_ON or RCC_LSE_Bypass), the application + * software should wait on LSERDY flag to be set indicating that LSE clock + * is stable and can be used to clock the RTC. + * @param RCC_LSE: specifies the new state of the LSE. + * This parameter can be one of the following values: + * @arg RCC_LSE_OFF: turn OFF the LSE oscillator, LSERDY flag goes low after + * 6 LSE oscillator clock cycles. + * @arg RCC_LSE_ON: turn ON the LSE oscillator + * @arg RCC_LSE_Bypass: LSE oscillator bypassed with external clock + * @retval None + */ +void RCC_LSEConfig(uint32_t RCC_LSE) +{ + /* Check the parameters */ + assert_param(IS_RCC_LSE(RCC_LSE)); + + /* Reset LSEON and LSEBYP bits before configuring the LSE ------------------*/ + /* Reset LSEON bit */ + RCC->BDCR &= ~(RCC_BDCR_LSEON); + + /* Reset LSEBYP bit */ + RCC->BDCR &= ~(RCC_BDCR_LSEBYP); + + /* Configure LSE */ + RCC->BDCR |= RCC_LSE; +} + +/** + * @brief Configures the External Low Speed oscillator (LSE) drive capability. + * @param RCC_LSEDrive: specifies the new state of the LSE drive capability. + * This parameter can be one of the following values: + * @arg RCC_LSEDrive_Low: LSE oscillator low drive capability. + * @arg RCC_LSEDrive_MediumLow: LSE oscillator medium low drive capability. + * @arg RCC_LSEDrive_MediumHigh: LSE oscillator medium high drive capability. + * @arg RCC_LSEDrive_High: LSE oscillator high drive capability. + * @retval None + */ +void RCC_LSEDriveConfig(uint32_t RCC_LSEDrive) +{ + /* Check the parameters */ + assert_param(IS_RCC_LSE_DRIVE(RCC_LSEDrive)); + + /* Clear LSEDRV[1:0] bits */ + RCC->BDCR &= ~(RCC_BDCR_LSEDRV); + + /* Set the LSE Drive */ + RCC->BDCR |= RCC_LSEDrive; +} + +/** + * @brief Enables or disables the Internal Low Speed oscillator (LSI). + * @note After enabling the LSI, the application software should wait on + * LSIRDY flag to be set indicating that LSI clock is stable and can + * be used to clock the IWDG and/or the RTC. + * @note LSI can not be disabled if the IWDG is running. + * @note When the LSI is stopped, LSIRDY flag goes low after 6 LSI oscillator + * clock cycles. + * @param NewState: new state of the LSI. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_LSICmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + *(__IO uint32_t *) CSR_LSION_BB = (uint32_t)NewState; +} + +/** + * @brief Configures the PLL clock source and multiplication factor. + * @note This function must be used only when the PLL is disabled. + * @note The minimum input clock frequency for PLL is 2 MHz (when using HSE as + * PLL source). + * @param RCC_PLLSource: specifies the PLL entry clock source. + * This parameter can be one of the following values: + * @arg RCC_PLLSource_HSI: HSI oscillator clockselected as PLL clock entry + * @arg RCC_PLLSource_HSI_Div2: HSI oscillator clock divided by 2 selected as + * PLL clock entry + * @arg RCC_PLLSource_PREDIV1: PREDIV1 clock selected as PLL clock source + * @param RCC_PLLMul: specifies the PLL multiplication factor, which drive the PLLVCO clock + * This parameter can be RCC_PLLMul_x where x:[2,16] + * + * @retval None + */ +void RCC_PLLConfig(uint32_t RCC_PLLSource, uint32_t RCC_PLLMul) +{ + /* Check the parameters */ + assert_param(IS_RCC_PLL_SOURCE(RCC_PLLSource)); + assert_param(IS_RCC_PLL_MUL(RCC_PLLMul)); + + /* Clear PLL Source [16] and Multiplier [21:18] bits */ + RCC->CFGR &= ~(RCC_CFGR_PLLMULL | RCC_CFGR_PLLSRC); + + /* Set the PLL Source and Multiplier */ + RCC->CFGR |= (uint32_t)(RCC_PLLSource | RCC_PLLMul); +} + +/** + * @brief Enables or disables the PLL. + * @note After enabling the PLL, the application software should wait on + * PLLRDY flag to be set indicating that PLL clock is stable and can + * be used as system clock source. + * @note The PLL can not be disabled if it is used as system clock source + * @note The PLL is disabled by hardware when entering STOP and STANDBY modes. + * @param NewState: new state of the PLL. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_PLLCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + *(__IO uint32_t *) CR_PLLON_BB = (uint32_t)NewState; +} + +/** + * @brief Configures the PREDIV1 division factor. + * @note This function must be used only when the PLL is disabled. + * @param RCC_PREDIV1_Div: specifies the PREDIV1 clock division factor. + * This parameter can be RCC_PREDIV1_Divx where x:[1,16] + * @retval None + */ +void RCC_PREDIV1Config(uint32_t RCC_PREDIV1_Div) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_PREDIV1(RCC_PREDIV1_Div)); + + tmpreg = RCC->CFGR2; + /* Clear PREDIV1[3:0] bits */ + tmpreg &= ~(RCC_CFGR2_PREDIV1); + + /* Set the PREDIV1 division factor */ + tmpreg |= RCC_PREDIV1_Div; + + /* Store the new value */ + RCC->CFGR2 = tmpreg; +} + +/** + * @brief Enables or disables the Clock Security System. + * @note If a failure is detected on the HSE oscillator clock, this oscillator + * is automatically disabled and an interrupt is generated to inform the + * software about the failure (Clock Security System Interrupt, CSSI), + * allowing the MCU to perform rescue operations. The CSSI is linked to + * the Cortex-M4 NMI (Non-Maskable Interrupt) exception vector. + * @param NewState: new state of the Clock Security System. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_ClockSecuritySystemCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + *(__IO uint32_t *) CR_CSSON_BB = (uint32_t)NewState; +} + +#ifdef STM32F303xC +/** + * @brief Selects the clock source to output on MCO pin (PA8). + * @note PA8 should be configured in alternate function mode. + * @param RCC_MCOSource: specifies the clock source to output. + * This parameter can be one of the following values: + * @arg RCC_MCOSource_NoClock: No clock selected. + * @arg RCC_MCOSource_LSI: LSI oscillator clock selected. + * @arg RCC_MCOSource_LSE: LSE oscillator clock selected. + * @arg RCC_MCOSource_SYSCLK: System clock selected. + * @arg RCC_MCOSource_HSI: HSI oscillator clock selected. + * @arg RCC_MCOSource_HSE: HSE oscillator clock selected. + * @arg RCC_MCOSource_PLLCLK_Div2: PLL clock divided by 2 selected. + * @retval None + */ +void RCC_MCOConfig(uint8_t RCC_MCOSource) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_MCO_SOURCE(RCC_MCOSource)); + + /* Get CFGR value */ + tmpreg = RCC->CFGR; + /* Clear MCO[3:0] bits */ + tmpreg &= ~(RCC_CFGR_MCO | RCC_CFGR_PLLNODIV); + /* Set the RCC_MCOSource */ + tmpreg |= RCC_MCOSource<<24; + /* Store the new value */ + RCC->CFGR = tmpreg; +} +#else + +/** + * @brief Selects the clock source to output on MCO pin (PA8) and the corresponding + * prescsaler. + * @note PA8 should be configured in alternate function mode. + * @param RCC_MCOSource: specifies the clock source to output. + * This parameter can be one of the following values: + * @arg RCC_MCOSource_NoClock: No clock selected. + * @arg RCC_MCOSource_LSI: LSI oscillator clock selected. + * @arg RCC_MCOSource_LSE: LSE oscillator clock selected. + * @arg RCC_MCOSource_SYSCLK: System clock selected. + * @arg RCC_MCOSource_HSI: HSI oscillator clock selected. + * @arg RCC_MCOSource_HSE: HSE oscillator clock selected. + * @arg RCC_MCOSource_PLLCLK_Div2: PLL clock divided by 2 selected. + * @arg RCC_MCOSource_PLLCLK: PLL clock selected. + * @param RCC_MCOPrescaler: specifies the prescaler on MCO pin. + * This parameter can be one of the following values: + * @arg RCC_MCOPrescaler_1: MCO clock is divided by 1. + * @arg RCC_MCOPrescaler_2: MCO clock is divided by 2. + * @arg RCC_MCOPrescaler_4: MCO clock is divided by 4. + * @arg RCC_MCOPrescaler_8: MCO clock is divided by 8. + * @arg RCC_MCOPrescaler_16: MCO clock is divided by 16. + * @arg RCC_MCOPrescaler_32: MCO clock is divided by 32. + * @arg RCC_MCOPrescaler_64: MCO clock is divided by 64. + * @arg RCC_MCOPrescaler_128: MCO clock is divided by 128. + * @retval None + */ +void RCC_MCOConfig(uint8_t RCC_MCOSource, uint32_t RCC_MCOPrescaler) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_MCO_SOURCE(RCC_MCOSource)); + assert_param(IS_RCC_MCO_PRESCALER(RCC_MCOPrescaler)); + + /* Get CFGR value */ + tmpreg = RCC->CFGR; + /* Clear MCOPRE[2:0] bits */ + tmpreg &= ~(RCC_CFGR_MCO_PRE | RCC_CFGR_MCO | RCC_CFGR_PLLNODIV); + /* Set the RCC_MCOSource and RCC_MCOPrescaler */ + tmpreg |= (RCC_MCOPrescaler | RCC_MCOSource<<24); + /* Store the new value */ + RCC->CFGR = tmpreg; +} +#endif /* STM32F303xC */ + +/** + * @} + */ + +/** @defgroup RCC_Group2 System AHB, APB1 and APB2 busses clocks configuration functions + * @brief System, AHB and APB busses clocks configuration functions + * +@verbatim + =============================================================================== + ##### System, AHB, APB1 and APB2 busses clocks configuration functions ##### + =============================================================================== + [..] This section provide functions allowing to configure the System, AHB, APB1 and + APB2 busses clocks. + (#) Several clock sources can be used to drive the System clock (SYSCLK): HSI, + HSE and PLL. + The AHB clock (HCLK) is derived from System clock through configurable prescaler + and used to clock the CPU, memory and peripherals mapped on AHB bus (DMA and GPIO). + APB1 (PCLK1) and APB2 (PCLK2) clocks are derived from AHB clock through + configurable prescalers and used to clock the peripherals mapped on these busses. + You can use "RCC_GetClocksFreq()" function to retrieve the frequencies of these clocks. + + (#) The maximum frequency of the SYSCLK, HCLK, PCLK1 and PCLK2 is 72 MHz. + Depending on the maximum frequency, the FLASH wait states (WS) should be + adapted accordingly: + +---------------------------------+ + | Wait states | HCLK clock | + | (Latency) | frequency (MHz) | + |-------------- |-----------------| + |0WS(1CPU cycle)| 0 < HCLK <= 24 | + |---------------|-----------------| + |1WS(2CPU cycle)|24 < HCLK <=48 | + |---------------|-----------------| + |2WS(3CPU cycle)|48 < HCLK <= 72 | + +---------------------------------+ + + (#) After reset, the System clock source is the HSI (8 MHz) with 0 WS and + prefetch is disabled. + [..] + (@) All the peripheral clocks are derived from the System clock (SYSCLK) + except: + (+@) The FLASH program/erase clock which is always HSI 8MHz clock. + (+@) The USB 48 MHz clock which is derived from the PLL VCO clock. + (+@) The USART clock which can be derived as well from HSI 8MHz, LSI or LSE. + (+@) The I2C clock which can be derived as well from HSI 8MHz clock. + (+@) The ADC clock which is derived from PLL output. + (+@) The RTC clock which is derived from the LSE, LSI or 1 MHz HSE_RTC + (HSE divided by a programmable prescaler). The System clock (SYSCLK) + frequency must be higher or equal to the RTC clock frequency. + (+@) IWDG clock which is always the LSI clock. + [..] It is recommended to use the following software sequences to tune the number + of wait states needed to access the Flash memory with the CPU frequency (HCLK). + (+) Increasing the CPU frequency + (++) Program the Flash Prefetch buffer, using "FLASH_PrefetchBufferCmd(ENABLE)" + function + (++) Check that Flash Prefetch buffer activation is taken into account by + reading FLASH_ACR using the FLASH_GetPrefetchBufferStatus() function + (++) Program Flash WS to 1 or 2, using "FLASH_SetLatency()" function + (++) Check that the new number of WS is taken into account by reading FLASH_ACR + (++) Modify the CPU clock source, using "RCC_SYSCLKConfig()" function + (++) If needed, modify the CPU clock prescaler by using "RCC_HCLKConfig()" function + (++) Check that the new CPU clock source is taken into account by reading + the clock source status, using "RCC_GetSYSCLKSource()" function + (+) Decreasing the CPU frequency + (++) Modify the CPU clock source, using "RCC_SYSCLKConfig()" function + (++) If needed, modify the CPU clock prescaler by using "RCC_HCLKConfig()" function + (++) Check that the new CPU clock source is taken into account by reading + the clock source status, using "RCC_GetSYSCLKSource()" function + (++) Program the new number of WS, using "FLASH_SetLatency()" function + (++) Check that the new number of WS is taken into account by reading FLASH_ACR + (++) Disable the Flash Prefetch buffer using "FLASH_PrefetchBufferCmd(DISABLE)" + function + (++) Check that Flash Prefetch buffer deactivation is taken into account by reading FLASH_ACR + using the FLASH_GetPrefetchBufferStatus() function. + +@endverbatim + * @{ + */ + +/** + * @brief Configures the system clock (SYSCLK). + * @note The HSI is used (enabled by hardware) as system clock source after + * startup from Reset, wake-up from STOP and STANDBY mode, or in case + * of failure of the HSE used directly or indirectly as system clock + * (if the Clock Security System CSS is enabled). + * @note A switch from one clock source to another occurs only if the target + * clock source is ready (clock stable after startup delay or PLL locked). + * If a clock source which is not yet ready is selected, the switch will + * occur when the clock source will be ready. + * You can use RCC_GetSYSCLKSource() function to know which clock is + * currently used as system clock source. + * @param RCC_SYSCLKSource: specifies the clock source used as system clock source + * This parameter can be one of the following values: + * @arg RCC_SYSCLKSource_HSI: HSI selected as system clock source + * @arg RCC_SYSCLKSource_HSE: HSE selected as system clock source + * @arg RCC_SYSCLKSource_PLLCLK: PLL selected as system clock source + * @retval None + */ +void RCC_SYSCLKConfig(uint32_t RCC_SYSCLKSource) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_SYSCLK_SOURCE(RCC_SYSCLKSource)); + + tmpreg = RCC->CFGR; + + /* Clear SW[1:0] bits */ + tmpreg &= ~RCC_CFGR_SW; + + /* Set SW[1:0] bits according to RCC_SYSCLKSource value */ + tmpreg |= RCC_SYSCLKSource; + + /* Store the new value */ + RCC->CFGR = tmpreg; +} + +/** + * @brief Returns the clock source used as system clock. + * @param None + * @retval The clock source used as system clock. The returned value can be one + * of the following values: + * - 0x00: HSI used as system clock + * - 0x04: HSE used as system clock + * - 0x08: PLL used as system clock + */ +uint8_t RCC_GetSYSCLKSource(void) +{ + return ((uint8_t)(RCC->CFGR & RCC_CFGR_SWS)); +} + +/** + * @brief Configures the AHB clock (HCLK). + * @note Depending on the device voltage range, the software has to set correctly + * these bits to ensure that the system frequency does not exceed the + * maximum allowed frequency (for more details refer to section above + * "CPU, AHB and APB busses clocks configuration functions"). + * @param RCC_SYSCLK: defines the AHB clock divider. This clock is derived from + * the system clock (SYSCLK). + * This parameter can be one of the following values: + * @arg RCC_SYSCLK_Div1: AHB clock = SYSCLK + * @arg RCC_SYSCLK_Div2: AHB clock = SYSCLK/2 + * @arg RCC_SYSCLK_Div4: AHB clock = SYSCLK/4 + * @arg RCC_SYSCLK_Div8: AHB clock = SYSCLK/8 + * @arg RCC_SYSCLK_Div16: AHB clock = SYSCLK/16 + * @arg RCC_SYSCLK_Div64: AHB clock = SYSCLK/64 + * @arg RCC_SYSCLK_Div128: AHB clock = SYSCLK/128 + * @arg RCC_SYSCLK_Div256: AHB clock = SYSCLK/256 + * @arg RCC_SYSCLK_Div512: AHB clock = SYSCLK/512 + * @retval None + */ +void RCC_HCLKConfig(uint32_t RCC_SYSCLK) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_HCLK(RCC_SYSCLK)); + + tmpreg = RCC->CFGR; + + /* Clear HPRE[3:0] bits */ + tmpreg &= ~RCC_CFGR_HPRE; + + /* Set HPRE[3:0] bits according to RCC_SYSCLK value */ + tmpreg |= RCC_SYSCLK; + + /* Store the new value */ + RCC->CFGR = tmpreg; +} + +/** + * @brief Configures the Low Speed APB clock (PCLK1). + * @param RCC_HCLK: defines the APB1 clock divider. This clock is derived from + * the AHB clock (HCLK). + * This parameter can be one of the following values: + * @arg RCC_HCLK_Div1: APB1 clock = HCLK + * @arg RCC_HCLK_Div2: APB1 clock = HCLK/2 + * @arg RCC_HCLK_Div4: APB1 clock = HCLK/4 + * @arg RCC_HCLK_Div8: APB1 clock = HCLK/8 + * @arg RCC_HCLK_Div16: APB1 clock = HCLK/16 + * @retval None + */ +void RCC_PCLK1Config(uint32_t RCC_HCLK) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_PCLK(RCC_HCLK)); + + tmpreg = RCC->CFGR; + /* Clear PPRE1[2:0] bits */ + tmpreg &= ~RCC_CFGR_PPRE1; + + /* Set PPRE1[2:0] bits according to RCC_HCLK value */ + tmpreg |= RCC_HCLK; + + /* Store the new value */ + RCC->CFGR = tmpreg; +} + +/** + * @brief Configures the High Speed APB clock (PCLK2). + * @param RCC_HCLK: defines the APB2 clock divider. This clock is derived from + * the AHB clock (HCLK). + * This parameter can be one of the following values: + * @arg RCC_HCLK_Div1: APB2 clock = HCLK + * @arg RCC_HCLK_Div2: APB2 clock = HCLK/2 + * @arg RCC_HCLK_Div4: APB2 clock = HCLK/4 + * @arg RCC_HCLK_Div8: APB2 clock = HCLK/8 + * @arg RCC_HCLK_Div16: APB2 clock = HCLK/16 + * @retval None + */ +void RCC_PCLK2Config(uint32_t RCC_HCLK) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RCC_PCLK(RCC_HCLK)); + + tmpreg = RCC->CFGR; + /* Clear PPRE2[2:0] bits */ + tmpreg &= ~RCC_CFGR_PPRE2; + /* Set PPRE2[2:0] bits according to RCC_HCLK value */ + tmpreg |= RCC_HCLK << 3; + /* Store the new value */ + RCC->CFGR = tmpreg; +} + +/** + * @brief Returns the frequencies of the System, AHB, APB2 and APB1 busses clocks. + * + * @note This function returns the frequencies of : + * System, AHB, APB2 and APB1 busses clocks, ADC1/2/3/4 clocks, + * USART1/2/3/4/5 clocks, I2C1/2 clocks and TIM1/8 Clocks. + * + * @note The frequency returned by this function is not the real frequency + * in the chip. It is calculated based on the predefined constant and + * the source selected by RCC_SYSCLKConfig(). + * + * @note If SYSCLK source is HSI, function returns constant HSI_VALUE(*) + * + * @note If SYSCLK source is HSE, function returns constant HSE_VALUE(**) + * + * @note If SYSCLK source is PLL, function returns constant HSE_VALUE(**) + * or HSI_VALUE(*) multiplied by the PLL factors. + * + * @note (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature, refer to RCC_AdjustHSICalibrationValue(). + * + * @note (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * return wrong result. + * + * @note The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param RCC_Clocks: pointer to a RCC_ClocksTypeDef structure which will hold + * the clocks frequencies. + * + * @note This function can be used by the user application to compute the + * baudrate for the communication peripherals or configure other parameters. + * @note Each time SYSCLK, HCLK, PCLK1 and/or PCLK2 clock changes, this function + * must be called to update the structure's field. Otherwise, any + * configuration based on this function will be incorrect. + * + * @retval None + */ +void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0, presc = 0, pllclk = 0; + uint32_t apb2presc = 0, ahbpresc = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + RCC_Clocks->SYSCLK_Frequency = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + RCC_Clocks->SYSCLK_Frequency = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + pllclk = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + pllclk = (HSE_VALUE / prediv1factor) * pllmull; + } + RCC_Clocks->SYSCLK_Frequency = pllclk; + break; + default: /* HSI used as system clock */ + RCC_Clocks->SYSCLK_Frequency = HSI_VALUE; + break; + } + /* Compute HCLK, PCLK clocks frequencies -----------------------------------*/ + /* Get HCLK prescaler */ + tmp = RCC->CFGR & RCC_CFGR_HPRE; + tmp = tmp >> 4; + ahbpresc = APBAHBPrescTable[tmp]; + /* HCLK clock frequency */ + RCC_Clocks->HCLK_Frequency = RCC_Clocks->SYSCLK_Frequency >> ahbpresc; + + /* Get PCLK1 prescaler */ + tmp = RCC->CFGR & RCC_CFGR_PPRE1; + tmp = tmp >> 8; + presc = APBAHBPrescTable[tmp]; + /* PCLK1 clock frequency */ + RCC_Clocks->PCLK1_Frequency = RCC_Clocks->HCLK_Frequency >> presc; + + /* Get PCLK2 prescaler */ + tmp = RCC->CFGR & RCC_CFGR_PPRE2; + tmp = tmp >> 11; + apb2presc = APBAHBPrescTable[tmp]; + + /* PCLK2 clock frequency */ + RCC_Clocks->PCLK2_Frequency = RCC_Clocks->HCLK_Frequency >> apb2presc; + + /* Get ADC12CLK prescaler */ + tmp = RCC->CFGR2 & RCC_CFGR2_ADCPRE12; + tmp = tmp >> 4; + presc = ADCPrescTable[tmp & 0x0F]; + if (((tmp & 0x10) != 0) && (presc != 0)) + { + /* ADC12CLK clock frequency is derived from PLL clock */ + RCC_Clocks->ADC12CLK_Frequency = pllclk / presc; + } + else + { + /* ADC12CLK clock frequency is AHB clock */ + RCC_Clocks->ADC12CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + + /* Get ADC34CLK prescaler */ + tmp = RCC->CFGR2 & RCC_CFGR2_ADCPRE34; + tmp = tmp >> 9; + presc = ADCPrescTable[tmp & 0x0F]; + if (((tmp & 0x10) != 0) && (presc != 0)) + { + /* ADC34CLK clock frequency is derived from PLL clock */ + RCC_Clocks->ADC34CLK_Frequency = pllclk / presc; + } + else + { + /* ADC34CLK clock frequency is AHB clock */ + RCC_Clocks->ADC34CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + + /* I2C1CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_I2C1SW) != RCC_CFGR3_I2C1SW) + { + /* I2C1 Clock is HSI Osc. */ + RCC_Clocks->I2C1CLK_Frequency = HSI_VALUE; + } + else + { + /* I2C1 Clock is System Clock */ + RCC_Clocks->I2C1CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + + /* I2C2CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_I2C2SW) != RCC_CFGR3_I2C2SW) + { + /* I2C2 Clock is HSI Osc. */ + RCC_Clocks->I2C2CLK_Frequency = HSI_VALUE; + } + else + { + /* I2C2 Clock is System Clock */ + RCC_Clocks->I2C2CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + + /* I2C3CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_I2C3SW) != RCC_CFGR3_I2C3SW) + { + /* I2C3 Clock is HSI Osc. */ + RCC_Clocks->I2C3CLK_Frequency = HSI_VALUE; + } + else + { + /* I2C3 Clock is System Clock */ + RCC_Clocks->I2C3CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + + /* TIM1CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM1SW) == RCC_CFGR3_TIM1SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* TIM1 Clock is 2 * pllclk */ + RCC_Clocks->TIM1CLK_Frequency = pllclk * 2; + } + else + { + /* TIM1 Clock is APB2 clock. */ + RCC_Clocks->TIM1CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + +#ifdef STM32F303xE + uint32_t apb1presc = 0; + apb1presc = APBAHBPrescTable[tmp]; + /* TIM2CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM2SW) == RCC_CFGR3_TIM2SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb1presc == ahbpresc)) + { + /* TIM2 Clock is pllclk */ + RCC_Clocks->TIM2CLK_Frequency = pllclk * 2 ; + } + else + { + /* TIM2 Clock is APB2 clock. */ + RCC_Clocks->TIM2CLK_Frequency = RCC_Clocks->PCLK1_Frequency; + } + + /* TIM3CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM3SW) == RCC_CFGR3_TIM3SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb1presc == ahbpresc)) + { + /* TIM3 Clock is pllclk */ + RCC_Clocks->TIM3CLK_Frequency = pllclk * 2; + } + else + { + /* TIM3 Clock is APB2 clock. */ + RCC_Clocks->TIM3CLK_Frequency = RCC_Clocks->PCLK1_Frequency; + } +#endif /* STM32F303xE */ + + /* TIM1CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_HRTIM1SW) == RCC_CFGR3_HRTIM1SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* HRTIM1 Clock is 2 * pllclk */ + RCC_Clocks->HRTIM1CLK_Frequency = pllclk * 2; + } + else + { + /* HRTIM1 Clock is APB2 clock. */ + RCC_Clocks->HRTIM1CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + + /* TIM8CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM8SW) == RCC_CFGR3_TIM8SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* TIM8 Clock is 2 * pllclk */ + RCC_Clocks->TIM8CLK_Frequency = pllclk * 2; + } + else + { + /* TIM8 Clock is APB2 clock. */ + RCC_Clocks->TIM8CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + + /* TIM15CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM15SW) == RCC_CFGR3_TIM15SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* TIM15 Clock is 2 * pllclk */ + RCC_Clocks->TIM15CLK_Frequency = pllclk * 2; + } + else + { + /* TIM15 Clock is APB2 clock. */ + RCC_Clocks->TIM15CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + + /* TIM16CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM16SW) == RCC_CFGR3_TIM16SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* TIM16 Clock is 2 * pllclk */ + RCC_Clocks->TIM16CLK_Frequency = pllclk * 2; + } + else + { + /* TIM16 Clock is APB2 clock. */ + RCC_Clocks->TIM16CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + + /* TIM17CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM17SW) == RCC_CFGR3_TIM17SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* TIM17 Clock is 2 * pllclk */ + RCC_Clocks->TIM17CLK_Frequency = pllclk * 2; + } + else + { + /* TIM17 Clock is APB2 clock. */ + RCC_Clocks->TIM16CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + + /* TIM20CLK clock frequency */ + if(((RCC->CFGR3 & RCC_CFGR3_TIM20SW) == RCC_CFGR3_TIM20SW)&& (RCC_Clocks->SYSCLK_Frequency == pllclk) \ + && (apb2presc == ahbpresc)) + { + /* TIM20 Clock is 2 * pllclk */ + RCC_Clocks->TIM20CLK_Frequency = pllclk * 2; + } + else + { + /* TIM20 Clock is APB2 clock. */ + RCC_Clocks->TIM20CLK_Frequency = RCC_Clocks->PCLK2_Frequency; + } + + /* USART1CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == 0x0) + { +#if defined(STM32F303x8) || defined(STM32F334x8) || defined(STM32F301x8) || defined(STM32F302x8) + /* USART1 Clock is PCLK1 instead of PCLK2 (limitation described in the + STM32F302/01/34 x4/x6/x8 respective erratasheets) */ + RCC_Clocks->USART1CLK_Frequency = RCC_Clocks->PCLK1_Frequency; +#else + /* USART Clock is PCLK2 */ + RCC_Clocks->USART1CLK_Frequency = RCC_Clocks->PCLK2_Frequency; +#endif + } + else if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == RCC_CFGR3_USART1SW_0) + { + /* USART Clock is System Clock */ + RCC_Clocks->USART1CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == RCC_CFGR3_USART1SW_1) + { + /* USART Clock is LSE Osc. */ + RCC_Clocks->USART1CLK_Frequency = LSE_VALUE; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART1SW) == RCC_CFGR3_USART1SW) + { + /* USART Clock is HSI Osc. */ + RCC_Clocks->USART1CLK_Frequency = HSI_VALUE; + } + + /* USART2CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == 0x0) + { + /* USART Clock is PCLK */ + RCC_Clocks->USART2CLK_Frequency = RCC_Clocks->PCLK1_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == RCC_CFGR3_USART2SW_0) + { + /* USART Clock is System Clock */ + RCC_Clocks->USART2CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == RCC_CFGR3_USART2SW_1) + { + /* USART Clock is LSE Osc. */ + RCC_Clocks->USART2CLK_Frequency = LSE_VALUE; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART2SW) == RCC_CFGR3_USART2SW) + { + /* USART Clock is HSI Osc. */ + RCC_Clocks->USART2CLK_Frequency = HSI_VALUE; + } + + /* USART3CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == 0x0) + { + /* USART Clock is PCLK */ + RCC_Clocks->USART3CLK_Frequency = RCC_Clocks->PCLK1_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == RCC_CFGR3_USART3SW_0) + { + /* USART Clock is System Clock */ + RCC_Clocks->USART3CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == RCC_CFGR3_USART3SW_1) + { + /* USART Clock is LSE Osc. */ + RCC_Clocks->USART3CLK_Frequency = LSE_VALUE; + } + else if((RCC->CFGR3 & RCC_CFGR3_USART3SW) == RCC_CFGR3_USART3SW) + { + /* USART Clock is HSI Osc. */ + RCC_Clocks->USART3CLK_Frequency = HSI_VALUE; + } + + /* UART4CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_UART4SW) == 0x0) + { + /* USART Clock is PCLK */ + RCC_Clocks->UART4CLK_Frequency = RCC_Clocks->PCLK1_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_UART4SW) == RCC_CFGR3_UART4SW_0) + { + /* USART Clock is System Clock */ + RCC_Clocks->UART4CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_UART4SW) == RCC_CFGR3_UART4SW_1) + { + /* USART Clock is LSE Osc. */ + RCC_Clocks->UART4CLK_Frequency = LSE_VALUE; + } + else if((RCC->CFGR3 & RCC_CFGR3_UART4SW) == RCC_CFGR3_UART4SW) + { + /* USART Clock is HSI Osc. */ + RCC_Clocks->UART4CLK_Frequency = HSI_VALUE; + } + + /* UART5CLK clock frequency */ + if((RCC->CFGR3 & RCC_CFGR3_UART5SW) == 0x0) + { + /* USART Clock is PCLK */ + RCC_Clocks->UART5CLK_Frequency = RCC_Clocks->PCLK1_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_UART5SW) == RCC_CFGR3_UART5SW_0) + { + /* USART Clock is System Clock */ + RCC_Clocks->UART5CLK_Frequency = RCC_Clocks->SYSCLK_Frequency; + } + else if((RCC->CFGR3 & RCC_CFGR3_UART5SW) == RCC_CFGR3_UART5SW_1) + { + /* USART Clock is LSE Osc. */ + RCC_Clocks->UART5CLK_Frequency = LSE_VALUE; + } + else if((RCC->CFGR3 & RCC_CFGR3_UART5SW) == RCC_CFGR3_UART5SW) + { + /* USART Clock is HSI Osc. */ + RCC_Clocks->UART5CLK_Frequency = HSI_VALUE; + } +} + +/** + * @} + */ + +/** @defgroup RCC_Group3 Peripheral clocks configuration functions + * @brief Peripheral clocks configuration functions + * +@verbatim + =============================================================================== + ##### Peripheral clocks configuration functions ##### + =============================================================================== + [..] This section provide functions allowing to configure the Peripheral clocks. + (#) The RTC clock which is derived from the LSE, LSI or HSE_Div32 + (HSE divided by 32). + (#) After restart from Reset or wakeup from STANDBY, all peripherals are + off except internal SRAM, Flash and SWD. Before to start using + a peripheral you have to enable its interface clock. You can do this + using RCC_AHBPeriphClockCmd(), RCC_APB2PeriphClockCmd() + and RCC_APB1PeriphClockCmd() functions. + (#) To reset the peripherals configuration (to the default state after + device reset) you can use RCC_AHBPeriphResetCmd(), RCC_APB2PeriphResetCmd() + and RCC_APB1PeriphResetCmd() functions. +@endverbatim + * @{ + */ + +/** + * @brief Configures the ADC clock (ADCCLK). + * @param RCC_PLLCLK: defines the ADC clock divider. This clock is derived from + * the PLL Clock. + * This parameter can be one of the following values: + * @arg RCC_ADC12PLLCLK_OFF: ADC12 clock disabled + * @arg RCC_ADC12PLLCLK_Div1: ADC12 clock = PLLCLK/1 + * @arg RCC_ADC12PLLCLK_Div2: ADC12 clock = PLLCLK/2 + * @arg RCC_ADC12PLLCLK_Div4: ADC12 clock = PLLCLK/4 + * @arg RCC_ADC12PLLCLK_Div6: ADC12 clock = PLLCLK/6 + * @arg RCC_ADC12PLLCLK_Div8: ADC12 clock = PLLCLK/8 + * @arg RCC_ADC12PLLCLK_Div10: ADC12 clock = PLLCLK/10 + * @arg RCC_ADC12PLLCLK_Div12: ADC12 clock = PLLCLK/12 + * @arg RCC_ADC12PLLCLK_Div16: ADC12 clock = PLLCLK/16 + * @arg RCC_ADC12PLLCLK_Div32: ADC12 clock = PLLCLK/32 + * @arg RCC_ADC12PLLCLK_Div64: ADC12 clock = PLLCLK/64 + * @arg RCC_ADC12PLLCLK_Div128: ADC12 clock = PLLCLK/128 + * @arg RCC_ADC12PLLCLK_Div256: ADC12 clock = PLLCLK/256 + * @arg RCC_ADC34PLLCLK_OFF: ADC34 clock disabled + * @arg RCC_ADC34PLLCLK_Div1: ADC34 clock = PLLCLK/1 + * @arg RCC_ADC34PLLCLK_Div2: ADC34 clock = PLLCLK/2 + * @arg RCC_ADC34PLLCLK_Div4: ADC34 clock = PLLCLK/4 + * @arg RCC_ADC34PLLCLK_Div6: ADC34 clock = PLLCLK/6 + * @arg RCC_ADC34PLLCLK_Div8: ADC34 clock = PLLCLK/8 + * @arg RCC_ADC34PLLCLK_Div10: ADC34 clock = PLLCLK/10 + * @arg RCC_ADC34PLLCLK_Div12: ADC34 clock = PLLCLK/12 + * @arg RCC_ADC34PLLCLK_Div16: ADC34 clock = PLLCLK/16 + * @arg RCC_ADC34PLLCLK_Div32: ADC34 clock = PLLCLK/32 + * @arg RCC_ADC34PLLCLK_Div64: ADC34 clock = PLLCLK/64 + * @arg RCC_ADC34PLLCLK_Div128: ADC34 clock = PLLCLK/128 + * @arg RCC_ADC34PLLCLK_Div256: ADC34 clock = PLLCLK/256 + * @retval None + */ +void RCC_ADCCLKConfig(uint32_t RCC_PLLCLK) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_RCC_ADCCLK(RCC_PLLCLK)); + + tmp = (RCC_PLLCLK >> 28); + + /* Clears ADCPRE34 bits */ + if (tmp != 0) + { + RCC->CFGR2 &= ~RCC_CFGR2_ADCPRE34; + } + /* Clears ADCPRE12 bits */ + else + { + RCC->CFGR2 &= ~RCC_CFGR2_ADCPRE12; + } + /* Set ADCPRE bits according to RCC_PLLCLK value */ + RCC->CFGR2 |= RCC_PLLCLK; +} + +/** + * @brief Configures the I2C clock (I2CCLK). + * @param RCC_I2CCLK: defines the I2C clock source. This clock is derived + * from the HSI or System clock. + * This parameter can be one of the following values: + * @arg RCC_I2CxCLK_HSI: I2Cx clock = HSI + * @arg RCC_I2CxCLK_SYSCLK: I2Cx clock = System Clock + * (x can be 1 or 2 or 3). + * @retval None + */ +void RCC_I2CCLKConfig(uint32_t RCC_I2CCLK) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_RCC_I2CCLK(RCC_I2CCLK)); + + tmp = (RCC_I2CCLK >> 28); + + /* Clear I2CSW bit */ + switch (tmp) + { + case 0x00: + RCC->CFGR3 &= ~RCC_CFGR3_I2C1SW; + break; + case 0x01: + RCC->CFGR3 &= ~RCC_CFGR3_I2C2SW; + break; + case 0x02: + RCC->CFGR3 &= ~RCC_CFGR3_I2C3SW; + break; + default: + break; + } + + /* Set I2CSW bits according to RCC_I2CCLK value */ + RCC->CFGR3 |= RCC_I2CCLK; +} + +/** + * @brief Configures the TIMx clock sources(TIMCLK). + * @note For STM32F303xC devices, TIMx can be clocked from the PLL running at 144 MHz + * when the system clock source is the PLL and HCLK & PCLK2 clocks are not divided in respect to SYSCLK. + * For the devices STM32F334x8, STM32F302x8 and STM32F303xE, TIMx can be clocked from the PLL running at + * 144 MHz when the system clock source is the PLL and AHB or APB2 subsystem clocks are not divided by + * more than 2 cumulatively. + * @note If one of the previous conditions is missed, the TIM clock source + * configuration is lost and calling again this function becomes mandatory. + * @param RCC_TIMCLK: defines the TIMx clock source. + * This parameter can be one of the following values: + * @arg RCC_TIMxCLK_PCLK: TIMx clock = APB clock (doubled frequency when prescaled) + * @arg RCC_TIMxCLK_PLLCLK: TIMx clock = PLL output (running up to 144 MHz) + * (x can be 1, 8, 15, 16, 17, 20, 2, 3,4). + * @note For STM32F303xC devices, TIM1 and TIM8 can be clocked at 144MHz. + * For STM32F303xE devices, TIM1/8/20/2/3/4/15/16/17 can be clocked at 144MHz. + * For STM32F334x8 devices , only TIM1 can be clocked at 144MHz. + * For STM32F302x8 devices, TIM1/15/16/17 can be clocked at 144MHz + * @retval None + */ +void RCC_TIMCLKConfig(uint32_t RCC_TIMCLK) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_RCC_TIMCLK(RCC_TIMCLK)); + + tmp = (RCC_TIMCLK >> 28); + + /* Clear TIMSW bit */ + + switch (tmp) + { + case 0x00: + RCC->CFGR3 &= ~RCC_CFGR3_TIM1SW; + break; + case 0x01: + RCC->CFGR3 &= ~RCC_CFGR3_TIM8SW; + break; + case 0x02: + RCC->CFGR3 &= ~RCC_CFGR3_TIM15SW; + break; + case 0x03: + RCC->CFGR3 &= ~RCC_CFGR3_TIM16SW; + break; + case 0x04: + RCC->CFGR3 &= ~RCC_CFGR3_TIM17SW; + break; + case 0x05: + RCC->CFGR3 &= ~RCC_CFGR3_TIM20SW; + case 0x06: + RCC->CFGR3 &= ~RCC_CFGR3_TIM2SW; + case 0x07: + RCC->CFGR3 &= ~RCC_CFGR3_TIM3SW; + break; + default: + break; + } + + /* Set I2CSW bits according to RCC_TIMCLK value */ + RCC->CFGR3 |= RCC_TIMCLK; +} + +/** + * @brief Configures the HRTIM1 clock sources(HRTIM1CLK). + * @note The configuration of the HRTIM1 clock source is only possible when the + * SYSCLK = PLL and HCLK and PCLK2 clocks are not divided in respect to SYSCLK + * @note If one of the previous conditions is missed, the TIM clock source + * configuration is lost and calling again this function becomes mandatory. + * @param RCC_HRTIMCLK: defines the TIMx clock source. + * This parameter can be one of the following values: + * @arg RCC_HRTIM1CLK_HCLK: TIMx clock = APB high speed clock (doubled frequency + * when prescaled) + * @arg RCC_HRTIM1CLK_PLLCLK: TIMx clock = PLL output (running up to 144 MHz) + * (x can be 1 or 8). + * @retval None + */ +void RCC_HRTIM1CLKConfig(uint32_t RCC_HRTIMCLK) +{ + /* Check the parameters */ + assert_param(IS_RCC_HRTIMCLK(RCC_HRTIMCLK)); + + /* Clear HRTIMSW bit */ + RCC->CFGR3 &= ~RCC_CFGR3_HRTIM1SW; + + /* Set HRTIMSW bits according to RCC_HRTIMCLK value */ + RCC->CFGR3 |= RCC_HRTIMCLK; +} + +/** + * @brief Configures the USART clock (USARTCLK). + * @param RCC_USARTCLK: defines the USART clock source. This clock is derived + * from the HSI or System clock. + * This parameter can be one of the following values: + * @arg RCC_USARTxCLK_PCLK: USART clock = APB Clock (PCLK) + * @arg RCC_USARTxCLK_SYSCLK: USART clock = System Clock + * @arg RCC_USARTxCLK_LSE: USART clock = LSE Clock + * @arg RCC_USARTxCLK_HSI: USART clock = HSI Clock + * (x can be 1, 2, 3, 4 or 5). + * @retval None + */ +void RCC_USARTCLKConfig(uint32_t RCC_USARTCLK) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_RCC_USARTCLK(RCC_USARTCLK)); + + tmp = (RCC_USARTCLK >> 28); + + /* Clear USARTSW[1:0] bit */ + switch (tmp) + { + case 0x01: /* clear USART1SW */ + RCC->CFGR3 &= ~RCC_CFGR3_USART1SW; + break; + case 0x02: /* clear USART2SW */ + RCC->CFGR3 &= ~RCC_CFGR3_USART2SW; + break; + case 0x03: /* clear USART3SW */ + RCC->CFGR3 &= ~RCC_CFGR3_USART3SW; + break; + case 0x04: /* clear UART4SW */ + RCC->CFGR3 &= ~RCC_CFGR3_UART4SW; + break; + case 0x05: /* clear UART5SW */ + RCC->CFGR3 &= ~RCC_CFGR3_UART5SW; + break; + default: + break; + } + + /* Set USARTSW bits according to RCC_USARTCLK value */ + RCC->CFGR3 |= RCC_USARTCLK; +} + +/** + * @brief Configures the USB clock (USBCLK). + * @param RCC_USBCLKSource: specifies the USB clock source. This clock is + * derived from the PLL output. + * This parameter can be one of the following values: + * @arg RCC_USBCLKSource_PLLCLK_1Div5: PLL clock divided by 1,5 selected as USB + * clock source + * @arg RCC_USBCLKSource_PLLCLK_Div1: PLL clock selected as USB clock source + * @retval None + */ +void RCC_USBCLKConfig(uint32_t RCC_USBCLKSource) +{ + /* Check the parameters */ + assert_param(IS_RCC_USBCLK_SOURCE(RCC_USBCLKSource)); + + *(__IO uint32_t *) CFGR_USBPRE_BB = RCC_USBCLKSource; +} + +/** + * @brief Configures the RTC clock (RTCCLK). + * @note As the RTC clock configuration bits are in the Backup domain and write + * access is denied to this domain after reset, you have to enable write + * access using PWR_BackupAccessCmd(ENABLE) function before to configure + * the RTC clock source (to be done once after reset). + * @note Once the RTC clock is configured it can't be changed unless the RTC + * is reset using RCC_BackupResetCmd function, or by a Power On Reset (POR) + * + * @param RCC_RTCCLKSource: specifies the RTC clock source. + * This parameter can be one of the following values: + * @arg RCC_RTCCLKSource_LSE: LSE selected as RTC clock + * @arg RCC_RTCCLKSource_LSI: LSI selected as RTC clock + * @arg RCC_RTCCLKSource_HSE_Div32: HSE divided by 32 selected as RTC clock + * + * @note If the LSE or LSI is used as RTC clock source, the RTC continues to + * work in STOP and STANDBY modes, and can be used as wakeup source. + * However, when the HSE clock is used as RTC clock source, the RTC + * cannot be used in STOP and STANDBY modes. + * @note The maximum input clock frequency for RTC is 2MHz (when using HSE as + * RTC clock source). + * @retval None + */ +void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource) +{ + /* Check the parameters */ + assert_param(IS_RCC_RTCCLK_SOURCE(RCC_RTCCLKSource)); + + /* Select the RTC clock source */ + RCC->BDCR |= RCC_RTCCLKSource; +} + +/** + * @brief Configures the I2S clock source (I2SCLK). + * @note This function must be called before enabling the SPI2 and SPI3 clocks. + * @param RCC_I2SCLKSource: specifies the I2S clock source. + * This parameter can be one of the following values: + * @arg RCC_I2S2CLKSource_SYSCLK: SYSCLK clock used as I2S clock source + * @arg RCC_I2S2CLKSource_Ext: External clock mapped on the I2S_CKIN pin + * used as I2S clock source + * @retval None + */ +void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource) +{ + /* Check the parameters */ + assert_param(IS_RCC_I2SCLK_SOURCE(RCC_I2SCLKSource)); + + *(__IO uint32_t *) CFGR_I2SSRC_BB = RCC_I2SCLKSource; +} + +/** + * @brief Enables or disables the RTC clock. + * @note This function must be used only after the RTC clock source was selected + * using the RCC_RTCCLKConfig function. + * @param NewState: new state of the RTC clock. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_RTCCLKCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + *(__IO uint32_t *) BDCR_RTCEN_BB = (uint32_t)NewState; +} + +/** + * @brief Forces or releases the Backup domain reset. + * @note This function resets the RTC peripheral (including the backup registers) + * and the RTC clock source selection in RCC_BDCR register. + * @param NewState: new state of the Backup domain reset. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_BackupResetCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + *(__IO uint32_t *) BDCR_BDRST_BB = (uint32_t)NewState; +} + +/** + * @brief Enables or disables the AHB peripheral clock. + * @note After reset, the peripheral clock (used for registers read/write access) + * is disabled and the application software has to enable this clock before + * using it. + * @param RCC_AHBPeriph: specifies the AHB peripheral to gates its clock. + * This parameter can be any combination of the following values: + * @arg RCC_AHBPeriph_GPIOA + * @arg RCC_AHBPeriph_GPIOB + * @arg RCC_AHBPeriph_GPIOC + * @arg RCC_AHBPeriph_GPIOD + * @arg RCC_AHBPeriph_GPIOE + * @arg RCC_AHBPeriph_GPIOF + * @arg RCC_AHBPeriph_GPIOG + * @arg RCC_AHBPeriph_GPIOH + * @arg RCC_AHBPeriph_TS + * @arg RCC_AHBPeriph_CRC + * @arg RCC_AHBPeriph_FMC + * @arg RCC_AHBPeriph_FLITF (has effect only when the Flash memory is in power down mode) + * @arg RCC_AHBPeriph_SRAM + * @arg RCC_AHBPeriph_DMA2 + * @arg RCC_AHBPeriph_DMA1 + * @arg RCC_AHBPeriph_ADC34 + * @arg RCC_AHBPeriph_ADC12 + * @param NewState: new state of the specified peripheral clock. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_AHBPeriphClockCmd(uint32_t RCC_AHBPeriph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_AHB_PERIPH(RCC_AHBPeriph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + RCC->AHBENR |= RCC_AHBPeriph; + } + else + { + RCC->AHBENR &= ~RCC_AHBPeriph; + } +} + +/** + * @brief Enables or disables the High Speed APB (APB2) peripheral clock. + * @note After reset, the peripheral clock (used for registers read/write access) + * is disabled and the application software has to enable this clock before + * using it. + * @param RCC_APB2Periph: specifies the APB2 peripheral to gates its clock. + * This parameter can be any combination of the following values: + * @arg RCC_APB2Periph_SYSCFG + * @arg RCC_APB2Periph_SPI1 + * @arg RCC_APB2Periph_USART1 + * @arg RCC_APB2Periph_SPI4 + * @arg RCC_APB2Periph_TIM15 + * @arg RCC_APB2Periph_TIM16 + * @arg RCC_APB2Periph_TIM17 + * @arg RCC_APB2Periph_TIM1 + * @arg RCC_APB2Periph_TIM8 + * @arg RCC_APB2Periph_HRTIM1 + * @arg RCC_APB2Periph_TIM20 + * @param NewState: new state of the specified peripheral clock. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_APB2_PERIPH(RCC_APB2Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + RCC->APB2ENR |= RCC_APB2Periph; + } + else + { + RCC->APB2ENR &= ~RCC_APB2Periph; + } +} + +/** + * @brief Enables or disables the Low Speed APB (APB1) peripheral clock. + * @note After reset, the peripheral clock (used for registers read/write access) + * is disabled and the application software has to enable this clock before + * using it. + * @param RCC_APB1Periph: specifies the APB1 peripheral to gates its clock. + * This parameter can be any combination of the following values: + * @arg RCC_APB1Periph_TIM2 + * @arg RCC_APB1Periph_TIM3 + * @arg RCC_APB1Periph_TIM4 + * @arg RCC_APB1Periph_TIM6 + * @arg RCC_APB1Periph_TIM7 + * @arg RCC_APB1Periph_WWDG + * @arg RCC_APB1Periph_SPI2 + * @arg RCC_APB1Periph_SPI3 + * @arg RCC_APB1Periph_USART2 + * @arg RCC_APB1Periph_USART3 + * @arg RCC_APB1Periph_UART4 + * @arg RCC_APB1Periph_UART5 + * @arg RCC_APB1Periph_I2C1 + * @arg RCC_APB1Periph_I2C2 + * @arg RCC_APB1Periph_USB + * @arg RCC_APB1Periph_CAN1 + * @arg RCC_APB1Periph_PWR + * @arg RCC_APB1Periph_DAC1 + * @arg RCC_APB1Periph_DAC2 + * @arg RCC_APB1Periph_I2C3 + * @param NewState: new state of the specified peripheral clock. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_APB1_PERIPH(RCC_APB1Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + RCC->APB1ENR |= RCC_APB1Periph; + } + else + { + RCC->APB1ENR &= ~RCC_APB1Periph; + } +} + +/** + * @brief Forces or releases AHB peripheral reset. + * @param RCC_AHBPeriph: specifies the AHB peripheral to reset. + * This parameter can be any combination of the following values: + * @arg RCC_AHBPeriph_FMC + * @arg RCC_AHBPeriph_GPIOH + * @arg RCC_AHBPeriph_GPIOA + * @arg RCC_AHBPeriph_GPIOB + * @arg RCC_AHBPeriph_GPIOC + * @arg RCC_AHBPeriph_GPIOD + * @arg RCC_AHBPeriph_GPIOE + * @arg RCC_AHBPeriph_GPIOF + * @arg RCC_AHBPeriph_GPIOG + * @arg RCC_AHBPeriph_TS + * @arg RCC_AHBPeriph_ADC34 + * @arg RCC_AHBPeriph_ADC12 + * @param NewState: new state of the specified peripheral reset. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_AHBPeriphResetCmd(uint32_t RCC_AHBPeriph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_AHB_RST_PERIPH(RCC_AHBPeriph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + RCC->AHBRSTR |= RCC_AHBPeriph; + } + else + { + RCC->AHBRSTR &= ~RCC_AHBPeriph; + } +} + +/** + * @brief Forces or releases High Speed APB (APB2) peripheral reset. + * @param RCC_APB2Periph: specifies the APB2 peripheral to reset. + * This parameter can be any combination of the following values: + * @arg RCC_APB2Periph_SYSCFG + * @arg RCC_APB2Periph_SPI1 + * @arg RCC_APB2Periph_USART1 + * @arg RCC_APB2Periph_SPI4 + * @arg RCC_APB2Periph_TIM15 + * @arg RCC_APB2Periph_TIM16 + * @arg RCC_APB2Periph_TIM17 + * @arg RCC_APB2Periph_TIM1 + * @arg RCC_APB2Periph_TIM8 + * @arg RCC_APB2Periph_TIM20 + * @arg RCC_APB2Periph_HRTIM1 + * @param NewState: new state of the specified peripheral reset. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_APB2PeriphResetCmd(uint32_t RCC_APB2Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_APB2_PERIPH(RCC_APB2Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + RCC->APB2RSTR |= RCC_APB2Periph; + } + else + { + RCC->APB2RSTR &= ~RCC_APB2Periph; + } +} + +/** + * @brief Forces or releases Low Speed APB (APB1) peripheral reset. + * @param RCC_APB1Periph: specifies the APB1 peripheral to reset. + * This parameter can be any combination of the following values: + * @arg RCC_APB1Periph_TIM2 + * @arg RCC_APB1Periph_TIM3 + * @arg RCC_APB1Periph_TIM4 + * @arg RCC_APB1Periph_TIM6 + * @arg RCC_APB1Periph_TIM7 + * @arg RCC_APB1Periph_WWDG + * @arg RCC_APB1Periph_SPI2 + * @arg RCC_APB1Periph_SPI3 + * @arg RCC_APB1Periph_USART2 + * @arg RCC_APB1Periph_USART3 + * @arg RCC_APB1Periph_UART4 + * @arg RCC_APB1Periph_UART5 + * @arg RCC_APB1Periph_I2C1 + * @arg RCC_APB1Periph_I2C2 + * @arg RCC_APB1Periph_I2C3 + * @arg RCC_APB1Periph_USB + * @arg RCC_APB1Periph_CAN1 + * @arg RCC_APB1Periph_PWR + * @arg RCC_APB1Periph_DAC + * @param NewState: new state of the specified peripheral clock. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_APB1PeriphResetCmd(uint32_t RCC_APB1Periph, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_APB1_PERIPH(RCC_APB1Periph)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + RCC->APB1RSTR |= RCC_APB1Periph; + } + else + { + RCC->APB1RSTR &= ~RCC_APB1Periph; + } +} + +/** + * @} + */ + +/** @defgroup RCC_Group4 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified RCC interrupts. + * @note The CSS interrupt doesn't have an enable bit; once the CSS is enabled + * and if the HSE clock fails, the CSS interrupt occurs and an NMI is + * automatically generated. The NMI will be executed indefinitely, and + * since NMI has higher priority than any other IRQ (and main program) + * the application will be stacked in the NMI ISR unless the CSS interrupt + * pending bit is cleared. + * @param RCC_IT: specifies the RCC interrupt sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg RCC_IT_LSIRDY: LSI ready interrupt + * @arg RCC_IT_LSERDY: LSE ready interrupt + * @arg RCC_IT_HSIRDY: HSI ready interrupt + * @arg RCC_IT_HSERDY: HSE ready interrupt + * @arg RCC_IT_PLLRDY: PLL ready interrupt + * @param NewState: new state of the specified RCC interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RCC_ITConfig(uint8_t RCC_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RCC_IT(RCC_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Perform Byte access to RCC_CIR[13:8] bits to enable the selected interrupts */ + *(__IO uint8_t *) CIR_BYTE2_ADDRESS |= RCC_IT; + } + else + { + /* Perform Byte access to RCC_CIR[13:8] bits to disable the selected interrupts */ + *(__IO uint8_t *) CIR_BYTE2_ADDRESS &= (uint8_t)~RCC_IT; + } +} + +/** + * @brief Checks whether the specified RCC flag is set or not. + * @param RCC_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg RCC_FLAG_HSIRDY: HSI oscillator clock ready + * @arg RCC_FLAG_HSERDY: HSE oscillator clock ready + * @arg RCC_FLAG_PLLRDY: PLL clock ready + * @arg RCC_FLAG_MCOF: MCO Flag + * @arg RCC_FLAG_LSERDY: LSE oscillator clock ready + * @arg RCC_FLAG_LSIRDY: LSI oscillator clock ready + * @arg RCC_FLAG_OBLRST: Option Byte Loader (OBL) reset + * @arg RCC_FLAG_PINRST: Pin reset + * @arg RCC_FLAG_PORRST: POR/PDR reset + * @arg RCC_FLAG_SFTRST: Software reset + * @arg RCC_FLAG_IWDGRST: Independent Watchdog reset + * @arg RCC_FLAG_WWDGRST: Window Watchdog reset + * @arg RCC_FLAG_LPWRRST: Low Power reset + * @retval The new state of RCC_FLAG (SET or RESET). + */ +FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG) +{ + uint32_t tmp = 0; + uint32_t statusreg = 0; + FlagStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_RCC_FLAG(RCC_FLAG)); + + /* Get the RCC register index */ + tmp = RCC_FLAG >> 5; + + if (tmp == 0) /* The flag to check is in CR register */ + { + statusreg = RCC->CR; + } + else if (tmp == 1) /* The flag to check is in BDCR register */ + { + statusreg = RCC->BDCR; + } + else if (tmp == 4) /* The flag to check is in CFGR register */ + { + statusreg = RCC->CFGR; + } + else /* The flag to check is in CSR register */ + { + statusreg = RCC->CSR; + } + + /* Get the flag position */ + tmp = RCC_FLAG & FLAG_MASK; + + if ((statusreg & ((uint32_t)1 << tmp)) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + /* Return the flag status */ + return bitstatus; +} + +/** + * @brief Clears the RCC reset flags. + * The reset flags are: RCC_FLAG_OBLRST, RCC_FLAG_PINRST, RCC_FLAG_PORRST, + * RCC_FLAG_SFTRST, RCC_FLAG_IWDGRST, RCC_FLAG_WWDGRST, RCC_FLAG_LPWRRST. + * @param None + * @retval None + */ +void RCC_ClearFlag(void) +{ + /* Set RMVF bit to clear the reset flags */ + RCC->CSR |= RCC_CSR_RMVF; +} + +/** + * @brief Checks whether the specified RCC interrupt has occurred or not. + * @param RCC_IT: specifies the RCC interrupt source to check. + * This parameter can be one of the following values: + * @arg RCC_IT_LSIRDY: LSI ready interrupt + * @arg RCC_IT_LSERDY: LSE ready interrupt + * @arg RCC_IT_HSIRDY: HSI ready interrupt + * @arg RCC_IT_HSERDY: HSE ready interrupt + * @arg RCC_IT_PLLRDY: PLL ready interrupt + * @arg RCC_IT_CSS: Clock Security System interrupt + * @retval The new state of RCC_IT (SET or RESET). + */ +ITStatus RCC_GetITStatus(uint8_t RCC_IT) +{ + ITStatus bitstatus = RESET; + + /* Check the parameters */ + assert_param(IS_RCC_GET_IT(RCC_IT)); + + /* Check the status of the specified RCC interrupt */ + if ((RCC->CIR & RCC_IT) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + /* Return the RCC_IT status */ + return bitstatus; +} + +/** + * @brief Clears the RCC's interrupt pending bits. + * @param RCC_IT: specifies the interrupt pending bit to clear. + * This parameter can be any combination of the following values: + * @arg RCC_IT_LSIRDY: LSI ready interrupt + * @arg RCC_IT_LSERDY: LSE ready interrupt + * @arg RCC_IT_HSIRDY: HSI ready interrupt + * @arg RCC_IT_HSERDY: HSE ready interrupt + * @arg RCC_IT_PLLRDY: PLL ready interrupt + * @arg RCC_IT_CSS: Clock Security System interrupt + * @retval None + */ +void RCC_ClearITPendingBit(uint8_t RCC_IT) +{ + /* Check the parameters */ + assert_param(IS_RCC_CLEAR_IT(RCC_IT)); + + /* Perform Byte access to RCC_CIR[23:16] bits to clear the selected interrupt + pending bits */ + *(__IO uint8_t *) CIR_BYTE3_ADDRESS = RCC_IT; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rtc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rtc.c new file mode 100755 index 0000000000..daf87f26d1 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rtc.c @@ -0,0 +1,2598 @@ +/** + ****************************************************************************** + * @file stm32f30x_rtc.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Real-Time Clock (RTC) peripheral: + * + Initialization + * + Calendar (Time and Date) configuration + * + Alarms (Alarm A and Alarm B) configuration + * + WakeUp Timer configuration + * + Daylight Saving configuration + * + Output pin Configuration + * + Smooth digital Calibration configuration + * + TimeStamp configuration + * + Tampers configuration + * + Backup Data Registers configuration + * + Output Type Config configuration + * + Shift control synchronisation + * + Interrupts and flags management + * + @verbatim + + =============================================================================== + ##### RTC Operating Condition ##### + =============================================================================== + [..] The real-time clock (RTC) and the RTC backup registers can be powered + from the VBAT voltage when the main VDD supply is powered off. + To retain the content of the RTC backup registers and supply the RTC + when VDD is turned off, VBAT pin can be connected to an optional + standby voltage supplied by a battery or by another source. + + [..] To allow the RTC to operate even when the main digital supply (VDD) + is turned off, the VBAT pin powers the following blocks: + (#) The RTC + (#) The LSE oscillator + (#) PC13 to PC15 I/Os (when available) + + [..] When the backup domain is supplied by VDD (analog switch connected + to VDD), the following functions are available: + (#) PC14 and PC15 can be used as either GPIO or LSE pins + (#) PC13 can be used as a GPIO or as the RTC_AF pin + + [..] When the backup domain is supplied by VBAT (analog switch connected + to VBAT because VDD is not present), the following functions are available: + (#) PC14 and PC15 can be used as LSE pins only + (#) PC13 can be used as the RTC_AF pin + + ##### Backup Domain Reset ##### + =============================================================================== + [..] The backup domain reset sets all RTC registers and the RCC_BDCR + register to their reset values. + A backup domain reset is generated when one of the following events + occurs: + (#) Software reset, triggered by setting the BDRST bit in the + RCC Backup domain control register (RCC_BDCR). You can use the + RCC_BackupResetCmd(). + (#) VDD or VBAT power on, if both supplies have previously been + powered off. + + ##### Backup Domain Access ##### + =============================================================================== + [..] After reset, the backup domain (RTC registers and RTC backup data + registers) is protected against possible unwanted write accesses. + [..] To enable access to the Backup Domain and RTC registers, proceed as follows: + (#) Enable the Power Controller (PWR) APB1 interface clock using the + RCC_APB1PeriphClockCmd() function. + (#) Enable access to Backup domain using the PWR_BackupAccessCmd() function. + (#) Select the RTC clock source using the RCC_RTCCLKConfig() function. + (#) Enable RTC Clock using the RCC_RTCCLKCmd() function. + + ##### How to use this driver ##### + =============================================================================== + [..] + (+) Enable the backup domain access (see description in the section above) + (+) Configure the RTC Prescaler (Asynchronous and Synchronous) and + RTC hour format using the RTC_Init() function. + + *** Time and Date configuration *** + =================================== + [..] + (+) To configure the RTC Calendar (Time and Date) use the RTC_SetTime() + and RTC_SetDate() functions. + (+) To read the RTC Calendar, use the RTC_GetTime() and RTC_GetDate() + functions. + (+) To read the RTC subsecond, use the RTC_GetSubSecond() function. + (+) Use the RTC_DayLightSavingConfig() function to add or sub one + hour to the RTC Calendar. + + *** Alarm configuration *** + =========================== + [..] + (+) To configure the RTC Alarm use the RTC_SetAlarm() function. + (+) Enable the selected RTC Alarm using the RTC_AlarmCmd() function. + (+) To read the RTC Alarm, use the RTC_GetAlarm() function. + (+) To read the RTC alarm SubSecond, use the RTC_GetAlarmSubSecond() function. + + *** RTC Wakeup configuration *** + ================================ + [..] + (+) Configure the RTC Wakeup Clock source use the RTC_WakeUpClockConfig() + function. + (+) Configure the RTC WakeUp Counter using the RTC_SetWakeUpCounter() + function + (+) Enable the RTC WakeUp using the RTC_WakeUpCmd() function + (+) To read the RTC WakeUp Counter register, use the RTC_GetWakeUpCounter() + function. + + *** Outputs configuration *** + ============================= + [..] The RTC has 2 different outputs: + (+) AFO_ALARM: this output is used to manage the RTC Alarm A, Alarm B + and WaKeUp signals. + To output the selected RTC signal on RTC_AF pin, use the + RTC_OutputConfig() function. + (+) AFO_CALIB: this output is 512Hz signal or 1Hz . + To output the RTC Clock on RTC_AF pin, use the RTC_CalibOutputCmd() + function. + + *** Smooth digital Calibration configuration *** + ================================================ + [..] + (+) Configure the RTC Original Digital Calibration Value and the corresponding + calibration cycle period (32s,16s and 8s) using the RTC_SmoothCalibConfig() + function. + + *** TimeStamp configuration *** + =============================== + [..] + (+) Configure the RTC_AF trigger and enables the RTC TimeStamp + using the RTC_TimeStampCmd() function. + (+) To read the RTC TimeStamp Time and Date register, use the + RTC_GetTimeStamp() function. + (+) To read the RTC TimeStamp SubSecond register, use the + RTC_GetTimeStampSubSecond() function. + + *** Tamper configuration *** + ============================ + [..] + (+) Configure the Tamper filter count using RTC_TamperFilterConfig() + function. + (+) Configure the RTC Tamper trigger Edge or Level according to the Tamper + filter (if equal to 0 Edge else Level) value using the RTC_TamperConfig() function. + (+) Configure the Tamper sampling frequency using RTC_TamperSamplingFreqConfig() + function. + (+) Configure the Tamper precharge or discharge duration using + RTC_TamperPinsPrechargeDuration() function. + (+) Enable the Tamper Pull-UP using RTC_TamperPullUpDisableCmd() function. + (+) Enable the RTC Tamper using the RTC_TamperCmd() function. + (+) Enable the Time stamp on Tamper detection event using + RTC_TSOnTamperDetecCmd() function. + + *** Backup Data Registers configuration *** + =========================================== + [..] + (+) To write to the RTC Backup Data registers, use the RTC_WriteBackupRegister() + function. + (+) To read the RTC Backup Data registers, use the RTC_ReadBackupRegister() + function. + + ##### RTC and low power modes ##### + =============================================================================== + [..] The MCU can be woken up from a low power mode by an RTC alternate + function. + [..] The RTC alternate functions are the RTC alarms (Alarm A and Alarm B), + RTC wakeup, RTC tamper event detection and RTC time stamp event detection. + These RTC alternate functions can wake up the system from the Stop + and Standby lowpower modes. + The system can also wake up from low power modes without depending + on an external interrupt (Auto-wakeup mode), by using the RTC alarm + or the RTC wakeup events. + [..] The RTC provides a programmable time base for waking up from the + Stop or Standby mode at regular intervals. + Wakeup from STOP and Standby modes is possible only when the RTC + clock source is LSE or LSI. + + ##### Selection of RTC_AF alternate functions ##### + =============================================================================== + [..] The RTC_AF pin (PC13) can be used for the following purposes: + (+) Wakeup pin 2 (WKUP2) using the PWR_WakeUpPinCmd() function. + (+) AFO_ALARM output + (+) AFO_CALIB output + (+) AFI_TAMPER + (+) AFI_TIMESTAMP + + +------------------------------------------------------------------------------------------+ + | Pin |RTC ALARM |RTC CALIB |RTC TAMPER |RTC TIMESTAMP |PC13MODE| PC13VALUE | + | configuration | OUTPUT | OUTPUT | INPUT | INPUT | bit | bit | + | and function | ENABLED | ENABLED | ENABLED | ENABLED | | | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | Alarm out | | | | | Don't | | + | output OD | 1 |Don't care|Don't care | Don't care | care | 0 | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | Alarm out | | | | | Don't | | + | output PP | 1 |Don't care|Don't care | Don't care | care | 1 | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | Calibration out | | | | | Don't | | + | output PP | 0 | 1 |Don't care | Don't care | care | Don't care | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | TAMPER input | | | | | Don't | | + | floating | 0 | 0 | 1 | 0 | care | Don't care | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | TIMESTAMP and | | | | | Don't | | + | TAMPER input | 0 | 0 | 1 | 1 | care | Don't care | + | floating | | | | | | | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | TIMESTAMP input | | | | | Don't | | + | floating | 0 | 0 | 0 | 1 | care | Don't care | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | Output PP | 0 | 0 | 0 | 0 | 1 | PC13 output | + | Forced | | | | | | | + |-----------------|----------|----------|-----------|--------------|--------|--------------| + | Wakeup Pin or | 0 | 0 | 0 | 0 | 0 | Don't care | + | Standard GPIO | | | | | | | + +------------------------------------------------------------------------------------------+ + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_rtc.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup RTC + * @brief RTC driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* Masks Definition */ +#define RTC_TR_RESERVED_MASK ((uint32_t)0x007F7F7F) +#define RTC_DR_RESERVED_MASK ((uint32_t)0x00FFFF3F) +#define RTC_INIT_MASK ((uint32_t)0xFFFFFFFF) +#define RTC_RSF_MASK ((uint32_t)0xFFFFFF5F) +#define RTC_FLAGS_MASK ((uint32_t)(RTC_FLAG_TSOVF | RTC_FLAG_TSF | RTC_FLAG_WUTF | \ + RTC_FLAG_ALRBF | RTC_FLAG_ALRAF | RTC_FLAG_INITF | \ + RTC_FLAG_RSF | RTC_FLAG_INITS | RTC_FLAG_WUTWF | \ + RTC_FLAG_ALRBWF | RTC_FLAG_ALRAWF | RTC_FLAG_TAMP1F | \ + RTC_FLAG_TAMP2F | RTC_FLAG_TAMP3F | RTC_FLAG_RECALPF | \ + RTC_FLAG_SHPF)) + +#define INITMODE_TIMEOUT ((uint32_t) 0x00002000) +#define SYNCHRO_TIMEOUT ((uint32_t) 0x00008000) +#define RECALPF_TIMEOUT ((uint32_t) 0x00001000) +#define SHPF_TIMEOUT ((uint32_t) 0x00002000) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +static uint8_t RTC_ByteToBcd2(uint8_t Value); +static uint8_t RTC_Bcd2ToByte(uint8_t Value); + +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup RTC_Private_Functions + * @{ + */ + +/** @defgroup RTC_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + [..] This section provide functions allowing to initialize and configure the RTC + Prescaler (Synchronous and Asynchronous), RTC Hour format, disable RTC registers + Write protection, enter and exit the RTC initialization mode, RTC registers + synchronization check and reference clock detection enable. + (#) The RTC Prescaler is programmed to generate the RTC 1Hz time base. It is + split into 2 programmable prescalers to minimize power consumption. + (++) A 7-bit asynchronous prescaler and A 13-bit synchronous prescaler. + (++) When both prescalers are used, it is recommended to configure the + asynchronous prescaler to a high value to minimize consumption. + (#) All RTC registers are Write protected. Writing to the RTC registers + is enabled by writing a key into the Write Protection register, RTC_WPR. + (#) To Configure the RTC Calendar, user application should enter initialization + mode. In this mode, the calendar counter is stopped and its value + can be updated. When the initialization sequence is complete, the + calendar restarts counting after 4 RTCCLK cycles. + (#) To read the calendar through the shadow registers after Calendar + initialization, calendar update or after wakeup from low power modes + the software must first clear the RSF flag. The software must then + wait until it is set again before reading the calendar, which means + that the calendar registers have been correctly copied into the RTC_TR + and RTC_DR shadow registers. The RTC_WaitForSynchro() function + implements the above software sequence (RSF clear and RSF check). + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the RTC registers to their default reset values. + * @note This function doesn't reset the RTC Clock source and RTC Backup Data + * registers. + * @param None + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC registers are deinitialized + * - ERROR: RTC registers are not deinitialized + */ +ErrorStatus RTC_DeInit(void) +{ + __IO uint32_t wutcounter = 0x00; + uint32_t wutwfstatus = 0x00; + ErrorStatus status = ERROR; + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Set Initialization mode */ + if (RTC_EnterInitMode() == ERROR) + { + status = ERROR; + } + else + { + /* Reset TR, DR and CR registers */ + RTC->TR = (uint32_t)0x00000000; + RTC->DR = (uint32_t)0x00002101; + + /* Reset All CR bits except CR[2:0] */ + RTC->CR &= (uint32_t)0x00000007; + + /* Wait till RTC WUTWF flag is set and if Time out is reached exit */ + do + { + wutwfstatus = RTC->ISR & RTC_ISR_WUTWF; + wutcounter++; + } while((wutcounter != INITMODE_TIMEOUT) && (wutwfstatus == 0x00)); + + if ((RTC->ISR & RTC_ISR_WUTWF) == RESET) + { + status = ERROR; + } + else + { + /* Reset all RTC CR register bits */ + RTC->CR &= (uint32_t)0x00000000; + RTC->WUTR = (uint32_t)0x0000FFFF; + RTC->PRER = (uint32_t)0x007F00FF; + RTC->ALRMAR = (uint32_t)0x00000000; + RTC->ALRMBR = (uint32_t)0x00000000; + RTC->SHIFTR = (uint32_t)0x00000000; + RTC->CALR = (uint32_t)0x00000000; + RTC->ALRMASSR = (uint32_t)0x00000000; + RTC->ALRMBSSR = (uint32_t)0x00000000; + + /* Reset ISR register and exit initialization mode */ + RTC->ISR = (uint32_t)0x00000000; + + /* Reset Tamper and alternate functions configuration register */ + RTC->TAFCR = 0x00000000; + + /* Wait till the RTC RSF flag is set */ + if (RTC_WaitForSynchro() == ERROR) + { + status = ERROR; + } + else + { + status = SUCCESS; + } + } + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @brief Initializes the RTC registers according to the specified parameters + * in RTC_InitStruct. + * @param RTC_InitStruct: pointer to a RTC_InitTypeDef structure that contains + * the configuration information for the RTC peripheral. + * @note The RTC Prescaler register is write protected and can be written in + * initialization mode only. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC registers are initialized + * - ERROR: RTC registers are not initialized + */ +ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct) +{ + ErrorStatus status = ERROR; + + /* Check the parameters */ + assert_param(IS_RTC_HOUR_FORMAT(RTC_InitStruct->RTC_HourFormat)); + assert_param(IS_RTC_ASYNCH_PREDIV(RTC_InitStruct->RTC_AsynchPrediv)); + assert_param(IS_RTC_SYNCH_PREDIV(RTC_InitStruct->RTC_SynchPrediv)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Set Initialization mode */ + if (RTC_EnterInitMode() == ERROR) + { + status = ERROR; + } + else + { + /* Clear RTC CR FMT Bit */ + RTC->CR &= ((uint32_t)~(RTC_CR_FMT)); + /* Set RTC_CR register */ + RTC->CR |= ((uint32_t)(RTC_InitStruct->RTC_HourFormat)); + + /* Configure the RTC PRER */ + RTC->PRER = (uint32_t)(RTC_InitStruct->RTC_SynchPrediv); + RTC->PRER |= (uint32_t)(RTC_InitStruct->RTC_AsynchPrediv << 16); + + /* Exit Initialization mode */ + RTC_ExitInitMode(); + + status = SUCCESS; + } + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @brief Fills each RTC_InitStruct member with its default value. + * @param RTC_InitStruct: pointer to a RTC_InitTypeDef structure which will be + * initialized. + * @retval None + */ +void RTC_StructInit(RTC_InitTypeDef* RTC_InitStruct) +{ + /* Initialize the RTC_HourFormat member */ + RTC_InitStruct->RTC_HourFormat = RTC_HourFormat_24; + + /* Initialize the RTC_AsynchPrediv member */ + RTC_InitStruct->RTC_AsynchPrediv = (uint32_t)0x7F; + + /* Initialize the RTC_SynchPrediv member */ + RTC_InitStruct->RTC_SynchPrediv = (uint32_t)0xFF; +} + +/** + * @brief Enables or disables the RTC registers write protection. + * @note All the RTC registers are write protected except for RTC_ISR[13:8], + * RTC_TAFCR and RTC_BKPxR. + * @note Writing a wrong key reactivates the write protection. + * @note The protection mechanism is not affected by system reset. + * @param NewState: new state of the write protection. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_WriteProtectionCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + } + else + { + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + } +} + +/** + * @brief Enters the RTC Initialization mode. + * @note The RTC Initialization mode is write protected, use the + * RTC_WriteProtectionCmd(DISABLE) before calling this function. + * @param None + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC is in Init mode + * - ERROR: RTC is not in Init mode + */ +ErrorStatus RTC_EnterInitMode(void) +{ + __IO uint32_t initcounter = 0x00; + ErrorStatus status = ERROR; + uint32_t initstatus = 0x00; + + /* Check if the Initialization mode is set */ + if ((RTC->ISR & RTC_ISR_INITF) == (uint32_t)RESET) + { + /* Set the Initialization mode */ + RTC->ISR = (uint32_t)RTC_INIT_MASK; + + /* Wait till RTC is in INIT state and if Time out is reached exit */ + do + { + initstatus = RTC->ISR & RTC_ISR_INITF; + initcounter++; + } while((initcounter != INITMODE_TIMEOUT) && (initstatus == 0x00)); + + if ((RTC->ISR & RTC_ISR_INITF) != RESET) + { + status = SUCCESS; + } + else + { + status = ERROR; + } + } + else + { + status = SUCCESS; + } + + return (status); +} + +/** + * @brief Exits the RTC Initialization mode. + * @note When the initialization sequence is complete, the calendar restarts + * counting after 4 RTCCLK cycles. + * @note The RTC Initialization mode is write protected, use the + * RTC_WriteProtectionCmd(DISABLE) before calling this function. + * @param None + * @retval None + */ +void RTC_ExitInitMode(void) +{ + /* Exit Initialization mode */ + RTC->ISR &= (uint32_t)~RTC_ISR_INIT; +} + +/** + * @brief Waits until the RTC Time and Date registers (RTC_TR and RTC_DR) are + * synchronized with RTC APB clock. + * @note The RTC Resynchronization mode is write protected, use the + * RTC_WriteProtectionCmd(DISABLE) before calling this function. + * @note To read the calendar through the shadow registers after Calendar + * initialization, calendar update or after wakeup from low power modes + * the software must first clear the RSF flag. + * The software must then wait until it is set again before reading + * the calendar, which means that the calendar registers have been + * correctly copied into the RTC_TR and RTC_DR shadow registers. + * @param None + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC registers are synchronised + * - ERROR: RTC registers are not synchronised + */ +ErrorStatus RTC_WaitForSynchro(void) +{ + __IO uint32_t synchrocounter = 0; + ErrorStatus status = ERROR; + uint32_t synchrostatus = 0x00; + + if ((RTC->CR & RTC_CR_BYPSHAD) != RESET) + { + /* Bypass shadow mode */ + status = SUCCESS; + } + else + { + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Clear RSF flag */ + RTC->ISR &= (uint32_t)RTC_RSF_MASK; + + /* Wait the registers to be synchronised */ + do + { + synchrostatus = RTC->ISR & RTC_ISR_RSF; + synchrocounter++; + } while((synchrocounter != SYNCHRO_TIMEOUT) && (synchrostatus == 0x00)); + + if ((RTC->ISR & RTC_ISR_RSF) != RESET) + { + status = SUCCESS; + } + else + { + status = ERROR; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + } + + return (status); +} + +/** + * @brief Enables or disables the RTC reference clock detection. + * @param NewState: new state of the RTC reference clock. + * This parameter can be: ENABLE or DISABLE. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC reference clock detection is enabled + * - ERROR: RTC reference clock detection is disabled + */ +ErrorStatus RTC_RefClockCmd(FunctionalState NewState) +{ + ErrorStatus status = ERROR; + + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Set Initialization mode */ + if (RTC_EnterInitMode() == ERROR) + { + status = ERROR; + } + else + { + if (NewState != DISABLE) + { + /* Enable the RTC reference clock detection */ + RTC->CR |= RTC_CR_REFCKON; + } + else + { + /* Disable the RTC reference clock detection */ + RTC->CR &= ~RTC_CR_REFCKON; + } + /* Exit Initialization mode */ + RTC_ExitInitMode(); + + status = SUCCESS; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @brief Enables or Disables the Bypass Shadow feature. + * @note When the Bypass Shadow is enabled the calendar value are taken + * directly from the Calendar counter. + * @param NewState: new state of the Bypass Shadow feature. + * This parameter can be: ENABLE or DISABLE. + * @retval None +*/ +void RTC_BypassShadowCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + if (NewState != DISABLE) + { + /* Set the BYPSHAD bit */ + RTC->CR |= (uint8_t)RTC_CR_BYPSHAD; + } + else + { + /* Reset the BYPSHAD bit */ + RTC->CR &= (uint8_t)~RTC_CR_BYPSHAD; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @} + */ + +/** @defgroup RTC_Group2 Time and Date configuration functions + * @brief Time and Date configuration functions + * +@verbatim + =============================================================================== + ##### Time and Date configuration functions ##### + =============================================================================== + [..] This section provide functions allowing to program and read the RTC Calendar + (Time and Date). + +@endverbatim + * @{ + */ + +/** + * @brief Set the RTC current time. + * @param RTC_Format: specifies the format of the entered parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_TimeStruct: pointer to a RTC_TimeTypeDef structure that contains + * the time configuration information for the RTC. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC Time register is configured + * - ERROR: RTC Time register is not configured + */ +ErrorStatus RTC_SetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct) +{ + uint32_t tmpreg = 0; + ErrorStatus status = ERROR; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + + if (RTC_Format == RTC_Format_BIN) + { + if ((RTC->CR & RTC_CR_FMT) != (uint32_t)RESET) + { + assert_param(IS_RTC_HOUR12(RTC_TimeStruct->RTC_Hours)); + assert_param(IS_RTC_H12(RTC_TimeStruct->RTC_H12)); + } + else + { + RTC_TimeStruct->RTC_H12 = 0x00; + assert_param(IS_RTC_HOUR24(RTC_TimeStruct->RTC_Hours)); + } + assert_param(IS_RTC_MINUTES(RTC_TimeStruct->RTC_Minutes)); + assert_param(IS_RTC_SECONDS(RTC_TimeStruct->RTC_Seconds)); + } + else + { + if ((RTC->CR & RTC_CR_FMT) != (uint32_t)RESET) + { + tmpreg = RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Hours); + assert_param(IS_RTC_HOUR12(tmpreg)); + assert_param(IS_RTC_H12(RTC_TimeStruct->RTC_H12)); + } + else + { + RTC_TimeStruct->RTC_H12 = 0x00; + assert_param(IS_RTC_HOUR24(RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Hours))); + } + assert_param(IS_RTC_MINUTES(RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Minutes))); + assert_param(IS_RTC_SECONDS(RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Seconds))); + } + + /* Check the input parameters format */ + if (RTC_Format != RTC_Format_BIN) + { + tmpreg = (((uint32_t)(RTC_TimeStruct->RTC_Hours) << 16) | \ + ((uint32_t)(RTC_TimeStruct->RTC_Minutes) << 8) | \ + ((uint32_t)RTC_TimeStruct->RTC_Seconds) | \ + ((uint32_t)(RTC_TimeStruct->RTC_H12) << 16)); + } + else + { + tmpreg = (uint32_t)(((uint32_t)RTC_ByteToBcd2(RTC_TimeStruct->RTC_Hours) << 16) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_TimeStruct->RTC_Minutes) << 8) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_TimeStruct->RTC_Seconds)) | \ + (((uint32_t)RTC_TimeStruct->RTC_H12) << 16)); + } + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Set Initialization mode */ + if (RTC_EnterInitMode() == ERROR) + { + status = ERROR; + } + else + { + /* Set the RTC_TR register */ + RTC->TR = (uint32_t)(tmpreg & RTC_TR_RESERVED_MASK); + + /* Exit Initialization mode */ + RTC_ExitInitMode(); + + /* If RTC_CR_BYPSHAD bit = 0, wait for synchro else this check is not needed */ + if ((RTC->CR & RTC_CR_BYPSHAD) == RESET) + { + if (RTC_WaitForSynchro() == ERROR) + { + status = ERROR; + } + else + { + status = SUCCESS; + } + } + else + { + status = SUCCESS; + } + + } + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @brief Fills each RTC_TimeStruct member with its default value + * (Time = 00h:00min:00sec). + * @param RTC_TimeStruct: pointer to a RTC_TimeTypeDef structure which will be + * initialized. + * @retval None + */ +void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct) +{ + /* Time = 00h:00min:00sec */ + RTC_TimeStruct->RTC_H12 = RTC_H12_AM; + RTC_TimeStruct->RTC_Hours = 0; + RTC_TimeStruct->RTC_Minutes = 0; + RTC_TimeStruct->RTC_Seconds = 0; +} + +/** + * @brief Get the RTC current Time. + * @param RTC_Format: specifies the format of the returned parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_TimeStruct: pointer to a RTC_TimeTypeDef structure that will + * contain the returned current time configuration. + * @retval None + */ +void RTC_GetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + + /* Get the RTC_TR register */ + tmpreg = (uint32_t)(RTC->TR & RTC_TR_RESERVED_MASK); + + /* Fill the structure fields with the read parameters */ + RTC_TimeStruct->RTC_Hours = (uint8_t)((tmpreg & (RTC_TR_HT | RTC_TR_HU)) >> 16); + RTC_TimeStruct->RTC_Minutes = (uint8_t)((tmpreg & (RTC_TR_MNT | RTC_TR_MNU)) >>8); + RTC_TimeStruct->RTC_Seconds = (uint8_t)(tmpreg & (RTC_TR_ST | RTC_TR_SU)); + RTC_TimeStruct->RTC_H12 = (uint8_t)((tmpreg & (RTC_TR_PM)) >> 16); + + /* Check the input parameters format */ + if (RTC_Format == RTC_Format_BIN) + { + /* Convert the structure parameters to Binary format */ + RTC_TimeStruct->RTC_Hours = (uint8_t)RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Hours); + RTC_TimeStruct->RTC_Minutes = (uint8_t)RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Minutes); + RTC_TimeStruct->RTC_Seconds = (uint8_t)RTC_Bcd2ToByte(RTC_TimeStruct->RTC_Seconds); + } +} + +/** + * @brief Gets the RTC current Calendar Subseconds value. + * @note This function freeze the Time and Date registers after reading the + * SSR register. + * @param None + * @retval RTC current Calendar Subseconds value. + */ +uint32_t RTC_GetSubSecond(void) +{ + uint32_t tmpreg = 0; + + /* Get subseconds values from the correspondent registers*/ + tmpreg = (uint32_t)(RTC->SSR); + + /* Read DR register to unfroze calendar registers */ + (void) (RTC->DR); + + return (tmpreg); +} + +/** + * @brief Set the RTC current date. + * @param RTC_Format: specifies the format of the entered parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_DateStruct: pointer to a RTC_DateTypeDef structure that contains + * the date configuration information for the RTC. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC Date register is configured + * - ERROR: RTC Date register is not configured + */ +ErrorStatus RTC_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct) +{ + uint32_t tmpreg = 0; + ErrorStatus status = ERROR; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + + if ((RTC_Format == RTC_Format_BIN) && ((RTC_DateStruct->RTC_Month & 0x10) == 0x10)) + { + RTC_DateStruct->RTC_Month = (RTC_DateStruct->RTC_Month & (uint32_t)~(0x10)) + 0x0A; + } + if (RTC_Format == RTC_Format_BIN) + { + assert_param(IS_RTC_YEAR(RTC_DateStruct->RTC_Year)); + assert_param(IS_RTC_MONTH(RTC_DateStruct->RTC_Month)); + assert_param(IS_RTC_DATE(RTC_DateStruct->RTC_Date)); + } + else + { + assert_param(IS_RTC_YEAR(RTC_Bcd2ToByte(RTC_DateStruct->RTC_Year))); + tmpreg = RTC_Bcd2ToByte(RTC_DateStruct->RTC_Month); + assert_param(IS_RTC_MONTH(tmpreg)); + tmpreg = RTC_Bcd2ToByte(RTC_DateStruct->RTC_Date); + assert_param(IS_RTC_DATE(tmpreg)); + } + assert_param(IS_RTC_WEEKDAY(RTC_DateStruct->RTC_WeekDay)); + + /* Check the input parameters format */ + if (RTC_Format != RTC_Format_BIN) + { + tmpreg = ((((uint32_t)RTC_DateStruct->RTC_Year) << 16) | \ + (((uint32_t)RTC_DateStruct->RTC_Month) << 8) | \ + ((uint32_t)RTC_DateStruct->RTC_Date) | \ + (((uint32_t)RTC_DateStruct->RTC_WeekDay) << 13)); + } + else + { + tmpreg = (((uint32_t)RTC_ByteToBcd2(RTC_DateStruct->RTC_Year) << 16) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_DateStruct->RTC_Month) << 8) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_DateStruct->RTC_Date)) | \ + ((uint32_t)RTC_DateStruct->RTC_WeekDay << 13)); + } + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Set Initialization mode */ + if (RTC_EnterInitMode() == ERROR) + { + status = ERROR; + } + else + { + /* Set the RTC_DR register */ + RTC->DR = (uint32_t)(tmpreg & RTC_DR_RESERVED_MASK); + + /* Exit Initialization mode */ + RTC_ExitInitMode(); + + /* If RTC_CR_BYPSHAD bit = 0, wait for synchro else this check is not needed */ + if ((RTC->CR & RTC_CR_BYPSHAD) == RESET) + { + if (RTC_WaitForSynchro() == ERROR) + { + status = ERROR; + } + else + { + status = SUCCESS; + } + } + else + { + status = SUCCESS; + } + } + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @brief Fills each RTC_DateStruct member with its default value + * (Monday, January 01 xx00). + * @param RTC_DateStruct: pointer to a RTC_DateTypeDef structure which will be + * initialized. + * @retval None + */ +void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct) +{ + /* Monday, January 01 xx00 */ + RTC_DateStruct->RTC_WeekDay = RTC_Weekday_Monday; + RTC_DateStruct->RTC_Date = 1; + RTC_DateStruct->RTC_Month = RTC_Month_January; + RTC_DateStruct->RTC_Year = 0; +} + +/** + * @brief Get the RTC current date. + * @param RTC_Format: specifies the format of the returned parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_DateStruct: pointer to a RTC_DateTypeDef structure that will + * contain the returned current date configuration. + * @retval None + */ +void RTC_GetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + + /* Get the RTC_TR register */ + tmpreg = (uint32_t)(RTC->DR & RTC_DR_RESERVED_MASK); + + /* Fill the structure fields with the read parameters */ + RTC_DateStruct->RTC_Year = (uint8_t)((tmpreg & (RTC_DR_YT | RTC_DR_YU)) >> 16); + RTC_DateStruct->RTC_Month = (uint8_t)((tmpreg & (RTC_DR_MT | RTC_DR_MU)) >> 8); + RTC_DateStruct->RTC_Date = (uint8_t)(tmpreg & (RTC_DR_DT | RTC_DR_DU)); + RTC_DateStruct->RTC_WeekDay = (uint8_t)((tmpreg & (RTC_DR_WDU)) >> 13); + + /* Check the input parameters format */ + if (RTC_Format == RTC_Format_BIN) + { + /* Convert the structure parameters to Binary format */ + RTC_DateStruct->RTC_Year = (uint8_t)RTC_Bcd2ToByte(RTC_DateStruct->RTC_Year); + RTC_DateStruct->RTC_Month = (uint8_t)RTC_Bcd2ToByte(RTC_DateStruct->RTC_Month); + RTC_DateStruct->RTC_Date = (uint8_t)RTC_Bcd2ToByte(RTC_DateStruct->RTC_Date); + RTC_DateStruct->RTC_WeekDay = (uint8_t)(RTC_DateStruct->RTC_WeekDay); + } +} + +/** + * @} + */ + +/** @defgroup RTC_Group3 Alarms configuration functions + * @brief Alarms (Alarm A and Alarm B) configuration functions + * +@verbatim + =============================================================================== + ##### Alarms (Alarm A and Alarm B) configuration functions ##### + =============================================================================== + [..] This section provides functions allowing to program and read the RTC Alarms. + +@endverbatim + * @{ + */ + +/** + * @brief Set the specified RTC Alarm. + * @note The Alarm register can only be written when the corresponding Alarm + * is disabled (Use the RTC_AlarmCmd(DISABLE)). + * @param RTC_Format: specifies the format of the returned parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_Alarm: specifies the alarm to be configured. + * This parameter can be one of the following values: + * @arg RTC_Alarm_A: to select Alarm A + * @arg RTC_Alarm_B: to select Alarm B + * @param RTC_AlarmStruct: pointer to a RTC_AlarmTypeDef structure that + * contains the alarm configuration parameters. + * @retval None + */ +void RTC_SetAlarm(uint32_t RTC_Format, uint32_t RTC_Alarm, RTC_AlarmTypeDef* RTC_AlarmStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + assert_param(IS_RTC_ALARM(RTC_Alarm)); + assert_param(IS_ALARM_MASK(RTC_AlarmStruct->RTC_AlarmMask)); + assert_param(IS_RTC_ALARM_DATE_WEEKDAY_SEL(RTC_AlarmStruct->RTC_AlarmDateWeekDaySel)); + + if (RTC_Format == RTC_Format_BIN) + { + if ((RTC->CR & RTC_CR_FMT) != (uint32_t)RESET) + { + assert_param(IS_RTC_HOUR12(RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours)); + assert_param(IS_RTC_H12(RTC_AlarmStruct->RTC_AlarmTime.RTC_H12)); + } + else + { + RTC_AlarmStruct->RTC_AlarmTime.RTC_H12 = 0x00; + assert_param(IS_RTC_HOUR24(RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours)); + } + assert_param(IS_RTC_MINUTES(RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes)); + assert_param(IS_RTC_SECONDS(RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds)); + + if(RTC_AlarmStruct->RTC_AlarmDateWeekDaySel == RTC_AlarmDateWeekDaySel_Date) + { + assert_param(IS_RTC_ALARM_DATE_WEEKDAY_DATE(RTC_AlarmStruct->RTC_AlarmDateWeekDay)); + } + else + { + assert_param(IS_RTC_ALARM_DATE_WEEKDAY_WEEKDAY(RTC_AlarmStruct->RTC_AlarmDateWeekDay)); + } + } + else + { + if ((RTC->CR & RTC_CR_FMT) != (uint32_t)RESET) + { + tmpreg = RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours); + assert_param(IS_RTC_HOUR12(tmpreg)); + assert_param(IS_RTC_H12(RTC_AlarmStruct->RTC_AlarmTime.RTC_H12)); + } + else + { + RTC_AlarmStruct->RTC_AlarmTime.RTC_H12 = 0x00; + assert_param(IS_RTC_HOUR24(RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours))); + } + + assert_param(IS_RTC_MINUTES(RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes))); + assert_param(IS_RTC_SECONDS(RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds))); + + if(RTC_AlarmStruct->RTC_AlarmDateWeekDaySel == RTC_AlarmDateWeekDaySel_Date) + { + tmpreg = RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmDateWeekDay); + assert_param(IS_RTC_ALARM_DATE_WEEKDAY_DATE(tmpreg)); + } + else + { + tmpreg = RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmDateWeekDay); + assert_param(IS_RTC_ALARM_DATE_WEEKDAY_WEEKDAY(tmpreg)); + } + } + + /* Check the input parameters format */ + if (RTC_Format != RTC_Format_BIN) + { + tmpreg = (((uint32_t)(RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours) << 16) | \ + ((uint32_t)(RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes) << 8) | \ + ((uint32_t)RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds) | \ + ((uint32_t)(RTC_AlarmStruct->RTC_AlarmTime.RTC_H12) << 16) | \ + ((uint32_t)(RTC_AlarmStruct->RTC_AlarmDateWeekDay) << 24) | \ + ((uint32_t)RTC_AlarmStruct->RTC_AlarmDateWeekDaySel) | \ + ((uint32_t)RTC_AlarmStruct->RTC_AlarmMask)); + } + else + { + tmpreg = (((uint32_t)RTC_ByteToBcd2(RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours) << 16) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes) << 8) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds)) | \ + ((uint32_t)(RTC_AlarmStruct->RTC_AlarmTime.RTC_H12) << 16) | \ + ((uint32_t)RTC_ByteToBcd2(RTC_AlarmStruct->RTC_AlarmDateWeekDay) << 24) | \ + ((uint32_t)RTC_AlarmStruct->RTC_AlarmDateWeekDaySel) | \ + ((uint32_t)RTC_AlarmStruct->RTC_AlarmMask)); + } + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Configure the Alarm register */ + if (RTC_Alarm == RTC_Alarm_A) + { + RTC->ALRMAR = (uint32_t)tmpreg; + } + else + { + RTC->ALRMBR = (uint32_t)tmpreg; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Fills each RTC_AlarmStruct member with its default value + * (Time = 00h:00mn:00sec / Date = 1st day of the month/Mask = + * all fields are masked). + * @param RTC_AlarmStruct: pointer to a @ref RTC_AlarmTypeDef structure which + * will be initialized. + * @retval None + */ +void RTC_AlarmStructInit(RTC_AlarmTypeDef* RTC_AlarmStruct) +{ + /* Alarm Time Settings : Time = 00h:00mn:00sec */ + RTC_AlarmStruct->RTC_AlarmTime.RTC_H12 = RTC_H12_AM; + RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours = 0; + RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes = 0; + RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds = 0; + + /* Alarm Date Settings : Date = 1st day of the month */ + RTC_AlarmStruct->RTC_AlarmDateWeekDaySel = RTC_AlarmDateWeekDaySel_Date; + RTC_AlarmStruct->RTC_AlarmDateWeekDay = 1; + + /* Alarm Masks Settings : Mask = all fields are not masked */ + RTC_AlarmStruct->RTC_AlarmMask = RTC_AlarmMask_None; +} + +/** + * @brief Get the RTC Alarm value and masks. + * @param RTC_Format: specifies the format of the output parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_Alarm: specifies the alarm to be read. + * This parameter can be one of the following values: + * @arg RTC_Alarm_A: to select Alarm A + * @arg RTC_Alarm_B: to select Alarm B + * @param RTC_AlarmStruct: pointer to a RTC_AlarmTypeDef structure that will + * contains the output alarm configuration values. + * @retval None + */ +void RTC_GetAlarm(uint32_t RTC_Format, uint32_t RTC_Alarm, RTC_AlarmTypeDef* RTC_AlarmStruct) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + assert_param(IS_RTC_ALARM(RTC_Alarm)); + + /* Get the RTC_ALRMxR register */ + if (RTC_Alarm == RTC_Alarm_A) + { + tmpreg = (uint32_t)(RTC->ALRMAR); + } + else + { + tmpreg = (uint32_t)(RTC->ALRMBR); + } + + /* Fill the structure with the read parameters */ + RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours = (uint32_t)((tmpreg & (RTC_ALRMAR_HT | \ + RTC_ALRMAR_HU)) >> 16); + RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes = (uint32_t)((tmpreg & (RTC_ALRMAR_MNT | \ + RTC_ALRMAR_MNU)) >> 8); + RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds = (uint32_t)(tmpreg & (RTC_ALRMAR_ST | \ + RTC_ALRMAR_SU)); + RTC_AlarmStruct->RTC_AlarmTime.RTC_H12 = (uint32_t)((tmpreg & RTC_ALRMAR_PM) >> 16); + RTC_AlarmStruct->RTC_AlarmDateWeekDay = (uint32_t)((tmpreg & (RTC_ALRMAR_DT | RTC_ALRMAR_DU)) >> 24); + RTC_AlarmStruct->RTC_AlarmDateWeekDaySel = (uint32_t)(tmpreg & RTC_ALRMAR_WDSEL); + RTC_AlarmStruct->RTC_AlarmMask = (uint32_t)(tmpreg & RTC_AlarmMask_All); + + if (RTC_Format == RTC_Format_BIN) + { + RTC_AlarmStruct->RTC_AlarmTime.RTC_Hours = RTC_Bcd2ToByte(RTC_AlarmStruct-> \ + RTC_AlarmTime.RTC_Hours); + RTC_AlarmStruct->RTC_AlarmTime.RTC_Minutes = RTC_Bcd2ToByte(RTC_AlarmStruct-> \ + RTC_AlarmTime.RTC_Minutes); + RTC_AlarmStruct->RTC_AlarmTime.RTC_Seconds = RTC_Bcd2ToByte(RTC_AlarmStruct-> \ + RTC_AlarmTime.RTC_Seconds); + RTC_AlarmStruct->RTC_AlarmDateWeekDay = RTC_Bcd2ToByte(RTC_AlarmStruct->RTC_AlarmDateWeekDay); + } +} + +/** + * @brief Enables or disables the specified RTC Alarm. + * @param RTC_Alarm: specifies the alarm to be configured. + * This parameter can be any combination of the following values: + * @arg RTC_Alarm_A: to select Alarm A + * @arg RTC_Alarm_B: to select Alarm B + * @param NewState: new state of the specified alarm. + * This parameter can be: ENABLE or DISABLE. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC Alarm is enabled/disabled + * - ERROR: RTC Alarm is not enabled/disabled + */ +ErrorStatus RTC_AlarmCmd(uint32_t RTC_Alarm, FunctionalState NewState) +{ + __IO uint32_t alarmcounter = 0x00; + uint32_t alarmstatus = 0x00; + ErrorStatus status = ERROR; + + /* Check the parameters */ + assert_param(IS_RTC_CMD_ALARM(RTC_Alarm)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Configure the Alarm state */ + if (NewState != DISABLE) + { + RTC->CR |= (uint32_t)RTC_Alarm; + + status = SUCCESS; + } + else + { + /* Disable the Alarm in RTC_CR register */ + RTC->CR &= (uint32_t)~RTC_Alarm; + + /* Wait till RTC ALRxWF flag is set and if Time out is reached exit */ + do + { + alarmstatus = RTC->ISR & (RTC_Alarm >> 8); + alarmcounter++; + } while((alarmcounter != INITMODE_TIMEOUT) && (alarmstatus == 0x00)); + + if ((RTC->ISR & (RTC_Alarm >> 8)) == RESET) + { + status = ERROR; + } + else + { + status = SUCCESS; + } + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @brief Configures the RTC AlarmA/B Subseconds value and mask. + * @note This function is performed only when the Alarm is disabled. + * @param RTC_Alarm: specifies the alarm to be configured. + * This parameter can be one of the following values: + * @arg RTC_Alarm_A: to select Alarm A + * @arg RTC_Alarm_B: to select Alarm B + * @param RTC_AlarmSubSecondValue: specifies the Subseconds value. + * This parameter can be a value from 0 to 0x00007FFF. + * @param RTC_AlarmSubSecondMask: specifies the Subseconds Mask. + * This parameter can be any combination of the following values: + * @arg RTC_AlarmSubSecondMask_All : All Alarm SS fields are masked. + * There is no comparison on sub seconds for Alarm. + * @arg RTC_AlarmSubSecondMask_SS14_1 : SS[14:1] are don't care in Alarm comparison. + * Only SS[0] is compared + * @arg RTC_AlarmSubSecondMask_SS14_2 : SS[14:2] are don't care in Alarm comparison. + * Only SS[1:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_3 : SS[14:3] are don't care in Alarm comparison. + * Only SS[2:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_4 : SS[14:4] are don't care in Alarm comparison. + * Only SS[3:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_5 : SS[14:5] are don't care in Alarm comparison. + * Only SS[4:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_6 : SS[14:6] are don't care in Alarm comparison. + * Only SS[5:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_7 : SS[14:7] are don't care in Alarm comparison. + * Only SS[6:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_8 : SS[14:8] are don't care in Alarm comparison. + * Only SS[7:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_9 : SS[14:9] are don't care in Alarm comparison. + * Only SS[8:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_10: SS[14:10] are don't care in Alarm comparison. + * Only SS[9:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_11: SS[14:11] are don't care in Alarm comparison. + * Only SS[10:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_12: SS[14:12] are don't care in Alarm comparison. + * Only SS[11:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14_13: SS[14:13] are don't care in Alarm comparison. + * Only SS[12:0] are compared + * @arg RTC_AlarmSubSecondMask_SS14 : SS[14] is don't care in Alarm comparison. + * Only SS[13:0] are compared + * @arg RTC_AlarmSubSecondMask_None : SS[14:0] are compared and must match + * to activate alarm + * @retval None + */ +void RTC_AlarmSubSecondConfig(uint32_t RTC_Alarm, uint32_t RTC_AlarmSubSecondValue, uint32_t RTC_AlarmSubSecondMask) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_ALARM(RTC_Alarm)); + assert_param(IS_RTC_ALARM_SUB_SECOND_VALUE(RTC_AlarmSubSecondValue)); + assert_param(IS_RTC_ALARM_SUB_SECOND_MASK(RTC_AlarmSubSecondMask)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Configure the Alarm A or Alarm B SubSecond registers */ + tmpreg = (uint32_t) (uint32_t)(RTC_AlarmSubSecondValue) | (uint32_t)(RTC_AlarmSubSecondMask); + + if (RTC_Alarm == RTC_Alarm_A) + { + /* Configure the AlarmA SubSecond register */ + RTC->ALRMASSR = tmpreg; + } + else + { + /* Configure the Alarm B SubSecond register */ + RTC->ALRMBSSR = tmpreg; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + +} + +/** + * @brief Gets the RTC Alarm Subseconds value. + * @param RTC_Alarm: specifies the alarm to be read. + * This parameter can be one of the following values: + * @arg RTC_Alarm_A: to select Alarm A + * @arg RTC_Alarm_B: to select Alarm B + * @param None + * @retval RTC Alarm Subseconds value. + */ +uint32_t RTC_GetAlarmSubSecond(uint32_t RTC_Alarm) +{ + uint32_t tmpreg = 0; + + /* Get the RTC_ALRMxR register */ + if (RTC_Alarm == RTC_Alarm_A) + { + tmpreg = (uint32_t)((RTC->ALRMASSR) & RTC_ALRMASSR_SS); + } + else + { + tmpreg = (uint32_t)((RTC->ALRMBSSR) & RTC_ALRMBSSR_SS); + } + + return (tmpreg); +} + +/** + * @} + */ + +/** @defgroup RTC_Group4 WakeUp Timer configuration functions + * @brief WakeUp Timer configuration functions + * +@verbatim + =============================================================================== + ##### WakeUp Timer configuration functions ##### + =============================================================================== + [..] This section provide functions allowing to program and read the RTC WakeUp. + +@endverbatim + * @{ + */ + +/** + * @brief Configures the RTC Wakeup clock source. + * @note The WakeUp Clock source can only be changed when the RTC WakeUp + * is disabled (Use the RTC_WakeUpCmd(DISABLE)). + * @param RTC_WakeUpClock: Wakeup Clock source. + * This parameter can be one of the following values: + * @arg RTC_WakeUpClock_RTCCLK_Div16: RTC Wakeup Counter Clock = RTCCLK/16 + * @arg RTC_WakeUpClock_RTCCLK_Div8: RTC Wakeup Counter Clock = RTCCLK/8 + * @arg RTC_WakeUpClock_RTCCLK_Div4: RTC Wakeup Counter Clock = RTCCLK/4 + * @arg RTC_WakeUpClock_RTCCLK_Div2: RTC Wakeup Counter Clock = RTCCLK/2 + * @arg RTC_WakeUpClock_CK_SPRE_16bits: RTC Wakeup Counter Clock = CK_SPRE + * @arg RTC_WakeUpClock_CK_SPRE_17bits: RTC Wakeup Counter Clock = CK_SPRE + * @retval None + */ +void RTC_WakeUpClockConfig(uint32_t RTC_WakeUpClock) +{ + /* Check the parameters */ + assert_param(IS_RTC_WAKEUP_CLOCK(RTC_WakeUpClock)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Clear the Wakeup Timer clock source bits in CR register */ + RTC->CR &= (uint32_t)~RTC_CR_WUCKSEL; + + /* Configure the clock source */ + RTC->CR |= (uint32_t)RTC_WakeUpClock; + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Configures the RTC Wakeup counter. + * @note The RTC WakeUp counter can only be written when the RTC WakeUp + * is disabled (Use the RTC_WakeUpCmd(DISABLE)). + * @param RTC_WakeUpCounter: specifies the WakeUp counter. + * This parameter can be a value from 0x0000 to 0xFFFF. + * @retval None + */ +void RTC_SetWakeUpCounter(uint32_t RTC_WakeUpCounter) +{ + /* Check the parameters */ + assert_param(IS_RTC_WAKEUP_COUNTER(RTC_WakeUpCounter)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Configure the Wakeup Timer counter */ + RTC->WUTR = (uint32_t)RTC_WakeUpCounter; + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Returns the RTC WakeUp timer counter value. + * @param None + * @retval The RTC WakeUp Counter value. + */ +uint32_t RTC_GetWakeUpCounter(void) +{ + /* Get the counter value */ + return ((uint32_t)(RTC->WUTR & RTC_WUTR_WUT)); +} + +/** + * @brief Enables or Disables the RTC WakeUp timer. + * @param NewState: new state of the WakeUp timer. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +ErrorStatus RTC_WakeUpCmd(FunctionalState NewState) +{ + __IO uint32_t wutcounter = 0x00; + uint32_t wutwfstatus = 0x00; + ErrorStatus status = ERROR; + + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + if (NewState != DISABLE) + { + /* Enable the Wakeup Timer */ + RTC->CR |= (uint32_t)RTC_CR_WUTE; + status = SUCCESS; + } + else + { + /* Disable the Wakeup Timer */ + RTC->CR &= (uint32_t)~RTC_CR_WUTE; + /* Wait till RTC WUTWF flag is set and if Time out is reached exit */ + do + { + wutwfstatus = RTC->ISR & RTC_ISR_WUTWF; + wutcounter++; + } while((wutcounter != INITMODE_TIMEOUT) && (wutwfstatus == 0x00)); + + if ((RTC->ISR & RTC_ISR_WUTWF) == RESET) + { + status = ERROR; + } + else + { + status = SUCCESS; + } + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return status; +} + +/** + * @} + */ + +/** @defgroup RTC_Group5 Daylight Saving configuration functions + * @brief Daylight Saving configuration functions + * +@verbatim + =============================================================================== + ##### Daylight Saving configuration functions ##### + =============================================================================== + [..] This section provide functions allowing to configure the RTC DayLight Saving. + +@endverbatim + * @{ + */ + +/** + * @brief Adds or substract one hour from the current time. + * @param RTC_DayLightSaveOperation: the value of hour adjustment. + * This parameter can be one of the following values: + * @arg RTC_DayLightSaving_SUB1H: Substract one hour (winter time) + * @arg RTC_DayLightSaving_ADD1H: Add one hour (summer time) + * @param RTC_StoreOperation: Specifies the value to be written in the BCK bit + * in CR register to store the operation. + * This parameter can be one of the following values: + * @arg RTC_StoreOperation_Reset: BCK Bit Reset + * @arg RTC_StoreOperation_Set: BCK Bit Set + * @retval None + */ +void RTC_DayLightSavingConfig(uint32_t RTC_DayLightSaving, uint32_t RTC_StoreOperation) +{ + /* Check the parameters */ + assert_param(IS_RTC_DAYLIGHT_SAVING(RTC_DayLightSaving)); + assert_param(IS_RTC_STORE_OPERATION(RTC_StoreOperation)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Clear the bits to be configured */ + RTC->CR &= (uint32_t)~(RTC_CR_BCK); + + /* Configure the RTC_CR register */ + RTC->CR |= (uint32_t)(RTC_DayLightSaving | RTC_StoreOperation); + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Returns the RTC Day Light Saving stored operation. + * @param None + * @retval RTC Day Light Saving stored operation. + * - RTC_StoreOperation_Reset + * - RTC_StoreOperation_Set + */ +uint32_t RTC_GetStoreOperation(void) +{ + return (RTC->CR & RTC_CR_BCK); +} + +/** + * @} + */ + +/** @defgroup RTC_Group6 Output pin Configuration function + * @brief Output pin Configuration function + * +@verbatim + =============================================================================== + ##### Output pin Configuration function ##### + =============================================================================== + [..] This section provide functions allowing to configure the RTC Output source. + +@endverbatim + * @{ + */ + +/** + * @brief Configures the RTC output source (AFO_ALARM). + * @param RTC_Output: Specifies which signal will be routed to the RTC output. + * This parameter can be one of the following values: + * @arg RTC_Output_Disable: No output selected + * @arg RTC_Output_AlarmA: signal of AlarmA mapped to output + * @arg RTC_Output_AlarmB: signal of AlarmB mapped to output + * @arg RTC_Output_WakeUp: signal of WakeUp mapped to output + * @param RTC_OutputPolarity: Specifies the polarity of the output signal. + * This parameter can be one of the following: + * @arg RTC_OutputPolarity_High: The output pin is high when the + * ALRAF/ALRBF/WUTF is high (depending on OSEL) + * @arg RTC_OutputPolarity_Low: The output pin is low when the + * ALRAF/ALRBF/WUTF is high (depending on OSEL) + * @retval None + */ +void RTC_OutputConfig(uint32_t RTC_Output, uint32_t RTC_OutputPolarity) +{ + /* Check the parameters */ + assert_param(IS_RTC_OUTPUT(RTC_Output)); + assert_param(IS_RTC_OUTPUT_POL(RTC_OutputPolarity)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Clear the bits to be configured */ + RTC->CR &= (uint32_t)~(RTC_CR_OSEL | RTC_CR_POL); + + /* Configure the output selection and polarity */ + RTC->CR |= (uint32_t)(RTC_Output | RTC_OutputPolarity); + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @} + */ + +/** @defgroup RTC_Group7 Digital Calibration configuration functions + * @brief Digital Calibration configuration functions + * +@verbatim + =============================================================================== + ##### Digital Calibration configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the RTC clock to be output through the relative + * pin. + * @param NewState: new state of the digital calibration Output. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_CalibOutputCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + if (NewState != DISABLE) + { + /* Enable the RTC clock output */ + RTC->CR |= (uint32_t)RTC_CR_COE; + } + else + { + /* Disable the RTC clock output */ + RTC->CR &= (uint32_t)~RTC_CR_COE; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Configures the Calibration Pinout (RTC_CALIB) Selection (1Hz or 512Hz). + * @param RTC_CalibOutput : Select the Calibration output Selection . + * This parameter can be one of the following values: + * @arg RTC_CalibOutput_512Hz: A signal has a regular waveform at 512Hz. + * @arg RTC_CalibOutput_1Hz : A signal has a regular waveform at 1Hz. + * @retval None +*/ +void RTC_CalibOutputConfig(uint32_t RTC_CalibOutput) +{ + /* Check the parameters */ + assert_param(IS_RTC_CALIB_OUTPUT(RTC_CalibOutput)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /*clear flags before config*/ + RTC->CR &= (uint32_t)~(RTC_CR_COSEL); + + /* Configure the RTC_CR register */ + RTC->CR |= (uint32_t)RTC_CalibOutput; + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Configures the Smooth Calibration Settings. + * @param RTC_SmoothCalibPeriod : Select the Smooth Calibration Period. + * This parameter can be can be one of the following values: + * @arg RTC_SmoothCalibPeriod_32sec : The smooth calibration periode is 32s. + * @arg RTC_SmoothCalibPeriod_16sec : The smooth calibration periode is 16s. + * @arg RTC_SmoothCalibPeriod_8sec : The smooth calibration periode is 8s. + * @param RTC_SmoothCalibPlusPulses : Select to Set or reset the CALP bit. + * This parameter can be one of the following values: + * @arg RTC_SmoothCalibPlusPulses_Set : Add one RTCCLK puls every 2**11 pulses. + * @arg RTC_SmoothCalibPlusPulses_Reset: No RTCCLK pulses are added. + * @param RTC_SmouthCalibMinusPulsesValue: Select the value of CALM[8:0] bits. + * This parameter can be one any value from 0 to 0x000001FF. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC Calib registers are configured + * - ERROR: RTC Calib registers are not configured +*/ +ErrorStatus RTC_SmoothCalibConfig(uint32_t RTC_SmoothCalibPeriod, + uint32_t RTC_SmoothCalibPlusPulses, + uint32_t RTC_SmouthCalibMinusPulsesValue) +{ + ErrorStatus status = ERROR; + uint32_t recalpfcount = 0; + + /* Check the parameters */ + assert_param(IS_RTC_SMOOTH_CALIB_PERIOD(RTC_SmoothCalibPeriod)); + assert_param(IS_RTC_SMOOTH_CALIB_PLUS(RTC_SmoothCalibPlusPulses)); + assert_param(IS_RTC_SMOOTH_CALIB_MINUS(RTC_SmouthCalibMinusPulsesValue)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* check if a calibration is pending*/ + if ((RTC->ISR & RTC_ISR_RECALPF) != RESET) + { + /* wait until the Calibration is completed*/ + while (((RTC->ISR & RTC_ISR_RECALPF) != RESET) && (recalpfcount != RECALPF_TIMEOUT)) + { + recalpfcount++; + } + } + + /* check if the calibration pending is completed or if there is no calibration operation at all*/ + if ((RTC->ISR & RTC_ISR_RECALPF) == RESET) + { + /* Configure the Smooth calibration settings */ + RTC->CALR = (uint32_t)((uint32_t)RTC_SmoothCalibPeriod | (uint32_t)RTC_SmoothCalibPlusPulses | (uint32_t)RTC_SmouthCalibMinusPulsesValue); + + status = SUCCESS; + } + else + { + status = ERROR; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return (ErrorStatus)(status); +} + +/** + * @} + */ + + +/** @defgroup RTC_Group8 TimeStamp configuration functions + * @brief TimeStamp configuration functions + * +@verbatim + =============================================================================== + ##### TimeStamp configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or Disables the RTC TimeStamp functionality with the + * specified time stamp pin stimulating edge. + * @param RTC_TimeStampEdge: Specifies the pin edge on which the TimeStamp is + * activated. + * This parameter can be one of the following: + * @arg RTC_TimeStampEdge_Rising: the Time stamp event occurs on the rising + * edge of the related pin. + * @arg RTC_TimeStampEdge_Falling: the Time stamp event occurs on the + * falling edge of the related pin. + * @param NewState: new state of the TimeStamp. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_TimeStampCmd(uint32_t RTC_TimeStampEdge, FunctionalState NewState) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_TIMESTAMP_EDGE(RTC_TimeStampEdge)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Get the RTC_CR register and clear the bits to be configured */ + tmpreg = (uint32_t)(RTC->CR & (uint32_t)~(RTC_CR_TSEDGE | RTC_CR_TSE)); + + /* Get the new configuration */ + if (NewState != DISABLE) + { + tmpreg |= (uint32_t)(RTC_TimeStampEdge | RTC_CR_TSE); + } + else + { + tmpreg |= (uint32_t)(RTC_TimeStampEdge); + } + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Configure the Time Stamp TSEDGE and Enable bits */ + RTC->CR = (uint32_t)tmpreg; + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Gets the RTC TimeStamp value and masks. + * @param RTC_Format: specifies the format of the output parameters. + * This parameter can be one of the following values: + * @arg RTC_Format_BIN: Binary data format + * @arg RTC_Format_BCD: BCD data format + * @param RTC_StampTimeStruct: pointer to a RTC_TimeTypeDef structure that will + * contains the TimeStamp time values. + * @param RTC_StampDateStruct: pointer to a RTC_DateTypeDef structure that will + * contains the TimeStamp date values. + * @retval None + */ +void RTC_GetTimeStamp(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_StampTimeStruct, + RTC_DateTypeDef* RTC_StampDateStruct) +{ + uint32_t tmptime = 0, tmpdate = 0; + + /* Check the parameters */ + assert_param(IS_RTC_FORMAT(RTC_Format)); + + /* Get the TimeStamp time and date registers values */ + tmptime = (uint32_t)(RTC->TSTR & RTC_TR_RESERVED_MASK); + tmpdate = (uint32_t)(RTC->TSDR & RTC_DR_RESERVED_MASK); + + /* Fill the Time structure fields with the read parameters */ + RTC_StampTimeStruct->RTC_Hours = (uint8_t)((tmptime & (RTC_TR_HT | RTC_TR_HU)) >> 16); + RTC_StampTimeStruct->RTC_Minutes = (uint8_t)((tmptime & (RTC_TR_MNT | RTC_TR_MNU)) >> 8); + RTC_StampTimeStruct->RTC_Seconds = (uint8_t)(tmptime & (RTC_TR_ST | RTC_TR_SU)); + RTC_StampTimeStruct->RTC_H12 = (uint8_t)((tmptime & (RTC_TR_PM)) >> 16); + + /* Fill the Date structure fields with the read parameters */ + RTC_StampDateStruct->RTC_Year = 0; + RTC_StampDateStruct->RTC_Month = (uint8_t)((tmpdate & (RTC_DR_MT | RTC_DR_MU)) >> 8); + RTC_StampDateStruct->RTC_Date = (uint8_t)(tmpdate & (RTC_DR_DT | RTC_DR_DU)); + RTC_StampDateStruct->RTC_WeekDay = (uint8_t)((tmpdate & (RTC_DR_WDU)) >> 13); + + /* Check the input parameters format */ + if (RTC_Format == RTC_Format_BIN) + { + /* Convert the Time structure parameters to Binary format */ + RTC_StampTimeStruct->RTC_Hours = (uint8_t)RTC_Bcd2ToByte(RTC_StampTimeStruct->RTC_Hours); + RTC_StampTimeStruct->RTC_Minutes = (uint8_t)RTC_Bcd2ToByte(RTC_StampTimeStruct->RTC_Minutes); + RTC_StampTimeStruct->RTC_Seconds = (uint8_t)RTC_Bcd2ToByte(RTC_StampTimeStruct->RTC_Seconds); + + /* Convert the Date structure parameters to Binary format */ + RTC_StampDateStruct->RTC_Month = (uint8_t)RTC_Bcd2ToByte(RTC_StampDateStruct->RTC_Month); + RTC_StampDateStruct->RTC_Date = (uint8_t)RTC_Bcd2ToByte(RTC_StampDateStruct->RTC_Date); + RTC_StampDateStruct->RTC_WeekDay = (uint8_t)RTC_Bcd2ToByte(RTC_StampDateStruct->RTC_WeekDay); + } +} + +/** + * @brief Gets the RTC timestamp Subseconds value. + * @param None + * @retval RTC current timestamp Subseconds value. + */ +uint32_t RTC_GetTimeStampSubSecond(void) +{ + /* Get timestamp subseconds values from the correspondent registers */ + return (uint32_t)(RTC->TSSSR); +} + +/** + * @} + */ + +/** @defgroup RTC_Group9 Tampers configuration functions + * @brief Tampers configuration functions + * +@verbatim + =============================================================================== + ##### Tampers configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures the select Tamper pin edge. + * @param RTC_Tamper: Selected tamper pin. + * This parameter can be any combination of the following values: + * @arg RTC_Tamper_1: Select Tamper 1. + * @arg RTC_Tamper_2: Select Tamper 2. + * @arg RTC_Tamper_3: Select Tamper 3. + * @param RTC_TamperTrigger: Specifies the trigger on the tamper pin that + * stimulates tamper event. + * This parameter can be one of the following values: + * @arg RTC_TamperTrigger_RisingEdge: Rising Edge of the tamper pin causes tamper event. + * @arg RTC_TamperTrigger_FallingEdge: Falling Edge of the tamper pin causes tamper event. + * @arg RTC_TamperTrigger_LowLevel: Low Level of the tamper pin causes tamper event. + * @arg RTC_TamperTrigger_HighLevel: High Level of the tamper pin causes tamper event. + * @retval None + */ +void RTC_TamperTriggerConfig(uint32_t RTC_Tamper, uint32_t RTC_TamperTrigger) +{ + /* Check the parameters */ + assert_param(IS_RTC_TAMPER(RTC_Tamper)); + assert_param(IS_RTC_TAMPER_TRIGGER(RTC_TamperTrigger)); + + /* Check if the active level for Tamper is rising edge (Low level)*/ + if (RTC_TamperTrigger == RTC_TamperTrigger_RisingEdge) + { + /* Configure the RTC_TAFCR register */ + RTC->TAFCR &= (uint32_t)((uint32_t)~(RTC_Tamper << 1)); + } + else + { + /* Configure the RTC_TAFCR register */ + RTC->TAFCR |= (uint32_t)(RTC_Tamper << 1); + } +} + +/** + * @brief Enables or Disables the Tamper detection. + * @param RTC_Tamper: Selected tamper pin. + * This parameter can be any combination of the following values: + * @arg RTC_Tamper_1: Select Tamper 1. + * @arg RTC_Tamper_2: Select Tamper 2. + * @arg RTC_Tamper_3: Select Tamper 3. + * @param NewState: new state of the tamper pin. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_TamperCmd(uint32_t RTC_Tamper, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RTC_TAMPER(RTC_Tamper)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected Tamper pin */ + RTC->TAFCR |= (uint32_t)RTC_Tamper; + } + else + { + /* Disable the selected Tamper pin */ + RTC->TAFCR &= (uint32_t)~RTC_Tamper; + } +} + +/** + * @brief Configures the Tampers Filter. + * @param RTC_TamperFilter: Specifies the tampers filter. + * This parameter can be one of the following values: + * @arg RTC_TamperFilter_Disable: Tamper filter is disabled. + * @arg RTC_TamperFilter_2Sample: Tamper is activated after 2 consecutive + * samples at the active level + * @arg RTC_TamperFilter_4Sample: Tamper is activated after 4 consecutive + * samples at the active level + * @arg RTC_TamperFilter_8Sample: Tamper is activated after 8 consecutive + * samples at the active level + * @retval None + */ +void RTC_TamperFilterConfig(uint32_t RTC_TamperFilter) +{ + /* Check the parameters */ + assert_param(IS_RTC_TAMPER_FILTER(RTC_TamperFilter)); + + /* Clear TAMPFLT[1:0] bits in the RTC_TAFCR register */ + RTC->TAFCR &= (uint32_t)~(RTC_TAFCR_TAMPFLT); + + /* Configure the RTC_TAFCR register */ + RTC->TAFCR |= (uint32_t)RTC_TamperFilter; +} + +/** + * @brief Configures the Tampers Sampling Frequency. + * @param RTC_TamperSamplingFreq: Specifies the tampers Sampling Frequency. + * This parameter can be one of the following values: + * @arg RTC_TamperSamplingFreq_RTCCLK_Div32768: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 32768 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div16384: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 16384 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div8192: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 8192 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div4096: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 4096 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div2048: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 2048 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div1024: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 1024 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div512: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 512 + * @arg RTC_TamperSamplingFreq_RTCCLK_Div256: Each of the tamper inputs are sampled + * with a frequency = RTCCLK / 256 + * @retval None + */ +void RTC_TamperSamplingFreqConfig(uint32_t RTC_TamperSamplingFreq) +{ + /* Check the parameters */ + assert_param(IS_RTC_TAMPER_SAMPLING_FREQ(RTC_TamperSamplingFreq)); + + /* Clear TAMPFREQ[2:0] bits in the RTC_TAFCR register */ + RTC->TAFCR &= (uint32_t)~(RTC_TAFCR_TAMPFREQ); + + /* Configure the RTC_TAFCR register */ + RTC->TAFCR |= (uint32_t)RTC_TamperSamplingFreq; +} + +/** + * @brief Configures the Tampers Pins input Precharge Duration. + * @param RTC_TamperPrechargeDuration: Specifies the Tampers Pins input + * Precharge Duration. + * This parameter can be one of the following values: + * @arg RTC_TamperPrechargeDuration_1RTCCLK: Tamper pins are pre-charged before sampling during 1 RTCCLK cycle + * @arg RTC_TamperPrechargeDuration_2RTCCLK: Tamper pins are pre-charged before sampling during 2 RTCCLK cycle + * @arg RTC_TamperPrechargeDuration_4RTCCLK: Tamper pins are pre-charged before sampling during 4 RTCCLK cycle + * @arg RTC_TamperPrechargeDuration_8RTCCLK: Tamper pins are pre-charged before sampling during 8 RTCCLK cycle + * @retval None + */ +void RTC_TamperPinsPrechargeDuration(uint32_t RTC_TamperPrechargeDuration) +{ + /* Check the parameters */ + assert_param(IS_RTC_TAMPER_PRECHARGE_DURATION(RTC_TamperPrechargeDuration)); + + /* Clear TAMPPRCH[1:0] bits in the RTC_TAFCR register */ + RTC->TAFCR &= (uint32_t)~(RTC_TAFCR_TAMPPRCH); + + /* Configure the RTC_TAFCR register */ + RTC->TAFCR |= (uint32_t)RTC_TamperPrechargeDuration; +} + +/** + * @brief Enables or Disables the TimeStamp on Tamper Detection Event. + * @note The timestamp is valid even the TSE bit in tamper control register + * is reset. + * @param NewState: new state of the timestamp on tamper event. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_TimeStampOnTamperDetectionCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Save timestamp on tamper detection event */ + RTC->TAFCR |= (uint32_t)RTC_TAFCR_TAMPTS; + } + else + { + /* Tamper detection does not cause a timestamp to be saved */ + RTC->TAFCR &= (uint32_t)~RTC_TAFCR_TAMPTS; + } +} + +/** + * @brief Enables or Disables the Precharge of Tamper pin. + * @param NewState: new state of tamper pull up. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_TamperPullUpCmd(FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable precharge of the selected Tamper pin */ + RTC->TAFCR &= (uint32_t)~RTC_TAFCR_TAMPPUDIS; + } + else + { + /* Disable precharge of the selected Tamper pin */ + RTC->TAFCR |= (uint32_t)RTC_TAFCR_TAMPPUDIS; + } +} + +/** + * @} + */ + +/** @defgroup RTC_Group10 Backup Data Registers configuration functions + * @brief Backup Data Registers configuration functions + * +@verbatim + =============================================================================== + ##### Backup Data Registers configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Writes a data in a specified RTC Backup data register. + * @param RTC_BKP_DR: RTC Backup data Register number. + * This parameter can be: RTC_BKP_DRx where x can be from 0 to 15 to + * specify the register. + * @param Data: Data to be written in the specified RTC Backup data register. + * @retval None + */ +void RTC_WriteBackupRegister(uint32_t RTC_BKP_DR, uint32_t Data) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_RTC_BKP(RTC_BKP_DR)); + + tmp = RTC_BASE + 0x50; + tmp += (RTC_BKP_DR * 4); + + /* Write the specified register */ + *(__IO uint32_t *)tmp = (uint32_t)Data; +} + +/** + * @brief Reads data from the specified RTC Backup data Register. + * @param RTC_BKP_DR: RTC Backup data Register number. + * This parameter can be: RTC_BKP_DRx where x can be from 0 to 15 to + * specify the register. + * @retval None + */ +uint32_t RTC_ReadBackupRegister(uint32_t RTC_BKP_DR) +{ + __IO uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_RTC_BKP(RTC_BKP_DR)); + + tmp = RTC_BASE + 0x50; + tmp += (RTC_BKP_DR * 4); + + /* Read the specified register */ + return (*(__IO uint32_t *)tmp); +} + +/** + * @} + */ + +/** @defgroup RTC_Group11 Output Type Config configuration functions + * @brief Output Type Config configuration functions + * +@verbatim + =============================================================================== + ##### Output Type Config configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures the RTC Output Pin mode. + * @param RTC_OutputType: specifies the RTC Output (PC13) pin mode. + * This parameter can be one of the following values: + * @arg RTC_OutputType_OpenDrain: RTC Output (PC13) is configured in + * Open Drain mode. + * @arg RTC_OutputType_PushPull: RTC Output (PC13) is configured in + * Push Pull mode. + * @retval None + */ +void RTC_OutputTypeConfig(uint32_t RTC_OutputType) +{ + /* Check the parameters */ + assert_param(IS_RTC_OUTPUT_TYPE(RTC_OutputType)); + + RTC->TAFCR &= (uint32_t)~(RTC_TAFCR_ALARMOUTTYPE); + RTC->TAFCR |= (uint32_t)(RTC_OutputType); +} + +/** + * @} + */ + +/** @defgroup RTC_Group12 Shift control synchronisation functions + * @brief Shift control synchronisation functions + * +@verbatim + =============================================================================== + ##### Shift control synchronisation functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures the Synchronization Shift Control Settings. + * @note When REFCKON is set, firmware must not write to Shift control register + * @param RTC_ShiftAdd1S : Select to add or not 1 second to the time Calendar. + * This parameter can be one of the following values : + * @arg RTC_ShiftAdd1S_Set : Add one second to the clock calendar. + * @arg RTC_ShiftAdd1S_Reset: No effect. + * @param RTC_ShiftSubFS: Select the number of Second Fractions to Substitute. + * This parameter can be one any value from 0 to 0x7FFF. + * @retval An ErrorStatus enumeration value: + * - SUCCESS: RTC Shift registers are configured + * - ERROR: RTC Shift registers are not configured +*/ +ErrorStatus RTC_SynchroShiftConfig(uint32_t RTC_ShiftAdd1S, uint32_t RTC_ShiftSubFS) +{ + ErrorStatus status = ERROR; + uint32_t shpfcount = 0; + + /* Check the parameters */ + assert_param(IS_RTC_SHIFT_ADD1S(RTC_ShiftAdd1S)); + assert_param(IS_RTC_SHIFT_SUBFS(RTC_ShiftSubFS)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + /* Check if a Shift is pending*/ + if ((RTC->ISR & RTC_ISR_SHPF) != RESET) + { + /* Wait until the shift is completed*/ + while (((RTC->ISR & RTC_ISR_SHPF) != RESET) && (shpfcount != SHPF_TIMEOUT)) + { + shpfcount++; + } + } + + /* Check if the Shift pending is completed or if there is no Shift operation at all*/ + if ((RTC->ISR & RTC_ISR_SHPF) == RESET) + { + /* check if the reference clock detection is disabled */ + if((RTC->CR & RTC_CR_REFCKON) == RESET) + { + /* Configure the Shift settings */ + RTC->SHIFTR = (uint32_t)(uint32_t)(RTC_ShiftSubFS) | (uint32_t)(RTC_ShiftAdd1S); + + if(RTC_WaitForSynchro() == ERROR) + { + status = ERROR; + } + else + { + status = SUCCESS; + } + } + else + { + status = ERROR; + } + } + else + { + status = ERROR; + } + + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; + + return (ErrorStatus)(status); +} + +/** + * @} + */ + +/** @defgroup RTC_Group13 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] All RTC interrupts are connected to the EXTI controller. + (+) To enable the RTC Alarm interrupt, the following sequence is required: + (++) Configure and enable the EXTI Line 17 in interrupt mode and select + the rising edge sensitivity using the EXTI_Init() function. + (++) Configure and enable the RTC_Alarm IRQ channel in the NVIC using + the NVIC_Init() function. + (++) Configure the RTC to generate RTC alarms (Alarm A and/or Alarm B) + using the RTC_SetAlarm() and RTC_AlarmCmd() functions. + (+) To enable the RTC Wakeup interrupt, the following sequence is required: + (++) Configure and enable the EXTI Line 20 in interrupt mode and select + the rising edge sensitivity using the EXTI_Init() function. + (++) Configure and enable the RTC_WKUP IRQ channel in the NVIC using + the NVIC_Init() function. + (++) Configure the RTC to generate the RTC wakeup timer event using the + RTC_WakeUpClockConfig(), RTC_SetWakeUpCounter() and RTC_WakeUpCmd() + functions. + (+) To enable the RTC Tamper interrupt, the following sequence is required: + (++) Configure and enable the EXTI Line 19 in interrupt mode and select + the rising edge sensitivity using the EXTI_Init() function. + (++) Configure and enable the TAMP_STAMP IRQ channel in the NVIC using + the NVIC_Init() function. + (++) Configure the RTC to detect the RTC tamper event using the + RTC_TamperTriggerConfig() and RTC_TamperCmd() functions. + (+) To enable the RTC TimeStamp interrupt, the following sequence is required: + (++) Configure and enable the EXTI Line 19 in interrupt mode and select + the rising edge sensitivity using the EXTI_Init() function. + (++) Configure and enable the TAMP_STAMP IRQ channel in the NVIC using + the NVIC_Init() function. + (++) Configure the RTC to detect the RTC time-stamp event using the + RTC_TimeStampCmd() functions. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified RTC interrupts. + * @param RTC_IT: specifies the RTC interrupt sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg RTC_IT_TS: Time Stamp interrupt mask + * @arg RTC_IT_WUT: WakeUp Timer interrupt mask + * @arg RTC_IT_ALRB: Alarm B interrupt mask + * @arg RTC_IT_ALRA: Alarm A interrupt mask + * @arg RTC_IT_TAMP: Tamper event interrupt mask + * @param NewState: new state of the specified RTC interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void RTC_ITConfig(uint32_t RTC_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_RTC_CONFIG_IT(RTC_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Disable the write protection for RTC registers */ + RTC->WPR = 0xCA; + RTC->WPR = 0x53; + + if (NewState != DISABLE) + { + /* Configure the Interrupts in the RTC_CR register */ + RTC->CR |= (uint32_t)(RTC_IT & ~RTC_TAFCR_TAMPIE); + /* Configure the Tamper Interrupt in the RTC_TAFCR */ + RTC->TAFCR |= (uint32_t)(RTC_IT & RTC_TAFCR_TAMPIE); + } + else + { + /* Configure the Interrupts in the RTC_CR register */ + RTC->CR &= (uint32_t)~(RTC_IT & (uint32_t)~RTC_TAFCR_TAMPIE); + /* Configure the Tamper Interrupt in the RTC_TAFCR */ + RTC->TAFCR &= (uint32_t)~(RTC_IT & RTC_TAFCR_TAMPIE); + } + /* Enable the write protection for RTC registers */ + RTC->WPR = 0xFF; +} + +/** + * @brief Checks whether the specified RTC flag is set or not. + * @param RTC_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg RTC_FLAG_RECALPF: RECALPF event flag + * @arg RTC_FLAG_TAMP3F: Tamper 3 event flag + * @arg RTC_FLAG_TAMP2F: Tamper 2 event flag + * @arg RTC_FLAG_TAMP1F: Tamper 1 event flag + * @arg RTC_FLAG_TSOVF: Time Stamp OverFlow flag + * @arg RTC_FLAG_TSF: Time Stamp event flag + * @arg RTC_FLAG_WUTF: WakeUp Timer flag + * @arg RTC_FLAG_ALRBF: Alarm B flag + * @arg RTC_FLAG_ALRAF: Alarm A flag + * @arg RTC_FLAG_INITF: Initialization mode flag + * @arg RTC_FLAG_RSF: Registers Synchronized flag + * @arg RTC_FLAG_INITS: Registers Configured flag + * @argRTC_FLAG_SHPF : Shift operation pending flag. + * @arg RTC_FLAG_WUTWF: WakeUp Timer Write flag + * @arg RTC_FLAG_ALRBWF: Alarm B Write flag + * @arg RTC_FLAG_ALRAWF: Alarm A write flag + * @retval The new state of RTC_FLAG (SET or RESET). + */ +FlagStatus RTC_GetFlagStatus(uint32_t RTC_FLAG) +{ + FlagStatus bitstatus = RESET; + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_GET_FLAG(RTC_FLAG)); + + /* Get all the flags */ + tmpreg = (uint32_t)(RTC->ISR & RTC_FLAGS_MASK); + + /* Return the status of the flag */ + if ((tmpreg & RTC_FLAG) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the RTC's pending flags. + * @param RTC_FLAG: specifies the RTC flag to clear. + * This parameter can be any combination of the following values: + * @arg RTC_FLAG_TAMP3F: Tamper 3 event flag + * @arg RTC_FLAG_TAMP2F: Tamper 2 event flag + * @arg RTC_FLAG_TAMP1F: Tamper 1 event flag + * @arg RTC_FLAG_TSOVF: Time Stamp Overflow flag + * @arg RTC_FLAG_TSF: Time Stamp event flag + * @arg RTC_FLAG_WUTF: WakeUp Timer flag + * @arg RTC_FLAG_ALRBF: Alarm B flag + * @arg RTC_FLAG_ALRAF: Alarm A flag + * @arg RTC_FLAG_RSF: Registers Synchronized flag + * @retval None + */ +void RTC_ClearFlag(uint32_t RTC_FLAG) +{ + /* Check the parameters */ + assert_param(IS_RTC_CLEAR_FLAG(RTC_FLAG)); + + /* Clear the Flags in the RTC_ISR register */ + RTC->ISR = (uint32_t)((uint32_t)(~((RTC_FLAG | RTC_ISR_INIT)& 0x0001FFFF) | (uint32_t)(RTC->ISR & RTC_ISR_INIT))); +} + +/** + * @brief Checks whether the specified RTC interrupt has occurred or not. + * @param RTC_IT: specifies the RTC interrupt source to check. + * This parameter can be one of the following values: + * @arg RTC_IT_TS: Time Stamp interrupt + * @arg RTC_IT_WUT: WakeUp Timer interrupt + * @arg RTC_IT_ALRB: Alarm B interrupt + * @arg RTC_IT_ALRA: Alarm A interrupt + * @arg RTC_IT_TAMP1: Tamper1 event interrupt + * @arg RTC_IT_TAMP2: Tamper2 event interrupt + * @arg RTC_IT_TAMP3: Tamper3 event interrupt + * @retval The new state of RTC_IT (SET or RESET). + */ +ITStatus RTC_GetITStatus(uint32_t RTC_IT) +{ + ITStatus bitstatus = RESET; + uint32_t tmpreg = 0, enablestatus = 0; + + /* Check the parameters */ + assert_param(IS_RTC_GET_IT(RTC_IT)); + + /* Get the TAMPER Interrupt enable bit and pending bit */ + tmpreg = (uint32_t)(RTC->TAFCR & (RTC_TAFCR_TAMPIE)); + + /* Get the Interrupt enable Status */ + enablestatus = (uint32_t)((RTC->CR & RTC_IT) | (tmpreg & ((RTC_IT >> (RTC_IT >> 18)) >> 15))); + + /* Get the Interrupt pending bit */ + tmpreg = (uint32_t)((RTC->ISR & (uint32_t)(RTC_IT >> 4))); + + /* Get the status of the Interrupt */ + if ((enablestatus != (uint32_t)RESET) && ((tmpreg & 0x0000FFFF) != (uint32_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the RTC's interrupt pending bits. + * @param RTC_IT: specifies the RTC interrupt pending bit to clear. + * This parameter can be any combination of the following values: + * @arg RTC_IT_TS: Time Stamp interrupt + * @arg RTC_IT_WUT: WakeUp Timer interrupt + * @arg RTC_IT_ALRB: Alarm B interrupt + * @arg RTC_IT_ALRA: Alarm A interrupt + * @arg RTC_IT_TAMP1: Tamper1 event interrupt + * @arg RTC_IT_TAMP2: Tamper2 event interrupt + * @arg RTC_IT_TAMP3: Tamper3 event interrupt + * @retval None + */ +void RTC_ClearITPendingBit(uint32_t RTC_IT) +{ + uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_RTC_CLEAR_IT(RTC_IT)); + + /* Get the RTC_ISR Interrupt pending bits mask */ + tmpreg = (uint32_t)(RTC_IT >> 4); + + /* Clear the interrupt pending bits in the RTC_ISR register */ + RTC->ISR = (uint32_t)((uint32_t)(~((tmpreg | RTC_ISR_INIT)& 0x0000FFFF) | (uint32_t)(RTC->ISR & RTC_ISR_INIT))); +} + +/** + * @} + */ + +/** + * @brief Converts a 2 digit decimal to BCD format. + * @param Value: Byte to be converted. + * @retval Converted byte + */ +static uint8_t RTC_ByteToBcd2(uint8_t Value) +{ + uint8_t bcdhigh = 0; + + while (Value >= 10) + { + bcdhigh++; + Value -= 10; + } + + return ((uint8_t)(bcdhigh << 4) | Value); +} + +/** + * @brief Convert from 2 digit BCD to Binary. + * @param Value: BCD value to be converted. + * @retval Converted word + */ +static uint8_t RTC_Bcd2ToByte(uint8_t Value) +{ + uint8_t tmp = 0; + tmp = ((uint8_t)(Value & (uint8_t)0xF0) >> (uint8_t)0x4) * 10; + return (tmp + (Value & (uint8_t)0x0F)); +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_spi.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_spi.c new file mode 100755 index 0000000000..279324dc23 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_spi.c @@ -0,0 +1,1417 @@ +/** + ****************************************************************************** + * @file stm32f30x_spi.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Serial peripheral interface (SPI): + * + Initialization and Configuration + * + Data transfers functions + * + Hardware CRC Calculation + * + DMA transfers management + * + Interrupts and flags management + * + * @verbatim + + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] + (#) Enable peripheral clock using RCC_APBPeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE) + function for SPI1 or using RCC_APBPeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE) + function for SPI2. + (#) Enable SCK, MOSI, MISO and NSS GPIO clocks using RCC_AHBPeriphClockCmd() + function. + (#) Peripherals alternate function: + (++) Connect the pin to the desired peripherals' Alternate + Function (AF) using GPIO_PinAFConfig() function. + (++) Configure the desired pin in alternate function by: + GPIO_InitStruct->GPIO_Mode = GPIO_Mode_AF. + (++) Select the type, pull-up/pull-down and output speed via + GPIO_PuPd, GPIO_OType and GPIO_Speed members. + (++) Call GPIO_Init() function. + (#) Program the Polarity, Phase, First Data, Baud Rate Prescaler, Slave + Management, Peripheral Mode and CRC Polynomial values using the SPI_Init() + function in SPI mode. In I2S mode, program the Mode, Standard, Data Format, + MCLK Output, Audio frequency and Polarity using I2S_Init() function. + (#) Configure the FIFO threshold using SPI_RxFIFOThresholdConfig() to select + at which threshold the RXNE event is generated. + (#) Enable the NVIC and the corresponding interrupt using the function + SPI_I2S_ITConfig() if you need to use interrupt mode. + (#) When using the DMA mode + (++) Configure the DMA using DMA_Init() function. + (++) Active the needed channel Request using SPI_I2S_DMACmd() function. + (#) Enable the SPI using the SPI_Cmd() function or enable the I2S using + I2S_Cmd(). + (#) Enable the DMA using the DMA_Cmd() function when using DMA mode. + (#) Optionally you can enable/configure the following parameters without + re-initialization (i.e there is no need to call again SPI_Init() function): + (++) When bidirectional mode (SPI_Direction_1Line_Rx or SPI_Direction_1Line_Tx) + is programmed as Data direction parameter using the SPI_Init() function + it can be possible to switch between SPI_Direction_Tx or SPI_Direction_Rx + using the SPI_BiDirectionalLineConfig() function. + (++) When SPI_NSS_Soft is selected as Slave Select Management parameter + using the SPI_Init() function it can be possible to manage the + NSS internal signal using the SPI_NSSInternalSoftwareConfig() function. + (++) Reconfigure the data size using the SPI_DataSizeConfig() function. + (++) Enable or disable the SS output using the SPI_SSOutputCmd() function. + (#) To use the CRC Hardware calculation feature refer to the Peripheral + CRC hardware Calculation subsection. + [..] It is possible to use SPI in I2S full duplex mode, in this case, each SPI + peripheral is able to manage sending and receiving data simultaneously + using two data lines. Each SPI peripheral has an extended block called I2Sxext + (ie. I2S2ext for SPI2 and I2S3ext for SPI3). + The extension block is not a full SPI IP, it is used only as I2S slave to + implement full duplex mode. The extension block uses the same clock sources + as its master. + To configure I2S full duplex you have to: + (#) Configure SPIx in I2S mode (I2S_Init() function) as described above. + (#) Call the I2S_FullDuplexConfig() function using the same strucutre passed to + I2S_Init() function. + (#) Call I2S_Cmd() for SPIx then for its extended block. + (#) Configure interrupts or DMA requests and to get/clear flag status, + use I2Sxext instance for the extension block. + [..] Functions that can be called with I2Sxext instances are: + I2S_Cmd(), I2S_FullDuplexConfig(), SPI_I2S_ReceiveData16(), SPI_I2S_SendData16(), + SPI_I2S_DMACmd(), SPI_I2S_ITConfig(), SPI_I2S_GetFlagStatus(), SPI_I2S_ClearFlag(), + SPI_I2S_GetITStatus() and SPI_I2S_ClearITPendingBit(). + [..] Example: To use SPI3 in Full duplex mode (SPI3 is Master Tx, I2S3ext is Slave Rx): + [..] RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); + I2S_StructInit(&I2SInitStruct); + I2SInitStruct.Mode = I2S_Mode_MasterTx; + I2S_Init(SPI3, &I2SInitStruct); + I2S_FullDuplexConfig(SPI3ext, &I2SInitStruct) + I2S_Cmd(SPI3, ENABLE); + I2S_Cmd(SPI3ext, ENABLE); + ... + while (SPI_I2S_GetFlagStatus(SPI2, SPI_FLAG_TXE) == RESET) + {} + SPI_I2S_SendData16(SPI3, txdata[i]); + ... + while (SPI_I2S_GetFlagStatus(I2S3ext, SPI_FLAG_RXNE) == RESET) + {} + rxdata[i] = SPI_I2S_ReceiveData16(I2S3ext); + ... + [..] + (@) In SPI mode: To use the SPI TI mode, call the function SPI_TIModeCmd() + just after calling the function SPI_Init(). + + @endverbatim + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_spi.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup SPI + * @brief SPI driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* SPI registers Masks */ +#define CR1_CLEAR_MASK ((uint16_t)0x3040) +#define CR2_LDMA_MASK ((uint16_t)0x9FFF) + +#define I2SCFGR_CLEAR_MASK ((uint16_t)0xF040) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup SPI_Private_Functions + * @{ + */ + +/** @defgroup SPI_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + [..] This section provides a set of functions allowing to initialize the SPI Direction, + SPI Mode, SPI Data Size, SPI Polarity, SPI Phase, SPI NSS Management, SPI Baud + Rate Prescaler, SPI First Bit and SPI CRC Polynomial. + [..] The SPI_Init() function follows the SPI configuration procedures for Master mode + and Slave mode (details for these procedures are available in reference manual). + [..] When the Software NSS management (SPI_InitStruct->SPI_NSS = SPI_NSS_Soft) is selected, + use the following function to manage the NSS bit: + void SPI_NSSInternalSoftwareConfig(SPI_TypeDef* SPIx, uint16_t SPI_NSSInternalSoft); + [..] In Master mode, when the Hardware NSS management (SPI_InitStruct->SPI_NSS = SPI_NSS_Hard) + is selected, use the following function to enable the NSS output feature. + void SPI_SSOutputCmd(SPI_TypeDef* SPIx, FunctionalState NewState); + [..] The NSS pulse mode can be managed by the SPI TI mode when enabling it using the + following function: void SPI_TIModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState); + And it can be managed by software in the SPI Motorola mode using this function: + void SPI_NSSPulseModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState); + [..] This section provides also functions to initialize the I2S Mode, Standard, + Data Format, MCLK Output, Audio frequency and Polarity. + [..] The I2S_Init() function follows the I2S configuration procedures for Master mode + and Slave mode. + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the SPIx peripheral registers to their default + * reset values. + * @param SPIx: To select the SPIx peripheral, where x can be: 1, 2 or 3 + * in SPI mode. + * @retval None + */ +void SPI_I2S_DeInit(SPI_TypeDef* SPIx) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + + if (SPIx == SPI1) + { + /* Enable SPI1 reset state */ + RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI1, ENABLE); + /* Release SPI1 from reset state */ + RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI1, DISABLE); + } + else if (SPIx == SPI2) + { + /* Enable SPI2 reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_SPI2, ENABLE); + /* Release SPI2 from reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_SPI2, DISABLE); + } + else if (SPIx == SPI3) + { + /* Enable SPI3 reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_SPI3, ENABLE); + /* Release SPI3 from reset state */ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_SPI3, DISABLE); + } + else + { + if (SPIx == SPI4) + { + /* Enable SPI4 reset state */ + RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI4, ENABLE); + /* Release SPI4 from reset state */ + RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI4, DISABLE); + } + } +} + +/** + * @brief Fills each SPI_InitStruct member with its default value. + * @param SPI_InitStruct: pointer to a SPI_InitTypeDef structure which will be initialized. + * @retval None + */ +void SPI_StructInit(SPI_InitTypeDef* SPI_InitStruct) +{ +/*--------------- Reset SPI init structure parameters values -----------------*/ + /* Initialize the SPI_Direction member */ + SPI_InitStruct->SPI_Direction = SPI_Direction_2Lines_FullDuplex; + /* Initialize the SPI_Mode member */ + SPI_InitStruct->SPI_Mode = SPI_Mode_Slave; + /* Initialize the SPI_DataSize member */ + SPI_InitStruct->SPI_DataSize = SPI_DataSize_8b; + /* Initialize the SPI_CPOL member */ + SPI_InitStruct->SPI_CPOL = SPI_CPOL_Low; + /* Initialize the SPI_CPHA member */ + SPI_InitStruct->SPI_CPHA = SPI_CPHA_1Edge; + /* Initialize the SPI_NSS member */ + SPI_InitStruct->SPI_NSS = SPI_NSS_Hard; + /* Initialize the SPI_BaudRatePrescaler member */ + SPI_InitStruct->SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2; + /* Initialize the SPI_FirstBit member */ + SPI_InitStruct->SPI_FirstBit = SPI_FirstBit_MSB; + /* Initialize the SPI_CRCPolynomial member */ + SPI_InitStruct->SPI_CRCPolynomial = 7; +} + +/** + * @brief Initializes the SPIx peripheral according to the specified + * parameters in the SPI_InitStruct. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_InitStruct: pointer to a SPI_InitTypeDef structure that + * contains the configuration information for the specified SPI peripheral. + * @retval None + */ +void SPI_Init(SPI_TypeDef* SPIx, SPI_InitTypeDef* SPI_InitStruct) +{ + uint16_t tmpreg = 0; + + /* check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + + /* Check the SPI parameters */ + assert_param(IS_SPI_DIRECTION_MODE(SPI_InitStruct->SPI_Direction)); + assert_param(IS_SPI_MODE(SPI_InitStruct->SPI_Mode)); + assert_param(IS_SPI_DATA_SIZE(SPI_InitStruct->SPI_DataSize)); + assert_param(IS_SPI_CPOL(SPI_InitStruct->SPI_CPOL)); + assert_param(IS_SPI_CPHA(SPI_InitStruct->SPI_CPHA)); + assert_param(IS_SPI_NSS(SPI_InitStruct->SPI_NSS)); + assert_param(IS_SPI_BAUDRATE_PRESCALER(SPI_InitStruct->SPI_BaudRatePrescaler)); + assert_param(IS_SPI_FIRST_BIT(SPI_InitStruct->SPI_FirstBit)); + assert_param(IS_SPI_CRC_POLYNOMIAL(SPI_InitStruct->SPI_CRCPolynomial)); + + /* Configuring the SPI in master mode */ + if(SPI_InitStruct->SPI_Mode == SPI_Mode_Master) + { +/*---------------------------- SPIx CR1 Configuration ------------------------*/ + /* Get the SPIx CR1 value */ + tmpreg = SPIx->CR1; + /* Clear BIDIMode, BIDIOE, RxONLY, SSM, SSI, LSBFirst, BR, MSTR, CPOL and CPHA bits */ + tmpreg &= CR1_CLEAR_MASK; + /* Configure SPIx: direction, NSS management, first transmitted bit, BaudRate prescaler + master/slave mode, CPOL and CPHA */ + /* Set BIDImode, BIDIOE and RxONLY bits according to SPI_Direction value */ + /* Set SSM, SSI and MSTR bits according to SPI_Mode and SPI_NSS values */ + /* Set LSBFirst bit according to SPI_FirstBit value */ + /* Set BR bits according to SPI_BaudRatePrescaler value */ + /* Set CPOL bit according to SPI_CPOL value */ + /* Set CPHA bit according to SPI_CPHA value */ + tmpreg |= (uint16_t)((uint16_t)(SPI_InitStruct->SPI_Direction | SPI_InitStruct->SPI_Mode) | + (uint16_t)((uint16_t)(SPI_InitStruct->SPI_CPOL | SPI_InitStruct->SPI_CPHA) | + (uint16_t)((uint16_t)(SPI_InitStruct->SPI_NSS | SPI_InitStruct->SPI_BaudRatePrescaler) | + SPI_InitStruct->SPI_FirstBit))); + /* Write to SPIx CR1 */ + SPIx->CR1 = tmpreg; + /*-------------------------Data Size Configuration -----------------------*/ + /* Get the SPIx CR2 value */ + tmpreg = SPIx->CR2; + /* Clear DS[3:0] bits */ + tmpreg &= (uint16_t)~SPI_CR2_DS; + /* Configure SPIx: Data Size */ + tmpreg |= (uint16_t)(SPI_InitStruct->SPI_DataSize); + /* Write to SPIx CR2 */ + SPIx->CR2 = tmpreg; + } + /* Configuring the SPI in slave mode */ + else + { +/*---------------------------- Data size Configuration -----------------------*/ + /* Get the SPIx CR2 value */ + tmpreg = SPIx->CR2; + /* Clear DS[3:0] bits */ + tmpreg &= (uint16_t)~SPI_CR2_DS; + /* Configure SPIx: Data Size */ + tmpreg |= (uint16_t)(SPI_InitStruct->SPI_DataSize); + /* Write to SPIx CR2 */ + SPIx->CR2 = tmpreg; +/*---------------------------- SPIx CR1 Configuration ------------------------*/ + /* Get the SPIx CR1 value */ + tmpreg = SPIx->CR1; + /* Clear BIDIMode, BIDIOE, RxONLY, SSM, SSI, LSBFirst, BR, MSTR, CPOL and CPHA bits */ + tmpreg &= CR1_CLEAR_MASK; + /* Configure SPIx: direction, NSS management, first transmitted bit, BaudRate prescaler + master/salve mode, CPOL and CPHA */ + /* Set BIDImode, BIDIOE and RxONLY bits according to SPI_Direction value */ + /* Set SSM, SSI and MSTR bits according to SPI_Mode and SPI_NSS values */ + /* Set LSBFirst bit according to SPI_FirstBit value */ + /* Set BR bits according to SPI_BaudRatePrescaler value */ + /* Set CPOL bit according to SPI_CPOL value */ + /* Set CPHA bit according to SPI_CPHA value */ + tmpreg |= (uint16_t)((uint16_t)(SPI_InitStruct->SPI_Direction | SPI_InitStruct->SPI_Mode) | + (uint16_t)((uint16_t)(SPI_InitStruct->SPI_CPOL | SPI_InitStruct->SPI_CPHA) | + (uint16_t)((uint16_t)(SPI_InitStruct->SPI_NSS | SPI_InitStruct->SPI_BaudRatePrescaler) | + SPI_InitStruct->SPI_FirstBit))); + + /* Write to SPIx CR1 */ + SPIx->CR1 = tmpreg; + } + + /* Activate the SPI mode (Reset I2SMOD bit in I2SCFGR register) */ + SPIx->I2SCFGR &= (uint16_t)~((uint16_t)SPI_I2SCFGR_I2SMOD); + +/*---------------------------- SPIx CRCPOLY Configuration --------------------*/ + /* Write to SPIx CRCPOLY */ + SPIx->CRCPR = SPI_InitStruct->SPI_CRCPolynomial; +} + +/** + * @brief Fills each I2S_InitStruct member with its default value. + * @param I2S_InitStruct : pointer to a I2S_InitTypeDef structure which will be initialized. + * @retval None + */ +void I2S_StructInit(I2S_InitTypeDef* I2S_InitStruct) +{ +/*--------------- Reset I2S init structure parameters values -----------------*/ + /* Initialize the I2S_Mode member */ + I2S_InitStruct->I2S_Mode = I2S_Mode_SlaveTx; + + /* Initialize the I2S_Standard member */ + I2S_InitStruct->I2S_Standard = I2S_Standard_Phillips; + + /* Initialize the I2S_DataFormat member */ + I2S_InitStruct->I2S_DataFormat = I2S_DataFormat_16b; + + /* Initialize the I2S_MCLKOutput member */ + I2S_InitStruct->I2S_MCLKOutput = I2S_MCLKOutput_Disable; + + /* Initialize the I2S_AudioFreq member */ + I2S_InitStruct->I2S_AudioFreq = I2S_AudioFreq_Default; + + /* Initialize the I2S_CPOL member */ + I2S_InitStruct->I2S_CPOL = I2S_CPOL_Low; +} + +/** + * @brief Initializes the SPIx peripheral according to the specified + * parameters in the I2S_InitStruct. + * @param SPIx:To select the SPIx peripheral, where x can be: 2 or 3 + * in I2S mode. + * @param I2S_InitStruct: pointer to an I2S_InitTypeDef structure that + * contains the configuration information for the specified SPI peripheral + * configured in I2S mode. + * @note + * The function calculates the optimal prescaler needed to obtain the most + * accurate audio frequency (depending on the I2S clock source, the PLL values + * and the product configuration). But in case the prescaler value is greater + * than 511, the default value (0x02) will be configured instead. + * @retval None + */ +void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct) +{ + uint16_t tmpreg = 0, i2sdiv = 2, i2sodd = 0, packetlength = 1; + uint32_t tmp = 0; + RCC_ClocksTypeDef RCC_Clocks; + uint32_t sourceclock = 0; + + /* Check the I2S parameters */ + assert_param(IS_SPI_23_PERIPH(SPIx)); + assert_param(IS_I2S_MODE(I2S_InitStruct->I2S_Mode)); + assert_param(IS_I2S_STANDARD(I2S_InitStruct->I2S_Standard)); + assert_param(IS_I2S_DATA_FORMAT(I2S_InitStruct->I2S_DataFormat)); + assert_param(IS_I2S_MCLK_OUTPUT(I2S_InitStruct->I2S_MCLKOutput)); + assert_param(IS_I2S_AUDIO_FREQ(I2S_InitStruct->I2S_AudioFreq)); + assert_param(IS_I2S_CPOL(I2S_InitStruct->I2S_CPOL)); + +/*----------------------- SPIx I2SCFGR & I2SPR Configuration -----------------*/ + /* Clear I2SMOD, I2SE, I2SCFG, PCMSYNC, I2SSTD, CKPOL, DATLEN and CHLEN bits */ + SPIx->I2SCFGR &= I2SCFGR_CLEAR_MASK; + SPIx->I2SPR = 0x0002; + + /* Get the I2SCFGR register value */ + tmpreg = SPIx->I2SCFGR; + + /* If the default value has to be written, reinitialize i2sdiv and i2sodd*/ + if(I2S_InitStruct->I2S_AudioFreq == I2S_AudioFreq_Default) + { + i2sodd = (uint16_t)0; + i2sdiv = (uint16_t)2; + } + /* If the requested audio frequency is not the default, compute the prescaler */ + else + { + /* Check the frame length (For the Prescaler computing) */ + if(I2S_InitStruct->I2S_DataFormat == I2S_DataFormat_16b) + { + /* Packet length is 16 bits */ + packetlength = 1; + } + else + { + /* Packet length is 32 bits */ + packetlength = 2; + } + + /* I2S Clock source is System clock: Get System Clock frequency */ + RCC_GetClocksFreq(&RCC_Clocks); + + /* Get the source clock value: based on System Clock value */ + sourceclock = RCC_Clocks.SYSCLK_Frequency; + + /* Compute the Real divider depending on the MCLK output state with a floating point */ + if(I2S_InitStruct->I2S_MCLKOutput == I2S_MCLKOutput_Enable) + { + /* MCLK output is enabled */ + tmp = (uint16_t)(((((sourceclock / 256) * 10) / I2S_InitStruct->I2S_AudioFreq)) + 5); + } + else + { + /* MCLK output is disabled */ + tmp = (uint16_t)(((((sourceclock / (32 * packetlength)) *10 ) / I2S_InitStruct->I2S_AudioFreq)) + 5); + } + + /* Remove the floating point */ + tmp = tmp / 10; + + /* Check the parity of the divider */ + i2sodd = (uint16_t)(tmp & (uint16_t)0x0001); + + /* Compute the i2sdiv prescaler */ + i2sdiv = (uint16_t)((tmp - i2sodd) / 2); + + /* Get the Mask for the Odd bit (SPI_I2SPR[8]) register */ + i2sodd = (uint16_t) (i2sodd << 8); + } + + /* Test if the divider is 1 or 0 or greater than 0xFF */ + if ((i2sdiv < 2) || (i2sdiv > 0xFF)) + { + /* Set the default values */ + i2sdiv = 2; + i2sodd = 0; + } + + /* Write to SPIx I2SPR register the computed value */ + SPIx->I2SPR = (uint16_t)(i2sdiv | (uint16_t)(i2sodd | (uint16_t)I2S_InitStruct->I2S_MCLKOutput)); + + /* Configure the I2S with the SPI_InitStruct values */ + tmpreg |= (uint16_t)((uint16_t)(SPI_I2SCFGR_I2SMOD | I2S_InitStruct->I2S_Mode) | \ + (uint16_t)((uint16_t)((uint16_t)(I2S_InitStruct->I2S_Standard |I2S_InitStruct->I2S_DataFormat) |\ + I2S_InitStruct->I2S_CPOL))); + + /* Write to SPIx I2SCFGR */ + SPIx->I2SCFGR = tmpreg; +} + +/** + * @brief Enables or disables the specified SPI peripheral. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param NewState: new state of the SPIx peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected SPI peripheral */ + SPIx->CR1 |= SPI_CR1_SPE; + } + else + { + /* Disable the selected SPI peripheral */ + SPIx->CR1 &= (uint16_t)~((uint16_t)SPI_CR1_SPE); + } +} + +/** + * @brief Enables or disables the TI Mode. + * @note This function can be called only after the SPI_Init() function has + * been called. + * @note When TI mode is selected, the control bits SSM, SSI, CPOL and CPHA + * are not taken into consideration and are configured by hardware + * respectively to the TI mode requirements. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param NewState: new state of the selected SPI TI communication mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_TIModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the TI mode for the selected SPI peripheral */ + SPIx->CR2 |= SPI_CR2_FRF; + } + else + { + /* Disable the TI mode for the selected SPI peripheral */ + SPIx->CR2 &= (uint16_t)~((uint16_t)SPI_CR2_FRF); + } +} + +/** + * @brief Enables or disables the specified SPI peripheral (in I2S mode). + * @param SPIx:To select the SPIx peripheral, where x can be: 2 or 3 in + * I2S mode or I2Sxext for I2S full duplex mode. + * @param NewState: new state of the SPIx peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_23_PERIPH_EXT(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected SPI peripheral in I2S mode */ + SPIx->I2SCFGR |= SPI_I2SCFGR_I2SE; + } + else + { + /* Disable the selected SPI peripheral in I2S mode */ + SPIx->I2SCFGR &= (uint16_t)~((uint16_t)SPI_I2SCFGR_I2SE); + } +} + +/** + * @brief Configures the data size for the selected SPI. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_DataSize: specifies the SPI data size. + * For the SPIx peripheral this parameter can be one of the following values: + * @arg SPI_DataSize_4b: Set data size to 4 bits + * @arg SPI_DataSize_5b: Set data size to 5 bits + * @arg SPI_DataSize_6b: Set data size to 6 bits + * @arg SPI_DataSize_7b: Set data size to 7 bits + * @arg SPI_DataSize_8b: Set data size to 8 bits + * @arg SPI_DataSize_9b: Set data size to 9 bits + * @arg SPI_DataSize_10b: Set data size to 10 bits + * @arg SPI_DataSize_11b: Set data size to 11 bits + * @arg SPI_DataSize_12b: Set data size to 12 bits + * @arg SPI_DataSize_13b: Set data size to 13 bits + * @arg SPI_DataSize_14b: Set data size to 14 bits + * @arg SPI_DataSize_15b: Set data size to 15 bits + * @arg SPI_DataSize_16b: Set data size to 16 bits + * @retval None + */ +void SPI_DataSizeConfig(SPI_TypeDef* SPIx, uint16_t SPI_DataSize) +{ + uint16_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_DATA_SIZE(SPI_DataSize)); + /* Read the CR2 register */ + tmpreg = SPIx->CR2; + /* Clear DS[3:0] bits */ + tmpreg &= (uint16_t)~SPI_CR2_DS; + /* Set new DS[3:0] bits value */ + tmpreg |= SPI_DataSize; + SPIx->CR2 = tmpreg; +} + +/** + * @brief Configures the FIFO reception threshold for the selected SPI. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_RxFIFOThreshold: specifies the FIFO reception threshold. + * This parameter can be one of the following values: + * @arg SPI_RxFIFOThreshold_HF: RXNE event is generated if the FIFO + * level is greater or equal to 1/2. + * @arg SPI_RxFIFOThreshold_QF: RXNE event is generated if the FIFO + * level is greater or equal to 1/4. + * @retval None + */ +void SPI_RxFIFOThresholdConfig(SPI_TypeDef* SPIx, uint16_t SPI_RxFIFOThreshold) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_RX_FIFO_THRESHOLD(SPI_RxFIFOThreshold)); + + /* Clear FRXTH bit */ + SPIx->CR2 &= (uint16_t)~((uint16_t)SPI_CR2_FRXTH); + + /* Set new FRXTH bit value */ + SPIx->CR2 |= SPI_RxFIFOThreshold; +} + +/** + * @brief Selects the data transfer direction in bidirectional mode for the specified SPI. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_Direction: specifies the data transfer direction in bidirectional mode. + * This parameter can be one of the following values: + * @arg SPI_Direction_Tx: Selects Tx transmission direction + * @arg SPI_Direction_Rx: Selects Rx receive direction + * @retval None + */ +void SPI_BiDirectionalLineConfig(SPI_TypeDef* SPIx, uint16_t SPI_Direction) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_DIRECTION(SPI_Direction)); + if (SPI_Direction == SPI_Direction_Tx) + { + /* Set the Tx only mode */ + SPIx->CR1 |= SPI_Direction_Tx; + } + else + { + /* Set the Rx only mode */ + SPIx->CR1 &= SPI_Direction_Rx; + } +} + +/** + * @brief Configures internally by software the NSS pin for the selected SPI. + * @note This function can be called only after the SPI_Init() function has + * been called. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_NSSInternalSoft: specifies the SPI NSS internal state. + * This parameter can be one of the following values: + * @arg SPI_NSSInternalSoft_Set: Set NSS pin internally + * @arg SPI_NSSInternalSoft_Reset: Reset NSS pin internally + * @retval None + */ +void SPI_NSSInternalSoftwareConfig(SPI_TypeDef* SPIx, uint16_t SPI_NSSInternalSoft) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_NSS_INTERNAL(SPI_NSSInternalSoft)); + + if (SPI_NSSInternalSoft != SPI_NSSInternalSoft_Reset) + { + /* Set NSS pin internally by software */ + SPIx->CR1 |= SPI_NSSInternalSoft_Set; + } + else + { + /* Reset NSS pin internally by software */ + SPIx->CR1 &= SPI_NSSInternalSoft_Reset; + } +} + +/** + * @brief Configures the full duplex mode for the I2Sx peripheral using its + * extension I2Sxext according to the specified parameters in the + * I2S_InitStruct. + * @param I2Sxext: where x can be 2 or 3 to select the I2S peripheral extension block. + * @param I2S_InitStruct: pointer to an I2S_InitTypeDef structure that + * contains the configuration information for the specified I2S peripheral + * extension. + * + * @note The structure pointed by I2S_InitStruct parameter should be the same + * used for the master I2S peripheral. In this case, if the master is + * configured as transmitter, the slave will be receiver and vice versa. + * Or you can force a different mode by modifying the field I2S_Mode to the + * value I2S_SlaveRx or I2S_SlaveTx independently of the master configuration. + * + * @note The I2S full duplex extension can be configured in slave mode only. + * + * @retval None + */ +void I2S_FullDuplexConfig(SPI_TypeDef* I2Sxext, I2S_InitTypeDef* I2S_InitStruct) +{ + uint16_t tmpreg = 0, tmp = 0; + + /* Check the I2S parameters */ + assert_param(IS_I2S_EXT_PERIPH(I2Sxext)); + assert_param(IS_I2S_MODE(I2S_InitStruct->I2S_Mode)); + assert_param(IS_I2S_STANDARD(I2S_InitStruct->I2S_Standard)); + assert_param(IS_I2S_DATA_FORMAT(I2S_InitStruct->I2S_DataFormat)); + assert_param(IS_I2S_CPOL(I2S_InitStruct->I2S_CPOL)); + +/*----------------------- SPIx I2SCFGR & I2SPR Configuration -----------------*/ + /* Clear I2SMOD, I2SE, I2SCFG, PCMSYNC, I2SSTD, CKPOL, DATLEN and CHLEN bits */ + I2Sxext->I2SCFGR &= I2SCFGR_CLEAR_MASK; + I2Sxext->I2SPR = 0x0002; + + /* Get the I2SCFGR register value */ + tmpreg = I2Sxext->I2SCFGR; + + /* Get the mode to be configured for the extended I2S */ + if ((I2S_InitStruct->I2S_Mode == I2S_Mode_MasterTx) || (I2S_InitStruct->I2S_Mode == I2S_Mode_SlaveTx)) + { + tmp = I2S_Mode_SlaveRx; + } + else + { + if ((I2S_InitStruct->I2S_Mode == I2S_Mode_MasterRx) || (I2S_InitStruct->I2S_Mode == I2S_Mode_SlaveRx)) + { + tmp = I2S_Mode_SlaveTx; + } + } + + + /* Configure the I2S with the SPI_InitStruct values */ + tmpreg |= (uint16_t)((uint16_t)SPI_I2SCFGR_I2SMOD | (uint16_t)(tmp | \ + (uint16_t)(I2S_InitStruct->I2S_Standard | (uint16_t)(I2S_InitStruct->I2S_DataFormat | \ + (uint16_t)I2S_InitStruct->I2S_CPOL)))); + + /* Write to SPIx I2SCFGR */ + I2Sxext->I2SCFGR = tmpreg; +} + +/** + * @brief Enables or disables the SS output for the selected SPI. + * @note This function can be called only after the SPI_Init() function has + * been called and the NSS hardware management mode is selected. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param NewState: new state of the SPIx SS output. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_SSOutputCmd(SPI_TypeDef* SPIx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the selected SPI SS output */ + SPIx->CR2 |= (uint16_t)SPI_CR2_SSOE; + } + else + { + /* Disable the selected SPI SS output */ + SPIx->CR2 &= (uint16_t)~((uint16_t)SPI_CR2_SSOE); + } +} + +/** + * @brief Enables or disables the NSS pulse management mode. + * @note This function can be called only after the SPI_Init() function has + * been called. + * @note When TI mode is selected, the control bits NSSP is not taken into + * consideration and are configured by hardware respectively to the + * TI mode requirements. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param NewState: new state of the NSS pulse management mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_NSSPulseModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the NSS pulse management mode */ + SPIx->CR2 |= SPI_CR2_NSSP; + } + else + { + /* Disable the NSS pulse management mode */ + SPIx->CR2 &= (uint16_t)~((uint16_t)SPI_CR2_NSSP); + } +} + +/** + * @} + */ + +/** @defgroup SPI_Group2 Data transfers functions + * @brief Data transfers functions + * +@verbatim + =============================================================================== + ##### Data transfers functions ##### + =============================================================================== + [..] This section provides a set of functions allowing to manage the SPI or I2S + data transfers. + [..] In reception, data are received and then stored into an internal Rx buffer while + In transmission, data are first stored into an internal Tx buffer before being + transmitted. + [..] The read access of the SPI_DR register can be done using the SPI_I2S_ReceiveData() + function and returns the Rx buffered value. Whereas a write access to the SPI_DR + can be done using SPI_I2S_SendData() function and stores the written data into + Tx buffer. + +@endverbatim + * @{ + */ + +/** + * @brief Transmits a Data through the SPIx peripheral. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param Data: Data to be transmitted. + * @retval None + */ +void SPI_SendData8(SPI_TypeDef* SPIx, uint8_t Data) +{ + uint32_t spixbase = 0x00; + + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + + spixbase = (uint32_t)SPIx; + spixbase += 0x0C; + + *(__IO uint8_t *) spixbase = Data; +} + +/** + * @brief Transmits a Data through the SPIx/I2Sx peripheral. + * @param SPIx: To select the SPIx/I2Sx peripheral, where x can be: 1, 2 or 3 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @param Data: Data to be transmitted. + * @retval None + */ +void SPI_I2S_SendData16(SPI_TypeDef* SPIx, uint16_t Data) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + + SPIx->DR = (uint16_t)Data; +} + +/** + * @brief Returns the most recent received data by the SPIx peripheral. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @retval The value of the received data. + */ +uint8_t SPI_ReceiveData8(SPI_TypeDef* SPIx) +{ + uint32_t spixbase = 0x00; + + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + + spixbase = (uint32_t)SPIx; + spixbase += 0x0C; + + return *(__IO uint8_t *) spixbase; +} + +/** + * @brief Returns the most recent received data by the SPIx peripheral. + * @param SPIx: To select the SPIx/I2Sx peripheral, where x can be: 1, 2, 3 or 4 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @retval The value of the received data. + */ +uint16_t SPI_I2S_ReceiveData16(SPI_TypeDef* SPIx) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + + return SPIx->DR; +} +/** + * @} + */ + +/** @defgroup SPI_Group3 Hardware CRC Calculation functions + * @brief Hardware CRC Calculation functions + * +@verbatim + =============================================================================== + ##### Hardware CRC Calculation functions ##### + =============================================================================== + [..] This section provides a set of functions allowing to manage the SPI CRC hardware + calculation. + [..] SPI communication using CRC is possible through the following procedure: + (#) Program the Data direction, Polarity, Phase, First Data, Baud Rate Prescaler, + Slave Management, Peripheral Mode and CRC Polynomial values using the SPI_Init() + function. + (#) Enable the CRC calculation using the SPI_CalculateCRC() function. + (#) Enable the SPI using the SPI_Cmd() function + (#) Before writing the last data to the TX buffer, set the CRCNext bit using the + SPI_TransmitCRC() function to indicate that after transmission of the last + data, the CRC should be transmitted. + (#) After transmitting the last data, the SPI transmits the CRC. The SPI_CR1_CRCNEXT + bit is reset. The CRC is also received and compared against the SPI_RXCRCR + value. + If the value does not match, the SPI_FLAG_CRCERR flag is set and an interrupt + can be generated when the SPI_I2S_IT_ERR interrupt is enabled. + [..] + (@) + (+@) It is advised to don't read the calculate CRC values during the communication. + (+@) When the SPI is in slave mode, be careful to enable CRC calculation only + when the clock is stable, that is, when the clock is in the steady state. + If not, a wrong CRC calculation may be done. In fact, the CRC is sensitive + to the SCK slave input clock as soon as CRCEN is set, and this, whatever + the value of the SPE bit. + (+@) With high bitrate frequencies, be careful when transmitting the CRC. + As the number of used CPU cycles has to be as low as possible in the CRC + transfer phase, it is forbidden to call software functions in the CRC + transmission sequence to avoid errors in the last data and CRC reception. + In fact, CRCNEXT bit has to be written before the end of the transmission/reception + of the last data. + (+@) For high bit rate frequencies, it is advised to use the DMA mode to avoid the + degradation of the SPI speed performance due to CPU accesses impacting the + SPI bandwidth. + (+@) When the STM32F30x are configured as slaves and the NSS hardware mode is + used, the NSS pin needs to be kept low between the data phase and the CRC + phase. + (+@) When the SPI is configured in slave mode with the CRC feature enabled, CRC + calculation takes place even if a high level is applied on the NSS pin. + This may happen for example in case of a multislave environment where the + communication master addresses slaves alternately. + (+@) Between a slave deselection (high level on NSS) and a new slave selection + (low level on NSS), the CRC value should be cleared on both master and slave + sides in order to resynchronize the master and slave for their respective + CRC calculation. + [..] + (@) To clear the CRC, follow the procedure below: + (#@) Disable SPI using the SPI_Cmd() function. + (#@) Disable the CRC calculation using the SPI_CalculateCRC() function. + (#@) Enable the CRC calculation using the SPI_CalculateCRC() function. + (#@) Enable SPI using the SPI_Cmd() function. + +@endverbatim + * @{ + */ + +/** + * @brief Configures the CRC calculation length for the selected SPI. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_CRCLength: specifies the SPI CRC calculation length. + * This parameter can be one of the following values: + * @arg SPI_CRCLength_8b: Set CRC Calculation to 8 bits + * @arg SPI_CRCLength_16b: Set CRC Calculation to 16 bits + * @retval None + */ +void SPI_CRCLengthConfig(SPI_TypeDef* SPIx, uint16_t SPI_CRCLength) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_CRC_LENGTH(SPI_CRCLength)); + + /* Clear CRCL bit */ + SPIx->CR1 &= (uint16_t)~((uint16_t)SPI_CR1_CRCL); + + /* Set new CRCL bit value */ + SPIx->CR1 |= SPI_CRCLength; +} + +/** + * @brief Enables or disables the CRC value calculation of the transferred bytes. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param NewState: new state of the SPIx CRC value calculation. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_CalculateCRC(SPI_TypeDef* SPIx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected SPI CRC calculation */ + SPIx->CR1 |= SPI_CR1_CRCEN; + } + else + { + /* Disable the selected SPI CRC calculation */ + SPIx->CR1 &= (uint16_t)~((uint16_t)SPI_CR1_CRCEN); + } +} + +/** + * @brief Transmits the SPIx CRC value. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @retval None + */ +void SPI_TransmitCRC(SPI_TypeDef* SPIx) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + + /* Enable the selected SPI CRC transmission */ + SPIx->CR1 |= SPI_CR1_CRCNEXT; +} + +/** + * @brief Returns the transmit or the receive CRC register value for the specified SPI. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_CRC: specifies the CRC register to be read. + * This parameter can be one of the following values: + * @arg SPI_CRC_Tx: Selects Tx CRC register + * @arg SPI_CRC_Rx: Selects Rx CRC register + * @retval The selected CRC register value.. + */ +uint16_t SPI_GetCRC(SPI_TypeDef* SPIx, uint8_t SPI_CRC) +{ + uint16_t crcreg = 0; + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_CRC(SPI_CRC)); + + if (SPI_CRC != SPI_CRC_Rx) + { + /* Get the Tx CRC register */ + crcreg = SPIx->TXCRCR; + } + else + { + /* Get the Rx CRC register */ + crcreg = SPIx->RXCRCR; + } + /* Return the selected CRC register */ + return crcreg; +} + +/** + * @brief Returns the CRC Polynomial register value for the specified SPI. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @retval The CRC Polynomial register value. + */ +uint16_t SPI_GetCRCPolynomial(SPI_TypeDef* SPIx) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + + /* Return the CRC polynomial register */ + return SPIx->CRCPR; +} + +/** + * @} + */ + +/** @defgroup SPI_Group4 DMA transfers management functions + * @brief DMA transfers management functions + * +@verbatim + =============================================================================== + ##### DMA transfers management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the SPIx/I2Sx DMA interface. + * @param SPIx:To select the SPIx/I2Sx peripheral, where x can be: 1, 2, 3 or 4 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @param SPI_I2S_DMAReq: specifies the SPI DMA transfer request to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg SPI_I2S_DMAReq_Tx: Tx buffer DMA transfer request + * @arg SPI_I2S_DMAReq_Rx: Rx buffer DMA transfer request + * @param NewState: new state of the selected SPI DMA transfer request. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_I2S_DMACmd(SPI_TypeDef* SPIx, uint16_t SPI_I2S_DMAReq, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_SPI_I2S_DMA_REQ(SPI_I2S_DMAReq)); + + if (NewState != DISABLE) + { + /* Enable the selected SPI DMA requests */ + SPIx->CR2 |= SPI_I2S_DMAReq; + } + else + { + /* Disable the selected SPI DMA requests */ + SPIx->CR2 &= (uint16_t)~SPI_I2S_DMAReq; + } +} + +/** + * @brief Configures the number of data to transfer type(Even/Odd) for the DMA + * last transfers and for the selected SPI. + * @note This function have a meaning only if DMA mode is selected and if + * the packing mode is used (data length <= 8 and DMA transfer size halfword) + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @param SPI_LastDMATransfer: specifies the SPI last DMA transfers state. + * This parameter can be one of the following values: + * @arg SPI_LastDMATransfer_TxEvenRxEven: Number of data for transmission Even + * and number of data for reception Even. + * @arg SPI_LastDMATransfer_TxOddRxEven: Number of data for transmission Odd + * and number of data for reception Even. + * @arg SPI_LastDMATransfer_TxEvenRxOdd: Number of data for transmission Even + * and number of data for reception Odd. + * @arg SPI_LastDMATransfer_TxOddRxOdd: RNumber of data for transmission Odd + * and number of data for reception Odd. + * @retval None + */ +void SPI_LastDMATransferCmd(SPI_TypeDef* SPIx, uint16_t SPI_LastDMATransfer) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH(SPIx)); + assert_param(IS_SPI_LAST_DMA_TRANSFER(SPI_LastDMATransfer)); + + /* Clear LDMA_TX and LDMA_RX bits */ + SPIx->CR2 &= CR2_LDMA_MASK; + + /* Set new LDMA_TX and LDMA_RX bits value */ + SPIx->CR2 |= SPI_LastDMATransfer; +} + +/** + * @} + */ + +/** @defgroup SPI_Group5 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] This section provides a set of functions allowing to configure the SPI/I2S + Interrupts sources and check or clear the flags or pending bits status. + The user should identify which mode will be used in his application to manage + the communication: Polling mode, Interrupt mode or DMA mode. + + *** Polling Mode *** + ==================== + [..] In Polling Mode, the SPI/I2S communication can be managed by 9 flags: + (#) SPI_I2S_FLAG_TXE : to indicate the status of the transmit buffer register. + (#) SPI_I2S_FLAG_RXNE : to indicate the status of the receive buffer register. + (#) SPI_I2S_FLAG_BSY : to indicate the state of the communication layer of the SPI. + (#) SPI_FLAG_CRCERR : to indicate if a CRC Calculation error occur. + (#) SPI_FLAG_MODF : to indicate if a Mode Fault error occur. + (#) SPI_I2S_FLAG_OVR : to indicate if an Overrun error occur. + (#) SPI_I2S_FLAG_FRE: to indicate a Frame Format error occurs. + (#) I2S_FLAG_UDR: to indicate an Underrun error occurs. + (#) I2S_FLAG_CHSIDE: to indicate Channel Side. + [..] + (@) Do not use the BSY flag to handle each data transmission or reception. + It is better to use the TXE and RXNE flags instead. + [..] In this Mode it is advised to use the following functions: + (+) FlagStatus SPI_I2S_GetFlagStatus(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG); + (+) void SPI_I2S_ClearFlag(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG); + + *** Interrupt Mode *** + ====================== + [..] In Interrupt Mode, the SPI/I2S communication can be managed by 3 interrupt sources + and 5 pending bits: + [..] Pending Bits: + (#) SPI_I2S_IT_TXE : to indicate the status of the transmit buffer register. + (#) SPI_I2S_IT_RXNE : to indicate the status of the receive buffer register. + (#) SPI_I2S_IT_OVR : to indicate if an Overrun error occur. + (#) I2S_IT_UDR : to indicate an Underrun Error occurs. + (#) SPI_I2S_FLAG_FRE : to indicate a Frame Format error occurs. + [..] Interrupt Source: + (#) SPI_I2S_IT_TXE: specifies the interrupt source for the Tx buffer empty + interrupt. + (#) SPI_I2S_IT_RXNE : specifies the interrupt source for the Rx buffer not + empty interrupt. + (#) SPI_I2S_IT_ERR : specifies the interrupt source for the errors interrupt. + [..] In this Mode it is advised to use the following functions: + (+) void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState); + (+) ITStatus SPI_I2S_GetITStatus(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT); + + *** FIFO Status *** + =================== + [..] It is possible to monitor the FIFO status when a transfer is ongoing using the + following function: + (+) uint32_t SPI_GetFIFOStatus(uint8_t SPI_FIFO_Direction); + + *** DMA Mode *** + ================ + [..] In DMA Mode, the SPI communication can be managed by 2 DMA Channel requests: + (#) SPI_I2S_DMAReq_Tx: specifies the Tx buffer DMA transfer request. + (#) SPI_I2S_DMAReq_Rx: specifies the Rx buffer DMA transfer request. + [..] In this Mode it is advised to use the following function: + (+) void SPI_I2S_DMACmd(SPI_TypeDef* SPIx, uint16_t SPI_I2S_DMAReq, FunctionalState NewState); + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified SPI/I2S interrupts. + * @param SPIx: To select the SPIx/I2Sx peripheral, where x can be: 1, 2, 3 or 4 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @param SPI_I2S_IT: specifies the SPI interrupt source to be enabled or disabled. + * This parameter can be one of the following values: + * @arg SPI_I2S_IT_TXE: Tx buffer empty interrupt mask + * @arg SPI_I2S_IT_RXNE: Rx buffer not empty interrupt mask + * @arg SPI_I2S_IT_ERR: Error interrupt mask + * @param NewState: new state of the specified SPI interrupt. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState) +{ + uint16_t itpos = 0, itmask = 0 ; + + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_SPI_I2S_CONFIG_IT(SPI_I2S_IT)); + + /* Get the SPI IT index */ + itpos = SPI_I2S_IT >> 4; + + /* Set the IT mask */ + itmask = (uint16_t)1 << (uint16_t)itpos; + + if (NewState != DISABLE) + { + /* Enable the selected SPI interrupt */ + SPIx->CR2 |= itmask; + } + else + { + /* Disable the selected SPI interrupt */ + SPIx->CR2 &= (uint16_t)~itmask; + } +} + +/** + * @brief Returns the current SPIx Transmission FIFO filled level. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @retval The Transmission FIFO filling state. + * - SPI_TransmissionFIFOStatus_Empty: when FIFO is empty + * - SPI_TransmissionFIFOStatus_1QuarterFull: if more than 1 quarter-full. + * - SPI_TransmissionFIFOStatus_HalfFull: if more than 1 half-full. + * - SPI_TransmissionFIFOStatus_Full: when FIFO is full. + */ +uint16_t SPI_GetTransmissionFIFOStatus(SPI_TypeDef* SPIx) +{ + /* Get the SPIx Transmission FIFO level bits */ + return (uint16_t)((SPIx->SR & SPI_SR_FTLVL)); +} + +/** + * @brief Returns the current SPIx Reception FIFO filled level. + * @param SPIx: where x can be 1, 2, 3 or 4 to select the SPI peripheral. + * @retval The Reception FIFO filling state. + * - SPI_ReceptionFIFOStatus_Empty: when FIFO is empty + * - SPI_ReceptionFIFOStatus_1QuarterFull: if more than 1 quarter-full. + * - SPI_ReceptionFIFOStatus_HalfFull: if more than 1 half-full. + * - SPI_ReceptionFIFOStatus_Full: when FIFO is full. + */ +uint16_t SPI_GetReceptionFIFOStatus(SPI_TypeDef* SPIx) +{ + /* Get the SPIx Reception FIFO level bits */ + return (uint16_t)((SPIx->SR & SPI_SR_FRLVL)); +} + +/** + * @brief Checks whether the specified SPI flag is set or not. + * @param SPIx: To select the SPIx/I2Sx peripheral, where x can be: 1, 2, 3 or 4 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @param SPI_I2S_FLAG: specifies the SPI flag to check. + * This parameter can be one of the following values: + * @arg SPI_I2S_FLAG_TXE: Transmit buffer empty flag. + * @arg SPI_I2S_FLAG_RXNE: Receive buffer not empty flag. + * @arg SPI_I2S_FLAG_BSY: Busy flag. + * @arg SPI_I2S_FLAG_OVR: Overrun flag. + * @arg SPI_I2S_FLAG_MODF: Mode Fault flag. + * @arg SPI_I2S_FLAG_CRCERR: CRC Error flag. + * @arg SPI_I2S_FLAG_FRE: TI frame format error flag. + * @arg I2S_FLAG_UDR: Underrun Error flag. + * @arg I2S_FLAG_CHSIDE: Channel Side flag. + * @retval The new state of SPI_I2S_FLAG (SET or RESET). + */ +FlagStatus SPI_I2S_GetFlagStatus(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + assert_param(IS_SPI_I2S_GET_FLAG(SPI_I2S_FLAG)); + + /* Check the status of the specified SPI flag */ + if ((SPIx->SR & SPI_I2S_FLAG) != (uint16_t)RESET) + { + /* SPI_I2S_FLAG is set */ + bitstatus = SET; + } + else + { + /* SPI_I2S_FLAG is reset */ + bitstatus = RESET; + } + /* Return the SPI_I2S_FLAG status */ + return bitstatus; +} + +/** + * @brief Clears the SPIx CRC Error (CRCERR) flag. + * @param SPIx: To select the SPIx/I2Sx peripheral, where x can be: 1, 2, 3 or 4 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @param SPI_I2S_FLAG: specifies the SPI flag to clear. + * This function clears only CRCERR flag. + * @note OVR (OverRun error) flag is cleared by software sequence: a read + * operation to SPI_DR register (SPI_I2S_ReceiveData()) followed by a read + * operation to SPI_SR register (SPI_I2S_GetFlagStatus()). + * @note MODF (Mode Fault) flag is cleared by software sequence: a read/write + * operation to SPI_SR register (SPI_I2S_GetFlagStatus()) followed by a + * write operation to SPI_CR1 register (SPI_Cmd() to enable the SPI). + * @retval None + */ +void SPI_I2S_ClearFlag(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG) +{ + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + assert_param(IS_SPI_CLEAR_FLAG(SPI_I2S_FLAG)); + + /* Clear the selected SPI CRC Error (CRCERR) flag */ + SPIx->SR = (uint16_t)~SPI_I2S_FLAG; +} + +/** + * @brief Checks whether the specified SPI/I2S interrupt has occurred or not. + * @param SPIx: To select the SPIx/I2Sx peripheral, where x can be: 1, 2, 3 or 4 + * in SPI mode or 2 or 3 in I2S mode or I2Sxext for I2S full duplex mode. + * @param SPI_I2S_IT: specifies the SPI interrupt source to check. + * This parameter can be one of the following values: + * @arg SPI_I2S_IT_TXE: Transmit buffer empty interrupt. + * @arg SPI_I2S_IT_RXNE: Receive buffer not empty interrupt. + * @arg SPI_IT_MODF: Mode Fault interrupt. + * @arg SPI_I2S_IT_OVR: Overrun interrupt. + * @arg I2S_IT_UDR: Underrun interrupt. + * @arg SPI_I2S_IT_FRE: Format Error interrupt. + * @retval The new state of SPI_I2S_IT (SET or RESET). + */ +ITStatus SPI_I2S_GetITStatus(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT) +{ + ITStatus bitstatus = RESET; + uint16_t itpos = 0, itmask = 0, enablestatus = 0; + + /* Check the parameters */ + assert_param(IS_SPI_ALL_PERIPH_EXT(SPIx)); + assert_param(IS_SPI_I2S_GET_IT(SPI_I2S_IT)); + + /* Get the SPI_I2S_IT index */ + itpos = 0x01 << (SPI_I2S_IT & 0x0F); + + /* Get the SPI_I2S_IT IT mask */ + itmask = SPI_I2S_IT >> 4; + + /* Set the IT mask */ + itmask = 0x01 << itmask; + + /* Get the SPI_I2S_IT enable bit status */ + enablestatus = (SPIx->CR2 & itmask) ; + + /* Check the status of the specified SPI interrupt */ + if (((SPIx->SR & itpos) != (uint16_t)RESET) && enablestatus) + { + /* SPI_I2S_IT is set */ + bitstatus = SET; + } + else + { + /* SPI_I2S_IT is reset */ + bitstatus = RESET; + } + /* Return the SPI_I2S_IT status */ + return bitstatus; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_syscfg.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_syscfg.c new file mode 100755 index 0000000000..6787719782 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_syscfg.c @@ -0,0 +1,569 @@ +/** + ****************************************************************************** + * @file stm32f30x_syscfg.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the SYSCFG peripheral: + * + Remapping the memory mapped at 0x00000000 + * + Remapping the DMA channels + * + Enabling I2C fast mode plus driving capability for I2C plus + * + Remapping USB interrupt line + * + Configuring the EXTI lines connection to the GPIO port + * + Configuring the CLASSB requirements + * + @verbatim + + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] The SYSCFG registers can be accessed only when the SYSCFG + interface APB clock is enabled. + [..] To enable SYSCFG APB clock use: + RCC_APBPeriphClockCmd(RCC_APBPeriph_SYSCFG, ENABLE); + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_syscfg.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup SYSCFG + * @brief SYSCFG driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Reset value of SYSCFG_CFGR1 register */ +#define CFGR1_CLEAR_MASK ((uint32_t)0x7C000000) + +/* ------------ SYSCFG registers bit address in the alias region -------------*/ +#define SYSCFG_OFFSET (SYSCFG_BASE - PERIPH_BASE) + +/* --- CFGR1 Register ---*/ +/* Alias word address of USB_IT_RMP bit */ +#define CFGR1_OFFSET (SYSCFG_OFFSET + 0x00) +#define USBITRMP_BitNumber 0x05 +#define CFGR1_USBITRMP_BB (PERIPH_BB_BASE + (CFGR1_OFFSET * 32) + (USBITRMP_BitNumber * 4)) + +/* --- CFGR2 Register ---*/ +/* Alias word address of BYP_ADDR_PAR bit */ +#define CFGR2_OFFSET (SYSCFG_OFFSET + 0x18) +#define BYPADDRPAR_BitNumber 0x04 +#define CFGR1_BYPADDRPAR_BB (PERIPH_BB_BASE + (CFGR2_OFFSET * 32) + (BYPADDRPAR_BitNumber * 4)) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup SYSCFG_Private_Functions + * @{ + */ + +/** @defgroup SYSCFG_Group1 SYSCFG Initialization and Configuration functions + * @brief SYSCFG Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### SYSCFG Initialization and Configuration functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the SYSCFG registers to their default reset values. + * @param None + * @retval None + * @note MEM_MODE bits are not affected by APB reset. + * MEM_MODE bits took the value from the user option bytes. + */ +void SYSCFG_DeInit(void) +{ + /* Reset SYSCFG_CFGR1 register to reset value without affecting MEM_MODE bits */ + SYSCFG->CFGR1 &= SYSCFG_CFGR1_MEM_MODE; + /* Set FPU Interrupt Enable bits to default value */ + SYSCFG->CFGR1 |= 0x7C000000; + /* Reset RAM Write protection bits to default value */ + SYSCFG->RCR = 0x00000000; + /* Set EXTICRx registers to reset value */ + SYSCFG->EXTICR[0] = 0; + SYSCFG->EXTICR[1] = 0; + SYSCFG->EXTICR[2] = 0; + SYSCFG->EXTICR[3] = 0; + /* Set CFGR2 register to reset value */ + SYSCFG->CFGR2 = 0; + /* Set CFGR3 register to reset value */ + SYSCFG->CFGR3 = 0; + /* Set CFGR4 register to reset value */ + SYSCFG->CFGR4 = 0; +} + +/** + * @brief Configures the memory mapping at address 0x00000000. + * @param SYSCFG_MemoryRemap: selects the memory remapping. + * This parameter can be one of the following values: + * @arg SYSCFG_MemoryRemap_Flash: Main Flash memory mapped at 0x00000000 + * @arg SYSCFG_MemoryRemap_SystemMemory: System Flash memory mapped at 0x00000000 + * @arg SYSCFG_MemoryRemap_SRAM: Embedded SRAM mapped at 0x00000000 + * @arg SYSCFG_MemoryRemap_FMC: External memory through FMC + * @retval None + */ +void SYSCFG_MemoryRemapConfig(uint32_t SYSCFG_MemoryRemap) +{ + uint32_t tmpcfgr1 = 0; + + /* Check the parameter */ + assert_param(IS_SYSCFG_MEMORY_REMAP(SYSCFG_MemoryRemap)); + + /* Get CFGR1 register value */ + tmpcfgr1 = SYSCFG->CFGR1; + + /* Clear MEM_MODE bits */ + tmpcfgr1 &= (uint32_t) (~SYSCFG_CFGR1_MEM_MODE); + + /* Set the new MEM_MODE bits value */ + tmpcfgr1 |= (uint32_t) SYSCFG_MemoryRemap; + + /* Set CFGR1 register with the new memory remap configuration */ + SYSCFG->CFGR1 = tmpcfgr1; +} + +/** + * @brief Configures the DMA channels remapping. + * @param SYSCFG_DMARemap: selects the DMA channels remap. + * This parameter can be one of the following values: + * @arg SYSCFG_DMARemap_TIM17: Remap TIM17 DMA requests from DMA1 channel1 to channel2 + * @arg SYSCFG_DMARemap_TIM16: Remap TIM16 DMA requests from DMA1 channel3 to channel4 + * @arg SYSCFG_DMARemap_TIM6DAC1Ch1: Remap TIM6/DAC1 DMA requests from DMA2 channel 3 to DMA1 channel 3 + * @arg SYSCFG_DMARemap_TIM7DAC1Ch2: Remap TIM7/DAC2 DMA requests from DMA2 channel 4 to DMA1 channel 4 + * @arg SYSCFG_DMARemap_ADC2ADC4: Remap ADC2 and ADC4 DMA requests from DMA2 channel1/channel3 to channel3/channel4 + * @arg SYSCFG_DMARemap_DAC2Ch1: Remap DAC2 DMA requests to DMA1 channel5 + * @arg SYSCFG_DMARemapCh2_SPI1_RX: Remap SPI1 RX DMA1 CH2 requests + * @arg SYSCFG_DMARemapCh4_SPI1_RX: Remap SPI1 RX DMA CH4 requests + * @arg SYSCFG_DMARemapCh6_SPI1_RX: Remap SPI1 RX DMA CH6 requests + * @arg SYSCFG_DMARemapCh3_SPI1_TX: Remap SPI1 TX DMA CH2 requests + * @arg SYSCFG_DMARemapCh5_SPI1_TX: Remap SPI1 TX DMA CH5 requests + * @arg SYSCFG_DMARemapCh7_SPI1_TX: Remap SPI1 TX DMA CH7 requests + * @arg SYSCFG_DMARemapCh7_I2C1_RX: Remap I2C1 RX DMA CH7 requests + * @arg SYSCFG_DMARemapCh3_I2C1_RX: Remap I2C1 RX DMA CH3 requests + * @arg SYSCFG_DMARemapCh5_I2C1_RX: Remap I2C1 RX DMA CH5 requests + * @arg SYSCFG_DMARemapCh6_I2C1_TX: Remap I2C1 TX DMA CH6 requests + * @arg SYSCFG_DMARemapCh2_I2C1_TX: Remap I2C1 TX DMA CH2 requests + * @arg SYSCFG_DMARemapCh4_I2C1_TX: Remap I2C1 TX DMA CH4 requests + * @arg SYSCFG_DMARemapCh4_ADC2: Remap ADC2 DMA1 Ch4 requests + * @arg SYSCFG_DMARemapCh2_ADC2: Remap ADC2 DMA1 Ch2 requests + * @param NewState: new state of the DMA channel remapping. + * This parameter can be: Enable or Disable. + * @note When enabled, DMA channel of the selected peripheral is remapped + * @note When disabled, Default DMA channel is mapped to the selected peripheral + * @note + * By default TIM17 DMA requests is mapped to channel 1 + * use SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM17, Enable) + * to remap TIM17 DMA requests to DMA1 channel 2 + * use SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM17, Disable) + * to map TIM17 DMA requests to DMA1 channel 1 (default mapping) + * @retval None + */ +void SYSCFG_DMAChannelRemapConfig(uint32_t SYSCFG_DMARemap, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SYSCFG_DMA_REMAP(SYSCFG_DMARemap)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if ((SYSCFG_DMARemap & 0x80000000)!= 0x80000000) + { + if (NewState != DISABLE) + { + /* Remap the DMA channel */ + SYSCFG->CFGR1 |= (uint32_t)SYSCFG_DMARemap; + } + else + { + /* use the default DMA channel mapping */ + SYSCFG->CFGR1 &= (uint32_t)(~SYSCFG_DMARemap); + } + } + else + { + if (NewState != DISABLE) + { + /* Remap the DMA channel */ + SYSCFG->CFGR3 |= (uint32_t)SYSCFG_DMARemap; + } + else + { + /* use the default DMA channel mapping */ + SYSCFG->CFGR3 &= (uint32_t)(~SYSCFG_DMARemap); + } + } +} + +/** + * @brief Configures the remapping capabilities of DAC/TIM triggers. + * @param SYSCFG_TriggerRemap: selects the trigger to be remapped. + * This parameter can be one of the following values: + * @arg SYSCFG_TriggerRemap_DACTIM3: Remap DAC trigger from TIM8 to TIM3 + * @arg SYSCFG_TriggerRemap_TIM1TIM17: Remap TIM1 ITR3 from TIM4 TRGO to TIM17 OC + * @arg SYSCFG_TriggerRemap_DACHRTIM1_TRIG1: Remap DAC trigger to HRTIM1 TRIG1 + * @arg SYSCFG_TriggerRemap_DACHRTIM1_TRIG2: Remap DAC trigger to HRTIM1 TRIG2 + * @param NewState: new state of the trigger mapping. + * This parameter can be: ENABLE or DISABLE. + * @note ENABLE: Enable fast mode plus driving capability for selected pin + * @note DISABLE: Disable fast mode plus driving capability for selected pin + * @retval None + */ +void SYSCFG_TriggerRemapConfig(uint32_t SYSCFG_TriggerRemap, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SYSCFG_TRIGGER_REMAP(SYSCFG_TriggerRemap)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if ((SYSCFG_TriggerRemap & 0x80000000)!= 0x80000000) + { + if (NewState != DISABLE) + { + /* Remap the trigger */ + SYSCFG->CFGR1 |= (uint32_t)SYSCFG_TriggerRemap; + } + else + { + /* Use the default trigger mapping */ + SYSCFG->CFGR1 &= (uint32_t)(~SYSCFG_TriggerRemap); + } + } + else + { + if (NewState != DISABLE) + { + /* Remap the trigger */ + SYSCFG->CFGR3 |= (uint32_t)SYSCFG_TriggerRemap; + } + else + { + /* Use the default trigger mapping */ + SYSCFG->CFGR3 &= (uint32_t)(~SYSCFG_TriggerRemap); + } + } +} + +/** + * @brief Configures the remapping capabilities of encoder mode. + * @ note This feature implement the so-called M/T method for measuring speed + * and position using quadrature encoders. + * @param SYSCFG_EncoderRemap: selects the remap option for encoder mode. + * This parameter can be one of the following values: + * @arg SYSCFG_EncoderRemap_No: No remap + * @arg SYSCFG_EncoderRemap_TIM2: Timer 2 IC1 and IC2 connected to TIM15 IC1 and IC2 + * @arg SYSCFG_EncoderRemap_TIM3: Timer 3 IC1 and IC2 connected to TIM15 IC1 and IC2 + * @arg SYSCFG_EncoderRemap_TIM4: Timer 4 IC1 and IC2 connected to TIM15 IC1 and IC2 + * @retval None + */ +void SYSCFG_EncoderRemapConfig(uint32_t SYSCFG_EncoderRemap) +{ + /* Check the parameter */ + assert_param(IS_SYSCFG_ENCODER_REMAP(SYSCFG_EncoderRemap)); + + /* Reset the encoder mode remapping bits */ + SYSCFG->CFGR1 &= (uint32_t)(~SYSCFG_CFGR1_ENCODER_MODE); + + /* Set the selected configuration */ + SYSCFG->CFGR1 |= (uint32_t)(SYSCFG_EncoderRemap); +} + +/** + * @brief Remaps the USB interrupt lines. + * @param NewState: new state of the mapping of USB interrupt lines. + * This parameter can be: + * @param ENABLE: Remap the USB interrupt line as following: + * @arg USB Device High Priority (USB_HP) interrupt mapped to line 74. + * @arg USB Device Low Priority (USB_LP) interrupt mapped to line 75. + * @arg USB Wakeup Interrupt (USB_WKUP) interrupt mapped to line 76. + * @param DISABLE: Use the default USB interrupt line: + * @arg USB Device High Priority (USB_HP) interrupt mapped to line 19. + * @arg USB Device Low Priority (USB_LP) interrupt mapped to line 20. + * @arg USB Wakeup Interrupt (USB_WKUP) interrupt mapped to line 42. + * @retval None + */ +void SYSCFG_USBInterruptLineRemapCmd(FunctionalState NewState) +{ + /* Check the parameter */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + /* Remap the USB interrupt lines */ + *(__IO uint32_t *) CFGR1_USBITRMP_BB = (uint32_t)NewState; +} + +/** + * @brief Configures the I2C fast mode plus driving capability. + * @param SYSCFG_I2CFastModePlus: selects the pin. + * This parameter can be one of the following values: + * @arg SYSCFG_I2CFastModePlus_PB6: Configure fast mode plus driving capability for PB6 + * @arg SYSCFG_I2CFastModePlus_PB7: Configure fast mode plus driving capability for PB7 + * @arg SYSCFG_I2CFastModePlus_PB8: Configure fast mode plus driving capability for PB8 + * @arg SYSCFG_I2CFastModePlus_PB9: Configure fast mode plus driving capability for PB9 + * @arg SYSCFG_I2CFastModePlus_I2C1: Configure fast mode plus driving capability for I2C1 pins + * @arg SYSCFG_I2CFastModePlus_I2C2: Configure fast mode plus driving capability for I2C2 pins + * @arg SYSCFG_I2CFastModePlus_I2C3: Configure fast mode plus driving capability for I2C3 pins + * @param NewState: new state of the DMA channel remapping. + * This parameter can be: + * @arg ENABLE: Enable fast mode plus driving capability for selected I2C pin + * @arg DISABLE: Disable fast mode plus driving capability for selected I2C pin + * @note For I2C1, fast mode plus driving capability can be enabled on all selected + * I2C1 pins using SYSCFG_I2CFastModePlus_I2C1 parameter or independently + * on each one of the following pins PB6, PB7, PB8 and PB9. + * @note For remaining I2C1 pins (PA14, PA15...) fast mode plus driving capability + * can be enabled only by using SYSCFG_I2CFastModePlus_I2C1 parameter. + * @note For all I2C2 pins fast mode plus driving capability can be enabled + * only by using SYSCFG_I2CFastModePlus_I2C2 parameter. + * @retval None + */ +void SYSCFG_I2CFastModePlusConfig(uint32_t SYSCFG_I2CFastModePlus, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SYSCFG_I2C_FMP(SYSCFG_I2CFastModePlus)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable fast mode plus driving capability for selected I2C pin */ + SYSCFG->CFGR1 |= (uint32_t)SYSCFG_I2CFastModePlus; + } + else + { + /* Disable fast mode plus driving capability for selected I2C pin */ + SYSCFG->CFGR1 &= (uint32_t)(~SYSCFG_I2CFastModePlus); + } +} + +/** + * @brief Enables or disables the selected SYSCFG interrupts. + * @param SYSCFG_IT: specifies the SYSCFG interrupt sources to be enabled or disabled. + * This parameter can be one of the following values: + * @arg SYSCFG_IT_IXC: Inexact Interrupt + * @arg SYSCFG_IT_IDC: Input denormal Interrupt + * @arg SYSCFG_IT_OFC: Overflow Interrupt + * @arg SYSCFG_IT_UFC: Underflow Interrupt + * @arg SYSCFG_IT_DZC: Divide-by-zero Interrupt + * @arg SYSCFG_IT_IOC: Invalid operation Interrupt + * @param NewState: new state of the specified SYSCFG interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void SYSCFG_ITConfig(uint32_t SYSCFG_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_FUNCTIONAL_STATE(NewState)); + assert_param(IS_SYSCFG_IT(SYSCFG_IT)); + + if (NewState != DISABLE) + { + /* Enable the selected SYSCFG interrupts */ + SYSCFG->CFGR1 |= SYSCFG_IT; + } + else + { + /* Disable the selected SYSCFG interrupts */ + SYSCFG->CFGR1 &= ((uint32_t)~SYSCFG_IT); + } +} + +/** + * @brief Selects the GPIO pin used as EXTI Line. + * @param EXTI_PortSourceGPIOx : selects the GPIO port to be used as source + * for EXTI lines where x can be (A, B, C, D, E, F, G, H). + * @param EXTI_PinSourcex: specifies the EXTI line to be configured. + * This parameter can be EXTI_PinSourcex where x can be (0..15) + * @retval None + */ +void SYSCFG_EXTILineConfig(uint8_t EXTI_PortSourceGPIOx, uint8_t EXTI_PinSourcex) +{ + uint32_t tmp = 0x00; + + /* Check the parameters */ + assert_param(IS_EXTI_PORT_SOURCE(EXTI_PortSourceGPIOx)); + assert_param(IS_EXTI_PIN_SOURCE(EXTI_PinSourcex)); + + tmp = ((uint32_t)0x0F) << (0x04 * (EXTI_PinSourcex & (uint8_t)0x03)); + SYSCFG->EXTICR[EXTI_PinSourcex >> 0x02] &= ~tmp; + SYSCFG->EXTICR[EXTI_PinSourcex >> 0x02] |= (((uint32_t)EXTI_PortSourceGPIOx) << (0x04 * (EXTI_PinSourcex & (uint8_t)0x03))); +} + +/** + * @brief Connects the selected parameter to the break input of TIM1. + * @note The selected configuration is locked and can be unlocked by system reset + * @param SYSCFG_Break: selects the configuration to be connected to break + * input of TIM1 + * This parameter can be any combination of the following values: + * @arg SYSCFG_Break_PVD: PVD interrupt is connected to the break input of TIM1. + * @arg SYSCFG_Break_SRAMParity: SRAM Parity error is connected to the break input of TIM1. + * @arg SYSCFG_Break_HardFault: Lockup output of CortexM4 is connected to the break input of TIM1. + * @retval None + */ +void SYSCFG_BreakConfig(uint32_t SYSCFG_Break) +{ + /* Check the parameter */ + assert_param(IS_SYSCFG_LOCK_CONFIG(SYSCFG_Break)); + + SYSCFG->CFGR2 |= (uint32_t) SYSCFG_Break; +} + +/** + * @brief Disables the parity check on RAM. + * @note Disabling the parity check on RAM locks the configuration bit. + * To re-enable the parity check on RAM perform a system reset. + * @param None + * @retval None + */ +void SYSCFG_BypassParityCheckDisable(void) +{ + /* Disable the address parity check on RAM */ + *(__IO uint32_t *) CFGR1_BYPADDRPAR_BB = (uint32_t)0x00000001; +} + +/** + * @brief Configures the remapping capabilities of DAC/TIM triggers. + * @param SYSCFG_ADCTriggerRemap: selects the ADC trigger to be remapped. + * This parameter can be one of the following values: + * @arg REMAPADCTRIGGER_ADC12_EXT2: Input trigger of ADC12 regular channel EXT2 + * @arg REMAPADCTRIGGER_ADC12_EXT3: Input trigger of ADC12 regular channel EXT3 + * @arg REMAPADCTRIGGER_ADC12_EXT5: Input trigger of ADC12 regular channel EXT5 + * @arg REMAPADCTRIGGER_ADC12_EXT13: Input trigger of ADC12 regular channel EXT13 + * @arg REMAPADCTRIGGER_ADC12_EXT15: Input trigger of ADC12 regular channel EXT15 + * @arg REMAPADCTRIGGER_ADC12_JEXT3: Input trigger of ADC12 injected channel JEXT3 + * @arg REMAPADCTRIGGER_ADC12_JEXT6: Input trigger of ADC12 injected channel JEXT6 + * @arg REMAPADCTRIGGER_ADC12_JEXT13: Input trigger of ADC12 injected channel JEXT16 + * @arg REMAPADCTRIGGER_ADC34_EXT5: Input trigger of ADC34 regular channel EXT5 + * @arg REMAPADCTRIGGER_ADC34_EXT6: Input trigger of ADC34 regular channel EXT6 + * @arg REMAPADCTRIGGER_ADC34_EXT15: Input trigger of ADC34 regular channel EXT15 + * @arg REMAPADCTRIGGER_ADC34_JEXT5: Input trigger of ADC34 injected channel JEXT5 + * @arg REMAPADCTRIGGER_ADC34_JEXT11: Input trigger of ADC34 injected channel JEXT11 + * @arg REMAPADCTRIGGER_ADC34_JEXT14: Input trigger of ADC34 injected channel JEXT14 + * @param NewState: new state of the trigger mapping. + * This parameter can be: ENABLE or DISABLE. + * @note ENABLE: Enable fast mode plus driving capability for selected pin + * @note DISABLE: Disable fast mode plus driving capability for selected pin + * @retval None + */ +void SYSCFG_ADCTriggerRemapConfig(uint32_t SYSCFG_ADCTriggerRemap, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_SYSCFG_ADC_TRIGGER_REMAP(SYSCFG_ADCTriggerRemap)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Remap the trigger */ + SYSCFG->CFGR4 |= (uint32_t)SYSCFG_ADCTriggerRemap; + } + else + { + /* Use the default trigger mapping */ + SYSCFG->CFGR4 &= (uint32_t)(~SYSCFG_ADCTriggerRemap); + } +} + +/** + * @brief Enables the ICODE SRAM write protection. + * @note Enabling the ICODE SRAM write protection locks the configuration bit. + * To disable the ICODE SRAM write protection perform a system reset. + * @param None + * @retval None + */ +void SYSCFG_SRAMWRPEnable(uint32_t SYSCFG_SRAMWRP) +{ + /* Check the parameter */ + assert_param(IS_SYSCFG_PAGE(SYSCFG_SRAMWRP)); + + /* Enable the write-protection on the selected ICODE SRAM page */ + SYSCFG->RCR |= (uint32_t)SYSCFG_SRAMWRP; +} + +/** + * @brief Checks whether the specified SYSCFG flag is set or not. + * @param SYSCFG_Flag: specifies the SYSCFG flag to check. + * This parameter can be one of the following values: + * @arg SYSCFG_FLAG_PE: SRAM parity error flag. + * @retval The new state of SYSCFG_Flag (SET or RESET). + */ +FlagStatus SYSCFG_GetFlagStatus(__attribute__((unused)) uint32_t SYSCFG_Flag) +{ + FlagStatus bitstatus = RESET; + + /* Check the parameter */ + assert_param(IS_SYSCFG_FLAG(SYSCFG_Flag)); + + /* Check the status of the specified SPI flag */ + if ((SYSCFG->CFGR2 & SYSCFG_CFGR2_SRAM_PE) != (uint32_t)RESET) + { + /* SYSCFG_Flag is set */ + bitstatus = SET; + } + else + { + /* SYSCFG_Flag is reset */ + bitstatus = RESET; + } + /* Return the SYSCFG_Flag status */ + return bitstatus; +} + +/** + * @brief Clears the selected SYSCFG flag. + * @param SYSCFG_Flag: selects the flag to be cleared. + * This parameter can be any combination of the following values: + * @arg SYSCFG_FLAG_PE: SRAM parity error flag. + * @retval None + */ +void SYSCFG_ClearFlag(uint32_t SYSCFG_Flag) +{ + /* Check the parameter */ + assert_param(IS_SYSCFG_FLAG(SYSCFG_Flag)); + + SYSCFG->CFGR2 |= (uint32_t) SYSCFG_Flag; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_tim.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_tim.c new file mode 100755 index 0000000000..3c6589df58 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_tim.c @@ -0,0 +1,4006 @@ +/** + ****************************************************************************** + * @file stm32f30x_tim.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the TIM peripheral: + * + TimeBase management + * + Output Compare management + * + Input Capture management + * + Advanced-control timers (TIM1 and TIM8) specific features + * + Interrupts, DMA and flags management + * + Clocks management + * + Synchronization management + * + Specific interface management + * + Specific remapping management + * + @verbatim + + ============================================================================== + ##### How to use this driver ##### + ============================================================================== + [..] This driver provides functions to configure and program the TIM + of all stm32f30x devices. + These functions are split in 9 groups: + + (#) TIM TimeBase management: this group includes all needed functions + to configure the TM Timebase unit: + (++) Set/Get Prescaler + (++) Set/Get Autoreload + (++) Counter modes configuration + (++) Set Clock division + (++) Select the One Pulse mode + (++) Update Request Configuration + (++) Update Disable Configuration + (++) Auto-Preload Configuration + (++) Enable/Disable the counter + + (#) TIM Output Compare management: this group includes all needed + functions to configure the Capture/Compare unit used in Output + compare mode: + (++) Configure each channel, independently, in Output Compare mode + (++) Select the output compare modes + (++) Select the Polarities of each channel + (++) Set/Get the Capture/Compare register values + (++) Select the Output Compare Fast mode + (++) Select the Output Compare Forced mode + (++) Output Compare-Preload Configuration + (++) Clear Output Compare Reference + (++) Select the OCREF Clear signal + (++) Enable/Disable the Capture/Compare Channels + + (#) TIM Input Capture management: this group includes all needed + functions to configure the Capture/Compare unit used in + Input Capture mode: + (++) Configure each channel in input capture mode + (++) Configure Channel1/2 in PWM Input mode + (++) Set the Input Capture Prescaler + (++) Get the Capture/Compare values + + (#) Advanced-control timers (TIM1 and TIM8) specific features + (++) Configures the Break input, dead time, Lock level, the OSSI, + the OSSR State and the AOE(automatic output enable) + (++) Enable/Disable the TIM peripheral Main Outputs + (++) Select the Commutation event + (++) Set/Reset the Capture Compare Preload Control bit + + (#) TIM interrupts, DMA and flags management + (++) Enable/Disable interrupt sources + (++) Get flags status + (++) Clear flags/ Pending bits + (++) Enable/Disable DMA requests + (++) Configure DMA burst mode + (++) Select CaptureCompare DMA request + + (#) TIM clocks management: this group includes all needed functions + to configure the clock controller unit: + (++) Select internal/External clock + (++) Select the external clock mode: ETR(Mode1/Mode2), TIx or ITRx + + (#) TIM synchronization management: this group includes all needed + functions to configure the Synchronization unit: + (++) Select Input Trigger + (++) Select Output Trigger + (++) Select Master Slave Mode + (++) ETR Configuration when used as external trigger + + (#) TIM specific interface management, this group includes all + needed functions to use the specific TIM interface: + (++) Encoder Interface Configuration + (++) Select Hall Sensor + + (#) TIM specific remapping management includes the Remapping + configuration of specific timers + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_tim.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup TIM + * @brief TIM driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/* ---------------------- TIM registers bit mask ------------------------ */ +#define SMCR_ETR_MASK ((uint16_t)0x00FF) +#define CCMR_OFFSET ((uint16_t)0x0018) +#define CCER_CCE_SET ((uint16_t)0x0001) +#define CCER_CCNE_SET ((uint16_t)0x0004) +#define CCMR_OC13M_MASK ((uint32_t)0xFFFEFF8F) +#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +static void TI1_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter); +static void TI2_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter); +static void TI3_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter); +static void TI4_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter); + +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup TIM_Private_Functions + * @{ + */ + +/** @defgroup TIM_Group1 TimeBase management functions + * @brief TimeBase management functions + * +@verbatim + =============================================================================== + ##### TimeBase management functions ##### + =============================================================================== + + + *** TIM Driver: how to use it in Timing(Time base) Mode *** + ============================================================ + [..] + To use the Timer in Timing(Time base) mode, the following steps are mandatory: + + (#) Enable TIM clock using + RCC_APBxPeriphClockCmd(RCC_APBxPeriph_TIMx, ENABLE) function + (#) Fill the TIM_TimeBaseInitStruct with the desired parameters. + (#) Call TIM_TimeBaseInit(TIMx, &TIM_TimeBaseInitStruct) to configure + the Time Base unit + with the corresponding configuration + (#) Enable the NVIC if you need to generate the update interrupt. + (#) Enable the corresponding interrupt using the function + TIM_ITConfig(TIMx, TIM_IT_Update) + (#) Call the TIM_Cmd(ENABLE) function to enable the TIM counter. + [..] + (@) All other functions can be used separately to modify, if needed, + a specific feature of the Timer. + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the TIMx peripheral registers to their default reset values. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16,17 or 20 to select the TIM peripheral. + * @retval None + + */ +void TIM_DeInit(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + if (TIMx == TIM1) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM1, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM1, DISABLE); + } + else if (TIMx == TIM2) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM2, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM2, DISABLE); + } + else if (TIMx == TIM3) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM3, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM3, DISABLE); + } + else if (TIMx == TIM4) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM4, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM4, DISABLE); + } + else if (TIMx == TIM6) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM6, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM6, DISABLE); + } + else if (TIMx == TIM7) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM7, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM7, DISABLE); + } + else if (TIMx == TIM8) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM8, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM8, DISABLE); + } + else if (TIMx == TIM15) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM15, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM15, DISABLE); + } + else if (TIMx == TIM16) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM16, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM16, DISABLE); + } + else if (TIMx == TIM17) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM17, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM17, DISABLE); + } + else + { + if (TIMx == TIM20) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM20, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM20, DISABLE); + } + } +} + +/** + * @brief Initializes the TIMx Time Base Unit peripheral according to + * the specified parameters in the TIM_TimeBaseInitStruct. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param TIM_TimeBaseInitStruct: pointer to a TIM_TimeBaseInitTypeDef structure + * that contains the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct) +{ + uint16_t tmpcr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_COUNTER_MODE(TIM_TimeBaseInitStruct->TIM_CounterMode)); + assert_param(IS_TIM_CKD_DIV(TIM_TimeBaseInitStruct->TIM_ClockDivision)); + + tmpcr1 = TIMx->CR1; + + if((TIMx == TIM1) || (TIMx == TIM8)|| (TIMx == TIM2) || + (TIMx == TIM3) || (TIMx == TIM4) || (TIMx == TIM20)) + { + /* Select the Counter Mode */ + tmpcr1 &= (uint16_t)(~(TIM_CR1_DIR | TIM_CR1_CMS)); + tmpcr1 |= (uint32_t)TIM_TimeBaseInitStruct->TIM_CounterMode; + } + + if((TIMx != TIM6) && (TIMx != TIM7)) + { + /* Set the clock division */ + tmpcr1 &= (uint16_t)(~TIM_CR1_CKD); + tmpcr1 |= (uint32_t)TIM_TimeBaseInitStruct->TIM_ClockDivision; + } + + TIMx->CR1 = tmpcr1; + + /* Set the Autoreload value */ + TIMx->ARR = TIM_TimeBaseInitStruct->TIM_Period ; + + /* Set the Prescaler value */ + TIMx->PSC = TIM_TimeBaseInitStruct->TIM_Prescaler; + + if ((TIMx == TIM1) || (TIMx == TIM8)|| (TIMx == TIM15) || + (TIMx == TIM16) || (TIMx == TIM17)|| (TIMx == TIM20)) + { + /* Set the Repetition Counter value */ + TIMx->RCR = TIM_TimeBaseInitStruct->TIM_RepetitionCounter; + } + + /* Generate an update event to reload the Prescaler + and the repetition counter(only for TIM1 and TIM8) value immediately */ + TIMx->EGR = TIM_PSCReloadMode_Immediate; +} + +/** + * @brief Fills each TIM_TimeBaseInitStruct member with its default value. + * @param TIM_TimeBaseInitStruct : pointer to a TIM_TimeBaseInitTypeDef + * structure which will be initialized. + * @retval None + */ +void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct) +{ + /* Set the default configuration */ + TIM_TimeBaseInitStruct->TIM_Period = 0xFFFFFFFF; + TIM_TimeBaseInitStruct->TIM_Prescaler = 0x0000; + TIM_TimeBaseInitStruct->TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseInitStruct->TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInitStruct->TIM_RepetitionCounter = 0x0000; +} + +/** + * @brief Configures the TIMx Prescaler. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16 or 17 to select the TIM peripheral. + * @param Prescaler: specifies the Prescaler Register value + * @param TIM_PSCReloadMode: specifies the TIM Prescaler Reload mode + * This parameter can be one of the following values: + * @arg TIM_PSCReloadMode_Update: The Prescaler is loaded at the update event. + * @arg TIM_PSCReloadMode_Immediate: The Prescaler is loaded immediately. + * @retval None + */ +void TIM_PrescalerConfig(TIM_TypeDef* TIMx, uint16_t Prescaler, uint16_t TIM_PSCReloadMode) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_PRESCALER_RELOAD(TIM_PSCReloadMode)); + /* Set the Prescaler value */ + TIMx->PSC = Prescaler; + /* Set or reset the UG Bit */ + TIMx->EGR = TIM_PSCReloadMode; +} + +/** + * @brief Specifies the TIMx Counter Mode to be used. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_CounterMode: specifies the Counter Mode to be used + * This parameter can be one of the following values: + * @arg TIM_CounterMode_Up: TIM Up Counting Mode + * @arg TIM_CounterMode_Down: TIM Down Counting Mode + * @arg TIM_CounterMode_CenterAligned1: TIM Center Aligned Mode1 + * @arg TIM_CounterMode_CenterAligned2: TIM Center Aligned Mode2 + * @arg TIM_CounterMode_CenterAligned3: TIM Center Aligned Mode3 + * @retval None + */ +void TIM_CounterModeConfig(TIM_TypeDef* TIMx, uint16_t TIM_CounterMode) +{ + uint16_t tmpcr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_COUNTER_MODE(TIM_CounterMode)); + + tmpcr1 = TIMx->CR1; + + /* Reset the CMS and DIR Bits */ + tmpcr1 &= (uint16_t)~(TIM_CR1_DIR | TIM_CR1_CMS); + + /* Set the Counter Mode */ + tmpcr1 |= TIM_CounterMode; + + /* Write to TIMx CR1 register */ + TIMx->CR1 = tmpcr1; +} + +/** + * @brief Sets the TIMx Counter Register value + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param Counter: specifies the Counter register new value. + * @retval None + */ +void TIM_SetCounter(TIM_TypeDef* TIMx, uint32_t Counter) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + /* Set the Counter Register value */ + TIMx->CNT = Counter; +} + +/** + * @brief Sets the TIMx Autoreload Register value + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param Autoreload: specifies the Autoreload register new value. + * @retval None + */ +void TIM_SetAutoreload(TIM_TypeDef* TIMx, uint32_t Autoreload) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + /* Set the Autoreload Register value */ + TIMx->ARR = Autoreload; +} + +/** + * @brief Gets the TIMx Counter value. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @retval Counter Register value + */ +uint32_t TIM_GetCounter(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + /* Get the Counter Register value */ + return TIMx->CNT; +} + +/** + * @brief Gets the TIMx Prescaler value. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @retval Prescaler Register value. + */ +uint16_t TIM_GetPrescaler(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + /* Get the Prescaler Register value */ + return TIMx->PSC; +} + +/** + * @brief Enables or Disables the TIMx Update event. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param NewState: new state of the TIMx UDIS bit + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_UpdateDisableConfig(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the Update Disable Bit */ + TIMx->CR1 |= TIM_CR1_UDIS; + } + else + { + /* Reset the Update Disable Bit */ + TIMx->CR1 &= (uint16_t)~TIM_CR1_UDIS; + } +} + +/** + * @brief Configures the TIMx Update Request Interrupt source. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param TIM_UpdateSource: specifies the Update source. + * This parameter can be one of the following values: + * @arg TIM_UpdateSource_Regular: Source of update is the counter + * overflow/underflow or the setting of UG bit, or an update + * generation through the slave mode controller. + * @arg TIM_UpdateSource_Global: Source of update is counter overflow/underflow. + * @retval None + */ +void TIM_UpdateRequestConfig(TIM_TypeDef* TIMx, uint16_t TIM_UpdateSource) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_UPDATE_SOURCE(TIM_UpdateSource)); + + if (TIM_UpdateSource != TIM_UpdateSource_Global) + { + /* Set the URS Bit */ + TIMx->CR1 |= TIM_CR1_URS; + } + else + { + /* Reset the URS Bit */ + TIMx->CR1 &= (uint16_t)~TIM_CR1_URS; + } +} + +/** + * @brief Sets or resets the update interrupt flag (UIF)status bit Remapping. + * when sets, reading TIMx_CNT register returns UIF bit instead of CNT[31] + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param NewState: new state of the UIFREMAP bit. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_UIFRemap(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the TIM Counter */ + TIMx->CR1 |= TIM_CR1_UIFREMAP; + } + else + { + /* Disable the TIM Counter */ + TIMx->CR1 &= (uint16_t)~TIM_CR1_UIFREMAP; + } +} + +/** + * @brief Enables or disables TIMx peripheral Preload register on ARR. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param NewState: new state of the TIMx peripheral Preload register + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_ARRPreloadConfig(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the ARR Preload Bit */ + TIMx->CR1 |= TIM_CR1_ARPE; + } + else + { + /* Reset the ARR Preload Bit */ + TIMx->CR1 &= (uint16_t)~TIM_CR1_ARPE; + } +} + +/** + * @brief Selects the TIMx's One Pulse Mode. + * @param TIMx: where x can be 1, 2, 3, 4, 6 ,7 ,8, 15, 16, 17 or 20 to select the TIM peripheral. + * @param TIM_OPMode: specifies the OPM Mode to be used. + * This parameter can be one of the following values: + * @arg TIM_OPMode_Single + * @arg TIM_OPMode_Repetitive + * @retval None + */ +void TIM_SelectOnePulseMode(TIM_TypeDef* TIMx, uint16_t TIM_OPMode) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_OPM_MODE(TIM_OPMode)); + + /* Reset the OPM Bit */ + TIMx->CR1 &= (uint16_t)~TIM_CR1_OPM; + + /* Configure the OPM Mode */ + TIMx->CR1 |= TIM_OPMode; +} + +/** + * @brief Sets the TIMx Clock Division value. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16 or 17, to select the TIM peripheral. + * @param TIM_CKD: specifies the clock division value. + * This parameter can be one of the following value: + * @arg TIM_CKD_DIV1: TDTS = Tck_tim + * @arg TIM_CKD_DIV2: TDTS = 2*Tck_tim + * @arg TIM_CKD_DIV4: TDTS = 4*Tck_tim + * @retval None + */ +void TIM_SetClockDivision(TIM_TypeDef* TIMx, uint16_t TIM_CKD) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_CKD_DIV(TIM_CKD)); + + /* Reset the CKD Bits */ + TIMx->CR1 &= (uint16_t)(~TIM_CR1_CKD); + + /* Set the CKD value */ + TIMx->CR1 |= TIM_CKD; +} + +/** + * @brief Enables or disables the specified TIM peripheral. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 17 and 20 to select + * the TIMx peripheral. + * @param NewState: new state of the TIMx peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_Cmd(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the TIM Counter */ + TIMx->CR1 |= TIM_CR1_CEN; + } + else + { + /* Disable the TIM Counter */ + TIMx->CR1 &= (uint16_t)~TIM_CR1_CEN; + } +} +/** + * @} + */ + +/** @defgroup TIM_Group2 Output Compare management functions + * @brief Output Compare management functions + * +@verbatim + =============================================================================== + ##### Output Compare management functions ##### + =============================================================================== + + *** TIM Driver: how to use it in Output Compare Mode *** + ======================================================== + [..] + To use the Timer in Output Compare mode, the following steps are mandatory: + + (#) Enable TIM clock using RCC_APBxPeriphClockCmd(RCC_APBxPeriph_TIMx, ENABLE) function + + (#) Configure the TIM pins by configuring the corresponding GPIO pins + + (#) Configure the Time base unit as described in the first part of this driver, + if needed, else the Timer will run with the default configuration: + (++) Autoreload value = 0xFFFF + (++) Prescaler value = 0x0000 + (++) Counter mode = Up counting + (++) Clock Division = TIM_CKD_DIV1 + (#) Fill the TIM_OCInitStruct with the desired parameters including: + (++) The TIM Output Compare mode: TIM_OCMode + (++) TIM Output State: TIM_OutputState + (++) TIM Pulse value: TIM_Pulse + (++) TIM Output Compare Polarity : TIM_OCPolarity + + (#) Call TIM_OCxInit(TIMx, &TIM_OCInitStruct) to configure the desired channel with the + corresponding configuration + + (#) Call the TIM_Cmd(ENABLE) function to enable the TIM counter. + [..] + (@) All other functions can be used separately to modify, if needed, + a specific feature of the Timer. + + (@) In case of PWM mode, this function is mandatory: + TIM_OCxPreloadConfig(TIMx, TIM_OCPreload_ENABLE); + + (@) If the corresponding interrupt or DMA request are needed, the user should: + (#@) Enable the NVIC (or the DMA) to use the TIM interrupts (or DMA requests). + (#@) Enable the corresponding interrupt (or DMA request) using the function + TIM_ITConfig(TIMx, TIM_IT_CCx) (or TIM_DMA_Cmd(TIMx, TIM_DMA_CCx)) + +@endverbatim + * @{ + */ + +/** + * @brief Initializes the TIMx Channel1 according to the specified parameters in + * the TIM_OCInitStruct. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17, to select the TIM peripheral. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + uint32_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_OC_MODE(TIM_OCInitStruct->TIM_OCMode)); + assert_param(IS_TIM_OUTPUT_STATE(TIM_OCInitStruct->TIM_OutputState)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCInitStruct->TIM_OCPolarity)); + + /* Disable the Channel 1: Reset the CC1E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC1E; + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Get the TIMx CR2 register value */ + tmpcr2 = TIMx->CR2; + + /* Get the TIMx CCMR1 register value */ + tmpccmrx = TIMx->CCMR1; + + /* Reset the Output Compare Mode Bits */ + tmpccmrx &= (uint32_t)~TIM_CCMR1_OC1M; + tmpccmrx &= (uint32_t)~TIM_CCMR1_CC1S; + /* Select the Output Compare Mode */ + tmpccmrx |= TIM_OCInitStruct->TIM_OCMode; + + /* Reset the Output Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC1P; + /* Set the Output Compare Polarity */ + tmpccer |= TIM_OCInitStruct->TIM_OCPolarity; + + /* Set the Output State */ + tmpccer |= TIM_OCInitStruct->TIM_OutputState; + + if((TIMx == TIM1) || (TIMx == TIM8) || (TIMx == TIM15) || (TIMx == TIM16) || (TIMx == TIM17)) + { + assert_param(IS_TIM_OUTPUTN_STATE(TIM_OCInitStruct->TIM_OutputNState)); + assert_param(IS_TIM_OCN_POLARITY(TIM_OCInitStruct->TIM_OCNPolarity)); + assert_param(IS_TIM_OCNIDLE_STATE(TIM_OCInitStruct->TIM_OCNIdleState)); + assert_param(IS_TIM_OCIDLE_STATE(TIM_OCInitStruct->TIM_OCIdleState)); + + /* Reset the Output N Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC1NP; + /* Set the Output N Polarity */ + tmpccer |= TIM_OCInitStruct->TIM_OCNPolarity; + /* Reset the Output N State */ + tmpccer &= (uint32_t)~TIM_CCER_CC1NE; + + /* Set the Output N State */ + tmpccer |= TIM_OCInitStruct->TIM_OutputNState; + /* Reset the Output Compare and Output Compare N IDLE State */ + tmpcr2 &= (uint32_t)~TIM_CR2_OIS1; + tmpcr2 &= (uint32_t)~TIM_CR2_OIS1N; + /* Set the Output Idle state */ + tmpcr2 |= TIM_OCInitStruct->TIM_OCIdleState; + /* Set the Output N Idle state */ + tmpcr2 |= TIM_OCInitStruct->TIM_OCNIdleState; + } + /* Write to TIMx CR2 */ + TIMx->CR2 = tmpcr2; + + /* Write to TIMx CCMR1 */ + TIMx->CCMR1 = tmpccmrx; + + /* Set the Capture Compare Register value */ + TIMx->CCR1 = TIM_OCInitStruct->TIM_Pulse; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Initializes the TIMx Channel2 according to the specified parameters + * in the TIM_OCInitStruct. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM peripheral. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + uint32_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_OC_MODE(TIM_OCInitStruct->TIM_OCMode)); + assert_param(IS_TIM_OUTPUT_STATE(TIM_OCInitStruct->TIM_OutputState)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCInitStruct->TIM_OCPolarity)); + + /* Disable the Channel 2: Reset the CC2E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC2E; + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Get the TIMx CR2 register value */ + tmpcr2 = TIMx->CR2; + + /* Get the TIMx CCMR1 register value */ + tmpccmrx = TIMx->CCMR1; + + /* Reset the Output Compare mode and Capture/Compare selection Bits */ + tmpccmrx &= (uint32_t)~TIM_CCMR1_OC2M; + tmpccmrx &= (uint32_t)~TIM_CCMR1_CC2S; + + /* Select the Output Compare Mode */ + tmpccmrx |= (uint32_t)(TIM_OCInitStruct->TIM_OCMode << 8); + + /* Reset the Output Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC2P; + /* Set the Output Compare Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCPolarity << 4); + + /* Set the Output State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputState << 4); + + if((TIMx == TIM1) || (TIMx == TIM8)) + { + assert_param(IS_TIM_OUTPUTN_STATE(TIM_OCInitStruct->TIM_OutputNState)); + assert_param(IS_TIM_OCN_POLARITY(TIM_OCInitStruct->TIM_OCNPolarity)); + assert_param(IS_TIM_OCNIDLE_STATE(TIM_OCInitStruct->TIM_OCNIdleState)); + assert_param(IS_TIM_OCIDLE_STATE(TIM_OCInitStruct->TIM_OCIdleState)); + + /* Reset the Output N Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC2NP; + /* Set the Output N Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCNPolarity << 4); + /* Reset the Output N State */ + tmpccer &= (uint32_t)~TIM_CCER_CC2NE; + + /* Set the Output N State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputNState << 4); + /* Reset the Output Compare and Output Compare N IDLE State */ + tmpcr2 &= (uint32_t)~TIM_CR2_OIS2; + tmpcr2 &= (uint32_t)~TIM_CR2_OIS2N; + /* Set the Output Idle state */ + tmpcr2 |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCIdleState << 2); + /* Set the Output N Idle state */ + tmpcr2 |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCNIdleState << 2); + } + /* Write to TIMx CR2 */ + TIMx->CR2 = tmpcr2; + + /* Write to TIMx CCMR1 */ + TIMx->CCMR1 = tmpccmrx; + + /* Set the Capture Compare Register value */ + TIMx->CCR2 = TIM_OCInitStruct->TIM_Pulse; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Initializes the TIMx Channel3 according to the specified parameters + * in the TIM_OCInitStruct. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + uint32_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OC_MODE(TIM_OCInitStruct->TIM_OCMode)); + assert_param(IS_TIM_OUTPUT_STATE(TIM_OCInitStruct->TIM_OutputState)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCInitStruct->TIM_OCPolarity)); + + /* Disable the Channel 3: Reset the CC2E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC3E; + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Get the TIMx CR2 register value */ + tmpcr2 = TIMx->CR2; + + /* Get the TIMx CCMR2 register value */ + tmpccmrx = TIMx->CCMR2; + + /* Reset the Output Compare mode and Capture/Compare selection Bits */ + tmpccmrx &= (uint32_t)~TIM_CCMR2_OC3M; + tmpccmrx &= (uint32_t)~TIM_CCMR2_CC3S; + /* Select the Output Compare Mode */ + tmpccmrx |= TIM_OCInitStruct->TIM_OCMode; + + /* Reset the Output Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC3P; + /* Set the Output Compare Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCPolarity << 8); + + /* Set the Output State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputState << 8); + + if((TIMx == TIM1) || (TIMx == TIM8)) + { + assert_param(IS_TIM_OUTPUTN_STATE(TIM_OCInitStruct->TIM_OutputNState)); + assert_param(IS_TIM_OCN_POLARITY(TIM_OCInitStruct->TIM_OCNPolarity)); + assert_param(IS_TIM_OCNIDLE_STATE(TIM_OCInitStruct->TIM_OCNIdleState)); + assert_param(IS_TIM_OCIDLE_STATE(TIM_OCInitStruct->TIM_OCIdleState)); + + /* Reset the Output N Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC3NP; + /* Set the Output N Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCNPolarity << 8); + /* Reset the Output N State */ + tmpccer &= (uint32_t)~TIM_CCER_CC3NE; + + /* Set the Output N State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputNState << 8); + /* Reset the Output Compare and Output Compare N IDLE State */ + tmpcr2 &= (uint32_t)~TIM_CR2_OIS3; + tmpcr2 &= (uint32_t)~TIM_CR2_OIS3N; + /* Set the Output Idle state */ + tmpcr2 |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCIdleState << 4); + /* Set the Output N Idle state */ + tmpcr2 |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCNIdleState << 4); + } + /* Write to TIMx CR2 */ + TIMx->CR2 = tmpcr2; + + /* Write to TIMx CCMR2 */ + TIMx->CCMR2 = tmpccmrx; + + /* Set the Capture Compare Register value */ + TIMx->CCR3 = TIM_OCInitStruct->TIM_Pulse; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Initializes the TIMx Channel4 according to the specified parameters + * in the TIM_OCInitStruct. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + uint32_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OC_MODE(TIM_OCInitStruct->TIM_OCMode)); + assert_param(IS_TIM_OUTPUT_STATE(TIM_OCInitStruct->TIM_OutputState)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCInitStruct->TIM_OCPolarity)); + + /* Disable the Channel 4: Reset the CC4E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC4E; + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Get the TIMx CR2 register value */ + tmpcr2 = TIMx->CR2; + + /* Get the TIMx CCMR2 register value */ + tmpccmrx = TIMx->CCMR2; + + /* Reset the Output Compare mode and Capture/Compare selection Bits */ + tmpccmrx &= (uint32_t)~TIM_CCMR2_OC4M; + tmpccmrx &= (uint32_t)~TIM_CCMR2_CC4S; + + /* Select the Output Compare Mode */ + tmpccmrx |= (uint32_t)(TIM_OCInitStruct->TIM_OCMode << 8); + + /* Reset the Output Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC4P; + /* Set the Output Compare Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCPolarity << 12); + + /* Set the Output State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputState << 12); + + if((TIMx == TIM1) || (TIMx == TIM8)) + { + assert_param(IS_TIM_OCIDLE_STATE(TIM_OCInitStruct->TIM_OCIdleState)); + /* Reset the Output Compare IDLE State */ + tmpcr2 &=(uint32_t) ~TIM_CR2_OIS4; + /* Set the Output Idle state */ + tmpcr2 |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCIdleState << 6); + } + /* Write to TIMx CR2 */ + TIMx->CR2 = tmpcr2; + + /* Write to TIMx CCMR2 */ + TIMx->CCMR2 = tmpccmrx; + + /* Set the Capture Compare Register value */ + TIMx->CCR4 = TIM_OCInitStruct->TIM_Pulse; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Initializes the TIMx Channel5 according to the specified parameters + * in the TIM_OCInitStruct. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_OC5Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + uint32_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OC_MODE(TIM_OCInitStruct->TIM_OCMode)); + assert_param(IS_TIM_OUTPUT_STATE(TIM_OCInitStruct->TIM_OutputState)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCInitStruct->TIM_OCPolarity)); + + /* Disable the Channel 5: Reset the CC5E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC5E; /* to be verified*/ + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Get the TIMx CR2 register value */ + tmpcr2 = TIMx->CR2; + + /* Get the TIMx CCMR3 register value */ + tmpccmrx = TIMx->CCMR3; + + /* Reset the Output Compare mode and Capture/Compare selection Bits */ + tmpccmrx &= (uint32_t)~TIM_CCMR3_OC5M; + + /* Select the Output Compare Mode */ + tmpccmrx |= (uint32_t)(TIM_OCInitStruct->TIM_OCMode); + + /* Reset the Output Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC5P; + /* Set the Output Compare Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCPolarity << 16); + + /* Set the Output State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputState << 16); + + if((TIMx == TIM1) || (TIMx == TIM8)) + { + assert_param(IS_TIM_OCIDLE_STATE(TIM_OCInitStruct->TIM_OCIdleState)); + /* Reset the Output Compare IDLE State */ + tmpcr2 &=(uint32_t) ~TIM_CR2_OIS5; + /* Set the Output Idle state */ + tmpcr2 |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCIdleState << 16); + } + /* Write to TIMx CR2 */ + TIMx->CR2 = tmpcr2; + + /* Write to TIMx CCMR2 */ + TIMx->CCMR3 = tmpccmrx; + + /* Set the Capture Compare Register value */ + TIMx->CCR5 = TIM_OCInitStruct->TIM_Pulse; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Initializes the TIMx Channel6 according to the specified parameters + * in the TIM_OCInitStruct. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_OC6Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + uint32_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OC_MODE(TIM_OCInitStruct->TIM_OCMode)); + assert_param(IS_TIM_OUTPUT_STATE(TIM_OCInitStruct->TIM_OutputState)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCInitStruct->TIM_OCPolarity)); + + /* Disable the Channel 5: Reset the CC5E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC6E; /* to be verified*/ + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + /* Get the TIMx CR2 register value */ + tmpcr2 = TIMx->CR2; + + /* Get the TIMx CCMR3 register value */ + tmpccmrx = TIMx->CCMR3; + + /* Reset the Output Compare mode and Capture/Compare selection Bits */ + tmpccmrx &= (uint32_t)~TIM_CCMR3_OC6M; + + /* Select the Output Compare Mode */ + tmpccmrx |= (uint32_t)(TIM_OCInitStruct->TIM_OCMode << 8); + + /* Reset the Output Polarity level */ + tmpccer &= (uint32_t)~TIM_CCER_CC6P; + /* Set the Output Compare Polarity */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OCPolarity << 20); + + /* Set the Output State */ + tmpccer |= (uint32_t)((uint32_t)TIM_OCInitStruct->TIM_OutputState << 20); + + if((TIMx == TIM1) || (TIMx == TIM8)) + { + assert_param(IS_TIM_OCIDLE_STATE(TIM_OCInitStruct->TIM_OCIdleState)); + /* Reset the Output Compare IDLE State */ + tmpcr2 &=(uint32_t) ~TIM_CR2_OIS6; + /* Set the Output Idle state */ + tmpcr2 |= (uint16_t)(TIM_OCInitStruct->TIM_OCIdleState << 18); + } + /* Write to TIMx CR2 */ + TIMx->CR2 = tmpcr2; + + /* Write to TIMx CCMR2 */ + TIMx->CCMR3 = tmpccmrx; + + /* Set the Capture Compare Register value */ + TIMx->CCR6 = TIM_OCInitStruct->TIM_Pulse; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Selects the TIM Group Channel 5 and Channel 1, + OC1REFC is the logical AND of OC1REFC and OC5REF. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIMx peripheral + * @param NewState: new state of the Commutation event. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_SelectGC5C1(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the GC5C1 Bit */ + TIMx->CCR5 |= TIM_CCR5_GC5C1; + } + else + { + /* Reset the GC5C1 Bit */ + TIMx->CCR5 &= (uint32_t)~TIM_CCR5_GC5C1; + } +} + +/** + * @brief Selects the TIM Group Channel 5 and Channel 2, + OC2REFC is the logical AND of OC2REFC and OC5REF. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIMx peripheral + * @param NewState: new state of the Commutation event. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_SelectGC5C2(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the GC5C2 Bit */ + TIMx->CCR5 |= TIM_CCR5_GC5C2; + } + else + { + /* Reset the GC5C2 Bit */ + TIMx->CCR5 &= (uint32_t)~TIM_CCR5_GC5C2; + } +} + + +/** + * @brief Selects the TIM Group Channel 5 and Channel 3, + OC3REFC is the logical AND of OC3REFC and OC5REF. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIMx peripheral + * @param NewState: new state of the Commutation event. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_SelectGC5C3(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the GC5C3 Bit */ + TIMx->CCR5 |= TIM_CCR5_GC5C3; + } + else + { + /* Reset the GC5C3 Bit */ + TIMx->CCR5 &= (uint32_t)~TIM_CCR5_GC5C3; + } +} + +/** + * @brief Fills each TIM_OCInitStruct member with its default value. + * @param TIM_OCInitStruct: pointer to a TIM_OCInitTypeDef structure which will + * be initialized. + * @retval None + */ +void TIM_OCStructInit(TIM_OCInitTypeDef* TIM_OCInitStruct) +{ + /* Set the default configuration */ + TIM_OCInitStruct->TIM_OCMode = TIM_OCMode_Timing; + TIM_OCInitStruct->TIM_OutputState = TIM_OutputState_Disable; + TIM_OCInitStruct->TIM_OutputNState = TIM_OutputNState_Disable; + TIM_OCInitStruct->TIM_Pulse = 0x00000000; + TIM_OCInitStruct->TIM_OCPolarity = TIM_OCPolarity_High; + TIM_OCInitStruct->TIM_OCNPolarity = TIM_OCPolarity_High; + TIM_OCInitStruct->TIM_OCIdleState = TIM_OCIdleState_Reset; + TIM_OCInitStruct->TIM_OCNIdleState = TIM_OCNIdleState_Reset; +} + +/** + * @brief Selects the TIM Output Compare Mode. + * @note This function disables the selected channel before changing the Output + * Compare Mode. If needed, user has to enable this channel using + * TIM_CCxCmd() and TIM_CCxNCmd() functions. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_Channel: specifies the TIM Channel + * This parameter can be one of the following values: + * @arg TIM_Channel_1: TIM Channel 1 + * @arg TIM_Channel_2: TIM Channel 2 + * @arg TIM_Channel_3: TIM Channel 3 + * @arg TIM_Channel_4: TIM Channel 4 + * @param TIM_OCMode: specifies the TIM Output Compare Mode. + * This parameter can be one of the following values: + * @arg TIM_OCMode_Timing + * @arg TIM_OCMode_Active + * @arg TIM_OCMode_Toggle + * @arg TIM_OCMode_PWM1 + * @arg TIM_OCMode_PWM2 + * @arg TIM_ForcedAction_Active + * @arg TIM_ForcedAction_InActive + * @arg TIM_OCMode_Retrigerrable_OPM1 + * @arg TIM_OCMode_Retrigerrable_OPM2 + * @arg TIM_OCMode_Combined_PWM1 + * @arg TIM_OCMode_Combined_PWM2 + * @arg TIM_OCMode_Asymmetric_PWM1 + * @arg TIM_OCMode_Asymmetric_PWM2 + * @retval None + */ +void TIM_SelectOCxM(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode) +{ + uint32_t tmp = 0; + uint16_t tmp1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); + + tmp = (uint32_t) TIMx; + tmp += CCMR_OFFSET; + + tmp1 = CCER_CCE_SET << (uint16_t)TIM_Channel; + + /* Disable the Channel: Reset the CCxE Bit */ + TIMx->CCER &= (uint16_t) ~tmp1; + + if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) + { + tmp += (TIM_Channel>>1); + + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK; + + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } + else + { + tmp += (uint32_t)(TIM_Channel - (uint32_t)4)>> (uint32_t)1; + + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK; + + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8); + } +} + +/** + * @brief Sets the TIMx Capture Compare1 Register value + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param Compare1: specifies the Capture Compare1 register new value. + * @retval None + */ +void TIM_SetCompare1(TIM_TypeDef* TIMx, uint32_t Compare1) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + + /* Set the Capture Compare1 Register value */ + TIMx->CCR1 = Compare1; +} + +/** + * @brief Sets the TIMx Capture Compare2 Register value + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param Compare2: specifies the Capture Compare2 register new value. + * @retval None + */ +void TIM_SetCompare2(TIM_TypeDef* TIMx, uint32_t Compare2) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + + /* Set the Capture Compare2 Register value */ + TIMx->CCR2 = Compare2; +} + +/** + * @brief Sets the TIMx Capture Compare3 Register value + * @param TIMx: where x can be 1, 2, 3, 4, 5, 20 or 8 to select the TIM peripheral. + * @param Compare3: specifies the Capture Compare3 register new value. + * @retval None + */ +void TIM_SetCompare3(TIM_TypeDef* TIMx, uint32_t Compare3) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + + /* Set the Capture Compare3 Register value */ + TIMx->CCR3 = Compare3; +} + +/** + * @brief Sets the TIMx Capture Compare4 Register value + * @param TIMx: where x can be 1, 2, 3, 4, 5, 20 or 8 to select the TIM peripheral. + * @param Compare4: specifies the Capture Compare4 register new value. + * @retval None + */ +void TIM_SetCompare4(TIM_TypeDef* TIMx, uint32_t Compare4) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + + /* Set the Capture Compare4 Register value */ + TIMx->CCR4 = Compare4; +} + +/** + * @brief Sets the TIMx Capture Compare5 Register value + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param Compare5: specifies the Capture Compare5 register new value. + * @retval None + */ +void TIM_SetCompare5(TIM_TypeDef* TIMx, uint32_t Compare5) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + + /* Set the Capture Compare5 Register value */ + TIMx->CCR5 = Compare5; +} + +/** + * @brief Sets the TIMx Capture Compare6 Register value + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param Compare6: specifies the Capture Compare5 register new value. + * @retval None + */ +void TIM_SetCompare6(TIM_TypeDef* TIMx, uint32_t Compare6) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + + /* Set the Capture Compare6 Register value */ + TIMx->CCR6 = Compare6; +} + +/** + * @brief Forces the TIMx output 1 waveform to active or inactive level. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_ForcedAction: specifies the forced Action to be set to the output waveform. + * This parameter can be one of the following values: + * @arg TIM_ForcedAction_Active: Force active level on OC1REF + * @arg TIM_ForcedAction_InActive: Force inactive level on OC1REF. + * @retval None + */ +void TIM_ForcedOC1Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_FORCED_ACTION(TIM_ForcedAction)); + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC1M Bits */ + tmpccmr1 &= (uint32_t)~TIM_CCMR1_OC1M; + + /* Configure The Forced output Mode */ + tmpccmr1 |= TIM_ForcedAction; + + /* Write to TIMx CCMR1 register */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Forces the TIMx output 2 waveform to active or inactive level. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_ForcedAction: specifies the forced Action to be set to the output waveform. + * This parameter can be one of the following values: + * @arg TIM_ForcedAction_Active: Force active level on OC2REF + * @arg TIM_ForcedAction_InActive: Force inactive level on OC2REF. + * @retval None + */ +void TIM_ForcedOC2Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_FORCED_ACTION(TIM_ForcedAction)); + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC2M Bits */ + tmpccmr1 &= (uint32_t)~TIM_CCMR1_OC2M; + + /* Configure The Forced output Mode */ + tmpccmr1 |= ((uint32_t)TIM_ForcedAction << 8); + + /* Write to TIMx CCMR1 register */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Forces the TIMx output 3 waveform to active or inactive level. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ForcedAction: specifies the forced Action to be set to the output waveform. + * This parameter can be one of the following values: + * @arg TIM_ForcedAction_Active: Force active level on OC3REF + * @arg TIM_ForcedAction_InActive: Force inactive level on OC3REF. + * @retval None + */ +void TIM_ForcedOC3Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_FORCED_ACTION(TIM_ForcedAction)); + + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC1M Bits */ + tmpccmr2 &= (uint32_t)~TIM_CCMR2_OC3M; + + /* Configure The Forced output Mode */ + tmpccmr2 |= TIM_ForcedAction; + + /* Write to TIMx CCMR2 register */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Forces the TIMx output 4 waveform to active or inactive level. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ForcedAction: specifies the forced Action to be set to the output waveform. + * This parameter can be one of the following values: + * @arg TIM_ForcedAction_Active: Force active level on OC4REF + * @arg TIM_ForcedAction_InActive: Force inactive level on OC4REF. + * @retval None + */ +void TIM_ForcedOC4Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_FORCED_ACTION(TIM_ForcedAction)); + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC2M Bits */ + tmpccmr2 &= (uint32_t)~TIM_CCMR2_OC4M; + + /* Configure The Forced output Mode */ + tmpccmr2 |= ((uint32_t)TIM_ForcedAction << 8); + + /* Write to TIMx CCMR2 register */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Forces the TIMx output 5 waveform to active or inactive level. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_ForcedAction: specifies the forced Action to be set to the output waveform. + * This parameter can be one of the following values: + * @arg TIM_ForcedAction_Active: Force active level on OC5REF + * @arg TIM_ForcedAction_InActive: Force inactive level on OC5REF. + * @retval None + */ +void TIM_ForcedOC5Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction) +{ + uint32_t tmpccmr3 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_FORCED_ACTION(TIM_ForcedAction)); + tmpccmr3 = TIMx->CCMR3; + + /* Reset the OC5M Bits */ + tmpccmr3 &= (uint32_t)~TIM_CCMR3_OC5M; + + /* Configure The Forced output Mode */ + tmpccmr3 |= (uint32_t)(TIM_ForcedAction); + + /* Write to TIMx CCMR3 register */ + TIMx->CCMR3 = tmpccmr3; +} + +/** + * @brief Forces the TIMx output 6 waveform to active or inactive level. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_ForcedAction: specifies the forced Action to be set to the output waveform. + * This parameter can be one of the following values: + * @arg TIM_ForcedAction_Active: Force active level on OC5REF + * @arg TIM_ForcedAction_InActive: Force inactive level on OC5REF. + * @retval None + */ +void TIM_ForcedOC6Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction) +{ + uint32_t tmpccmr3 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_FORCED_ACTION(TIM_ForcedAction)); + tmpccmr3 = TIMx->CCMR3; + + /* Reset the OC6M Bits */ + tmpccmr3 &= (uint32_t)~TIM_CCMR3_OC6M; + + /* Configure The Forced output Mode */ + tmpccmr3 |= ((uint32_t)TIM_ForcedAction << 8); + + /* Write to TIMx CCMR3 register */ + TIMx->CCMR3 = tmpccmr3; +} + +/** + * @brief Enables or disables the TIMx peripheral Preload register on CCR1. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_OCPreload: new state of the TIMx peripheral Preload register + * This parameter can be one of the following values: + * @arg TIM_OCPreload_Enable + * @arg TIM_OCPreload_Disable + * @retval None + */ +void TIM_OC1PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_OCPRELOAD_STATE(TIM_OCPreload)); + + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC1PE Bit */ + tmpccmr1 &= (uint32_t)(~TIM_CCMR1_OC1PE); + + /* Enable or Disable the Output Compare Preload feature */ + tmpccmr1 |= TIM_OCPreload; + + /* Write to TIMx CCMR1 register */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Enables or disables the TIMx peripheral Preload register on CCR2. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_OCPreload: new state of the TIMx peripheral Preload register + * This parameter can be one of the following values: + * @arg TIM_OCPreload_Enable + * @arg TIM_OCPreload_Disable + * @retval None + */ +void TIM_OC2PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_OCPRELOAD_STATE(TIM_OCPreload)); + + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC2PE Bit */ + tmpccmr1 &= (uint32_t)(~TIM_CCMR1_OC2PE); + + /* Enable or Disable the Output Compare Preload feature */ + tmpccmr1 |= ((uint32_t)TIM_OCPreload << 8); + + /* Write to TIMx CCMR1 register */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Enables or disables the TIMx peripheral Preload register on CCR3. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCPreload: new state of the TIMx peripheral Preload register + * This parameter can be one of the following values: + * @arg TIM_OCPreload_Enable + * @arg TIM_OCPreload_Disable + * @retval None + */ +void TIM_OC3PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OCPRELOAD_STATE(TIM_OCPreload)); + + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC3PE Bit */ + tmpccmr2 &= (uint32_t)(~TIM_CCMR2_OC3PE); + + /* Enable or Disable the Output Compare Preload feature */ + tmpccmr2 |= TIM_OCPreload; + + /* Write to TIMx CCMR2 register */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Enables or disables the TIMx peripheral Preload register on CCR4. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCPreload: new state of the TIMx peripheral Preload register + * This parameter can be one of the following values: + * @arg TIM_OCPreload_Enable + * @arg TIM_OCPreload_Disable + * @retval None + */ +void TIM_OC4PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OCPRELOAD_STATE(TIM_OCPreload)); + + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC4PE Bit */ + tmpccmr2 &= (uint32_t)(~TIM_CCMR2_OC4PE); + + /* Enable or Disable the Output Compare Preload feature */ + tmpccmr2 |= ((uint32_t)TIM_OCPreload << 8); + + /* Write to TIMx CCMR2 register */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Enables or disables the TIMx peripheral Preload register on CCR5. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCPreload: new state of the TIMx peripheral Preload register + * This parameter can be one of the following values: + * @arg TIM_OCPreload_Enable + * @arg TIM_OCPreload_Disable + * @retval None + */ +void TIM_OC5PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload) +{ + uint32_t tmpccmr3 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OCPRELOAD_STATE(TIM_OCPreload)); + + tmpccmr3 = TIMx->CCMR3; + + /* Reset the OC5PE Bit */ + tmpccmr3 &= (uint32_t)(~TIM_CCMR3_OC5PE); + + /* Enable or Disable the Output Compare Preload feature */ + tmpccmr3 |= (uint32_t)(TIM_OCPreload); + + /* Write to TIMx CCMR3 register */ + TIMx->CCMR3 = tmpccmr3; +} + +/** + * @brief Enables or disables the TIMx peripheral Preload register on CCR6. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCPreload: new state of the TIMx peripheral Preload register + * This parameter can be one of the following values: + * @arg TIM_OCPreload_Enable + * @arg TIM_OCPreload_Disable + * @retval None + */ +void TIM_OC6PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload) +{ + uint32_t tmpccmr3 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OCPRELOAD_STATE(TIM_OCPreload)); + + tmpccmr3 = TIMx->CCMR3; + + /* Reset the OC5PE Bit */ + tmpccmr3 &= (uint32_t)(~TIM_CCMR3_OC6PE); + + /* Enable or Disable the Output Compare Preload feature */ + tmpccmr3 |= ((uint32_t)TIM_OCPreload << 8); + + /* Write to TIMx CCMR3 register */ + TIMx->CCMR3 = tmpccmr3; +} + +/** + * @brief Configures the TIMx Output Compare 1 Fast feature. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_OCFast: new state of the Output Compare Fast Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCFast_Enable: TIM output compare fast enable + * @arg TIM_OCFast_Disable: TIM output compare fast disable + * @retval None + */ +void TIM_OC1FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_OCFAST_STATE(TIM_OCFast)); + + /* Get the TIMx CCMR1 register value */ + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC1FE Bit */ + tmpccmr1 &= (uint32_t)~TIM_CCMR1_OC1FE; + + /* Enable or Disable the Output Compare Fast Bit */ + tmpccmr1 |= TIM_OCFast; + + /* Write to TIMx CCMR1 */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Configures the TIMx Output Compare 2 Fast feature. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_OCFast: new state of the Output Compare Fast Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCFast_Enable: TIM output compare fast enable + * @arg TIM_OCFast_Disable: TIM output compare fast disable + * @retval None + */ +void TIM_OC2FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_OCFAST_STATE(TIM_OCFast)); + + /* Get the TIMx CCMR1 register value */ + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC2FE Bit */ + tmpccmr1 &= (uint32_t)(~TIM_CCMR1_OC2FE); + + /* Enable or Disable the Output Compare Fast Bit */ + tmpccmr1 |= ((uint32_t)TIM_OCFast << 8); + + /* Write to TIMx CCMR1 */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Configures the TIMx Output Compare 3 Fast feature. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCFast: new state of the Output Compare Fast Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCFast_Enable: TIM output compare fast enable + * @arg TIM_OCFast_Disable: TIM output compare fast disable + * @retval None + */ +void TIM_OC3FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OCFAST_STATE(TIM_OCFast)); + + /* Get the TIMx CCMR2 register value */ + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC3FE Bit */ + tmpccmr2 &= (uint32_t)~TIM_CCMR2_OC3FE; + + /* Enable or Disable the Output Compare Fast Bit */ + tmpccmr2 |= TIM_OCFast; + + /* Write to TIMx CCMR2 */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Configures the TIMx Output Compare 4 Fast feature. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCFast: new state of the Output Compare Fast Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCFast_Enable: TIM output compare fast enable + * @arg TIM_OCFast_Disable: TIM output compare fast disable + * @retval None + */ +void TIM_OC4FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OCFAST_STATE(TIM_OCFast)); + + /* Get the TIMx CCMR2 register value */ + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC4FE Bit */ + tmpccmr2 &= (uint32_t)(~TIM_CCMR2_OC4FE); + + /* Enable or Disable the Output Compare Fast Bit */ + tmpccmr2 |= ((uint32_t)TIM_OCFast << 8); + + /* Write to TIMx CCMR2 */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Clears or safeguards the OCREF1 signal on an external event + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_OCClear: new state of the Output Compare Clear Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCClear_Enable: TIM Output clear enable + * @arg TIM_OCClear_Disable: TIM Output clear disable + * @retval None + */ +void TIM_ClearOC1Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_OCCLEAR_STATE(TIM_OCClear)); + + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC1CE Bit */ + tmpccmr1 &= (uint32_t)~TIM_CCMR1_OC1CE; + + /* Enable or Disable the Output Compare Clear Bit */ + tmpccmr1 |= TIM_OCClear; + + /* Write to TIMx CCMR1 register */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Clears or safeguards the OCREF2 signal on an external event + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_OCClear: new state of the Output Compare Clear Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCClear_Enable: TIM Output clear enable + * @arg TIM_OCClear_Disable: TIM Output clear disable + * @retval None + */ +void TIM_ClearOC2Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear) +{ + uint32_t tmpccmr1 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_OCCLEAR_STATE(TIM_OCClear)); + + tmpccmr1 = TIMx->CCMR1; + + /* Reset the OC2CE Bit */ + tmpccmr1 &= (uint32_t)~TIM_CCMR1_OC2CE; + + /* Enable or Disable the Output Compare Clear Bit */ + tmpccmr1 |= ((uint32_t)TIM_OCClear << 8); + + /* Write to TIMx CCMR1 register */ + TIMx->CCMR1 = tmpccmr1; +} + +/** + * @brief Clears or safeguards the OCREF3 signal on an external event + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCClear: new state of the Output Compare Clear Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCClear_Enable: TIM Output clear enable + * @arg TIM_OCClear_Disable: TIM Output clear disable + * @retval None + */ +void TIM_ClearOC3Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OCCLEAR_STATE(TIM_OCClear)); + + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC3CE Bit */ + tmpccmr2 &= (uint32_t)~TIM_CCMR2_OC3CE; + + /* Enable or Disable the Output Compare Clear Bit */ + tmpccmr2 |= TIM_OCClear; + + /* Write to TIMx CCMR2 register */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Clears or safeguards the OCREF4 signal on an external event + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCClear: new state of the Output Compare Clear Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCClear_Enable: TIM Output clear enable + * @arg TIM_OCClear_Disable: TIM Output clear disable + * @retval None + */ +void TIM_ClearOC4Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear) +{ + uint32_t tmpccmr2 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OCCLEAR_STATE(TIM_OCClear)); + + tmpccmr2 = TIMx->CCMR2; + + /* Reset the OC4CE Bit */ + tmpccmr2 &= (uint32_t)~TIM_CCMR2_OC4CE; + + /* Enable or Disable the Output Compare Clear Bit */ + tmpccmr2 |= ((uint32_t)TIM_OCClear << 8); + + /* Write to TIMx CCMR2 register */ + TIMx->CCMR2 = tmpccmr2; +} + +/** + * @brief Clears or safeguards the OCREF5 signal on an external event + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCClear: new state of the Output Compare Clear Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCClear_Enable: TIM Output clear enable + * @arg TIM_OCClear_Disable: TIM Output clear disable + * @retval None + */ +void TIM_ClearOC5Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear) +{ + uint32_t tmpccmr3 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OCCLEAR_STATE(TIM_OCClear)); + + tmpccmr3 = TIMx->CCMR3; + + /* Reset the OC5CE Bit */ + tmpccmr3 &= (uint32_t)~TIM_CCMR3_OC5CE; + + /* Enable or Disable the Output Compare Clear Bit */ + tmpccmr3 |= (uint32_t)(TIM_OCClear); + + /* Write to TIMx CCMR3 register */ + TIMx->CCMR3 = tmpccmr3; +} + +/** + * @brief Clears or safeguards the OCREF6 signal on an external event + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCClear: new state of the Output Compare Clear Enable Bit. + * This parameter can be one of the following values: + * @arg TIM_OCClear_Enable: TIM Output clear enable + * @arg TIM_OCClear_Disable: TIM Output clear disable + * @retval None + */ +void TIM_ClearOC6Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear) +{ + uint32_t tmpccmr3 = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OCCLEAR_STATE(TIM_OCClear)); + + tmpccmr3 = TIMx->CCMR3; + + /* Reset the OC5CE Bit */ + tmpccmr3 &= (uint32_t)~TIM_CCMR3_OC6CE; + + /* Enable or Disable the Output Compare Clear Bit */ + tmpccmr3 |= ((uint32_t)TIM_OCClear << 8); + + /* Write to TIMx CCMR3 register */ + TIMx->CCMR3 = tmpccmr3; +} + +/** + * @brief Selects the OCReference Clear source. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_OCReferenceClear: specifies the OCReference Clear source. + * This parameter can be one of the following values: + * @arg TIM_OCReferenceClear_ETRF: The internal OCreference clear input is connected to ETRF. + * @arg TIM_OCReferenceClear_OCREFCLR: The internal OCreference clear input is connected to OCREF_CLR input. + * @retval None + */ +void TIM_SelectOCREFClear(TIM_TypeDef* TIMx, uint16_t TIM_OCReferenceClear) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(TIM_OCREFERENCECECLEAR_SOURCE(TIM_OCReferenceClear)); + + /* Set the TIM_OCReferenceClear source */ + TIMx->SMCR &= (uint16_t)~((uint16_t)TIM_SMCR_OCCS); + TIMx->SMCR |= TIM_OCReferenceClear; +} + +/** + * @brief Configures the TIMx channel 1 polarity. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_OCPolarity: specifies the OC1 Polarity + * This parameter can be one of the following values: + * @arg TIM_OCPolarity_High: Output Compare active high + * @arg TIM_OCPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC1PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC1P Bit */ + tmpccer &= (uint32_t)(~TIM_CCER_CC1P); + tmpccer |= TIM_OCPolarity; + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx Channel 1N polarity. + * @param TIMx: where x can be 1, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_OCNPolarity: specifies the OC1N Polarity + * This parameter can be one of the following values: + * @arg TIM_OCNPolarity_High: Output Compare active high + * @arg TIM_OCNPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC1NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity) +{ + uint32_t tmpccer = 0; + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_TIM_OCN_POLARITY(TIM_OCNPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC1NP Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC1NP; + tmpccer |= TIM_OCNPolarity; + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx channel 2 polarity. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_OCPolarity: specifies the OC2 Polarity + * This parameter can be one of the following values: + * @arg TIM_OCPolarity_High: Output Compare active high + * @arg TIM_OCPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC2PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC2P Bit */ + tmpccer &= (uint32_t)(~TIM_CCER_CC2P); + tmpccer |= ((uint32_t)TIM_OCPolarity << 4); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx Channel 2N polarity. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCNPolarity: specifies the OC2N Polarity + * This parameter can be one of the following values: + * @arg TIM_OCNPolarity_High: Output Compare active high + * @arg TIM_OCNPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC2NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OCN_POLARITY(TIM_OCNPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC2NP Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC2NP; + tmpccer |= ((uint32_t)TIM_OCNPolarity << 4); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx channel 3 polarity. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCPolarity: specifies the OC3 Polarity + * This parameter can be one of the following values: + * @arg TIM_OCPolarity_High: Output Compare active high + * @arg TIM_OCPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC3PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC3P Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC3P; + tmpccer |= ((uint32_t)TIM_OCPolarity << 8); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx Channel 3N polarity. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCNPolarity: specifies the OC3N Polarity + * This parameter can be one of the following values: + * @arg TIM_OCNPolarity_High: Output Compare active high + * @arg TIM_OCNPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC3NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OCN_POLARITY(TIM_OCNPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC3NP Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC3NP; + tmpccer |= ((uint32_t)TIM_OCNPolarity << 8); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx channel 4 polarity. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_OCPolarity: specifies the OC4 Polarity + * This parameter can be one of the following values: + * @arg TIM_OCPolarity_High: Output Compare active high + * @arg TIM_OCPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC4PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC4P Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC4P; + tmpccer |= ((uint32_t)TIM_OCPolarity << 12); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx channel 5 polarity. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCPolarity: specifies the OC5 Polarity + * This parameter can be one of the following values: + * @arg TIM_OCPolarity_High: Output Compare active high + * @arg TIM_OCPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC5PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC5P Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC5P; + tmpccer |= ((uint32_t)TIM_OCPolarity << 16); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Configures the TIMx channel 6 polarity. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * @param TIM_OCPolarity: specifies the OC6 Polarity + * This parameter can be one of the following values: + * @arg TIM_OCPolarity_High: Output Compare active high + * @arg TIM_OCPolarity_Low: Output Compare active low + * @retval None + */ +void TIM_OC6PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity) +{ + uint32_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_OC_POLARITY(TIM_OCPolarity)); + + tmpccer = TIMx->CCER; + + /* Set or Reset the CC6P Bit */ + tmpccer &= (uint32_t)~TIM_CCER_CC6P; + tmpccer |= ((uint32_t)TIM_OCPolarity << 20); + + /* Write to TIMx CCER register */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Enables or disables the TIM Capture Compare Channel x. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_Channel: specifies the TIM Channel + * This parameter can be one of the following values: + * @arg TIM_Channel_1: TIM Channel 1 + * @arg TIM_Channel_2: TIM Channel 2 + * @arg TIM_Channel_3: TIM Channel 3 + * @arg TIM_Channel_4: TIM Channel 4 + * @arg TIM_Channel_5: TIM Channel 5 + * @arg TIM_Channel_6: TIM Channel 6 + * @param TIM_CCx: specifies the TIM Channel CCxE bit new state. + * This parameter can be: TIM_CCx_Enable or TIM_CCx_Disable. + * @retval None + */ +void TIM_CCxCmd(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_CCx) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_CCX(TIM_CCx)); + + tmp = (uint32_t)CCER_CCE_SET << (uint32_t)TIM_Channel; + + /* Reset the CCxE Bit */ + TIMx->CCER &= (uint32_t)(~tmp); + + /* Set or reset the CCxE Bit */ + TIMx->CCER |= ((uint32_t)TIM_CCx << (uint32_t)TIM_Channel); +} + +/** + * @brief Enables or disables the TIM Capture Compare Channel xN. + * @param TIMx: where x can be 1, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_Channel: specifies the TIM Channel + * This parameter can be one of the following values: + * @arg TIM_Channel_1: TIM Channel 1 + * @arg TIM_Channel_2: TIM Channel 2 + * @arg TIM_Channel_3: TIM Channel 3 + * @param TIM_CCxN: specifies the TIM Channel CCxNE bit new state. + * This parameter can be: TIM_CCxN_Enable or TIM_CCxN_Disable. + * @retval None + */ +void TIM_CCxNCmd(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_CCxN) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_TIM_COMPLEMENTARY_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_CCXN(TIM_CCxN)); + + tmp = (uint32_t)CCER_CCNE_SET << (uint32_t)TIM_Channel; + + /* Reset the CCxNE Bit */ + TIMx->CCER &= (uint32_t) ~tmp; + + /* Set or reset the CCxNE Bit */ + TIMx->CCER |= ((uint32_t)TIM_CCxN << (uint32_t)TIM_Channel); +} +/** + * @} + */ + +/** @defgroup TIM_Group3 Input Capture management functions + * @brief Input Capture management functions + * +@verbatim + =============================================================================== + ##### Input Capture management functions ##### + =============================================================================== + + *** TIM Driver: how to use it in Input Capture Mode *** + ======================================================= + [..] + To use the Timer in Input Capture mode, the following steps are mandatory: + + (#) Enable TIM clock using RCC_APBxPeriphClockCmd(RCC_APBxPeriph_TIMx, ENABLE) function + + (#) Configure the TIM pins by configuring the corresponding GPIO pins + + (#) Configure the Time base unit as described in the first part of this driver, + if needed, else the Timer will run with the default configuration: + (++) Autoreload value = 0xFFFF + (++) Prescaler value = 0x0000 + (++) Counter mode = Up counting + (++) Clock Division = TIM_CKD_DIV1 + + (#) Fill the TIM_ICInitStruct with the desired parameters including: + (++) TIM Channel: TIM_Channel + (++) TIM Input Capture polarity: TIM_ICPolarity + (++) TIM Input Capture selection: TIM_ICSelection + (++) TIM Input Capture Prescaler: TIM_ICPrescaler + (++) TIM Input CApture filter value: TIM_ICFilter + + (#) Call TIM_ICInit(TIMx, &TIM_ICInitStruct) to configure the desired channel with the + corresponding configuration and to measure only frequency or duty cycle of the input signal, + or, + Call TIM_PWMIConfig(TIMx, &TIM_ICInitStruct) to configure the desired channels with the + corresponding configuration and to measure the frequency and the duty cycle of the input signal + + (#) Enable the NVIC or the DMA to read the measured frequency. + + (#) Enable the corresponding interrupt (or DMA request) to read the Captured value, + using the function TIM_ITConfig(TIMx, TIM_IT_CCx) (or TIM_DMA_Cmd(TIMx, TIM_DMA_CCx)) + + (#) Call the TIM_Cmd(ENABLE) function to enable the TIM counter. + + (#) Use TIM_GetCapturex(TIMx); to read the captured value. + [..] + (@) All other functions can be used separately to modify, if needed, + a specific feature of the Timer. + +@endverbatim + * @{ + */ + +/** + * @brief Initializes the TIM peripheral according to the specified parameters + * in the TIM_ICInitStruct. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_ICInitStruct: pointer to a TIM_ICInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_IC_POLARITY(TIM_ICInitStruct->TIM_ICPolarity)); + assert_param(IS_TIM_IC_SELECTION(TIM_ICInitStruct->TIM_ICSelection)); + assert_param(IS_TIM_IC_PRESCALER(TIM_ICInitStruct->TIM_ICPrescaler)); + assert_param(IS_TIM_IC_FILTER(TIM_ICInitStruct->TIM_ICFilter)); + + if (TIM_ICInitStruct->TIM_Channel == TIM_Channel_1) + { + /* TI1 Configuration */ + TI1_Config(TIMx, TIM_ICInitStruct->TIM_ICPolarity, + TIM_ICInitStruct->TIM_ICSelection, + TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC1Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + } + else if (TIM_ICInitStruct->TIM_Channel == TIM_Channel_2) + { + /* TI2 Configuration */ + TI2_Config(TIMx, TIM_ICInitStruct->TIM_ICPolarity, + TIM_ICInitStruct->TIM_ICSelection, + TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC2Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + } + else if (TIM_ICInitStruct->TIM_Channel == TIM_Channel_3) + { + /* TI3 Configuration */ + TI3_Config(TIMx, TIM_ICInitStruct->TIM_ICPolarity, + TIM_ICInitStruct->TIM_ICSelection, + TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC3Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + } + else + { + /* TI4 Configuration */ + TI4_Config(TIMx, TIM_ICInitStruct->TIM_ICPolarity, + TIM_ICInitStruct->TIM_ICSelection, + TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC4Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + } +} + +/** + * @brief Fills each TIM_ICInitStruct member with its default value. + * @param TIM_ICInitStruct: pointer to a TIM_ICInitTypeDef structure which will + * be initialized. + * @retval None + */ +void TIM_ICStructInit(TIM_ICInitTypeDef* TIM_ICInitStruct) +{ + /* Set the default configuration */ + TIM_ICInitStruct->TIM_Channel = TIM_Channel_1; + TIM_ICInitStruct->TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStruct->TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStruct->TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStruct->TIM_ICFilter = 0x00; +} + +/** + * @brief Configures the TIM peripheral according to the specified parameters + * in the TIM_ICInitStruct to measure an external PWM signal. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_ICInitStruct: pointer to a TIM_ICInitTypeDef structure that contains + * the configuration information for the specified TIM peripheral. + * @retval None + */ +void TIM_PWMIConfig(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct) +{ + uint16_t icoppositepolarity = TIM_ICPolarity_Rising; + uint16_t icoppositeselection = TIM_ICSelection_DirectTI; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + + /* Select the Opposite Input Polarity */ + if (TIM_ICInitStruct->TIM_ICPolarity == TIM_ICPolarity_Rising) + { + icoppositepolarity = TIM_ICPolarity_Falling; + } + else + { + icoppositepolarity = TIM_ICPolarity_Rising; + } + /* Select the Opposite Input */ + if (TIM_ICInitStruct->TIM_ICSelection == TIM_ICSelection_DirectTI) + { + icoppositeselection = TIM_ICSelection_IndirectTI; + } + else + { + icoppositeselection = TIM_ICSelection_DirectTI; + } + if (TIM_ICInitStruct->TIM_Channel == TIM_Channel_1) + { + /* TI1 Configuration */ + TI1_Config(TIMx, TIM_ICInitStruct->TIM_ICPolarity, TIM_ICInitStruct->TIM_ICSelection, + TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC1Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + /* TI2 Configuration */ + TI2_Config(TIMx, icoppositepolarity, icoppositeselection, TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC2Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + } + else + { + /* TI2 Configuration */ + TI2_Config(TIMx, TIM_ICInitStruct->TIM_ICPolarity, TIM_ICInitStruct->TIM_ICSelection, + TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC2Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + /* TI1 Configuration */ + TI1_Config(TIMx, icoppositepolarity, icoppositeselection, TIM_ICInitStruct->TIM_ICFilter); + /* Set the Input Capture Prescaler value */ + TIM_SetIC1Prescaler(TIMx, TIM_ICInitStruct->TIM_ICPrescaler); + } +} + +/** + * @brief Gets the TIMx Input Capture 1 value. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @retval Capture Compare 1 Register value. + */ +uint32_t TIM_GetCapture1(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + + /* Get the Capture 1 Register value */ + return TIMx->CCR1; +} + +/** + * @brief Gets the TIMx Input Capture 2 value. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @retval Capture Compare 2 Register value. + */ +uint32_t TIM_GetCapture2(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + + /* Get the Capture 2 Register value */ + return TIMx->CCR2; +} + +/** + * @brief Gets the TIMx Input Capture 3 value. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @retval Capture Compare 3 Register value. + */ +uint32_t TIM_GetCapture3(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + + /* Get the Capture 3 Register value */ + return TIMx->CCR3; +} + +/** + * @brief Gets the TIMx Input Capture 4 value. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @retval Capture Compare 4 Register value. + */ +uint32_t TIM_GetCapture4(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + + /* Get the Capture 4 Register value */ + return TIMx->CCR4; +} + +/** + * @brief Sets the TIMx Input Capture 1 prescaler. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_ICPSC: specifies the Input Capture1 prescaler new value. + * This parameter can be one of the following values: + * @arg TIM_ICPSC_DIV1: no prescaler + * @arg TIM_ICPSC_DIV2: capture is done once every 2 events + * @arg TIM_ICPSC_DIV4: capture is done once every 4 events + * @arg TIM_ICPSC_DIV8: capture is done once every 8 events + * @retval None + */ +void TIM_SetIC1Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_IC_PRESCALER(TIM_ICPSC)); + + /* Reset the IC1PSC Bits */ + TIMx->CCMR1 &= (uint32_t)~TIM_CCMR1_IC1PSC; + + /* Set the IC1PSC value */ + TIMx->CCMR1 |= TIM_ICPSC; +} + +/** + * @brief Sets the TIMx Input Capture 2 prescaler. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_ICPSC: specifies the Input Capture2 prescaler new value. + * This parameter can be one of the following values: + * @arg TIM_ICPSC_DIV1: no prescaler + * @arg TIM_ICPSC_DIV2: capture is done once every 2 events + * @arg TIM_ICPSC_DIV4: capture is done once every 4 events + * @arg TIM_ICPSC_DIV8: capture is done once every 8 events + * @retval None + */ +void TIM_SetIC2Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_IC_PRESCALER(TIM_ICPSC)); + + /* Reset the IC2PSC Bits */ + TIMx->CCMR1 &= (uint32_t)~TIM_CCMR1_IC2PSC; + + /* Set the IC2PSC value */ + TIMx->CCMR1 |= (uint32_t)((uint32_t)TIM_ICPSC << 8); +} + +/** + * @brief Sets the TIMx Input Capture 3 prescaler. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ICPSC: specifies the Input Capture3 prescaler new value. + * This parameter can be one of the following values: + * @arg TIM_ICPSC_DIV1: no prescaler + * @arg TIM_ICPSC_DIV2: capture is done once every 2 events + * @arg TIM_ICPSC_DIV4: capture is done once every 4 events + * @arg TIM_ICPSC_DIV8: capture is done once every 8 events + * @retval None + */ +void TIM_SetIC3Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_IC_PRESCALER(TIM_ICPSC)); + + /* Reset the IC3PSC Bits */ + TIMx->CCMR2 &= (uint16_t)~TIM_CCMR2_IC3PSC; + + /* Set the IC3PSC value */ + TIMx->CCMR2 |= TIM_ICPSC; +} + +/** + * @brief Sets the TIMx Input Capture 4 prescaler. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ICPSC: specifies the Input Capture4 prescaler new value. + * This parameter can be one of the following values: + * @arg TIM_ICPSC_DIV1: no prescaler + * @arg TIM_ICPSC_DIV2: capture is done once every 2 events + * @arg TIM_ICPSC_DIV4: capture is done once every 4 events + * @arg TIM_ICPSC_DIV8: capture is done once every 8 events + * @retval None + */ +void TIM_SetIC4Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_IC_PRESCALER(TIM_ICPSC)); + + /* Reset the IC4PSC Bits */ + TIMx->CCMR2 &= (uint16_t)~TIM_CCMR2_IC4PSC; + + /* Set the IC4PSC value */ + TIMx->CCMR2 |= (uint16_t)(TIM_ICPSC << 8); +} +/** + * @} + */ + +/** @defgroup TIM_Group4 Advanced-control timers (TIM1 and TIM8) specific features + * @brief Advanced-control timers (TIM1 and TIM8) specific features + * +@verbatim + =============================================================================== + ##### Advanced-control timers (TIM1 and TIM8) specific features ##### + =============================================================================== + + *** TIM Driver: how to use the Break feature *** + ================================================ + [..] + After configuring the Timer channel(s) in the appropriate Output Compare mode: + + (#) Fill the TIM_BDTRInitStruct with the desired parameters for the Timer + Break Polarity, dead time, Lock level, the OSSI/OSSR State and the + AOE(automatic output enable). + + (#) Call TIM_BDTRConfig(TIMx, &TIM_BDTRInitStruct) to configure the Timer + + (#) Enable the Main Output using TIM_CtrlPWMOutputs(TIM1, ENABLE) + + (#) Once the break even occurs, the Timer's output signals are put in reset + state or in a known state (according to the configuration made in + TIM_BDTRConfig() function). + +@endverbatim + * @{ + */ + +/** + * @brief Configures the Break feature, dead time, Lock level, OSSI/OSSR State + * and the AOE(automatic output enable). + * @param TIMx: where x can be 1, 8, 15, 16, 20 or 17 to select the TIM + * @param TIM_BDTRInitStruct: pointer to a TIM_BDTRInitTypeDef structure that + * contains the BDTR Register configuration information for the TIM peripheral. + * @retval None + */ +void TIM_BDTRConfig(TIM_TypeDef* TIMx, TIM_BDTRInitTypeDef *TIM_BDTRInitStruct) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_TIM_OSSR_STATE(TIM_BDTRInitStruct->TIM_OSSRState)); + assert_param(IS_TIM_OSSI_STATE(TIM_BDTRInitStruct->TIM_OSSIState)); + assert_param(IS_TIM_LOCK_LEVEL(TIM_BDTRInitStruct->TIM_LOCKLevel)); + assert_param(IS_TIM_BREAK_STATE(TIM_BDTRInitStruct->TIM_Break)); + assert_param(IS_TIM_BREAK_POLARITY(TIM_BDTRInitStruct->TIM_BreakPolarity)); + assert_param(IS_TIM_AUTOMATIC_OUTPUT_STATE(TIM_BDTRInitStruct->TIM_AutomaticOutput)); + + /* Set the Lock level, the Break enable Bit and the Polarity, the OSSR State, + the OSSI State, the dead time value and the Automatic Output Enable Bit */ + TIMx->BDTR = (uint32_t)TIM_BDTRInitStruct->TIM_OSSRState | TIM_BDTRInitStruct->TIM_OSSIState | + TIM_BDTRInitStruct->TIM_LOCKLevel | TIM_BDTRInitStruct->TIM_DeadTime | + TIM_BDTRInitStruct->TIM_Break | TIM_BDTRInitStruct->TIM_BreakPolarity | + TIM_BDTRInitStruct->TIM_AutomaticOutput; +} + +/** + * @brief Configures the Break1 feature. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM + * @param TIM_Break1Polarity: specifies the Break1 polarity. + * This parameter can be one of the following values: + * @arg TIM_Break1Polarity_Low: Break1 input is active low + * @arg TIM_Break1Polarity_High: Break1 input is active high + * @param TIM_Break1Filter: specifies the Break1 filter value. + * This parameter must be a value between 0x00 and 0x0F + * @retval None + */ +void TIM_Break1Config(TIM_TypeDef* TIMx, uint32_t TIM_Break1Polarity, uint8_t TIM_Break1Filter) +{ /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_BREAK1_FILTER(TIM_Break1Filter)); + + /* Reset the BKP and BKF Bits */ + TIMx->BDTR &= (uint32_t)~ (TIM_BDTR_BKP | TIM_BDTR_BKF); + /* Configure the Break1 polarity and filter */ + TIMx->BDTR |= TIM_Break1Polarity |((uint32_t)TIM_Break1Filter << 16); +} + +/** + * @brief Configures the Break2 feature. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM + * @param TIM_Break2Polarity: specifies the Break2 polarity. + * This parameter can be one of the following values: + * @arg TIM_Break2Polarity_Low: Break2 input is active low + * @arg TIM_Break2Polarity_High: Break2 input is active high + * @param TIM_Break2Filter: specifies the Break2 filter value. + * This parameter must be a value between 0x00 and 0x0F + * @retval None + */ +void TIM_Break2Config(TIM_TypeDef* TIMx, uint32_t TIM_Break2Polarity, uint8_t TIM_Break2Filter) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_BREAK2_FILTER(TIM_Break2Filter)); + + /* Reset the BKP and BKF Bits */ + TIMx->BDTR &= (uint32_t)~ (TIM_BDTR_BK2P | TIM_BDTR_BK2F); + + /* Configure the Break1 polarity and filter */ + TIMx->BDTR |= TIM_Break2Polarity |((uint32_t)TIM_Break2Filter << 20); +} + +/** + * @brief Enables or disables the TIM Break1 input. + * @param TIMx: where x can be 1, 8, 20, 16 or 17 to select the TIMx peripheral. + * @param NewState: new state of the TIM Break1 input. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_Break1Cmd(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the Break1 */ + TIMx->BDTR |= TIM_BDTR_BKE; + } + else + { + /* Disable the Break1 */ + TIMx->BDTR &= (uint32_t)~TIM_BDTR_BKE; + } +} + +/** + * @brief Enables or disables the TIM Break2 input. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIMx peripheral. + * @param NewState: new state of the TIM Break2 input. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_Break2Cmd(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the Break1 */ + TIMx->BDTR |= TIM_BDTR_BK2E; + } + else + { + /* Disable the Break1 */ + TIMx->BDTR &= (uint32_t)~TIM_BDTR_BK2E; + } +} + +/** + * @brief Fills each TIM_BDTRInitStruct member with its default value. + * @param TIM_BDTRInitStruct: pointer to a TIM_BDTRInitTypeDef structure which + * will be initialized. + * @retval None + */ +void TIM_BDTRStructInit(TIM_BDTRInitTypeDef* TIM_BDTRInitStruct) +{ + /* Set the default configuration */ + TIM_BDTRInitStruct->TIM_OSSRState = TIM_OSSRState_Disable; + TIM_BDTRInitStruct->TIM_OSSIState = TIM_OSSIState_Disable; + TIM_BDTRInitStruct->TIM_LOCKLevel = TIM_LOCKLevel_OFF; + TIM_BDTRInitStruct->TIM_DeadTime = 0x00; + TIM_BDTRInitStruct->TIM_Break = TIM_Break_Disable; + TIM_BDTRInitStruct->TIM_BreakPolarity = TIM_BreakPolarity_Low; + TIM_BDTRInitStruct->TIM_AutomaticOutput = TIM_AutomaticOutput_Disable; +} + +/** + * @brief Enables or disables the TIM peripheral Main Outputs. + * @param TIMx: where x can be 1, 8, 15, 16, 20 or 17 to select the TIMx peripheral. + * @param NewState: new state of the TIM peripheral Main Outputs. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_CtrlPWMOutputs(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the TIM Main Output */ + TIMx->BDTR |= TIM_BDTR_MOE; + } + else + { + /* Disable the TIM Main Output */ + TIMx->BDTR &= (uint16_t)~TIM_BDTR_MOE; + } +} + +/** + * @brief Selects the TIM peripheral Commutation event. + * @param TIMx: where x can be 1, 8, 15, 16, 20 or 17 to select the TIMx peripheral + * @param NewState: new state of the Commutation event. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_SelectCOM(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the COM Bit */ + TIMx->CR2 |= TIM_CR2_CCUS; + } + else + { + /* Reset the COM Bit */ + TIMx->CR2 &= (uint16_t)~TIM_CR2_CCUS; + } +} + +/** + * @brief Sets or Resets the TIM peripheral Capture Compare Preload Control bit. + * @param TIMx: where x can be 1 or 8 or 20 to select the TIMx peripheral + * @param NewState: new state of the Capture Compare Preload Control bit + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_CCPreloadControl(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST6_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Set the CCPC Bit */ + TIMx->CR2 |= TIM_CR2_CCPC; + } + else + { + /* Reset the CCPC Bit */ + TIMx->CR2 &= (uint16_t)~TIM_CR2_CCPC; + } +} +/** + * @} + */ + +/** @defgroup TIM_Group5 Interrupts DMA and flags management functions + * @brief Interrupts, DMA and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts, DMA and flags management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified TIM interrupts. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIMx peripheral. + * @param TIM_IT: specifies the TIM interrupts sources to be enabled or disabled. + * This parameter can be any combination of the following values: + * @arg TIM_IT_Update: TIM update Interrupt source + * @arg TIM_IT_CC1: TIM Capture Compare 1 Interrupt source + * @arg TIM_IT_CC2: TIM Capture Compare 2 Interrupt source + * @arg TIM_IT_CC3: TIM Capture Compare 3 Interrupt source + * @arg TIM_IT_CC4: TIM Capture Compare 4 Interrupt source + * @arg TIM_IT_COM: TIM Commutation Interrupt source + * @arg TIM_IT_Trigger: TIM Trigger Interrupt source + * @arg TIM_IT_Break: TIM Break Interrupt source + * + * @note For TIM6 and TIM7 only the parameter TIM_IT_Update can be used + * @note For TIM9 and TIM12 only one of the following parameters can be used: TIM_IT_Update, + * TIM_IT_CC1, TIM_IT_CC2 or TIM_IT_Trigger. + * @note For TIM10, TIM11, TIM13 and TIM14 only one of the following parameters can + * be used: TIM_IT_Update or TIM_IT_CC1 + * @note TIM_IT_COM and TIM_IT_Break can be used only with TIM1 and TIM8 + * + * @param NewState: new state of the TIM interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_ITConfig(TIM_TypeDef* TIMx, uint16_t TIM_IT, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_IT(TIM_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the Interrupt sources */ + TIMx->DIER |= TIM_IT; + } + else + { + /* Disable the Interrupt sources */ + TIMx->DIER &= (uint16_t)~TIM_IT; + } +} + +/** + * @brief Configures the TIMx event to be generate by software. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_EventSource: specifies the event source. + * This parameter can be one or more of the following values: + * @arg TIM_EventSource_Update: Timer update Event source + * @arg TIM_EventSource_CC1: Timer Capture Compare 1 Event source + * @arg TIM_EventSource_CC2: Timer Capture Compare 2 Event source + * @arg TIM_EventSource_CC3: Timer Capture Compare 3 Event source + * @arg TIM_EventSource_CC4: Timer Capture Compare 4 Event source + * @arg TIM_EventSource_COM: Timer COM event source + * @arg TIM_EventSource_Trigger: Timer Trigger Event source + * @arg TIM_EventSource_Break: Timer Break event source + * + * @note TIM6 and TIM7 can only generate an update event. + * @note TIM_EventSource_COM and TIM_EventSource_Break are used only with TIM1 and TIM8. + * + * @retval None + */ +void TIM_GenerateEvent(TIM_TypeDef* TIMx, uint16_t TIM_EventSource) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_EVENT_SOURCE(TIM_EventSource)); + + /* Set the event sources */ + TIMx->EGR = TIM_EventSource; +} + +/** + * @brief Checks whether the specified TIM flag is set or not. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg TIM_FLAG_Update: TIM update Flag + * @arg TIM_FLAG_CC1: TIM Capture Compare 1 Flag + * @arg TIM_FLAG_CC2: TIM Capture Compare 2 Flag + * @arg TIM_FLAG_CC3: TIM Capture Compare 3 Flag + * @arg TIM_FLAG_CC4: TIM Capture Compare 4 Flag + * @arg TIM_FLAG_CC5: TIM Capture Compare 5 Flag + * @arg TIM_FLAG_CC6: TIM Capture Compare 6 Flag + * @arg TIM_FLAG_COM: TIM Commutation Flag + * @arg TIM_FLAG_Trigger: TIM Trigger Flag + * @arg TIM_FLAG_Break: TIM Break Flag + * @arg TIM_FLAG_CC1OF: TIM Capture Compare 1 over capture Flag + * @arg TIM_FLAG_CC2OF: TIM Capture Compare 2 over capture Flag + * @arg TIM_FLAG_CC3OF: TIM Capture Compare 3 over capture Flag + * @arg TIM_FLAG_CC4OF: TIM Capture Compare 4 over capture Flag + * + * @note TIM6 and TIM7 can have only one update flag. + * @note TIM_FLAG_COM and TIM_FLAG_Break are used only with TIM1 and TIM8. + * + * @retval The new state of TIM_FLAG (SET or RESET). + */ +FlagStatus TIM_GetFlagStatus(TIM_TypeDef* TIMx, uint32_t TIM_FLAG) +{ + ITStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_GET_FLAG(TIM_FLAG)); + + + if ((TIMx->SR & TIM_FLAG) != RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the TIMx's pending flags. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_FLAG: specifies the flag bit to clear. + * This parameter can be any combination of the following values: + * @arg TIM_FLAG_Update: TIM update Flag + * @arg TIM_FLAG_CC1: TIM Capture Compare 1 Flag + * @arg TIM_FLAG_CC2: TIM Capture Compare 2 Flag + * @arg TIM_FLAG_CC3: TIM Capture Compare 3 Flag + * @arg TIM_FLAG_CC4: TIM Capture Compare 4 Flag + * @arg TIM_FLAG_CC5: TIM Capture Compare 5 Flag + * @arg TIM_FLAG_CC6: TIM Capture Compare 6 Flag + * @arg TIM_FLAG_COM: TIM Commutation Flag + * @arg TIM_FLAG_Trigger: TIM Trigger Flag + * @arg TIM_FLAG_Break: TIM Break Flag + * @arg TIM_FLAG_CC1OF: TIM Capture Compare 1 over capture Flag + * @arg TIM_FLAG_CC2OF: TIM Capture Compare 2 over capture Flag + * @arg TIM_FLAG_CC3OF: TIM Capture Compare 3 over capture Flag + * @arg TIM_FLAG_CC4OF: TIM Capture Compare 4 over capture Flag + * + * @note TIM6 and TIM7 can have only one update flag. + * @note TIM_FLAG_COM and TIM_FLAG_Break are used only with TIM1 and TIM8. + * + * @retval None + */ +void TIM_ClearFlag(TIM_TypeDef* TIMx, uint16_t TIM_FLAG) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + /* Clear the flags */ + TIMx->SR = (uint16_t)~TIM_FLAG; +} + +/** + * @brief Checks whether the TIM interrupt has occurred or not. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_IT: specifies the TIM interrupt source to check. + * This parameter can be one of the following values: + * @arg TIM_IT_Update: TIM update Interrupt source + * @arg TIM_IT_CC1: TIM Capture Compare 1 Interrupt source + * @arg TIM_IT_CC2: TIM Capture Compare 2 Interrupt source + * @arg TIM_IT_CC3: TIM Capture Compare 3 Interrupt source + * @arg TIM_IT_CC4: TIM Capture Compare 4 Interrupt source + * @arg TIM_IT_COM: TIM Commutation Interrupt source + * @arg TIM_IT_Trigger: TIM Trigger Interrupt source + * @arg TIM_IT_Break: TIM Break Interrupt source + * + * @note TIM6 and TIM7 can generate only an update interrupt. + * @note TIM_IT_COM and TIM_IT_Break are used only with TIM1 and TIM8. + * + * @retval The new state of the TIM_IT(SET or RESET). + */ +ITStatus TIM_GetITStatus(TIM_TypeDef* TIMx, uint16_t TIM_IT) +{ + ITStatus bitstatus = RESET; + uint16_t itstatus = 0x0, itenable = 0x0; + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_GET_IT(TIM_IT)); + + itstatus = TIMx->SR & TIM_IT; + + itenable = TIMx->DIER & TIM_IT; + if ((itstatus != (uint16_t)RESET) && (itenable != (uint16_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the TIMx's interrupt pending bits. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_IT: specifies the pending bit to clear. + * This parameter can be any combination of the following values: + * @arg TIM_IT_Update: TIM1 update Interrupt source + * @arg TIM_IT_CC1: TIM Capture Compare 1 Interrupt source + * @arg TIM_IT_CC2: TIM Capture Compare 2 Interrupt source + * @arg TIM_IT_CC3: TIM Capture Compare 3 Interrupt source + * @arg TIM_IT_CC4: TIM Capture Compare 4 Interrupt source + * @arg TIM_IT_COM: TIM Commutation Interrupt source + * @arg TIM_IT_Trigger: TIM Trigger Interrupt source + * @arg TIM_IT_Break: TIM Break Interrupt source + * + * @note TIM6 and TIM7 can generate only an update interrupt. + * @note TIM_IT_COM and TIM_IT_Break are used only with TIM1 and TIM8. + * + * @retval None + */ +void TIM_ClearITPendingBit(TIM_TypeDef* TIMx, uint16_t TIM_IT) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + + /* Clear the IT pending Bit */ + TIMx->SR = (uint16_t)~TIM_IT; +} + +/** + * @brief Configures the TIMx's DMA interface. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_DMABase: DMA Base address. + * This parameter can be one of the following values: + * @arg TIM_DMABase_CR1 + * @arg TIM_DMABase_CR2 + * @arg TIM_DMABase_SMCR + * @arg TIM_DMABase_DIER + * @arg TIM1_DMABase_SR + * @arg TIM_DMABase_EGR + * @arg TIM_DMABase_CCMR1 + * @arg TIM_DMABase_CCMR2 + * @arg TIM_DMABase_CCER + * @arg TIM_DMABase_CNT + * @arg TIM_DMABase_PSC + * @arg TIM_DMABase_ARR + * @arg TIM_DMABase_RCR + * @arg TIM_DMABase_CCR1 + * @arg TIM_DMABase_CCR2 + * @arg TIM_DMABase_CCR3 + * @arg TIM_DMABase_CCR4 + * @arg TIM_DMABase_BDTR + * @arg TIM_DMABase_DCR + * @param TIM_DMABurstLength: DMA Burst length. This parameter can be one value + * between: TIM_DMABurstLength_1Transfer and TIM_DMABurstLength_18Transfers. + * @retval None + */ +void TIM_DMAConfig(TIM_TypeDef* TIMx, uint16_t TIM_DMABase, uint16_t TIM_DMABurstLength) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_DMA_BASE(TIM_DMABase)); + assert_param(IS_TIM_DMA_LENGTH(TIM_DMABurstLength)); + + /* Set the DMA Base and the DMA Burst Length */ + TIMx->DCR = TIM_DMABase | TIM_DMABurstLength; +} + +/** + * @brief Enables or disables the TIMx's DMA Requests. + * @param TIMx: where x can be 1, 2, 3, 4, 6, 7, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param TIM_DMASource: specifies the DMA Request sources. + * This parameter can be any combination of the following values: + * @arg TIM_DMA_Update: TIM update Interrupt source + * @arg TIM_DMA_CC1: TIM Capture Compare 1 DMA source + * @arg TIM_DMA_CC2: TIM Capture Compare 2 DMA source + * @arg TIM_DMA_CC3: TIM Capture Compare 3 DMA source + * @arg TIM_DMA_CC4: TIM Capture Compare 4 DMA source + * @arg TIM_DMA_COM: TIM Commutation DMA source + * @arg TIM_DMA_Trigger: TIM Trigger DMA source + * @param NewState: new state of the DMA Request sources. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_ALL_PERIPH(TIMx)); + assert_param(IS_TIM_DMA_SOURCE(TIM_DMASource)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the DMA sources */ + TIMx->DIER |= TIM_DMASource; + } + else + { + /* Disable the DMA sources */ + TIMx->DIER &= (uint16_t)~TIM_DMASource; + } +} + +/** + * @brief Selects the TIMx peripheral Capture Compare DMA source. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16, 20 or 17 to select the TIM peripheral. + * @param NewState: new state of the Capture Compare DMA source + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_SelectCCDMA(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the CCDS Bit */ + TIMx->CR2 |= TIM_CR2_CCDS; + } + else + { + /* Reset the CCDS Bit */ + TIMx->CR2 &= (uint16_t)~TIM_CR2_CCDS; + } +} +/** + * @} + */ + +/** @defgroup TIM_Group6 Clocks management functions + * @brief Clocks management functions + * +@verbatim + =============================================================================== + ##### Clocks management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures the TIMx internal Clock + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @retval None + */ +void TIM_InternalClockConfig(TIM_TypeDef* TIMx) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + + /* Disable slave mode to clock the prescaler directly with the internal clock */ + TIMx->SMCR &= (uint16_t)~TIM_SMCR_SMS; +} + +/** + * @brief Configures the TIMx Internal Trigger as External Clock + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param TIM_InputTriggerSource: Trigger source. + * This parameter can be one of the following values: + * @arg TIM_TS_ITR0: Internal Trigger 0 + * @arg TIM_TS_ITR1: Internal Trigger 1 + * @arg TIM_TS_ITR2: Internal Trigger 2 + * @arg TIM_TS_ITR3: Internal Trigger 3 + * @retval None + */ +void TIM_ITRxExternalClockConfig(TIM_TypeDef* TIMx, uint16_t TIM_InputTriggerSource) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_INTERNAL_TRIGGER_SELECTION(TIM_InputTriggerSource)); + + /* Select the Internal Trigger */ + TIM_SelectInputTrigger(TIMx, TIM_InputTriggerSource); + + /* Select the External clock mode1 */ + TIMx->SMCR |= TIM_SlaveMode_External1; +} + +/** + * @brief Configures the TIMx Trigger as External Clock + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 + * to select the TIM peripheral. + * @param TIM_TIxExternalCLKSource: Trigger source. + * This parameter can be one of the following values: + * @arg TIM_TIxExternalCLK1Source_TI1ED: TI1 Edge Detector + * @arg TIM_TIxExternalCLK1Source_TI1: Filtered Timer Input 1 + * @arg TIM_TIxExternalCLK1Source_TI2: Filtered Timer Input 2 + * @param TIM_ICPolarity: specifies the TIx Polarity. + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Rising + * @arg TIM_ICPolarity_Falling + * @param ICFilter: specifies the filter value. + * This parameter must be a value between 0x0 and 0xF. + * @retval None + */ +void TIM_TIxExternalClockConfig(TIM_TypeDef* TIMx, uint16_t TIM_TIxExternalCLKSource, + uint16_t TIM_ICPolarity, uint16_t ICFilter) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_IC_POLARITY(TIM_ICPolarity)); + assert_param(IS_TIM_IC_FILTER(ICFilter)); + + /* Configure the Timer Input Clock Source */ + if (TIM_TIxExternalCLKSource == TIM_TIxExternalCLK1Source_TI2) + { + TI2_Config(TIMx, TIM_ICPolarity, TIM_ICSelection_DirectTI, ICFilter); + } + else + { + TI1_Config(TIMx, TIM_ICPolarity, TIM_ICSelection_DirectTI, ICFilter); + } + /* Select the Trigger source */ + TIM_SelectInputTrigger(TIMx, TIM_TIxExternalCLKSource); + /* Select the External clock mode1 */ + TIMx->SMCR |= TIM_SlaveMode_External1; +} + +/** + * @brief Configures the External clock Mode1 + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ExtTRGPrescaler: The external Trigger Prescaler. + * This parameter can be one of the following values: + * @arg TIM_ExtTRGPSC_OFF: ETRP Prescaler OFF. + * @arg TIM_ExtTRGPSC_DIV2: ETRP frequency divided by 2. + * @arg TIM_ExtTRGPSC_DIV4: ETRP frequency divided by 4. + * @arg TIM_ExtTRGPSC_DIV8: ETRP frequency divided by 8. + * @param TIM_ExtTRGPolarity: The external Trigger Polarity. + * This parameter can be one of the following values: + * @arg TIM_ExtTRGPolarity_Inverted: active low or falling edge active. + * @arg TIM_ExtTRGPolarity_NonInverted: active high or rising edge active. + * @param ExtTRGFilter: External Trigger Filter. + * This parameter must be a value between 0x00 and 0x0F + * @retval None + */ +void TIM_ETRClockMode1Config(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, + uint16_t TIM_ExtTRGPolarity, uint16_t ExtTRGFilter) +{ + uint16_t tmpsmcr = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_EXT_PRESCALER(TIM_ExtTRGPrescaler)); + assert_param(IS_TIM_EXT_POLARITY(TIM_ExtTRGPolarity)); + assert_param(IS_TIM_EXT_FILTER(ExtTRGFilter)); + /* Configure the ETR Clock source */ + TIM_ETRConfig(TIMx, TIM_ExtTRGPrescaler, TIM_ExtTRGPolarity, ExtTRGFilter); + + /* Get the TIMx SMCR register value */ + tmpsmcr = TIMx->SMCR; + + /* Reset the SMS Bits */ + tmpsmcr &= (uint16_t)~TIM_SMCR_SMS; + + /* Select the External clock mode1 */ + tmpsmcr |= TIM_SlaveMode_External1; + + /* Select the Trigger selection : ETRF */ + tmpsmcr &= (uint16_t)~TIM_SMCR_TS; + tmpsmcr |= TIM_TS_ETRF; + + /* Write to TIMx SMCR */ + TIMx->SMCR = tmpsmcr; +} + +/** + * @brief Configures the External clock Mode2 + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ExtTRGPrescaler: The external Trigger Prescaler. + * This parameter can be one of the following values: + * @arg TIM_ExtTRGPSC_OFF: ETRP Prescaler OFF. + * @arg TIM_ExtTRGPSC_DIV2: ETRP frequency divided by 2. + * @arg TIM_ExtTRGPSC_DIV4: ETRP frequency divided by 4. + * @arg TIM_ExtTRGPSC_DIV8: ETRP frequency divided by 8. + * @param TIM_ExtTRGPolarity: The external Trigger Polarity. + * This parameter can be one of the following values: + * @arg TIM_ExtTRGPolarity_Inverted: active low or falling edge active. + * @arg TIM_ExtTRGPolarity_NonInverted: active high or rising edge active. + * @param ExtTRGFilter: External Trigger Filter. + * This parameter must be a value between 0x00 and 0x0F + * @retval None + */ +void TIM_ETRClockMode2Config(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, + uint16_t TIM_ExtTRGPolarity, uint16_t ExtTRGFilter) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_EXT_PRESCALER(TIM_ExtTRGPrescaler)); + assert_param(IS_TIM_EXT_POLARITY(TIM_ExtTRGPolarity)); + assert_param(IS_TIM_EXT_FILTER(ExtTRGFilter)); + + /* Configure the ETR Clock source */ + TIM_ETRConfig(TIMx, TIM_ExtTRGPrescaler, TIM_ExtTRGPolarity, ExtTRGFilter); + + /* Enable the External clock mode2 */ + TIMx->SMCR |= TIM_SMCR_ECE; +} +/** + * @} + */ + +/** @defgroup TIM_Group7 Synchronization management functions + * @brief Synchronization management functions + * +@verbatim + =============================================================================== + ##### Synchronization management functions ##### + =============================================================================== + + *** TIM Driver: how to use it in synchronization Mode *** + ========================================================= + [..] Case of two/several Timers + + (#) Configure the Master Timers using the following functions: + (++) void TIM_SelectOutputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_TRGOSource); + (++) void TIM_SelectMasterSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_MasterSlaveMode); + (#) Configure the Slave Timers using the following functions: + (++) void TIM_SelectInputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_InputTriggerSource); + (++) void TIM_SelectSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_SlaveMode); + + [..] Case of Timers and external trigger(ETR pin) + + (#) Configure the External trigger using this function: + (++) void TIM_ETRConfig(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, uint16_t TIM_ExtTRGPolarity, + uint16_t ExtTRGFilter); + (#) Configure the Slave Timers using the following functions: + (++) void TIM_SelectInputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_InputTriggerSource); + (++) void TIM_SelectSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_SlaveMode); + +@endverbatim + * @{ + */ + +/** + * @brief Selects the Input Trigger source + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 + * to select the TIM peripheral. + * @param TIM_InputTriggerSource: The Input Trigger source. + * This parameter can be one of the following values: + * @arg TIM_TS_ITR0: Internal Trigger 0 + * @arg TIM_TS_ITR1: Internal Trigger 1 + * @arg TIM_TS_ITR2: Internal Trigger 2 + * @arg TIM_TS_ITR3: Internal Trigger 3 + * @arg TIM_TS_TI1F_ED: TI1 Edge Detector + * @arg TIM_TS_TI1FP1: Filtered Timer Input 1 + * @arg TIM_TS_TI2FP2: Filtered Timer Input 2 + * @arg TIM_TS_ETRF: External Trigger input + * @retval None + */ +void TIM_SelectInputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_InputTriggerSource) +{ + uint16_t tmpsmcr = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_TRIGGER_SELECTION(TIM_InputTriggerSource)); + + /* Get the TIMx SMCR register value */ + tmpsmcr = TIMx->SMCR; + + /* Reset the TS Bits */ + tmpsmcr &= (uint16_t)~TIM_SMCR_TS; + + /* Set the Input Trigger source */ + tmpsmcr |= TIM_InputTriggerSource; + + /* Write to TIMx SMCR */ + TIMx->SMCR = tmpsmcr; +} + +/** + * @brief Selects the TIMx Trigger Output Mode. + * @param TIMx: where x can be 1, 2, 3, 4, 5, 6, 7, 8 or 15 to select the TIM peripheral. + * + * @param TIM_TRGOSource: specifies the Trigger Output source. + * This parameter can be one of the following values: + * + * - For all TIMx + * @arg TIM_TRGOSource_Reset: The UG bit in the TIM_EGR register is used as the trigger output(TRGO) + * @arg TIM_TRGOSource_Enable: The Counter Enable CEN is used as the trigger output(TRGO) + * @arg TIM_TRGOSource_Update: The update event is selected as the trigger output(TRGO) + * + * - For all TIMx except TIM6 and TIM7 + * @arg TIM_TRGOSource_OC1: The trigger output sends a positive pulse when the CC1IF flag + * is to be set, as soon as a capture or compare match occurs(TRGO) + * @arg TIM_TRGOSource_OC1Ref: OC1REF signal is used as the trigger output(TRGO) + * @arg TIM_TRGOSource_OC2Ref: OC2REF signal is used as the trigger output(TRGO) + * @arg TIM_TRGOSource_OC3Ref: OC3REF signal is used as the trigger output(TRGO) + * @arg TIM_TRGOSource_OC4Ref: OC4REF signal is used as the trigger output(TRGO) + * + * @retval None + */ +void TIM_SelectOutputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_TRGOSource) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST7_PERIPH(TIMx)); + assert_param(IS_TIM_TRGO_SOURCE(TIM_TRGOSource)); + + /* Reset the MMS Bits */ + TIMx->CR2 &= (uint16_t)~TIM_CR2_MMS; + /* Select the TRGO source */ + TIMx->CR2 |= TIM_TRGOSource; +} + +/** + * @brief Selects the TIMx Trigger Output Mode2 (TRGO2). + * @param TIMx: where x can be 1 or 8 or 20 to select the TIM peripheral. + * + * @param TIM_TRGO2Source: specifies the Trigger Output source. + * This parameter can be one of the following values: + * + * - For all TIMx + * @arg TIM_TRGOSource_Reset: The UG bit in the TIM_EGR register is used as the trigger output(TRGO2) + * @arg TIM_TRGOSource_Enable: The Counter Enable CEN is used as the trigger output(TRGO2) + * @arg TIM_TRGOSource_Update: The update event is selected as the trigger output(TRGO2) + * @arg TIM_TRGOSource_OC1: The trigger output sends a positive pulse when the CC1IF flag + * is to be set, as soon as a capture or compare match occurs(TRGO2) + * @arg TIM_TRGOSource_OC1Ref: OC1REF signal is used as the trigger output(TRGO2) + * @arg TIM_TRGOSource_OC2Ref: OC2REF signal is used as the trigger output(TRGO2) + * @arg TIM_TRGOSource_OC3Ref: OC3REF signal is used as the trigger output(TRGO2) + * @arg TIM_TRGOSource_OC4Ref: OC4REF signal is used as the trigger output(TRGO2) + * @arg TIM_TRGO2Source_OC4Ref_RisingFalling: OC4Ref Rising and Falling are used as the trigger output(TRGO2) + * @arg TIM_TRGO2Source_OC6Ref_RisingFalling: OC6Ref Rising and Falling are used as the trigger output(TRGO2) + * @arg TIM_TRGO2Source_OC4RefRising_OC6RefRising: OC4Ref Rising and OC6Ref Rising are used as the trigger output(TRGO2) + * @arg TIM_TRGO2Source_OC4RefRising_OC6RefFalling: OC4Ref Rising and OC6Ref Falling are used as the trigger output(TRGO2) + * @arg TIM_TRGO2Source_OC5RefRising_OC6RefRising: OC5Ref Rising and OC6Ref Rising are used as the trigger output(TRGO2) + * @arg TIM_TRGO2Source_OC5RefRising_OC6RefFalling: OC5Ref Rising and OC6Ref Falling are used as the trigger output(TRGO2) + * + * @retval None + */ +void TIM_SelectOutputTrigger2(TIM_TypeDef* TIMx, uint32_t TIM_TRGO2Source) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST4_PERIPH(TIMx)); + assert_param(IS_TIM_TRGO2_SOURCE(TIM_TRGO2Source)); + + /* Reset the MMS Bits */ + TIMx->CR2 &= (uint32_t)~TIM_CR2_MMS2; + /* Select the TRGO source */ + TIMx->CR2 |= TIM_TRGO2Source; +} + +/** + * @brief Selects the TIMx Slave Mode. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM peripheral. + * @param TIM_SlaveMode: specifies the Timer Slave Mode. + * This parameter can be one of the following values: + * @arg TIM_SlaveMode_Reset: Rising edge of the selected trigger signal(TRGI) reinitialize + * the counter and triggers an update of the registers + * @arg TIM_SlaveMode_Gated: The counter clock is enabled when the trigger signal (TRGI) is high + * @arg TIM_SlaveMode_Trigger: The counter starts at a rising edge of the trigger TRGI + * @arg TIM_SlaveMode_External1: Rising edges of the selected trigger (TRGI) clock the counter + * @arg TIM_SlaveMode_Combined_ResetTrigger: Rising edge of the selected trigger input (TRGI) + * reinitializes the counter, generates an update + * of the registers and starts the counter. + * @retval None + */ +void TIM_SelectSlaveMode(TIM_TypeDef* TIMx, uint32_t TIM_SlaveMode) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_SLAVE_MODE(TIM_SlaveMode)); + + /* Reset the SMS Bits */ + TIMx->SMCR &= (uint32_t)~TIM_SMCR_SMS; + + /* Select the Slave Mode */ + TIMx->SMCR |= (uint32_t)TIM_SlaveMode; +} + +/** + * @brief Sets or Resets the TIMx Master/Slave Mode. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM peripheral. + * @param TIM_MasterSlaveMode: specifies the Timer Master Slave Mode. + * This parameter can be one of the following values: + * @arg TIM_MasterSlaveMode_Enable: synchronization between the current timer + * and its slaves (through TRGO) + * @arg TIM_MasterSlaveMode_Disable: No action + * @retval None + */ +void TIM_SelectMasterSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_MasterSlaveMode) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_TIM_MSM_STATE(TIM_MasterSlaveMode)); + + /* Reset the MSM Bit */ + TIMx->SMCR &= (uint16_t)~TIM_SMCR_MSM; + + /* Set or Reset the MSM Bit */ + TIMx->SMCR |= TIM_MasterSlaveMode; +} + +/** + * @brief Configures the TIMx External Trigger (ETR). + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM peripheral. + * @param TIM_ExtTRGPrescaler: The external Trigger Prescaler. + * This parameter can be one of the following values: + * @arg TIM_ExtTRGPSC_OFF: ETRP Prescaler OFF. + * @arg TIM_ExtTRGPSC_DIV2: ETRP frequency divided by 2. + * @arg TIM_ExtTRGPSC_DIV4: ETRP frequency divided by 4. + * @arg TIM_ExtTRGPSC_DIV8: ETRP frequency divided by 8. + * @param TIM_ExtTRGPolarity: The external Trigger Polarity. + * This parameter can be one of the following values: + * @arg TIM_ExtTRGPolarity_Inverted: active low or falling edge active. + * @arg TIM_ExtTRGPolarity_NonInverted: active high or rising edge active. + * @param ExtTRGFilter: External Trigger Filter. + * This parameter must be a value between 0x00 and 0x0F + * @retval None + */ +void TIM_ETRConfig(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler, + uint16_t TIM_ExtTRGPolarity, uint16_t ExtTRGFilter) +{ + uint16_t tmpsmcr = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_EXT_PRESCALER(TIM_ExtTRGPrescaler)); + assert_param(IS_TIM_EXT_POLARITY(TIM_ExtTRGPolarity)); + assert_param(IS_TIM_EXT_FILTER(ExtTRGFilter)); + + tmpsmcr = TIMx->SMCR; + + /* Reset the ETR Bits */ + tmpsmcr &= SMCR_ETR_MASK; + + /* Set the Prescaler, the Filter value and the Polarity */ + tmpsmcr |= (uint16_t)(TIM_ExtTRGPrescaler | (uint16_t)(TIM_ExtTRGPolarity | (uint16_t)(ExtTRGFilter << (uint16_t)8))); + + /* Write to TIMx SMCR */ + TIMx->SMCR = tmpsmcr; +} +/** + * @} + */ + +/** @defgroup TIM_Group8 Specific interface management functions + * @brief Specific interface management functions + * +@verbatim + =============================================================================== + ##### Specific interface management functions ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures the TIMx Encoder Interface. + * @param TIMx: where x can be 1, 2, 3, 4, 20 or 8 to select the TIM + * peripheral. + * @param TIM_EncoderMode: specifies the TIMx Encoder Mode. + * This parameter can be one of the following values: + * @arg TIM_EncoderMode_TI1: Counter counts on TI1FP1 edge depending on TI2FP2 level. + * @arg TIM_EncoderMode_TI2: Counter counts on TI2FP2 edge depending on TI1FP1 level. + * @arg TIM_EncoderMode_TI12: Counter counts on both TI1FP1 and TI2FP2 edges depending + * on the level of the other input. + * @param TIM_IC1Polarity: specifies the IC1 Polarity + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Falling: IC Falling edge. + * @arg TIM_ICPolarity_Rising: IC Rising edge. + * @param TIM_IC2Polarity: specifies the IC2 Polarity + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Falling: IC Falling edge. + * @arg TIM_ICPolarity_Rising: IC Rising edge. + * @retval None + */ +void TIM_EncoderInterfaceConfig(TIM_TypeDef* TIMx, uint16_t TIM_EncoderMode, + uint16_t TIM_IC1Polarity, uint16_t TIM_IC2Polarity) +{ + uint16_t tmpsmcr = 0; + uint16_t tmpccmr1 = 0; + uint16_t tmpccer = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST3_PERIPH(TIMx)); + assert_param(IS_TIM_ENCODER_MODE(TIM_EncoderMode)); + assert_param(IS_TIM_IC_POLARITY(TIM_IC1Polarity)); + assert_param(IS_TIM_IC_POLARITY(TIM_IC2Polarity)); + + /* Get the TIMx SMCR register value */ + tmpsmcr = TIMx->SMCR; + + /* Get the TIMx CCMR1 register value */ + tmpccmr1 = TIMx->CCMR1; + + /* Get the TIMx CCER register value */ + tmpccer = TIMx->CCER; + + /* Set the encoder Mode */ + tmpsmcr &= (uint16_t)~TIM_SMCR_SMS; + tmpsmcr |= TIM_EncoderMode; + + /* Select the Capture Compare 1 and the Capture Compare 2 as input */ + tmpccmr1 &= ((uint16_t)~TIM_CCMR1_CC1S) & ((uint16_t)~TIM_CCMR1_CC2S); + tmpccmr1 |= TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0; + + /* Set the TI1 and the TI2 Polarities */ + tmpccer &= ((uint16_t)~TIM_CCER_CC1P) & ((uint16_t)~TIM_CCER_CC2P); + tmpccer |= (uint16_t)(TIM_IC1Polarity | (uint16_t)(TIM_IC2Polarity << (uint16_t)4)); + + /* Write to TIMx SMCR */ + TIMx->SMCR = tmpsmcr; + + /* Write to TIMx CCMR1 */ + TIMx->CCMR1 = tmpccmr1; + + /* Write to TIMx CCER */ + TIMx->CCER = tmpccer; +} + +/** + * @brief Enables or disables the TIMx's Hall sensor interface. + * @param TIMx: where x can be 1, 2, 3, 4, 8, 20 or 15 to select the TIM + * peripheral. + * @param NewState: new state of the TIMx Hall sensor interface. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void TIM_SelectHallSensor(TIM_TypeDef* TIMx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST2_PERIPH(TIMx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Set the TI1S Bit */ + TIMx->CR2 |= TIM_CR2_TI1S; + } + else + { + /* Reset the TI1S Bit */ + TIMx->CR2 &= (uint16_t)~TIM_CR2_TI1S; + } +} +/** + * @} + */ + +/** @defgroup TIM_Group9 Specific remapping management function + * @brief Specific remapping management function + * +@verbatim + =============================================================================== + ##### Specific remapping management function ##### + =============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Configures the TIM16 Remapping input Capabilities. + * @param TIMx: where x can be 1, 8, 20 or 16 to select the TIM peripheral. + * @param TIM_Remap: specifies the TIM input remapping source. + * This parameter can be one of the following values: + * @arg TIM16_GPIO: TIM16 Channel 1 is connected to GPIO. + * @arg TIM16_RTC_CLK: TIM16 Channel 1 is connected to RTC input clock. + * @arg TIM16_HSE_DIV32: TIM16 Channel 1 is connected to HSE/32 clock. + * @arg TIM16_MCO: TIM16 Channel 1 is connected to MCO clock. + * @arg TIM1_ADC1_AWDG1: TIM1 ETR is connected to ADC1 AWDG1. + * @arg TIM1_ADC1_AWDG2: TIM1 ETR is connected to ADC1 AWDG2. + * @arg TIM1_ADC1_AWDG3: TIM1 ETR is connected to ADC1 AWDG3. + * @arg TIM1_ADC4_AWDG1: TIM1 ETR is connected to ADC4 AWDG1. + * @arg TIM1_ADC4_AWDG2: TIM1 ETR is connected to ADC4 AWDG2. + * @arg TIM1_ADC4_AWDG3: TIM1 ETR is connected to ADC4 AWDG3. + * @arg TIM8_ADC2_AWDG1: TIM8 ETR is connected to ADC2 AWDG1. + * @arg TIM8_ADC2_AWDG2: TIM8 ETR is connected to ADC2 AWDG2. + * @arg TIM8_ADC2_AWDG3: TIM8 ETR is connected to ADC2 AWDG3. + * @arg TIM8_ADC4_AWDG1: TIM8 ETR is connected to ADC4 AWDG1. + * @arg TIM8_ADC4_AWDG2: TIM8 ETR is connected to ADC4 AWDG2. + * @arg TIM8_ADC4_AWDG3: TIM8 ETR is connected to ADC4 AWDG3. + * @arg TIM20_ADC3_AWDG1: TIM20 ETR is connected to ADC3 AWDG1. + * @arg TIM20_ADC3_AWDG2: TIM20 ETR is connected to ADC3 AWDG2. + * @arg TIM20_ADC3_AWDG3: TIM20 ETR is connected to ADC3 AWDG3. + * @arg TIM20_ADC4_AWDG1: TIM20 ETR is connected to ADC4 AWDG1. + * @arg TIM20_ADC4_AWDG2: TIM20 ETR is connected to ADC4 AWDG2. + * @arg TIM20_ADC4_AWDG3: TIM20 ETR is connected to ADC4 AWDG3. + * @retval : None + */ +void TIM_RemapConfig(TIM_TypeDef* TIMx, uint16_t TIM_Remap) +{ + /* Check the parameters */ + assert_param(IS_TIM_LIST8_PERIPH(TIMx)); + assert_param(IS_TIM_REMAP(TIM_Remap)); + + /* Set the Timer remapping configuration */ + TIMx->OR = TIM_Remap; +} +/** + * @} + */ + +/** + * @brief Configure the TI1 as Input. + * @param TIMx: where x can be 1, 2, 3, 4, 5, 8, 9, 10, 11, 12, 13 or 14 + * to select the TIM peripheral. + * @param TIM_ICPolarity : The Input Polarity. + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Rising + * @arg TIM_ICPolarity_Falling + * @arg TIM_ICPolarity_BothEdge + * @param TIM_ICSelection: specifies the input to be used. + * This parameter can be one of the following values: + * @arg TIM_ICSelection_DirectTI: TIM Input 1 is selected to be connected to IC1. + * @arg TIM_ICSelection_IndirectTI: TIM Input 1 is selected to be connected to IC2. + * @arg TIM_ICSelection_TRC: TIM Input 1 is selected to be connected to TRC. + * @param TIM_ICFilter: Specifies the Input Capture Filter. + * This parameter must be a value between 0x00 and 0x0F. + * @retval None + */ +static void TI1_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter) +{ + uint32_t tmpccmr1 = 0, tmpccer = 0; + + /* Disable the Channel 1: Reset the CC1E Bit */ + TIMx->CCER &= (uint32_t)~TIM_CCER_CC1E; + tmpccmr1 = TIMx->CCMR1; + tmpccer = TIMx->CCER; + + /* Select the Input and set the filter */ + tmpccmr1 &= ((uint32_t)~TIM_CCMR1_CC1S) & ((uint32_t)~TIM_CCMR1_IC1F); + tmpccmr1 |= (uint32_t)(TIM_ICSelection | (uint32_t)((uint32_t)TIM_ICFilter << 4)); + + /* Select the Polarity and set the CC1E Bit */ + tmpccer &= (uint32_t)~(TIM_CCER_CC1P | TIM_CCER_CC1NP); + tmpccer |= (uint32_t)(TIM_ICPolarity | (uint32_t)TIM_CCER_CC1E); + + /* Write to TIMx CCMR1 and CCER registers */ + TIMx->CCMR1 = tmpccmr1; + TIMx->CCER = tmpccer; +} + +/** + * @brief Configure the TI2 as Input. + * @param TIMx: where x can be 1, 2, 3, 4, 5, 8, 9 or 12 to select the TIM + * peripheral. + * @param TIM_ICPolarity : The Input Polarity. + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Rising + * @arg TIM_ICPolarity_Falling + * @arg TIM_ICPolarity_BothEdge + * @param TIM_ICSelection: specifies the input to be used. + * This parameter can be one of the following values: + * @arg TIM_ICSelection_DirectTI: TIM Input 2 is selected to be connected to IC2. + * @arg TIM_ICSelection_IndirectTI: TIM Input 2 is selected to be connected to IC1. + * @arg TIM_ICSelection_TRC: TIM Input 2 is selected to be connected to TRC. + * @param TIM_ICFilter: Specifies the Input Capture Filter. + * This parameter must be a value between 0x00 and 0x0F. + * @retval None + */ +static void TI2_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter) +{ + uint32_t tmpccmr1 = 0, tmpccer = 0, tmp = 0; + + /* Disable the Channel 2: Reset the CC2E Bit */ + TIMx->CCER &= (uint16_t)~TIM_CCER_CC2E; + tmpccmr1 = TIMx->CCMR1; + tmpccer = TIMx->CCER; + tmp = (uint16_t)(TIM_ICPolarity << 4); + + /* Select the Input and set the filter */ + tmpccmr1 &= ((uint32_t)~TIM_CCMR1_CC2S) & ((uint32_t)~TIM_CCMR1_IC2F); + tmpccmr1 |= (uint32_t)((uint32_t)TIM_ICFilter << 12); + tmpccmr1 |= (uint32_t)((uint32_t)TIM_ICSelection << 8); + + /* Select the Polarity and set the CC2E Bit */ + tmpccer &= (uint16_t)~(TIM_CCER_CC2P | TIM_CCER_CC2NP); + tmpccer |= (uint16_t)(tmp | (uint16_t)TIM_CCER_CC2E); + + /* Write to TIMx CCMR1 and CCER registers */ + TIMx->CCMR1 = tmpccmr1 ; + TIMx->CCER = tmpccer; +} + +/** + * @brief Configure the TI3 as Input. + * @param TIMx: where x can be 1, 2, 3, 4, 5 or 8 to select the TIM peripheral. + * @param TIM_ICPolarity : The Input Polarity. + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Rising + * @arg TIM_ICPolarity_Falling + * @arg TIM_ICPolarity_BothEdge + * @param TIM_ICSelection: specifies the input to be used. + * This parameter can be one of the following values: + * @arg TIM_ICSelection_DirectTI: TIM Input 3 is selected to be connected to IC3. + * @arg TIM_ICSelection_IndirectTI: TIM Input 3 is selected to be connected to IC4. + * @arg TIM_ICSelection_TRC: TIM Input 3 is selected to be connected to TRC. + * @param TIM_ICFilter: Specifies the Input Capture Filter. + * This parameter must be a value between 0x00 and 0x0F. + * @retval None + */ +static void TI3_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter) +{ + uint16_t tmpccmr2 = 0, tmpccer = 0, tmp = 0; + + /* Disable the Channel 3: Reset the CC3E Bit */ + TIMx->CCER &= (uint16_t)~TIM_CCER_CC3E; + tmpccmr2 = TIMx->CCMR2; + tmpccer = TIMx->CCER; + tmp = (uint16_t)(TIM_ICPolarity << 8); + + /* Select the Input and set the filter */ + tmpccmr2 &= ((uint16_t)~TIM_CCMR1_CC1S) & ((uint16_t)~TIM_CCMR2_IC3F); + tmpccmr2 |= (uint16_t)(TIM_ICSelection | (uint16_t)(TIM_ICFilter << (uint16_t)4)); + + /* Select the Polarity and set the CC3E Bit */ + tmpccer &= (uint16_t)~(TIM_CCER_CC3P | TIM_CCER_CC3NP); + tmpccer |= (uint16_t)(tmp | (uint16_t)TIM_CCER_CC3E); + + /* Write to TIMx CCMR2 and CCER registers */ + TIMx->CCMR2 = tmpccmr2; + TIMx->CCER = tmpccer; +} + +/** + * @brief Configure the TI4 as Input. + * @param TIMx: where x can be 1, 2, 3, 4, 5 or 8 to select the TIM peripheral. + * @param TIM_ICPolarity : The Input Polarity. + * This parameter can be one of the following values: + * @arg TIM_ICPolarity_Rising + * @arg TIM_ICPolarity_Falling + * @arg TIM_ICPolarity_BothEdge + * @param TIM_ICSelection: specifies the input to be used. + * This parameter can be one of the following values: + * @arg TIM_ICSelection_DirectTI: TIM Input 4 is selected to be connected to IC4. + * @arg TIM_ICSelection_IndirectTI: TIM Input 4 is selected to be connected to IC3. + * @arg TIM_ICSelection_TRC: TIM Input 4 is selected to be connected to TRC. + * @param TIM_ICFilter: Specifies the Input Capture Filter. + * This parameter must be a value between 0x00 and 0x0F. + * @retval None + */ +static void TI4_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_ICSelection, + uint16_t TIM_ICFilter) +{ + uint16_t tmpccmr2 = 0, tmpccer = 0, tmp = 0; + + /* Disable the Channel 4: Reset the CC4E Bit */ + TIMx->CCER &= (uint16_t)~TIM_CCER_CC4E; + tmpccmr2 = TIMx->CCMR2; + tmpccer = TIMx->CCER; + tmp = (uint16_t)(TIM_ICPolarity << 12); + + /* Select the Input and set the filter */ + tmpccmr2 &= ((uint16_t)~TIM_CCMR1_CC2S) & ((uint16_t)~TIM_CCMR1_IC2F); + tmpccmr2 |= (uint16_t)(TIM_ICSelection << 8); + tmpccmr2 |= (uint16_t)(TIM_ICFilter << 12); + + /* Select the Polarity and set the CC4E Bit */ + tmpccer &= (uint16_t)~(TIM_CCER_CC4P | TIM_CCER_CC4NP); + tmpccer |= (uint16_t)(tmp | (uint16_t)TIM_CCER_CC4E); + + /* Write to TIMx CCMR2 and CCER registers */ + TIMx->CCMR2 = tmpccmr2; + TIMx->CCER = tmpccer ; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_usart.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_usart.c new file mode 100755 index 0000000000..8955c8d157 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_usart.c @@ -0,0 +1,2084 @@ +/** + ****************************************************************************** + * @file stm32f30x_usart.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Universal synchronous asynchronous receiver + * transmitter (USART): + * + Initialization and Configuration + * + STOP Mode + * + AutoBaudRate + * + Data transfers + * + Multi-Processor Communication + * + LIN mode + * + Half-duplex mode + * + Smartcard mode + * + IrDA mode + * + RS485 mode + * + DMA transfers management + * + Interrupts and flags management + * + * @verbatim + =============================================================================== + ##### How to use this driver ##### + =============================================================================== + [..] + (#) Enable peripheral clock using RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE) + function for USART1 or using RCC_APB1PeriphClockCmd(RCC_APB1Periph_USARTx, ENABLE) + function for USART2, USART3, UART4 and UART5. + (#) According to the USART mode, enable the GPIO clocks using + RCC_AHBPeriphClockCmd() function. (The I/O can be TX, RX, CTS, + or and SCLK). + (#) Peripheral's alternate function: + (++) Connect the pin to the desired peripherals' Alternate + Function (AF) using GPIO_PinAFConfig() function. + (++) Configure the desired pin in alternate function by: + GPIO_InitStruct->GPIO_Mode = GPIO_Mode_AF. + (++) Select the type, pull-up/pull-down and output speed via + GPIO_PuPd, GPIO_OType and GPIO_Speed members. + (++) Call GPIO_Init() function. + (#) Program the Baud Rate, Word Length , Stop Bit, Parity, Hardware + flow control and Mode(Receiver/Transmitter) using the SPI_Init() + function. + (#) For synchronous mode, enable the clock and program the polarity, + phase and last bit using the USART_ClockInit() function. + (#) Enable the USART using the USART_Cmd() function. + (#) Enable the NVIC and the corresponding interrupt using the function + USART_ITConfig() if you need to use interrupt mode. + (#) When using the DMA mode: + (++) Configure the DMA using DMA_Init() function. + (++) Activate the needed channel Request using USART_DMACmd() function. + (#) Enable the DMA using the DMA_Cmd() function, when using DMA mode. + [..] + Refer to Multi-Processor, LIN, half-duplex, Smartcard, IrDA sub-sections + for more details. + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_usart.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup USART + * @brief USART driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ + +/*!< USART CR1 register clear Mask ((~(uint32_t)0xFFFFE6F3)) */ +#define CR1_CLEAR_MASK ((uint32_t)(USART_CR1_M | USART_CR1_PCE | \ + USART_CR1_PS | USART_CR1_TE | \ + USART_CR1_RE)) + +/*!< USART CR2 register clock bits clear Mask ((~(uint32_t)0xFFFFF0FF)) */ +#define CR2_CLOCK_CLEAR_MASK ((uint32_t)(USART_CR2_CLKEN | USART_CR2_CPOL | \ + USART_CR2_CPHA | USART_CR2_LBCL)) + +/*!< USART CR3 register clear Mask ((~(uint32_t)0xFFFFFCFF)) */ +#define CR3_CLEAR_MASK ((uint32_t)(USART_CR3_RTSE | USART_CR3_CTSE)) + +/*!< USART Interrupts mask */ +#define IT_MASK ((uint32_t)0x000000FF) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup USART_Private_Functions + * @{ + */ + +/** @defgroup USART_Group1 Initialization and Configuration functions + * @brief Initialization and Configuration functions + * +@verbatim + =============================================================================== + ##### Initialization and Configuration functions ##### + =============================================================================== + [..] + This subsection provides a set of functions allowing to initialize the USART + in asynchronous and in synchronous modes. + (+) For the asynchronous mode only these parameters can be configured: + (++) Baud Rate. + (++) Word Length. + (++) Stop Bit. + (++) Parity: If the parity is enabled, then the MSB bit of the data written + in the data register is transmitted but is changed by the parity bit. + Depending on the frame length defined by the M bit (8-bits or 9-bits), + the possible USART frame formats are as listed in the following table: + [..] + +-------------------------------------------------------------+ + | M bit | PCE bit | USART frame | + |---------------------|---------------------------------------| + | 0 | 0 | | SB | 8 bit data | STB | | + |---------|-----------|---------------------------------------| + | 0 | 1 | | SB | 7 bit data | PB | STB | | + |---------|-----------|---------------------------------------| + | 1 | 0 | | SB | 9 bit data | STB | | + |---------|-----------|---------------------------------------| + | 1 | 1 | | SB | 8 bit data | PB | STB | | + +-------------------------------------------------------------+ + [..] + (++) Hardware flow control. + (++) Receiver/transmitter modes. + [..] The USART_Init() function follows the USART asynchronous configuration + procedure(details for the procedure are available in reference manual. + (+) For the synchronous mode in addition to the asynchronous mode parameters + these parameters should be also configured: + (++) USART Clock Enabled. + (++) USART polarity. + (++) USART phase. + (++) USART LastBit. + [..] These parameters can be configured using the USART_ClockInit() function. + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the USARTx peripheral registers to their default reset values. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @retval None + */ +void USART_DeInit(USART_TypeDef* USARTx) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + + if (USARTx == USART1) + { + RCC_APB2PeriphResetCmd(RCC_APB2Periph_USART1, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_USART1, DISABLE); + } + else if (USARTx == USART2) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_USART2, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_USART2, DISABLE); + } + else if (USARTx == USART3) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_USART3, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_USART3, DISABLE); + } + else if (USARTx == UART4) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_UART4, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_UART4, DISABLE); + } + else + { + if (USARTx == UART5) + { + RCC_APB1PeriphResetCmd(RCC_APB1Periph_UART5, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_UART5, DISABLE); + } + } +} + +/** + * @brief Initializes the USARTx peripheral according to the specified + * parameters in the USART_InitStruct . + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_InitStruct: pointer to a USART_InitTypeDef structure + * that contains the configuration information for the specified USART peripheral. + * @retval None + */ +void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct) +{ + uint32_t divider = 0, apbclock = 0, tmpreg = 0; + RCC_ClocksTypeDef RCC_ClocksStatus; + + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_BAUDRATE(USART_InitStruct->USART_BaudRate)); + assert_param(IS_USART_WORD_LENGTH(USART_InitStruct->USART_WordLength)); + assert_param(IS_USART_STOPBITS(USART_InitStruct->USART_StopBits)); + assert_param(IS_USART_PARITY(USART_InitStruct->USART_Parity)); + assert_param(IS_USART_MODE(USART_InitStruct->USART_Mode)); + assert_param(IS_USART_HARDWARE_FLOW_CONTROL(USART_InitStruct->USART_HardwareFlowControl)); + + /* Disable USART */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_UE); + + /*---------------------------- USART CR2 Configuration -----------------------*/ + tmpreg = USARTx->CR2; + /* Clear STOP[13:12] bits */ + tmpreg &= (uint32_t)~((uint32_t)USART_CR2_STOP); + + /* Configure the USART Stop Bits, Clock, CPOL, CPHA and LastBit ------------*/ + /* Set STOP[13:12] bits according to USART_StopBits value */ + tmpreg |= (uint32_t)USART_InitStruct->USART_StopBits; + + /* Write to USART CR2 */ + USARTx->CR2 = tmpreg; + + /*---------------------------- USART CR1 Configuration -----------------------*/ + tmpreg = USARTx->CR1; + /* Clear M, PCE, PS, TE and RE bits */ + tmpreg &= (uint32_t)~((uint32_t)CR1_CLEAR_MASK); + + /* Configure the USART Word Length, Parity and mode ----------------------- */ + /* Set the M bits according to USART_WordLength value */ + /* Set PCE and PS bits according to USART_Parity value */ + /* Set TE and RE bits according to USART_Mode value */ + tmpreg |= (uint32_t)USART_InitStruct->USART_WordLength | USART_InitStruct->USART_Parity | + USART_InitStruct->USART_Mode; + + /* Write to USART CR1 */ + USARTx->CR1 = tmpreg; + + /*---------------------------- USART CR3 Configuration -----------------------*/ + tmpreg = USARTx->CR3; + /* Clear CTSE and RTSE bits */ + tmpreg &= (uint32_t)~((uint32_t)CR3_CLEAR_MASK); + + /* Configure the USART HFC -------------------------------------------------*/ + /* Set CTSE and RTSE bits according to USART_HardwareFlowControl value */ + tmpreg |= USART_InitStruct->USART_HardwareFlowControl; + + /* Write to USART CR3 */ + USARTx->CR3 = tmpreg; + + /*---------------------------- USART BRR Configuration -----------------------*/ + /* Configure the USART Baud Rate -------------------------------------------*/ + RCC_GetClocksFreq(&RCC_ClocksStatus); + + if (USARTx == USART1) + { + apbclock = RCC_ClocksStatus.USART1CLK_Frequency; + } + else if (USARTx == USART2) + { + apbclock = RCC_ClocksStatus.USART2CLK_Frequency; + } + else if (USARTx == USART3) + { + apbclock = RCC_ClocksStatus.USART3CLK_Frequency; + } + else if (USARTx == UART4) + { + apbclock = RCC_ClocksStatus.UART4CLK_Frequency; + } + else + { + apbclock = RCC_ClocksStatus.UART5CLK_Frequency; + } + + /* Determine the integer part */ + if ((USARTx->CR1 & USART_CR1_OVER8) != 0) + { + /* (divider * 10) computing in case Oversampling mode is 8 Samples */ + divider = (uint32_t)((2 * apbclock) / (USART_InitStruct->USART_BaudRate)); + tmpreg = (uint32_t)((2 * apbclock) % (USART_InitStruct->USART_BaudRate)); + } + else /* if ((USARTx->CR1 & CR1_OVER8_Set) == 0) */ + { + /* (divider * 10) computing in case Oversampling mode is 16 Samples */ + divider = (uint32_t)((apbclock) / (USART_InitStruct->USART_BaudRate)); + tmpreg = (uint32_t)((apbclock) % (USART_InitStruct->USART_BaudRate)); + } + + /* round the divider : if fractional part i greater than 0.5 increment divider */ + if (tmpreg >= (USART_InitStruct->USART_BaudRate) / 2) + { + divider++; + } + + /* Implement the divider in case Oversampling mode is 8 Samples */ + if ((USARTx->CR1 & USART_CR1_OVER8) != 0) + { + /* get the LSB of divider and shift it to the right by 1 bit */ + tmpreg = (divider & (uint16_t)0x000F) >> 1; + + /* update the divider value */ + divider = (divider & (uint16_t)0xFFF0) | tmpreg; + } + + /* Write to USART BRR */ + USARTx->BRR = (uint16_t)divider; +} + +/** + * @brief Fills each USART_InitStruct member with its default value. + * @param USART_InitStruct: pointer to a USART_InitTypeDef structure + * which will be initialized. + * @retval None + */ +void USART_StructInit(USART_InitTypeDef* USART_InitStruct) +{ + /* USART_InitStruct members default value */ + USART_InitStruct->USART_BaudRate = 9600; + USART_InitStruct->USART_WordLength = USART_WordLength_8b; + USART_InitStruct->USART_StopBits = USART_StopBits_1; + USART_InitStruct->USART_Parity = USART_Parity_No ; + USART_InitStruct->USART_Mode = USART_Mode_Rx | USART_Mode_Tx; + USART_InitStruct->USART_HardwareFlowControl = USART_HardwareFlowControl_None; +} + +/** + * @brief Initializes the USARTx peripheral Clock according to the + * specified parameters in the USART_ClockInitStruct. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3. + * @param USART_ClockInitStruct: pointer to a USART_ClockInitTypeDef + * structure that contains the configuration information for the specified + * USART peripheral. + * @retval None + */ +void USART_ClockInit(USART_TypeDef* USARTx, USART_ClockInitTypeDef* USART_ClockInitStruct) +{ + uint32_t tmpreg = 0; + /* Check the parameters */ + assert_param(IS_USART_123_PERIPH(USARTx)); + assert_param(IS_USART_CLOCK(USART_ClockInitStruct->USART_Clock)); + assert_param(IS_USART_CPOL(USART_ClockInitStruct->USART_CPOL)); + assert_param(IS_USART_CPHA(USART_ClockInitStruct->USART_CPHA)); + assert_param(IS_USART_LASTBIT(USART_ClockInitStruct->USART_LastBit)); +/*---------------------------- USART CR2 Configuration -----------------------*/ + tmpreg = USARTx->CR2; + /* Clear CLKEN, CPOL, CPHA, LBCL and SSM bits */ + tmpreg &= (uint32_t)~((uint32_t)CR2_CLOCK_CLEAR_MASK); + /* Configure the USART Clock, CPOL, CPHA, LastBit and SSM ------------*/ + /* Set CLKEN bit according to USART_Clock value */ + /* Set CPOL bit according to USART_CPOL value */ + /* Set CPHA bit according to USART_CPHA value */ + /* Set LBCL bit according to USART_LastBit value */ + tmpreg |= (uint32_t)(USART_ClockInitStruct->USART_Clock | USART_ClockInitStruct->USART_CPOL | + USART_ClockInitStruct->USART_CPHA | USART_ClockInitStruct->USART_LastBit); + /* Write to USART CR2 */ + USARTx->CR2 = tmpreg; +} + +/** + * @brief Fills each USART_ClockInitStruct member with its default value. + * @param USART_ClockInitStruct: pointer to a USART_ClockInitTypeDef + * structure which will be initialized. + * @retval None + */ +void USART_ClockStructInit(USART_ClockInitTypeDef* USART_ClockInitStruct) +{ + /* USART_ClockInitStruct members default value */ + USART_ClockInitStruct->USART_Clock = USART_Clock_Disable; + USART_ClockInitStruct->USART_CPOL = USART_CPOL_Low; + USART_ClockInitStruct->USART_CPHA = USART_CPHA_1Edge; + USART_ClockInitStruct->USART_LastBit = USART_LastBit_Disable; +} + +/** + * @brief Enables or disables the specified USART peripheral. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USARTx peripheral. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected USART by setting the UE bit in the CR1 register */ + USARTx->CR1 |= USART_CR1_UE; + } + else + { + /* Disable the selected USART by clearing the UE bit in the CR1 register */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_UE); + } +} + +/** + * @brief Enables or disables the USART's transmitter or receiver. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_Direction: specifies the USART direction. + * This parameter can be any combination of the following values: + * @arg USART_Mode_Tx: USART Transmitter + * @arg USART_Mode_Rx: USART Receiver + * @param NewState: new state of the USART transfer direction. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_DirectionModeCmd(USART_TypeDef* USARTx, uint32_t USART_DirectionMode, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_MODE(USART_DirectionMode)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the USART's transfer interface by setting the TE and/or RE bits + in the USART CR1 register */ + USARTx->CR1 |= USART_DirectionMode; + } + else + { + /* Disable the USART's transfer interface by clearing the TE and/or RE bits + in the USART CR3 register */ + USARTx->CR1 &= (uint32_t)~USART_DirectionMode; + } +} + +/** + * @brief Enables or disables the USART's 8x oversampling mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USART 8x oversampling mode. + * This parameter can be: ENABLE or DISABLE. + * @note + * This function has to be called before calling USART_Init() + * function in order to have correct baudrate Divider value. + * @retval None + */ +void USART_OverSampling8Cmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the 8x Oversampling mode by setting the OVER8 bit in the CR1 register */ + USARTx->CR1 |= USART_CR1_OVER8; + } + else + { + /* Disable the 8x Oversampling mode by clearing the OVER8 bit in the CR1 register */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_OVER8); + } +} + +/** + * @brief Enables or disables the USART's one bit sampling method. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USART one bit sampling method. + * This parameter can be: ENABLE or DISABLE. + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_OneBitMethodCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the one bit method by setting the ONEBIT bit in the CR3 register */ + USARTx->CR3 |= USART_CR3_ONEBIT; + } + else + { + /* Disable the one bit method by clearing the ONEBIT bit in the CR3 register */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_ONEBIT); + } +} + +/** + * @brief Enables or disables the USART's most significant bit first + * transmitted/received following the start bit. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USART most significant bit first + * transmitted/received following the start bit. + * This parameter can be: ENABLE or DISABLE. + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_MSBFirstCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the most significant bit first transmitted/received following the + start bit by setting the MSBFIRST bit in the CR2 register */ + USARTx->CR2 |= USART_CR2_MSBFIRST; + } + else + { + /* Disable the most significant bit first transmitted/received following the + start bit by clearing the MSBFIRST bit in the CR2 register */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_MSBFIRST); + } +} + +/** + * @brief Enables or disables the binary data inversion. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new defined levels for the USART data. + * This parameter can be: ENABLE or DISABLE. + * @arg ENABLE: Logical data from the data register are send/received in negative + * logic. (1=L, 0=H). The parity bit is also inverted. + * @arg DISABLE: Logical data from the data register are send/received in positive + * logic. (1=H, 0=L) + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_DataInvCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the binary data inversion feature by setting the DATAINV bit in + the CR2 register */ + USARTx->CR2 |= USART_CR2_DATAINV; + } + else + { + /* Disable the binary data inversion feature by clearing the DATAINV bit in + the CR2 register */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_DATAINV); + } +} + +/** + * @brief Enables or disables the Pin(s) active level inversion. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_InvPin: specifies the USART pin(s) to invert. + * This parameter can be any combination of the following values: + * @arg USART_InvPin_Tx: USART Tx pin active level inversion. + * @arg USART_InvPin_Rx: USART Rx pin active level inversion. + * @param NewState: new active level status for the USART pin(s). + * This parameter can be: ENABLE or DISABLE. + * - ENABLE: pin(s) signal values are inverted (Vdd =0, Gnd =1). + * - DISABLE: pin(s) signal works using the standard logic levels (Vdd =1, Gnd =0). + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_InvPinCmd(USART_TypeDef* USARTx, uint32_t USART_InvPin, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_INVERSTION_PIN(USART_InvPin)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the active level inversion for selected pins by setting the TXINV + and/or RXINV bits in the USART CR2 register */ + USARTx->CR2 |= USART_InvPin; + } + else + { + /* Disable the active level inversion for selected requests by clearing the + TXINV and/or RXINV bits in the USART CR2 register */ + USARTx->CR2 &= (uint32_t)~USART_InvPin; + } +} + +/** + * @brief Enables or disables the swap Tx/Rx pins. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USARTx TX/RX pins pinout. + * This parameter can be: ENABLE or DISABLE. + * @arg ENABLE: The TX and RX pins functions are swapped. + * @arg DISABLE: TX/RX pins are used as defined in standard pinout + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_SWAPPinCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the SWAP feature by setting the SWAP bit in the CR2 register */ + USARTx->CR2 |= USART_CR2_SWAP; + } + else + { + /* Disable the SWAP feature by clearing the SWAP bit in the CR2 register */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_SWAP); + } +} + +/** + * @brief Enables or disables the receiver Time Out feature. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USARTx receiver Time Out. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_ReceiverTimeOutCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the receiver time out feature by setting the RTOEN bit in the CR2 + register */ + USARTx->CR2 |= USART_CR2_RTOEN; + } + else + { + /* Disable the receiver time out feature by clearing the RTOEN bit in the CR2 + register */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_RTOEN); + } +} + +/** + * @brief Sets the receiver Time Out value. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_ReceiverTimeOut: specifies the Receiver Time Out value. + * @retval None + */ +void USART_SetReceiverTimeOut(USART_TypeDef* USARTx, uint32_t USART_ReceiverTimeOut) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_TIMEOUT(USART_ReceiverTimeOut)); + + /* Clear the receiver Time Out value by clearing the RTO[23:0] bits in the RTOR + register */ + USARTx->RTOR &= (uint32_t)~((uint32_t)USART_RTOR_RTO); + /* Set the receiver Time Out value by setting the RTO[23:0] bits in the RTOR + register */ + USARTx->RTOR |= USART_ReceiverTimeOut; +} + +/** + * @brief Sets the system clock prescaler. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_Prescaler: specifies the prescaler clock. + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_SetPrescaler(USART_TypeDef* USARTx, uint8_t USART_Prescaler) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + + /* Clear the USART prescaler */ + USARTx->GTPR &= USART_GTPR_GT; + /* Set the USART prescaler */ + USARTx->GTPR |= USART_Prescaler; +} + +/** + * @} + */ + + +/** @defgroup USART_Group2 STOP Mode functions + * @brief STOP Mode functions + * +@verbatim + =============================================================================== + ##### STOP Mode functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage + WakeUp from STOP mode. + + [..] The USART is able to WakeUp from Stop Mode if USART clock is set to HSI + or LSI. + + [..] The WakeUp source is configured by calling USART_StopModeWakeUpSourceConfig() + function. + + [..] After configuring the source of WakeUp and before entering in Stop Mode + USART_STOPModeCmd() function should be called to allow USART WakeUp. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified USART peripheral in STOP Mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USARTx peripheral state in stop mode. + * This parameter can be: ENABLE or DISABLE. + * @note + * This function has to be called when USART clock is set to HSI or LSE. + * @retval None + */ +void USART_STOPModeCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the selected USART in STOP mode by setting the UESM bit in the CR1 + register */ + USARTx->CR1 |= USART_CR1_UESM; + } + else + { + /* Disable the selected USART in STOP mode by clearing the UE bit in the CR1 + register */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_UESM); + } +} + +/** + * @brief Selects the USART WakeUp method form stop mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_WakeUp: specifies the selected USART wakeup method. + * This parameter can be one of the following values: + * @arg USART_WakeUpSource_AddressMatch: WUF active on address match. + * @arg USART_WakeUpSource_StartBit: WUF active on Start bit detection. + * @arg USART_WakeUpSource_RXNE: WUF active on RXNE. + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_StopModeWakeUpSourceConfig(USART_TypeDef* USARTx, uint32_t USART_WakeUpSource) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_STOPMODE_WAKEUPSOURCE(USART_WakeUpSource)); + + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_WUS); + USARTx->CR3 |= USART_WakeUpSource; +} + +/** + * @} + */ + + +/** @defgroup USART_Group3 AutoBaudRate functions + * @brief AutoBaudRate functions + * +@verbatim + =============================================================================== + ##### AutoBaudRate functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage + the AutoBaudRate detections. + + [..] Before Enabling AutoBaudRate detection using USART_AutoBaudRateCmd () + The character patterns used to calculate baudrate must be chosen by calling + USART_AutoBaudRateConfig() function. These function take as parameter : + (#)USART_AutoBaudRate_StartBit : any character starting with a bit 1. + (#)USART_AutoBaudRate_FallingEdge : any character starting with a 10xx bit pattern. + + [..] At any later time, another request for AutoBaudRate detection can be performed + using USART_RequestCmd() function. + + [..] The AutoBaudRate detection is monitored by the status of ABRF flag which indicate + that the AutoBaudRate detection is completed. In addition to ABRF flag, the ABRE flag + indicate that this procedure is completed without success. USART_GetFlagStatus () + function should be used to monitor the status of these flags. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the Auto Baud Rate. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USARTx auto baud rate. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_AutoBaudRateCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the auto baud rate feature by setting the ABREN bit in the CR2 + register */ + USARTx->CR2 |= USART_CR2_ABREN; + } + else + { + /* Disable the auto baud rate feature by clearing the ABREN bit in the CR2 + register */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_ABREN); + } +} + +/** + * @brief Selects the USART auto baud rate method. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_AutoBaudRate: specifies the selected USART auto baud rate method. + * This parameter can be one of the following values: + * @arg USART_AutoBaudRate_StartBit: Start Bit duration measurement. + * @arg USART_AutoBaudRate_FallingEdge: Falling edge to falling edge measurement. + * @arg USART_AutoBaudRate_0x7FFrame: 0x7F frame. + * @arg USART_AutoBaudRate_0x55Frame: 0x55 frame. + * @note + * This function has to be called before calling USART_Cmd() function. + * @retval None + */ +void USART_AutoBaudRateConfig(USART_TypeDef* USARTx, uint32_t USART_AutoBaudRate) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_AUTOBAUDRATE_MODE(USART_AutoBaudRate)); + + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_ABRMODE); + USARTx->CR2 |= USART_AutoBaudRate; +} + +/** + * @} + */ + + +/** @defgroup USART_Group4 Data transfers functions + * @brief Data transfers functions + * +@verbatim + =============================================================================== + ##### Data transfers functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage + the USART data transfers. + [..] During an USART reception, data shifts in least significant bit first + through the RX pin. When a transmission is taking place, a write instruction to + the USART_TDR register stores the data in the shift register. + [..] The read access of the USART_RDR register can be done using + the USART_ReceiveData() function and returns the RDR value. + Whereas a write access to the USART_TDR can be done using USART_SendData() + function and stores the written data into TDR. + +@endverbatim + * @{ + */ + +/** + * @brief Transmits single data through the USARTx peripheral. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param Data: the data to transmit. + * @retval None + */ +void USART_SendData(USART_TypeDef* USARTx, uint16_t Data) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_DATA(Data)); + + /* Transmit Data */ + USARTx->TDR = (Data & (uint16_t)0x01FF); +} + +/** + * @brief Returns the most recent received data by the USARTx peripheral. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @retval The received data. + */ +uint16_t USART_ReceiveData(USART_TypeDef* USARTx) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + + /* Receive Data */ + return (uint16_t)(USARTx->RDR & (uint16_t)0x01FF); +} + +/** + * @} + */ + +/** @defgroup USART_Group5 MultiProcessor Communication functions + * @brief Multi-Processor Communication functions + * +@verbatim + =============================================================================== + ##### Multi-Processor Communication functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage the USART + multiprocessor communication. + [..] For instance one of the USARTs can be the master, its TX output is + connected to the RX input of the other USART. The others are slaves, + their respective TX outputs are logically ANDed together and connected + to the RX input of the master. USART multiprocessor communication is + possible through the following procedure: + (#) Program the Baud rate, Word length = 9 bits, Stop bits, Parity, + Mode transmitter or Mode receiver and hardware flow control values + using the USART_Init() function. + (#) Configures the USART address using the USART_SetAddress() function. + (#) Configures the wake up methode (USART_WakeUp_IdleLine or + USART_WakeUp_AddressMark) using USART_WakeUpConfig() function only + for the slaves. + (#) Enable the USART using the USART_Cmd() function. + (#) Enter the USART slaves in mute mode using USART_ReceiverWakeUpCmd() + function. + [..] The USART Slave exit from mute mode when receive the wake up condition. + +@endverbatim + * @{ + */ + +/** + * @brief Sets the address of the USART node. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_Address: Indicates the address of the USART node. + * @retval None + */ +void USART_SetAddress(USART_TypeDef* USARTx, uint8_t USART_Address) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + + /* Clear the USART address */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_ADD); + /* Set the USART address node */ + USARTx->CR2 |=((uint32_t)USART_Address << (uint32_t)0x18); +} + +/** + * @brief Enables or disables the USART's mute mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USART mute mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_MuteModeCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the USART mute mode by setting the MME bit in the CR1 register */ + USARTx->CR1 |= USART_CR1_MME; + } + else + { + /* Disable the USART mute mode by clearing the MME bit in the CR1 register */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_MME); + } +} + +/** + * @brief Selects the USART WakeUp method from mute mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_WakeUp: specifies the USART wakeup method. + * This parameter can be one of the following values: + * @arg USART_WakeUp_IdleLine: WakeUp by an idle line detection + * @arg USART_WakeUp_AddressMark: WakeUp by an address mark + * @retval None + */ +void USART_MuteModeWakeUpConfig(USART_TypeDef* USARTx, uint32_t USART_WakeUp) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_MUTEMODE_WAKEUP(USART_WakeUp)); + + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_WAKE); + USARTx->CR1 |= USART_WakeUp; +} + +/** + * @brief Configure the USART Address detection length. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_AddressLength: specifies the USART address length detection. + * This parameter can be one of the following values: + * @arg USART_AddressLength_4b: 4-bit address length detection + * @arg USART_AddressLength_7b: 7-bit address length detection + * @retval None + */ +void USART_AddressDetectionConfig(USART_TypeDef* USARTx, uint32_t USART_AddressLength) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_ADDRESS_DETECTION(USART_AddressLength)); + + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_ADDM7); + USARTx->CR2 |= USART_AddressLength; +} + +/** + * @} + */ + +/** @defgroup USART_Group6 LIN mode functions + * @brief LIN mode functions + * +@verbatim + =============================================================================== + ##### LIN mode functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage the USART + LIN Mode communication. + [..] In LIN mode, 8-bit data format with 1 stop bit is required in accordance + with the LIN standard. + [..] Only this LIN Feature is supported by the USART IP: + (+) LIN Master Synchronous Break send capability and LIN slave break + detection capability : 13-bit break generation and 10/11 bit break + detection. + [..] USART LIN Master transmitter communication is possible through the + following procedure: + (#) Program the Baud rate, Word length = 8bits, Stop bits = 1bit, Parity, + Mode transmitter or Mode receiver and hardware flow control values + using the USART_Init() function. + (#) Enable the LIN mode using the USART_LINCmd() function. + (#) Enable the USART using the USART_Cmd() function. + (#) Send the break character using USART_SendBreak() function. + [..] USART LIN Master receiver communication is possible through the + following procedure: + (#) Program the Baud rate, Word length = 8bits, Stop bits = 1bit, Parity, + Mode transmitter or Mode receiver and hardware flow control values + using the USART_Init() function. + (#) Configures the break detection length + using the USART_LINBreakDetectLengthConfig() function. + (#) Enable the LIN mode using the USART_LINCmd() function. + (#) Enable the USART using the USART_Cmd() function. + [..] + (@) In LIN mode, the following bits must be kept cleared: + (+@) CLKEN in the USART_CR2 register. + (+@) STOP[1:0], SCEN, HDSEL and IREN in the USART_CR3 register. + +@endverbatim + * @{ + */ + +/** + * @brief Sets the USART LIN Break detection length. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_LINBreakDetectLength: specifies the LIN break detection length. + * This parameter can be one of the following values: + * @arg USART_LINBreakDetectLength_10b: 10-bit break detection + * @arg USART_LINBreakDetectLength_11b: 11-bit break detection + * @retval None + */ +void USART_LINBreakDetectLengthConfig(USART_TypeDef* USARTx, uint32_t USART_LINBreakDetectLength) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_LIN_BREAK_DETECT_LENGTH(USART_LINBreakDetectLength)); + + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_LBDL); + USARTx->CR2 |= USART_LINBreakDetectLength; +} + +/** + * @brief Enables or disables the USART's LIN mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USART LIN mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_LINCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the LIN mode by setting the LINEN bit in the CR2 register */ + USARTx->CR2 |= USART_CR2_LINEN; + } + else + { + /* Disable the LIN mode by clearing the LINEN bit in the CR2 register */ + USARTx->CR2 &= (uint32_t)~((uint32_t)USART_CR2_LINEN); + } +} + +/** + * @} + */ + +/** @defgroup USART_Group7 Halfduplex mode function + * @brief Half-duplex mode function + * +@verbatim + =============================================================================== + ##### Half-duplex mode function ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage the USART + Half-duplex communication. + [..] The USART can be configured to follow a single-wire half-duplex protocol + where the TX and RX lines are internally connected. + [..] USART Half duplex communication is possible through the following procedure: + (#) Program the Baud rate, Word length, Stop bits, Parity, Mode transmitter + or Mode receiver and hardware flow control values using the USART_Init() + function. + (#) Configures the USART address using the USART_SetAddress() function. + (#) Enable the half duplex mode using USART_HalfDuplexCmd() function. + (#) Enable the USART using the USART_Cmd() function. + [..] + (@) The RX pin is no longer used. + (@) In Half-duplex mode the following bits must be kept cleared: + (+@) LINEN and CLKEN bits in the USART_CR2 register. + (+@) SCEN and IREN bits in the USART_CR3 register. + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the USART's Half Duplex communication. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the USART Communication. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_HalfDuplexCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the Half-Duplex mode by setting the HDSEL bit in the CR3 register */ + USARTx->CR3 |= USART_CR3_HDSEL; + } + else + { + /* Disable the Half-Duplex mode by clearing the HDSEL bit in the CR3 register */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_HDSEL); + } +} + +/** + * @} + */ + + +/** @defgroup USART_Group8 Smartcard mode functions + * @brief Smartcard mode functions + * +@verbatim + =============================================================================== + ##### Smartcard mode functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage the USART + Smartcard communication. + [..] The Smartcard interface is designed to support asynchronous protocol + Smartcards as defined in the ISO 7816-3 standard. The USART can provide + a clock to the smartcard through the SCLK output. In smartcard mode, + SCLK is not associated to the communication but is simply derived from + the internal peripheral input clock through a 5-bit prescaler. + [..] Smartcard communication is possible through the following procedure: + (#) Configures the Smartcard Prescaler using the USART_SetPrescaler() + function. + (#) Configures the Smartcard Guard Time using the USART_SetGuardTime() + function. + (#) Program the USART clock using the USART_ClockInit() function as following: + (++) USART Clock enabled. + (++) USART CPOL Low. + (++) USART CPHA on first edge. + (++) USART Last Bit Clock Enabled. + (#) Program the Smartcard interface using the USART_Init() function as + following: + (++) Word Length = 9 Bits. + (++) 1.5 Stop Bit. + (++) Even parity. + (++) BaudRate = 12096 baud. + (++) Hardware flow control disabled (RTS and CTS signals). + (++) Tx and Rx enabled + (#) Optionally you can enable the parity error interrupt using + the USART_ITConfig() function. + (#) Enable the Smartcard NACK using the USART_SmartCardNACKCmd() function. + (#) Enable the Smartcard interface using the USART_SmartCardCmd() function. + (#) Enable the USART using the USART_Cmd() function. + [..] + Please refer to the ISO 7816-3 specification for more details. + [..] + (@) It is also possible to choose 0.5 stop bit for receiving but it is + recommended to use 1.5 stop bits for both transmitting and receiving + to avoid switching between the two configurations. + (@) In smartcard mode, the following bits must be kept cleared: + (+@) LINEN bit in the USART_CR2 register. + (+@) HDSEL and IREN bits in the USART_CR3 register. + +@endverbatim + * @{ + */ + +/** + * @brief Sets the specified USART guard time. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3. + * @param USART_GuardTime: specifies the guard time. + * @retval None + */ +void USART_SetGuardTime(USART_TypeDef* USARTx, uint8_t USART_GuardTime) +{ + /* Check the parameters */ + assert_param(IS_USART_123_PERIPH(USARTx)); + + /* Clear the USART Guard time */ + USARTx->GTPR &= USART_GTPR_PSC; + /* Set the USART guard time */ + USARTx->GTPR |= (uint16_t)((uint16_t)USART_GuardTime << 0x08); +} + +/** + * @brief Enables or disables the USART's Smart Card mode. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3. + * @param NewState: new state of the Smart Card mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_SmartCardCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_123_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the SC mode by setting the SCEN bit in the CR3 register */ + USARTx->CR3 |= USART_CR3_SCEN; + } + else + { + /* Disable the SC mode by clearing the SCEN bit in the CR3 register */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_SCEN); + } +} + +/** + * @brief Enables or disables NACK transmission. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3. + * @param NewState: new state of the NACK transmission. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_SmartCardNACKCmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_123_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the NACK transmission by setting the NACK bit in the CR3 register */ + USARTx->CR3 |= USART_CR3_NACK; + } + else + { + /* Disable the NACK transmission by clearing the NACK bit in the CR3 register */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_NACK); + } +} + +/** + * @brief Sets the Smart Card number of retries in transmit and receive. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3. + * @param USART_AutoCount: specifies the Smart Card auto retry count. + * @retval None + */ +void USART_SetAutoRetryCount(USART_TypeDef* USARTx, uint8_t USART_AutoCount) +{ + /* Check the parameters */ + assert_param(IS_USART_123_PERIPH(USARTx)); + assert_param(IS_USART_AUTO_RETRY_COUNTER(USART_AutoCount)); + /* Clear the USART auto retry count */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_SCARCNT); + /* Set the USART auto retry count*/ + USARTx->CR3 |= (uint32_t)((uint32_t)USART_AutoCount << 0x11); +} + +/** + * @brief Sets the Smart Card Block length. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3. + * @param USART_BlockLength: specifies the Smart Card block length. + * @retval None + */ +void USART_SetBlockLength(USART_TypeDef* USARTx, uint8_t USART_BlockLength) +{ + /* Check the parameters */ + assert_param(IS_USART_123_PERIPH(USARTx)); + + /* Clear the Smart card block length */ + USARTx->RTOR &= (uint32_t)~((uint32_t)USART_RTOR_BLEN); + /* Set the Smart Card block length */ + USARTx->RTOR |= (uint32_t)((uint32_t)USART_BlockLength << 0x18); +} + +/** + * @} + */ + +/** @defgroup USART_Group9 IrDA mode functions + * @brief IrDA mode functions + * +@verbatim + =============================================================================== + ##### IrDA mode functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage the USART + IrDA communication. + [..] IrDA is a half duplex communication protocol. If the Transmitter is busy, + any data on the IrDA receive line will be ignored by the IrDA decoder + and if the Receiver is busy, data on the TX from the USART to IrDA will + not be encoded by IrDA. While receiving data, transmission should be + avoided as the data to be transmitted could be corrupted. + [..] IrDA communication is possible through the following procedure: + (#) Program the Baud rate, Word length = 8 bits, Stop bits, Parity, + Transmitter/Receiver modes and hardware flow control values using + the USART_Init() function. + (#) Configures the IrDA pulse width by configuring the prescaler using + the USART_SetPrescaler() function. + (#) Configures the IrDA USART_IrDAMode_LowPower or USART_IrDAMode_Normal + mode using the USART_IrDAConfig() function. + (#) Enable the IrDA using the USART_IrDACmd() function. + (#) Enable the USART using the USART_Cmd() function. + [..] + (@) A pulse of width less than two and greater than one PSC period(s) may or + may not be rejected. + (@) The receiver set up time should be managed by software. The IrDA physical + layer specification specifies a minimum of 10 ms delay between + transmission and reception (IrDA is a half duplex protocol). + (@) In IrDA mode, the following bits must be kept cleared: + (+@) LINEN, STOP and CLKEN bits in the USART_CR2 register. + (+@) SCEN and HDSEL bits in the USART_CR3 register. + +@endverbatim + * @{ + */ + +/** + * @brief Configures the USART's IrDA interface. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_IrDAMode: specifies the IrDA mode. + * This parameter can be one of the following values: + * @arg USART_IrDAMode_LowPower + * @arg USART_IrDAMode_Normal + * @retval None + */ +void USART_IrDAConfig(USART_TypeDef* USARTx, uint32_t USART_IrDAMode) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_IRDA_MODE(USART_IrDAMode)); + + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_IRLP); + USARTx->CR3 |= USART_IrDAMode; +} + +/** + * @brief Enables or disables the USART's IrDA interface. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the IrDA mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_IrDACmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the IrDA mode by setting the IREN bit in the CR3 register */ + USARTx->CR3 |= USART_CR3_IREN; + } + else + { + /* Disable the IrDA mode by clearing the IREN bit in the CR3 register */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_IREN); + } +} +/** + * @} + */ + +/** @defgroup USART_Group10 RS485 mode function + * @brief RS485 mode function + * +@verbatim + =============================================================================== + ##### RS485 mode functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to manage the USART + RS485 flow control. + [..] RS485 flow control (Driver enable feature) handling is possible through + the following procedure: + (#) Program the Baud rate, Word length = 8 bits, Stop bits, Parity, + Transmitter/Receiver modes and hardware flow control values using + the USART_Init() function. + (#) Enable the Driver Enable using the USART_DECmd() function. + (#) Configures the Driver Enable polarity using the USART_DEPolarityConfig() + function. + (#) Configures the Driver Enable assertion time using USART_SetDEAssertionTime() + function and deassertion time using the USART_SetDEDeassertionTime() + function. + (#) Enable the USART using the USART_Cmd() function. + [..] + (@) The assertion and dessertion times are expressed in sample time units (1/8 or + 1/16 bit time, depending on the oversampling rate). + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the USART's DE functionality. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param NewState: new state of the driver enable mode. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_DECmd(USART_TypeDef* USARTx, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + if (NewState != DISABLE) + { + /* Enable the DE functionality by setting the DEM bit in the CR3 register */ + USARTx->CR3 |= USART_CR3_DEM; + } + else + { + /* Disable the DE functionality by clearing the DEM bit in the CR3 register */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_DEM); + } +} + +/** + * @brief Configures the USART's DE polarity + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_DEPolarity: specifies the DE polarity. + * This parameter can be one of the following values: + * @arg USART_DEPolarity_Low + * @arg USART_DEPolarity_High + * @retval None + */ +void USART_DEPolarityConfig(USART_TypeDef* USARTx, uint32_t USART_DEPolarity) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_DE_POLARITY(USART_DEPolarity)); + + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_DEP); + USARTx->CR3 |= USART_DEPolarity; +} + +/** + * @brief Sets the specified RS485 DE assertion time + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_AssertionTime: specifies the time between the activation of the DE + * signal and the beginning of the start bit + * @retval None + */ +void USART_SetDEAssertionTime(USART_TypeDef* USARTx, uint32_t USART_DEAssertionTime) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_DE_ASSERTION_DEASSERTION_TIME(USART_DEAssertionTime)); + + /* Clear the DE assertion time */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_DEAT); + /* Set the new value for the DE assertion time */ + USARTx->CR1 |=((uint32_t)USART_DEAssertionTime << (uint32_t)0x15); +} + +/** + * @brief Sets the specified RS485 DE deassertion time + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_DeassertionTime: specifies the time between the middle of the last + * stop bit in a transmitted message and the de-activation of the DE signal + * @retval None + */ +void USART_SetDEDeassertionTime(USART_TypeDef* USARTx, uint32_t USART_DEDeassertionTime) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_DE_ASSERTION_DEASSERTION_TIME(USART_DEDeassertionTime)); + + /* Clear the DE deassertion time */ + USARTx->CR1 &= (uint32_t)~((uint32_t)USART_CR1_DEDT); + /* Set the new value for the DE deassertion time */ + USARTx->CR1 |=((uint32_t)USART_DEDeassertionTime << (uint32_t)0x10); +} + +/** + * @} + */ + +/** @defgroup USART_Group11 DMA transfers management functions + * @brief DMA transfers management functions + * +@verbatim + =============================================================================== + ##### DMA transfers management functions ##### + =============================================================================== + [..] This section provides two functions that can be used only in DMA mode. + [..] In DMA Mode, the USART communication can be managed by 2 DMA Channel + requests: + (#) USART_DMAReq_Tx: specifies the Tx buffer DMA transfer request. + (#) USART_DMAReq_Rx: specifies the Rx buffer DMA transfer request. + [..] In this Mode it is advised to use the following function: + (+) void USART_DMACmd(USART_TypeDef* USARTx, uint16_t USART_DMAReq, + FunctionalState NewState). +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the USART's DMA interface. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4. + * @param USART_DMAReq: specifies the DMA request. + * This parameter can be any combination of the following values: + * @arg USART_DMAReq_Tx: USART DMA transmit request + * @arg USART_DMAReq_Rx: USART DMA receive request + * @param NewState: new state of the DMA Request sources. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_DMACmd(USART_TypeDef* USARTx, uint32_t USART_DMAReq, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_1234_PERIPH(USARTx)); + assert_param(IS_USART_DMAREQ(USART_DMAReq)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the DMA transfer for selected requests by setting the DMAT and/or + DMAR bits in the USART CR3 register */ + USARTx->CR3 |= USART_DMAReq; + } + else + { + /* Disable the DMA transfer for selected requests by clearing the DMAT and/or + DMAR bits in the USART CR3 register */ + USARTx->CR3 &= (uint32_t)~USART_DMAReq; + } +} + +/** + * @brief Enables or disables the USART's DMA interface when reception error occurs. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4. + * @param USART_DMAOnError: specifies the DMA status in case of reception error. + * This parameter can be any combination of the following values: + * @arg USART_DMAOnError_Enable: DMA receive request enabled when the USART DMA + * reception error is asserted. + * @arg USART_DMAOnError_Disable: DMA receive request disabled when the USART DMA + * reception error is asserted. + * @retval None + */ +void USART_DMAReceptionErrorConfig(USART_TypeDef* USARTx, uint32_t USART_DMAOnError) +{ + /* Check the parameters */ + assert_param(IS_USART_1234_PERIPH(USARTx)); + assert_param(IS_USART_DMAONERROR(USART_DMAOnError)); + + /* Clear the DMA Reception error detection bit */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_DDRE); + /* Set the new value for the DMA Reception error detection bit */ + USARTx->CR3 |= USART_DMAOnError; +} + +/** + * @} + */ + +/** @defgroup USART_Group12 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + =============================================================================== + ##### Interrupts and flags management functions ##### + =============================================================================== + [..] This subsection provides a set of functions allowing to configure the + USART Interrupts sources, Requests and check or clear the flags or pending bits status. + The user should identify which mode will be used in his application to + manage the communication: Polling mode, Interrupt mode. + + *** Polling Mode *** + ==================== + [..] In Polling Mode, the SPI communication can be managed by these flags: + (#) USART_FLAG_REACK: to indicate the status of the Receive Enable + acknowledge flag + (#) USART_FLAG_TEACK: to indicate the status of the Transmit Enable + acknowledge flag. + (#) USART_FLAG_WUF: to indicate the status of the Wake up flag. + (#) USART_FLAG_RWU: to indicate the status of the Receive Wake up flag. + (#) USART_FLAG_SBK: to indicate the status of the Send Break flag. + (#) USART_FLAG_CMF: to indicate the status of the Character match flag. + (#) USART_FLAG_BUSY: to indicate the status of the Busy flag. + (#) USART_FLAG_ABRF: to indicate the status of the Auto baud rate flag. + (#) USART_FLAG_ABRE: to indicate the status of the Auto baud rate error flag. + (#) USART_FLAG_EOBF: to indicate the status of the End of block flag. + (#) USART_FLAG_RTOF: to indicate the status of the Receive time out flag. + (#) USART_FLAG_nCTSS: to indicate the status of the Inverted nCTS input + bit status. + (#) USART_FLAG_TXE: to indicate the status of the transmit buffer register. + (#) USART_FLAG_RXNE: to indicate the status of the receive buffer register. + (#) USART_FLAG_TC: to indicate the status of the transmit operation. + (#) USART_FLAG_IDLE: to indicate the status of the Idle Line. + (#) USART_FLAG_CTS: to indicate the status of the nCTS input. + (#) USART_FLAG_LBD: to indicate the status of the LIN break detection. + (#) USART_FLAG_NE: to indicate if a noise error occur. + (#) USART_FLAG_FE: to indicate if a frame error occur. + (#) USART_FLAG_PE: to indicate if a parity error occur. + (#) USART_FLAG_ORE: to indicate if an Overrun error occur. + [..] In this Mode it is advised to use the following functions: + (+) FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG). + (+) void USART_ClearFlag(USART_TypeDef* USARTx, uint16_t USART_FLAG). + + *** Interrupt Mode *** + ====================== + [..] In Interrupt Mode, the USART communication can be managed by 8 interrupt + sources and 10 pending bits: + (+) Pending Bits: + (##) USART_IT_WU: to indicate the status of the Wake up interrupt. + (##) USART_IT_CM: to indicate the status of Character match interrupt. + (##) USART_IT_EOB: to indicate the status of End of block interrupt. + (##) USART_IT_RTO: to indicate the status of Receive time out interrupt. + (##) USART_IT_CTS: to indicate the status of CTS change interrupt. + (##) USART_IT_LBD: to indicate the status of LIN Break detection interrupt. + (##) USART_IT_TC: to indicate the status of Transmission complete interrupt. + (##) USART_IT_IDLE: to indicate the status of IDLE line detected interrupt. + (##) USART_IT_ORE: to indicate the status of OverRun Error interrupt. + (##) USART_IT_NE: to indicate the status of Noise Error interrupt. + (##) USART_IT_FE: to indicate the status of Framing Error interrupt. + (##) USART_IT_PE: to indicate the status of Parity Error interrupt. + + (+) Interrupt Source: + (##) USART_IT_WU: specifies the interrupt source for Wake up interrupt. + (##) USART_IT_CM: specifies the interrupt source for Character match + interrupt. + (##) USART_IT_EOB: specifies the interrupt source for End of block + interrupt. + (##) USART_IT_RTO: specifies the interrupt source for Receive time-out + interrupt. + (##) USART_IT_CTS: specifies the interrupt source for CTS change interrupt. + (##) USART_IT_LBD: specifies the interrupt source for LIN Break + detection interrupt. + (##) USART_IT_TXE: specifies the interrupt source for Transmit Data + Register empty interrupt. + (##) USART_IT_TC: specifies the interrupt source for Transmission + complete interrupt. + (##) USART_IT_RXNE: specifies the interrupt source for Receive Data + register not empty interrupt. + (##) USART_IT_IDLE: specifies the interrupt source for Idle line + detection interrupt. + (##) USART_IT_PE: specifies the interrupt source for Parity Error interrupt. + (##) USART_IT_ERR: specifies the interrupt source for Error interrupt + (Frame error, noise error, overrun error) + -@@- Some parameters are coded in order to use them as interrupt + source or as pending bits. + [..] In this Mode it is advised to use the following functions: + (+) void USART_ITConfig(USART_TypeDef* USARTx, uint16_t USART_IT, FunctionalState NewState). + (+) ITStatus USART_GetITStatus(USART_TypeDef* USARTx, uint16_t USART_IT). + (+) void USART_ClearITPendingBit(USART_TypeDef* USARTx, uint16_t USART_IT). + +@endverbatim + * @{ + */ + +/** + * @brief Enables or disables the specified USART interrupts. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_IT: specifies the USART interrupt sources to be enabled or disabled. + * This parameter can be one of the following values: + * @arg USART_IT_WU: Wake up interrupt. + * @arg USART_IT_CM: Character match interrupt. + * @arg USART_IT_EOB: End of block interrupt. + * @arg USART_IT_RTO: Receive time out interrupt. + * @arg USART_IT_CTS: CTS change interrupt. + * @arg USART_IT_LBD: LIN Break detection interrupt. + * @arg USART_IT_TXE: Transmit Data Register empty interrupt. + * @arg USART_IT_TC: Transmission complete interrupt. + * @arg USART_IT_RXNE: Receive Data register not empty interrupt. + * @arg USART_IT_IDLE: Idle line detection interrupt. + * @arg USART_IT_PE: Parity Error interrupt. + * @arg USART_IT_ERR: Error interrupt(Frame error, noise error, overrun error) + * @param NewState: new state of the specified USARTx interrupts. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_ITConfig(USART_TypeDef* USARTx, uint32_t USART_IT, FunctionalState NewState) +{ + uint32_t usartreg = 0, itpos = 0, itmask = 0; + uint32_t usartxbase = 0; + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_CONFIG_IT(USART_IT)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + usartxbase = (uint32_t)USARTx; + + /* Get the USART register index */ + usartreg = (((uint16_t)USART_IT) >> 0x08); + + /* Get the interrupt position */ + itpos = USART_IT & IT_MASK; + itmask = (((uint32_t)0x01) << itpos); + + if (usartreg == 0x02) /* The IT is in CR2 register */ + { + usartxbase += 0x04; + } + else if (usartreg == 0x03) /* The IT is in CR3 register */ + { + usartxbase += 0x08; + } + else /* The IT is in CR1 register */ + { + } + if (NewState != DISABLE) + { + *(__IO uint32_t*)usartxbase |= itmask; + } + else + { + *(__IO uint32_t*)usartxbase &= ~itmask; + } +} + +/** + * @brief Enables the specified USART's Request. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_Request: specifies the USART request. + * This parameter can be any combination of the following values: + * @arg USART_Request_TXFRQ: Transmit data flush ReQuest + * @arg USART_Request_RXFRQ: Receive data flush ReQuest + * @arg USART_Request_MMRQ: Mute Mode ReQuest + * @arg USART_Request_SBKRQ: Send Break ReQuest + * @arg USART_Request_ABRRQ: Auto Baud Rate ReQuest + * @param NewState: new state of the DMA interface when reception error occurs. + * This parameter can be: ENABLE or DISABLE. + * @retval None + */ +void USART_RequestCmd(USART_TypeDef* USARTx, uint32_t USART_Request, FunctionalState NewState) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_REQUEST(USART_Request)); + assert_param(IS_FUNCTIONAL_STATE(NewState)); + + if (NewState != DISABLE) + { + /* Enable the USART ReQuest by setting the dedicated request bit in the RQR + register.*/ + USARTx->RQR |= USART_Request; + } + else + { + /* Disable the USART ReQuest by clearing the dedicated request bit in the RQR + register.*/ + USARTx->RQR &= (uint32_t)~USART_Request; + } +} + +/** + * @brief Enables or disables the USART's Overrun detection. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_OVRDetection: specifies the OVR detection status in case of OVR error. + * This parameter can be any combination of the following values: + * @arg USART_OVRDetection_Enable: OVR error detection enabled when the USART OVR error + * is asserted. + * @arg USART_OVRDetection_Disable: OVR error detection disabled when the USART OVR error + * is asserted. + * @retval None + */ +void USART_OverrunDetectionConfig(USART_TypeDef* USARTx, uint32_t USART_OVRDetection) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_OVRDETECTION(USART_OVRDetection)); + + /* Clear the OVR detection bit */ + USARTx->CR3 &= (uint32_t)~((uint32_t)USART_CR3_OVRDIS); + /* Set the new value for the OVR detection bit */ + USARTx->CR3 |= USART_OVRDetection; +} + +/** + * @brief Checks whether the specified USART flag is set or not. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_FLAG: specifies the flag to check. + * This parameter can be one of the following values: + * @arg USART_FLAG_REACK: Receive Enable acknowledge flag. + * @arg USART_FLAG_TEACK: Transmit Enable acknowledge flag. + * @arg USART_FLAG_WUF: Wake up flag. + * @arg USART_FLAG_RWU: Receive Wake up flag. + * @arg USART_FLAG_SBK: Send Break flag. + * @arg USART_FLAG_CMF: Character match flag. + * @arg USART_FLAG_BUSY: Busy flag. + * @arg USART_FLAG_ABRF: Auto baud rate flag. + * @arg USART_FLAG_ABRE: Auto baud rate error flag. + * @arg USART_FLAG_EOBF: End of block flag. + * @arg USART_FLAG_RTOF: Receive time out flag. + * @arg USART_FLAG_nCTSS: Inverted nCTS input bit status. + * @arg USART_FLAG_CTS: CTS Change flag. + * @arg USART_FLAG_LBD: LIN Break detection flag. + * @arg USART_FLAG_TXE: Transmit data register empty flag. + * @arg USART_FLAG_TC: Transmission Complete flag. + * @arg USART_FLAG_RXNE: Receive data register not empty flag. + * @arg USART_FLAG_IDLE: Idle Line detection flag. + * @arg USART_FLAG_ORE: OverRun Error flag. + * @arg USART_FLAG_NE: Noise Error flag. + * @arg USART_FLAG_FE: Framing Error flag. + * @arg USART_FLAG_PE: Parity Error flag. + * @retval The new state of USART_FLAG (SET or RESET). + */ +FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint32_t USART_FLAG) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_FLAG(USART_FLAG)); + + if ((USARTx->ISR & USART_FLAG) != (uint16_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the USARTx's pending flags. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_FLAG: specifies the flag to clear. + * This parameter can be any combination of the following values: + * @arg USART_FLAG_WUF: Wake up flag. + * @arg USART_FLAG_CMF: Character match flag. + * @arg USART_FLAG_EOBF: End of block flag. + * @arg USART_FLAG_RTOF: Receive time out flag. + * @arg USART_FLAG_CTS: CTS Change flag. + * @arg USART_FLAG_LBD: LIN Break detection flag. + * @arg USART_FLAG_TC: Transmission Complete flag. + * @arg USART_FLAG_IDLE: IDLE line detected flag. + * @arg USART_FLAG_ORE: OverRun Error flag. + * @arg USART_FLAG_NE: Noise Error flag. + * @arg USART_FLAG_FE: Framing Error flag. + * @arg USART_FLAG_PE: Parity Errorflag. + * + * @note + * - RXNE pending bit is cleared by a read to the USART_RDR register + * (USART_ReceiveData()) or by writing 1 to the RXFRQ in the register USART_RQR + * (USART_RequestCmd()). + * - TC flag can be also cleared by software sequence: a read operation to + * USART_SR register (USART_GetFlagStatus()) followed by a write operation + * to USART_TDR register (USART_SendData()). + * - TXE flag is cleared by a write to the USART_TDR register + * (USART_SendData()) or by writing 1 to the TXFRQ in the register USART_RQR + * (USART_RequestCmd()). + * - SBKF flag is cleared by 1 to the SBKRQ in the register USART_RQR + * (USART_RequestCmd()). + * @retval None + */ +void USART_ClearFlag(USART_TypeDef* USARTx, uint32_t USART_FLAG) +{ + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_CLEAR_FLAG(USART_FLAG)); + + USARTx->ICR = USART_FLAG; +} + +/** + * @brief Checks whether the specified USART interrupt has occurred or not. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_IT: specifies the USART interrupt source to check. + * This parameter can be one of the following values: + * @arg USART_IT_WU: Wake up interrupt. + * @arg USART_IT_CM: Character match interrupt. + * @arg USART_IT_EOB: End of block interrupt. + * @arg USART_IT_RTO: Receive time out interrupt. + * @arg USART_IT_CTS: CTS change interrupt. + * @arg USART_IT_LBD: LIN Break detection interrupt. + * @arg USART_IT_TXE: Transmit Data Register empty interrupt. + * @arg USART_IT_TC: Transmission complete interrupt. + * @arg USART_IT_RXNE: Receive Data register not empty interrupt. + * @arg USART_IT_IDLE: Idle line detection interrupt. + * @arg USART_IT_ORE: OverRun Error interrupt. + * @arg USART_IT_NE: Noise Error interrupt. + * @arg USART_IT_FE: Framing Error interrupt. + * @arg USART_IT_PE: Parity Error interrupt. + * @retval The new state of USART_IT (SET or RESET). + */ +ITStatus USART_GetITStatus(USART_TypeDef* USARTx, uint32_t USART_IT) +{ + uint32_t bitpos = 0, itmask = 0, usartreg = 0; + ITStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_GET_IT(USART_IT)); + + /* Get the USART register index */ + usartreg = (((uint16_t)USART_IT) >> 0x08); + /* Get the interrupt position */ + itmask = USART_IT & IT_MASK; + itmask = (uint32_t)0x01 << itmask; + + if (usartreg == 0x01) /* The IT is in CR1 register */ + { + itmask &= USARTx->CR1; + } + else if (usartreg == 0x02) /* The IT is in CR2 register */ + { + itmask &= USARTx->CR2; + } + else /* The IT is in CR3 register */ + { + itmask &= USARTx->CR3; + } + + bitpos = USART_IT >> 0x10; + bitpos = (uint32_t)0x01 << bitpos; + bitpos &= USARTx->ISR; + if ((itmask != (uint16_t)RESET)&&(bitpos != (uint16_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + + return bitstatus; +} + +/** + * @brief Clears the USARTx's interrupt pending bits. + * @param USARTx: Select the USART peripheral. This parameter can be one of the + * following values: USART1 or USART2 or USART3 or UART4 or UART5. + * @param USART_IT: specifies the interrupt pending bit to clear. + * This parameter can be one of the following values: + * @arg USART_IT_WU: Wake up interrupt. + * @arg USART_IT_CM: Character match interrupt. + * @arg USART_IT_EOB: End of block interrupt. + * @arg USART_IT_RTO: Receive time out interrupt. + * @arg USART_IT_CTS: CTS change interrupt. + * @arg USART_IT_LBD: LIN Break detection interrupt. + * @arg USART_IT_TC: Transmission complete interrupt. + * @arg USART_IT_IDLE: IDLE line detected interrupt. + * @arg USART_IT_ORE: OverRun Error interrupt. + * @arg USART_IT_NE: Noise Error interrupt. + * @arg USART_IT_FE: Framing Error interrupt. + * @arg USART_IT_PE: Parity Error interrupt. + * @note + * - RXNE pending bit is cleared by a read to the USART_RDR register + * (USART_ReceiveData()) or by writing 1 to the RXFRQ in the register USART_RQR + * (USART_RequestCmd()). + * - TC pending bit can be also cleared by software sequence: a read + * operation to USART_SR register (USART_GetITStatus()) followed by a write + * operation to USART_TDR register (USART_SendData()). + * - TXE pending bit is cleared by a write to the USART_TDR register + * (USART_SendData()) or by writing 1 to the TXFRQ in the register USART_RQR + * (USART_RequestCmd()). + * @retval None + */ +void USART_ClearITPendingBit(USART_TypeDef* USARTx, uint32_t USART_IT) +{ + uint32_t bitpos = 0, itmask = 0; + /* Check the parameters */ + assert_param(IS_USART_ALL_PERIPH(USARTx)); + assert_param(IS_USART_CLEAR_IT(USART_IT)); + + bitpos = USART_IT >> 0x10; + itmask = ((uint32_t)0x01 << (uint32_t)bitpos); + USARTx->ICR = (uint32_t)itmask; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_wwdg.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_wwdg.c new file mode 100755 index 0000000000..f51e887e63 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_wwdg.c @@ -0,0 +1,304 @@ +/** + ****************************************************************************** + * @file stm32f30x_wwdg.c + * @author MCD Application Team + * @version V1.2.3 + * @date 10-July-2015 + * @brief This file provides firmware functions to manage the following + * functionalities of the Window watchdog (WWDG) peripheral: + * + Prescaler, Refresh window and Counter configuration + * + WWDG activation + * + Interrupts and flags management + * + * @verbatim + * + ============================================================================== + ##### WWDG features ##### + ============================================================================== + + [..] Once enabled the WWDG generates a system reset on expiry of a programmed + time period, unless the program refreshes the counter (downcounter) + before to reach 0x3F value (i.e. a reset is generated when the counter + value rolls over from 0x40 to 0x3F). + [..] An MCU reset is also generated if the counter value is refreshed + before the counter has reached the refresh window value. This + implies that the counter must be refreshed in a limited window. + + [..] Once enabled the WWDG cannot be disabled except by a system reset. + + [..] WWDGRST flag in RCC_CSR register can be used to inform when a WWDG + reset occurs. + + [..] The WWDG counter input clock is derived from the APB clock divided + by a programmable prescaler. + + [..] WWDG counter clock = PCLK1 / Prescaler. + [..] WWDG timeout = (WWDG counter clock) * (counter value). + + [..] Min-max timeout value @36MHz (PCLK1): ~114us / ~58.3ms. + + ##### How to use this driver ##### + ============================================================================== + [..] + (#) Enable WWDG clock using RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE) + function. + + (#) Configure the WWDG prescaler using WWDG_SetPrescaler() function. + + (#) Configure the WWDG refresh window using WWDG_SetWindowValue() function. + + (#) Set the WWDG counter value and start it using WWDG_Enable() function. + When the WWDG is enabled the counter value should be configured to + a value greater than 0x40 to prevent generating an immediate reset. + + (#) Optionally you can enable the Early wakeup interrupt which is + generated when the counter reach 0x40. + Once enabled this interrupt cannot be disabled except by a system reset. + + (#) Then the application program must refresh the WWDG counter at regular + intervals during normal operation to prevent an MCU reset, using + WWDG_SetCounter() function. This operation must occur only when + the counter value is lower than the refresh window value, + programmed using WWDG_SetWindowValue(). + + @endverbatim + + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x_wwdg.h" +#include "stm32f30x_rcc.h" + +/** @addtogroup STM32F30x_StdPeriph_Driver + * @{ + */ + +/** @defgroup WWDG + * @brief WWDG driver modules + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* --------------------- WWDG registers bit mask ---------------------------- */ +/* CFR register bit mask */ +#define CFR_WDGTB_MASK ((uint32_t)0xFFFFFE7F) +#define CFR_W_MASK ((uint32_t)0xFFFFFF80) +#define BIT_MASK ((uint8_t)0x7F) + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/** @defgroup WWDG_Private_Functions + * @{ + */ + +/** @defgroup WWDG_Group1 Prescaler, Refresh window and Counter configuration functions + * @brief Prescaler, Refresh window and Counter configuration functions + * +@verbatim + ============================================================================== + ##### Prescaler, Refresh window and Counter configuration functions ##### + ============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Deinitializes the WWDG peripheral registers to their default reset values. + * @param None + * @retval None + */ +void WWDG_DeInit(void) +{ + RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, DISABLE); +} + +/** + * @brief Sets the WWDG Prescaler. + * @param WWDG_Prescaler: specifies the WWDG Prescaler. + * This parameter can be one of the following values: + * @arg WWDG_Prescaler_1: WWDG counter clock = (PCLK1/4096)/1 + * @arg WWDG_Prescaler_2: WWDG counter clock = (PCLK1/4096)/2 + * @arg WWDG_Prescaler_4: WWDG counter clock = (PCLK1/4096)/4 + * @arg WWDG_Prescaler_8: WWDG counter clock = (PCLK1/4096)/8 + * @retval None + */ +void WWDG_SetPrescaler(uint32_t WWDG_Prescaler) +{ + uint32_t tmpreg = 0; + /* Check the parameters */ + assert_param(IS_WWDG_PRESCALER(WWDG_Prescaler)); + /* Clear WDGTB[1:0] bits */ + tmpreg = WWDG->CFR & CFR_WDGTB_MASK; + /* Set WDGTB[1:0] bits according to WWDG_Prescaler value */ + tmpreg |= WWDG_Prescaler; + /* Store the new value */ + WWDG->CFR = tmpreg; +} + +/** + * @brief Sets the WWDG window value. + * @param WindowValue: specifies the window value to be compared to the downcounter. + * This parameter value must be lower than 0x80. + * @retval None + */ +void WWDG_SetWindowValue(uint8_t WindowValue) +{ + __IO uint32_t tmpreg = 0; + + /* Check the parameters */ + assert_param(IS_WWDG_WINDOW_VALUE(WindowValue)); + /* Clear W[6:0] bits */ + + tmpreg = WWDG->CFR & CFR_W_MASK; + + /* Set W[6:0] bits according to WindowValue value */ + tmpreg |= WindowValue & (uint32_t) BIT_MASK; + + /* Store the new value */ + WWDG->CFR = tmpreg; +} + +/** + * @brief Enables the WWDG Early Wakeup interrupt(EWI). + * @note Once enabled this interrupt cannot be disabled except by a system reset. + * @param None + * @retval None + */ +void WWDG_EnableIT(void) +{ + WWDG->CFR |= WWDG_CFR_EWI; +} + +/** + * @brief Sets the WWDG counter value. + * @param Counter: specifies the watchdog counter value. + * This parameter must be a number between 0x40 and 0x7F (to prevent generating + * an immediate reset). + * @retval None + */ +void WWDG_SetCounter(uint8_t Counter) +{ + /* Check the parameters */ + assert_param(IS_WWDG_COUNTER(Counter)); + /* Write to T[6:0] bits to configure the counter value, no need to do + a read-modify-write; writing a 0 to WDGA bit does nothing */ + WWDG->CR = Counter & BIT_MASK; +} + +/** + * @} + */ + +/** @defgroup WWDG_Group2 WWDG activation functions + * @brief WWDG activation functions + * +@verbatim + ============================================================================== + ##### WWDG activation function ##### + ============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Enables WWDG and load the counter value. + * @param Counter: specifies the watchdog counter value. + * This parameter must be a number between 0x40 and 0x7F (to prevent generating + * an immediate reset). + * @retval None + */ +void WWDG_Enable(uint8_t Counter) +{ + /* Check the parameters */ + assert_param(IS_WWDG_COUNTER(Counter)); + WWDG->CR = WWDG_CR_WDGA | Counter; +} + +/** + * @} + */ + +/** @defgroup WWDG_Group3 Interrupts and flags management functions + * @brief Interrupts and flags management functions + * +@verbatim + ============================================================================== + ##### Interrupts and flags management functions ##### + ============================================================================== + +@endverbatim + * @{ + */ + +/** + * @brief Checks whether the Early Wakeup interrupt flag is set or not. + * @param None + * @retval The new state of the Early Wakeup interrupt flag (SET or RESET). + */ +FlagStatus WWDG_GetFlagStatus(void) +{ + FlagStatus bitstatus = RESET; + + if ((WWDG->SR) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears Early Wakeup interrupt flag. + * @param None + * @retval None + */ +void WWDG_ClearFlag(void) +{ + WWDG->SR = (uint32_t)RESET; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h new file mode 100755 index 0000000000..4184a9c084 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h @@ -0,0 +1,259 @@ +/** + ****************************************************************************** + * @file usb_core.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Standard protocol processing functions prototypes + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_CORE_H +#define __USB_CORE_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +typedef enum _CONTROL_STATE +{ + WAIT_SETUP, /* 0 */ + SETTING_UP, /* 1 */ + IN_DATA, /* 2 */ + OUT_DATA, /* 3 */ + LAST_IN_DATA, /* 4 */ + LAST_OUT_DATA, /* 5 */ + WAIT_STATUS_IN, /* 7 */ + WAIT_STATUS_OUT, /* 8 */ + STALLED, /* 9 */ + PAUSE /* 10 */ +} CONTROL_STATE; /* The state machine states of a control pipe */ + +typedef struct OneDescriptor +{ + const uint8_t *Descriptor; + uint16_t Descriptor_Size; +} +ONE_DESCRIPTOR, *PONE_DESCRIPTOR; +/* All the request process routines return a value of this type + If the return value is not SUCCESS or NOT_READY, + the software will STALL the correspond endpoint */ +typedef enum _RESULT +{ + USB_SUCCESS = 0, /* Process successfully */ + USB_ERROR, + USB_UNSUPPORT, + USB_NOT_READY /* The process has not been finished, endpoint will be + NAK to further request */ +} RESULT; + + +/*-*-*-*-*-*-*-*-*-*-* Definitions for endpoint level -*-*-*-*-*-*-*-*-*-*-*-*/ +typedef struct _ENDPOINT_INFO +{ + /* When send data out of the device, + CopyData() is used to get data buffer 'Length' bytes data + if Length is 0, + CopyData() returns the total length of the data + if the request is not supported, returns 0 + (NEW Feature ) + if CopyData() returns -1, the calling routine should not proceed + further and will resume the SETUP process by the class device + if Length is not 0, + CopyData() returns a pointer to indicate the data location + Usb_wLength is the data remain to be sent, + Usb_wOffset is the Offset of original data + When receive data from the host, + CopyData() is used to get user data buffer which is capable + of Length bytes data to copy data from the endpoint buffer. + if Length is 0, + CopyData() returns the available data length, + if Length is not 0, + CopyData() returns user buffer address + Usb_rLength is the data remain to be received, + Usb_rPointer is the Offset of data buffer + */ + uint16_t Usb_wLength; + uint16_t Usb_wOffset; + uint16_t PacketSize; + uint8_t *(*CopyData)(uint16_t Length); +}ENDPOINT_INFO; + +/*-*-*-*-*-*-*-*-*-*-*-* Definitions for device level -*-*-*-*-*-*-*-*-*-*-*-*/ + +typedef struct _DEVICE +{ + uint8_t Total_Endpoint; /* Number of endpoints that are used */ + uint8_t Total_Configuration;/* Number of configuration available */ +} +DEVICE; + +typedef union +{ + uint16_t w; + struct BW + { + uint8_t bb1; + uint8_t bb0; + } + bw; +} uint16_t_uint8_t; + +typedef struct _DEVICE_INFO +{ + uint8_t USBbmRequestType; /* bmRequestType */ + uint8_t USBbRequest; /* bRequest */ + uint16_t_uint8_t USBwValues; /* wValue */ + uint16_t_uint8_t USBwIndexs; /* wIndex */ + uint16_t_uint8_t USBwLengths; /* wLength */ + + uint8_t ControlState; /* of type CONTROL_STATE */ + uint8_t Current_Feature; + uint8_t Current_Configuration; /* Selected configuration */ + uint8_t Current_Interface; /* Selected interface of current configuration */ + uint8_t Current_AlternateSetting;/* Selected Alternate Setting of current + interface*/ + + ENDPOINT_INFO Ctrl_Info; +}DEVICE_INFO; + +typedef struct _DEVICE_PROP +{ + void (*Init)(void); /* Initialize the device */ + void (*Reset)(void); /* Reset routine of this device */ + + /* Device dependent process after the status stage */ + void (*Process_Status_IN)(void); + void (*Process_Status_OUT)(void); + + /* Procedure of process on setup stage of a class specified request with data stage */ + /* All class specified requests with data stage are processed in Class_Data_Setup + Class_Data_Setup() + responses to check all special requests and fills ENDPOINT_INFO + according to the request + If IN tokens are expected, then wLength & wOffset will be filled + with the total transferring bytes and the starting position + If OUT tokens are expected, then rLength & rOffset will be filled + with the total expected bytes and the starting position in the buffer + + If the request is valid, Class_Data_Setup returns SUCCESS, else UNSUPPORT + + CAUTION: + Since GET_CONFIGURATION & GET_INTERFACE are highly related to + the individual classes, they will be checked and processed here. + */ + RESULT (*Class_Data_Setup)(uint8_t RequestNo); + + /* Procedure of process on setup stage of a class specified request without data stage */ + /* All class specified requests without data stage are processed in Class_NoData_Setup + Class_NoData_Setup + responses to check all special requests and perform the request + + CAUTION: + Since SET_CONFIGURATION & SET_INTERFACE are highly related to + the individual classes, they will be checked and processed here. + */ + RESULT (*Class_NoData_Setup)(uint8_t RequestNo); + + /*Class_Get_Interface_Setting + This function is used by the file usb_core.c to test if the selected Interface + and Alternate Setting (uint8_t Interface, uint8_t AlternateSetting) are supported by + the application. + This function is writing by user. It should return "SUCCESS" if the Interface + and Alternate Setting are supported by the application or "UNSUPPORT" if they + are not supported. */ + + RESULT (*Class_Get_Interface_Setting)(uint8_t Interface, uint8_t AlternateSetting); + + const uint8_t* (*GetDeviceDescriptor)(uint16_t Length); + const uint8_t* (*GetConfigDescriptor)(uint16_t Length); + const uint8_t* (*GetStringDescriptor)(uint16_t Length); + + /* This field is not used in current library version. It is kept only for + compatibility with previous versions */ + void* RxEP_buffer; + + uint8_t MaxPacketSize; + +}DEVICE_PROP; + +typedef struct _USER_STANDARD_REQUESTS +{ + void (*User_GetConfiguration)(void); /* Get Configuration */ + void (*User_SetConfiguration)(void); /* Set Configuration */ + void (*User_GetInterface)(void); /* Get Interface */ + void (*User_SetInterface)(void); /* Set Interface */ + void (*User_GetStatus)(void); /* Get Status */ + void (*User_ClearFeature)(void); /* Clear Feature */ + void (*User_SetEndPointFeature)(void); /* Set Endpoint Feature */ + void (*User_SetDeviceFeature)(void); /* Set Device Feature */ + void (*User_SetDeviceAddress)(void); /* Set Device Address */ +} +USER_STANDARD_REQUESTS; + +/* Exported constants --------------------------------------------------------*/ +#define Type_Recipient (pInformation->USBbmRequestType & (REQUEST_TYPE | RECIPIENT)) + +#define Usb_rLength Usb_wLength +#define Usb_rOffset Usb_wOffset + +#define USBwValue USBwValues.w +#define USBwValue0 USBwValues.bw.bb0 +#define USBwValue1 USBwValues.bw.bb1 +#define USBwIndex USBwIndexs.w +#define USBwIndex0 USBwIndexs.bw.bb0 +#define USBwIndex1 USBwIndexs.bw.bb1 +#define USBwLength USBwLengths.w +#define USBwLength0 USBwLengths.bw.bb0 +#define USBwLength1 USBwLengths.bw.bb1 + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +uint8_t Setup0_Process(void); +uint8_t Post0_Process(void); +uint8_t Out0_Process(void); +uint8_t In0_Process(void); + +RESULT Standard_SetEndPointFeature(void); +RESULT Standard_SetDeviceFeature(void); + +const uint8_t *Standard_GetConfiguration(uint16_t Length); +RESULT Standard_SetConfiguration(void); +const uint8_t *Standard_GetInterface(uint16_t Length); +RESULT Standard_SetInterface(void); +uint8_t *Standard_GetDescriptorData(uint16_t Length, PONE_DESCRIPTOR pDesc); + +const uint8_t *Standard_GetStatus(uint16_t Length); +RESULT Standard_ClearFeature(void); +void SetDeviceAddress(uint8_t); +void NOP_Process(void); + +extern DEVICE_PROP Device_Property; +extern USER_STANDARD_REQUESTS User_Standard_Requests; +extern DEVICE Device_Table; +extern DEVICE_INFO Device_Info; + +/* cells saving status during interrupt servicing */ +extern __IO uint16_t SaveRState; +extern __IO uint16_t SaveTState; + +#endif /* __USB_CORE_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h new file mode 100755 index 0000000000..7f6e1b508b --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_def.h @@ -0,0 +1,92 @@ +/** + ****************************************************************************** + * @file usb_def.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Definitions related to USB Core + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_DEF_H +#define __USB_DEF_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +typedef enum _RECIPIENT_TYPE +{ + DEVICE_RECIPIENT, /* Recipient device */ + INTERFACE_RECIPIENT, /* Recipient interface */ + ENDPOINT_RECIPIENT, /* Recipient endpoint */ + OTHER_RECIPIENT +} RECIPIENT_TYPE; + + +typedef enum _STANDARD_REQUESTS +{ + GET_STATUS = 0, + CLEAR_FEATURE, + RESERVED1, + SET_FEATURE, + RESERVED2, + SET_ADDRESS, + GET_DESCRIPTOR, + SET_DESCRIPTOR, + GET_CONFIGURATION, + SET_CONFIGURATION, + GET_INTERFACE, + SET_INTERFACE, + TOTAL_sREQUEST, /* Total number of Standard request */ + SYNCH_FRAME = 12 +} STANDARD_REQUESTS; + +/* Definition of "USBwValue" */ +typedef enum _DESCRIPTOR_TYPE +{ + DEVICE_DESCRIPTOR = 1, + CONFIG_DESCRIPTOR, + STRING_DESCRIPTOR, + INTERFACE_DESCRIPTOR, + ENDPOINT_DESCRIPTOR +} DESCRIPTOR_TYPE; + +/* Feature selector of a SET_FEATURE or CLEAR_FEATURE */ +typedef enum _FEATURE_SELECTOR +{ + ENDPOINT_STALL, + DEVICE_REMOTE_WAKEUP +} FEATURE_SELECTOR; + +/* Exported constants --------------------------------------------------------*/ +/* Definition of "USBbmRequestType" */ +#define REQUEST_TYPE 0x60 /* Mask to get request type */ +#define STANDARD_REQUEST 0x00 /* Standard request */ +#define CLASS_REQUEST 0x20 /* Class request */ +#define VENDOR_REQUEST 0x40 /* Vendor request */ + +#define RECIPIENT 0x1F /* Mask to get recipient */ + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +#endif /* __USB_DEF_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h new file mode 100755 index 0000000000..44b528cbd4 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_init.h @@ -0,0 +1,62 @@ +/** + ****************************************************************************** + * @file usb_init.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Initialization routines & global variables + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_INIT_H +#define __USB_INIT_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +void USB_Init(void); + +/* External variables --------------------------------------------------------*/ +/* The number of current endpoint, it will be used to specify an endpoint */ +extern uint8_t EPindex; +/* The number of current device, it is an index to the Device_Table */ +/*extern uint8_t Device_no; */ +/* Points to the DEVICE_INFO structure of current device */ +/* The purpose of this register is to speed up the execution */ +extern DEVICE_INFO* pInformation; +/* Points to the DEVICE_PROP structure of current device */ +/* The purpose of this register is to speed up the execution */ +extern DEVICE_PROP* pProperty; +/* Temporary save the state of Rx & Tx status. */ +/* Whenever the Rx or Tx state is changed, its value is saved */ +/* in this variable first and will be set to the EPRB or EPRA */ +/* at the end of interrupt process */ +extern USER_STANDARD_REQUESTS *pUser_Standard_Requests; + +extern uint16_t SaveState ; +extern uint16_t wInterrupt_Mask; + +#endif /* __USB_INIT_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h new file mode 100755 index 0000000000..307ab14ea7 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_int.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @file usb_int.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Endpoint CTR (Low and High) interrupt's service routines prototypes + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_INT_H +#define __USB_INT_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +void CTR_LP(void); +void CTR_HP(void); + +/* External variables --------------------------------------------------------*/ + +#endif /* __USB_INT_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h new file mode 100755 index 0000000000..d40336d196 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_lib.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @file usb_lib.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief USB library include files + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_LIB_H +#define __USB_LIB_H + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f30x.h" +#include "usb_type.h" +#include "usb_regs.h" +#include "usb_def.h" +#include "usb_core.h" +#include "usb_init.h" +#include "usb_sil.h" +#include "usb_mem.h" +#include "usb_int.h" + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* External variables --------------------------------------------------------*/ + +#endif /* __USB_LIB_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h new file mode 100755 index 0000000000..0b2910060a --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @file usb_mem.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Utility prototypes functions for memory/PMA transfers + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_MEM_H +#define __USB_MEM_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); +void PMAToUserBufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); + +/* External variables --------------------------------------------------------*/ + +#endif /*__USB_MEM_H*/ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h new file mode 100755 index 0000000000..3c2bcb0189 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_regs.h @@ -0,0 +1,703 @@ +/** + ****************************************************************************** + * @file usb_regs.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Interface prototype functions to USB cell registers + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_REGS_H +#define __USB_REGS_H + +#if defined(STM32F303xC) || \ + defined(STM32F303x8) || defined(STM32F334x8) || \ + defined(STM32F301x8) || \ + defined(STM32F373xC) || defined(STM32F378xx) || \ + defined(STM32F302xC) +#define __USB_PMA_1x16 +typedef uint32_t usb_pma_t; +#endif /* STM32F303xC || */ + /* STM32F303x8 || STM32F334x8 || */ + /* STM32F301x8 || */ + /* STM32F373xC || STM32F378xx */ +#if defined(STM32F302xE) || defined(STM32F303xD) || defined(STM32F303xE) +#define __USB_PMA_2x16 +typedef uint16_t usb_pma_t; +#endif /* STM32F302xE || STM32F303xD || STM32F303xE */ + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +typedef enum _EP_DBUF_DIR +{ + /* double buffered endpoint direction */ + EP_DBUF_ERR, + EP_DBUF_OUT, + EP_DBUF_IN +}EP_DBUF_DIR; + +/* endpoint buffer number */ +enum EP_BUF_NUM +{ + EP_NOBUF, + EP_BUF0, + EP_BUF1 +}; + +/* Exported constants --------------------------------------------------------*/ +#define RegBase (0x40005C00L) /* USB_IP Peripheral Registers base address */ +#define PMAAddr (0x40006000L) /* USB_IP Packet Memory Area base address */ + +/******************************************************************************/ +/* General registers */ +/******************************************************************************/ + +/* Control register */ +#define CNTR ((__IO unsigned *)(RegBase + 0x40)) +/* Interrupt status register */ +#define ISTR ((__IO unsigned *)(RegBase + 0x44)) +/* Frame number register */ +#define FNR ((__IO unsigned *)(RegBase + 0x48)) +/* Device address register */ +#define DADDR ((__IO unsigned *)(RegBase + 0x4C)) +/* Buffer Table address register */ +#define BTABLE ((__IO unsigned *)(RegBase + 0x50)) +/******************************************************************************/ +/* Endpoint registers */ +/******************************************************************************/ +#define EP0REG ((__IO unsigned *)(RegBase)) /* endpoint 0 register address */ + +/* Endpoint Addresses (w/direction) */ +#define EP0_OUT ((uint8_t)0x00) +#define EP0_IN ((uint8_t)0x80) +#define EP1_OUT ((uint8_t)0x01) +#define EP1_IN ((uint8_t)0x81) +#define EP2_OUT ((uint8_t)0x02) +#define EP2_IN ((uint8_t)0x82) +#define EP3_OUT ((uint8_t)0x03) +#define EP3_IN ((uint8_t)0x83) +#define EP4_OUT ((uint8_t)0x04) +#define EP4_IN ((uint8_t)0x84) +#define EP5_OUT ((uint8_t)0x05) +#define EP5_IN ((uint8_t)0x85) +#define EP6_OUT ((uint8_t)0x06) +#define EP6_IN ((uint8_t)0x86) +#define EP7_OUT ((uint8_t)0x07) +#define EP7_IN ((uint8_t)0x87) + +/* endpoints enumeration */ +#define ENDP0 ((uint8_t)0) +#define ENDP1 ((uint8_t)1) +#define ENDP2 ((uint8_t)2) +#define ENDP3 ((uint8_t)3) +#define ENDP4 ((uint8_t)4) +#define ENDP5 ((uint8_t)5) +#define ENDP6 ((uint8_t)6) +#define ENDP7 ((uint8_t)7) + +/******************************************************************************/ +/* ISTR interrupt events */ +/******************************************************************************/ +#define ISTR_CTR (0x8000) /* Correct TRansfer (clear-only bit) */ +#define ISTR_DOVR (0x4000) /* DMA OVeR/underrun (clear-only bit) */ +#define ISTR_ERR (0x2000) /* ERRor (clear-only bit) */ +#define ISTR_WKUP (0x1000) /* WaKe UP (clear-only bit) */ +#define ISTR_SUSP (0x0800) /* SUSPend (clear-only bit) */ +#define ISTR_RESET (0x0400) /* RESET (clear-only bit) */ +#define ISTR_SOF (0x0200) /* Start Of Frame (clear-only bit) */ +#define ISTR_ESOF (0x0100) /* Expected Start Of Frame (clear-only bit) */ + + +#define ISTR_DIR (0x0010) /* DIRection of transaction (read-only bit) */ +#define ISTR_EP_ID (0x000F) /* EndPoint IDentifier (read-only bit) */ + +#define CLR_CTR (~ISTR_CTR) /* clear Correct TRansfer bit */ +#define CLR_DOVR (~ISTR_DOVR) /* clear DMA OVeR/underrun bit*/ +#define CLR_ERR (~ISTR_ERR) /* clear ERRor bit */ +#define CLR_WKUP (~ISTR_WKUP) /* clear WaKe UP bit */ +#define CLR_SUSP (~ISTR_SUSP) /* clear SUSPend bit */ +#define CLR_RESET (~ISTR_RESET) /* clear RESET bit */ +#define CLR_SOF (~ISTR_SOF) /* clear Start Of Frame bit */ +#define CLR_ESOF (~ISTR_ESOF) /* clear Expected Start Of Frame bit */ + +/******************************************************************************/ +/* CNTR control register bits definitions */ +/******************************************************************************/ +#define CNTR_CTRM (0x8000) /* Correct TRansfer Mask */ +#define CNTR_DOVRM (0x4000) /* DMA OVeR/underrun Mask */ +#define CNTR_ERRM (0x2000) /* ERRor Mask */ +#define CNTR_WKUPM (0x1000) /* WaKe UP Mask */ +#define CNTR_SUSPM (0x0800) /* SUSPend Mask */ +#define CNTR_RESETM (0x0400) /* RESET Mask */ +#define CNTR_SOFM (0x0200) /* Start Of Frame Mask */ +#define CNTR_ESOFM (0x0100) /* Expected Start Of Frame Mask */ + + +#define CNTR_RESUME (0x0010) /* RESUME request */ +#define CNTR_FSUSP (0x0008) /* Force SUSPend */ +#define CNTR_LPMODE (0x0004) /* Low-power MODE */ +#define CNTR_PDWN (0x0002) /* Power DoWN */ +#define CNTR_FRES (0x0001) /* Force USB RESet */ + +/******************************************************************************/ +/* FNR Frame Number Register bit definitions */ +/******************************************************************************/ +#define FNR_RXDP (0x8000) /* status of D+ data line */ +#define FNR_RXDM (0x4000) /* status of D- data line */ +#define FNR_LCK (0x2000) /* LoCKed */ +#define FNR_LSOF (0x1800) /* Lost SOF */ +#define FNR_FN (0x07FF) /* Frame Number */ +/******************************************************************************/ +/* DADDR Device ADDRess bit definitions */ +/******************************************************************************/ +#define DADDR_EF (0x80) +#define DADDR_ADD (0x7F) +/******************************************************************************/ +/* Endpoint register */ +/******************************************************************************/ +/* bit positions */ +#define EP_CTR_RX (0x8000) /* EndPoint Correct TRansfer RX */ +#define EP_DTOG_RX (0x4000) /* EndPoint Data TOGGLE RX */ +#define EPRX_STAT (0x3000) /* EndPoint RX STATus bit field */ +#define EP_SETUP (0x0800) /* EndPoint SETUP */ +#define EP_T_FIELD (0x0600) /* EndPoint TYPE */ +#define EP_KIND (0x0100) /* EndPoint KIND */ +#define EP_CTR_TX (0x0080) /* EndPoint Correct TRansfer TX */ +#define EP_DTOG_TX (0x0040) /* EndPoint Data TOGGLE TX */ +#define EPTX_STAT (0x0030) /* EndPoint TX STATus bit field */ +#define EPADDR_FIELD (0x000F) /* EndPoint ADDRess FIELD */ + +/* EndPoint REGister MASK (no toggle fields) */ +#define EPREG_MASK (EP_CTR_RX|EP_SETUP|EP_T_FIELD|EP_KIND|EP_CTR_TX|EPADDR_FIELD) + +/* EP_TYPE[1:0] EndPoint TYPE */ +#define EP_TYPE_MASK (0x0600) /* EndPoint TYPE Mask */ +#define EP_BULK (0x0000) /* EndPoint BULK */ +#define EP_CONTROL (0x0200) /* EndPoint CONTROL */ +#define EP_ISOCHRONOUS (0x0400) /* EndPoint ISOCHRONOUS */ +#define EP_INTERRUPT (0x0600) /* EndPoint INTERRUPT */ +#define EP_T_MASK (~EP_T_FIELD & EPREG_MASK) + + +/* EP_KIND EndPoint KIND */ +#define EPKIND_MASK (~EP_KIND & EPREG_MASK) + +/* STAT_TX[1:0] STATus for TX transfer */ +#define EP_TX_DIS (0x0000) /* EndPoint TX DISabled */ +#define EP_TX_STALL (0x0010) /* EndPoint TX STALLed */ +#define EP_TX_NAK (0x0020) /* EndPoint TX NAKed */ +#define EP_TX_VALID (0x0030) /* EndPoint TX VALID */ +#define EPTX_DTOG1 (0x0010) /* EndPoint TX Data TOGgle bit1 */ +#define EPTX_DTOG2 (0x0020) /* EndPoint TX Data TOGgle bit2 */ +#define EPTX_DTOGMASK (EPTX_STAT|EPREG_MASK) + +/* STAT_RX[1:0] STATus for RX transfer */ +#define EP_RX_DIS (0x0000) /* EndPoint RX DISabled */ +#define EP_RX_STALL (0x1000) /* EndPoint RX STALLed */ +#define EP_RX_NAK (0x2000) /* EndPoint RX NAKed */ +#define EP_RX_VALID (0x3000) /* EndPoint RX VALID */ +#define EPRX_DTOG1 (0x1000) /* EndPoint RX Data TOGgle bit1 */ +#define EPRX_DTOG2 (0x2000) /* EndPoint RX Data TOGgle bit1 */ +#define EPRX_DTOGMASK (EPRX_STAT|EPREG_MASK) +/* Exported macro ------------------------------------------------------------*/ +/* SetCNTR */ +#define _SetCNTR(wRegValue) (*CNTR = (uint16_t)wRegValue) + +/* SetISTR */ +#define _SetISTR(wRegValue) (*ISTR = (uint16_t)wRegValue) + +/* SetDADDR */ +#define _SetDADDR(wRegValue) (*DADDR = (uint16_t)wRegValue) + +/* SetBTABLE */ +#define _SetBTABLE(wRegValue)(*BTABLE = (uint16_t)(wRegValue & 0xFFF8)) + +/* GetCNTR */ +#define _GetCNTR() ((uint16_t) *CNTR) + +/* GetISTR */ +#define _GetISTR() ((uint16_t) *ISTR) + +/* GetFNR */ +#define _GetFNR() ((uint16_t) *FNR) + +/* GetDADDR */ +#define _GetDADDR() ((uint16_t) *DADDR) + +/* GetBTABLE */ +#define _GetBTABLE() ((uint16_t) *BTABLE) + +/* SetENDPOINT */ +#define _SetENDPOINT(bEpNum,wRegValue) (*(EP0REG + bEpNum)= \ + (uint16_t)wRegValue) + +/* GetENDPOINT */ +#define _GetENDPOINT(bEpNum) ((uint16_t)(*(EP0REG + bEpNum))) + +/******************************************************************************* +* Macro Name : SetEPType +* Description : sets the type in the endpoint register(bits EP_TYPE[1:0]) +* Input : bEpNum: Endpoint Number. +* wType +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPType(bEpNum,wType) (_SetENDPOINT(bEpNum,\ + ((_GetENDPOINT(bEpNum) & EP_T_MASK) | wType ))) + +/******************************************************************************* +* Macro Name : GetEPType +* Description : gets the type in the endpoint register(bits EP_TYPE[1:0]) +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint Type +*******************************************************************************/ +#define _GetEPType(bEpNum) (_GetENDPOINT(bEpNum) & EP_T_FIELD) + +/******************************************************************************* +* Macro Name : SetEPTxStatus +* Description : sets the status for tx transfer (bits STAT_TX[1:0]). +* Input : bEpNum: Endpoint Number. +* wState: new state +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPTxStatus(bEpNum,wState) {\ + register uint16_t _wRegVal; \ + _wRegVal = _GetENDPOINT(bEpNum) & EPTX_DTOGMASK;\ + /* toggle first bit ? */ \ + if((EPTX_DTOG1 & wState)!= 0) \ + _wRegVal ^= EPTX_DTOG1; \ + /* toggle second bit ? */ \ + if((EPTX_DTOG2 & wState)!= 0) \ + _wRegVal ^= EPTX_DTOG2; \ + _SetENDPOINT(bEpNum, (_wRegVal | EP_CTR_RX|EP_CTR_TX)); \ + } /* _SetEPTxStatus */ + +/******************************************************************************* +* Macro Name : SetEPRxStatus +* Description : sets the status for rx transfer (bits STAT_TX[1:0]) +* Input : bEpNum: Endpoint Number. +* wState: new state. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPRxStatus(bEpNum,wState) {\ + register uint16_t _wRegVal; \ + \ + _wRegVal = _GetENDPOINT(bEpNum) & EPRX_DTOGMASK;\ + /* toggle first bit ? */ \ + if((EPRX_DTOG1 & wState)!= 0) \ + _wRegVal ^= EPRX_DTOG1; \ + /* toggle second bit ? */ \ + if((EPRX_DTOG2 & wState)!= 0) \ + _wRegVal ^= EPRX_DTOG2; \ + _SetENDPOINT(bEpNum, (_wRegVal | EP_CTR_RX|EP_CTR_TX)); \ + } /* _SetEPRxStatus */ + +/******************************************************************************* +* Macro Name : SetEPRxTxStatus +* Description : sets the status for rx & tx (bits STAT_TX[1:0] & STAT_RX[1:0]) +* Input : bEpNum: Endpoint Number. +* wStaterx: new state. +* wStatetx: new state. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPRxTxStatus(bEpNum,wStaterx,wStatetx) {\ + register uint32_t _wRegVal; \ + \ + _wRegVal = _GetENDPOINT(bEpNum) & (EPRX_DTOGMASK |EPTX_STAT) ;\ + /* toggle first bit ? */ \ + if((EPRX_DTOG1 & wStaterx)!= 0) \ + _wRegVal ^= EPRX_DTOG1; \ + /* toggle second bit ? */ \ + if((EPRX_DTOG2 & wStaterx)!= 0) \ + _wRegVal ^= EPRX_DTOG2; \ + /* toggle first bit ? */ \ + if((EPTX_DTOG1 & wStatetx)!= 0) \ + _wRegVal ^= EPTX_DTOG1; \ + /* toggle second bit ? */ \ + if((EPTX_DTOG2 & wStatetx)!= 0) \ + _wRegVal ^= EPTX_DTOG2; \ + _SetENDPOINT(bEpNum, _wRegVal | EP_CTR_RX|EP_CTR_TX); \ + } /* _SetEPRxTxStatus */ +/******************************************************************************* +* Macro Name : GetEPTxStatus / GetEPRxStatus +* Description : gets the status for tx/rx transfer (bits STAT_TX[1:0] +* /STAT_RX[1:0]) +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : status . +*******************************************************************************/ +#define _GetEPTxStatus(bEpNum) ((uint16_t)_GetENDPOINT(bEpNum) & EPTX_STAT) + +#define _GetEPRxStatus(bEpNum) ((uint16_t)_GetENDPOINT(bEpNum) & EPRX_STAT) + +/******************************************************************************* +* Macro Name : SetEPTxValid / SetEPRxValid +* Description : sets directly the VALID tx/rx-status into the enpoint register +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPTxValid(bEpNum) (_SetEPTxStatus(bEpNum, EP_TX_VALID)) + +#define _SetEPRxValid(bEpNum) (_SetEPRxStatus(bEpNum, EP_RX_VALID)) + +/******************************************************************************* +* Macro Name : GetTxStallStatus / GetRxStallStatus. +* Description : checks stall condition in an endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : TRUE = endpoint in stall condition. +*******************************************************************************/ +#define _GetTxStallStatus(bEpNum) (_GetEPTxStatus(bEpNum) \ + == EP_TX_STALL) +#define _GetRxStallStatus(bEpNum) (_GetEPRxStatus(bEpNum) \ + == EP_RX_STALL) + +/******************************************************************************* +* Macro Name : SetEP_KIND / ClearEP_KIND. +* Description : set & clear EP_KIND bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEP_KIND(bEpNum) (_SetENDPOINT(bEpNum, \ + (EP_CTR_RX|EP_CTR_TX|((_GetENDPOINT(bEpNum) | EP_KIND) & EPREG_MASK)))) +#define _ClearEP_KIND(bEpNum) (_SetENDPOINT(bEpNum, \ + (EP_CTR_RX|EP_CTR_TX|(_GetENDPOINT(bEpNum) & EPKIND_MASK)))) + +/******************************************************************************* +* Macro Name : Set_Status_Out / Clear_Status_Out. +* Description : Sets/clears directly STATUS_OUT bit in the endpoint register. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _Set_Status_Out(bEpNum) _SetEP_KIND(bEpNum) +#define _Clear_Status_Out(bEpNum) _ClearEP_KIND(bEpNum) + +/******************************************************************************* +* Macro Name : SetEPDoubleBuff / ClearEPDoubleBuff. +* Description : Sets/clears directly EP_KIND bit in the endpoint register. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPDoubleBuff(bEpNum) _SetEP_KIND(bEpNum) +#define _ClearEPDoubleBuff(bEpNum) _ClearEP_KIND(bEpNum) + +/******************************************************************************* +* Macro Name : ClearEP_CTR_RX / ClearEP_CTR_TX. +* Description : Clears bit CTR_RX / CTR_TX in the endpoint register. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _ClearEP_CTR_RX(bEpNum) (_SetENDPOINT(bEpNum,\ + _GetENDPOINT(bEpNum) & 0x7FFF & EPREG_MASK)) +#define _ClearEP_CTR_TX(bEpNum) (_SetENDPOINT(bEpNum,\ + _GetENDPOINT(bEpNum) & 0xFF7F & EPREG_MASK)) + +/******************************************************************************* +* Macro Name : ToggleDTOG_RX / ToggleDTOG_TX . +* Description : Toggles DTOG_RX / DTOG_TX bit in the endpoint register. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _ToggleDTOG_RX(bEpNum) (_SetENDPOINT(bEpNum, \ + EP_CTR_RX|EP_CTR_TX|EP_DTOG_RX | (_GetENDPOINT(bEpNum) & EPREG_MASK))) +#define _ToggleDTOG_TX(bEpNum) (_SetENDPOINT(bEpNum, \ + EP_CTR_RX|EP_CTR_TX|EP_DTOG_TX | (_GetENDPOINT(bEpNum) & EPREG_MASK))) + +/******************************************************************************* +* Macro Name : ClearDTOG_RX / ClearDTOG_TX. +* Description : Clears DTOG_RX / DTOG_TX bit in the endpoint register. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _ClearDTOG_RX(bEpNum) if((_GetENDPOINT(bEpNum) & EP_DTOG_RX) != 0)\ + _ToggleDTOG_RX(bEpNum) +#define _ClearDTOG_TX(bEpNum) if((_GetENDPOINT(bEpNum) & EP_DTOG_TX) != 0)\ + _ToggleDTOG_TX(bEpNum) +/******************************************************************************* +* Macro Name : SetEPAddress. +* Description : Sets address in an endpoint register. +* Input : bEpNum: Endpoint Number. +* bAddr: Address. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPAddress(bEpNum,bAddr) _SetENDPOINT(bEpNum,\ + EP_CTR_RX|EP_CTR_TX|(_GetENDPOINT(bEpNum) & EPREG_MASK) | bAddr) + +/******************************************************************************* +* Macro Name : GetEPAddress. +* Description : Gets address in an endpoint register. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _GetEPAddress(bEpNum) ((uint8_t)(_GetENDPOINT(bEpNum) & EPADDR_FIELD)) + +#ifdef __USB_PMA_1x16 +#define _pEPTxAddr(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8 )*2 + PMAAddr)) +#define _pEPTxCount(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8+2)*2 + PMAAddr)) +#define _pEPRxAddr(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8+4)*2 + PMAAddr)) +#define _pEPRxCount(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8+6)*2 + PMAAddr)) +#endif /* __USB_PMA_1x16 */ +#ifdef __USB_PMA_2x16 +#define _pEPTxAddr(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8 ) + PMAAddr)) +#define _pEPTxCount(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8+2) + PMAAddr)) +#define _pEPRxAddr(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8+4) + PMAAddr)) +#define _pEPRxCount(bEpNum) ((usb_pma_t *)((_GetBTABLE()+bEpNum*8+6) + PMAAddr)) +#endif /* __USB_PMA_2x16 */ +/******************************************************************************* +* Macro Name : SetEPTxAddr / SetEPRxAddr. +* Description : sets address of the tx/rx buffer. +* Input : bEpNum: Endpoint Number. +* wAddr: address to be set (must be word aligned). +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPTxAddr(bEpNum,wAddr) (*_pEPTxAddr(bEpNum) = ((wAddr >> 1) << 1)) +#define _SetEPRxAddr(bEpNum,wAddr) (*_pEPRxAddr(bEpNum) = ((wAddr >> 1) << 1)) + +/******************************************************************************* +* Macro Name : GetEPTxAddr / GetEPRxAddr. +* Description : Gets address of the tx/rx buffer. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : address of the buffer. +*******************************************************************************/ +#define _GetEPTxAddr(bEpNum) ((uint16_t)*_pEPTxAddr(bEpNum)) +#define _GetEPRxAddr(bEpNum) ((uint16_t)*_pEPRxAddr(bEpNum)) + +/******************************************************************************* +* Macro Name : SetEPCountRxReg. +* Description : Sets counter of rx buffer with no. of blocks. +* Input : pdwReg: pointer to counter. +* wCount: Counter. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _BlocksOf32(dwReg,wCount,wNBlocks) {\ + wNBlocks = wCount >> 5;\ + if((wCount & 0x1f) == 0)\ + wNBlocks--;\ + *pdwReg = (usb_pma_t)((wNBlocks << 10) | 0x8000);\ + }/* _BlocksOf32 */ + +#define _BlocksOf2(dwReg,wCount,wNBlocks) {\ + wNBlocks = wCount >> 1;\ + if((wCount & 0x1) != 0)\ + wNBlocks++;\ + *pdwReg = (usb_pma_t)(wNBlocks << 10);\ + }/* _BlocksOf2 */ + +#define _SetEPCountRxReg(dwReg,wCount) {\ + uint16_t wNBlocks;\ + if(wCount > 62){_BlocksOf32(dwReg,wCount,wNBlocks);}\ + else {_BlocksOf2(dwReg,wCount,wNBlocks);}\ + }/* _SetEPCountRxReg */ + + + +#define _SetEPRxDblBuf0Count(bEpNum,wCount) {\ + usb_pma_t *pdwReg = _pEPTxCount(bEpNum); \ + _SetEPCountRxReg(pdwReg, wCount);\ + } +/******************************************************************************* +* Macro Name : SetEPTxCount / SetEPRxCount. +* Description : sets counter for the tx/rx buffer. +* Input : bEpNum: endpoint number. +* wCount: Counter value. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPTxCount(bEpNum,wCount) (*_pEPTxCount(bEpNum) = wCount) +#define _SetEPRxCount(bEpNum,wCount) {\ + usb_pma_t *pdwReg = _pEPRxCount(bEpNum); \ + _SetEPCountRxReg(pdwReg, wCount);\ + } +/******************************************************************************* +* Macro Name : GetEPTxCount / GetEPRxCount. +* Description : gets counter of the tx buffer. +* Input : bEpNum: endpoint number. +* Output : None. +* Return : Counter value. +*******************************************************************************/ +#define _GetEPTxCount(bEpNum)((uint16_t)(*_pEPTxCount(bEpNum)) & 0x3ff) +#define _GetEPRxCount(bEpNum)((uint16_t)(*_pEPRxCount(bEpNum)) & 0x3ff) + +/******************************************************************************* +* Macro Name : SetEPDblBuf0Addr / SetEPDblBuf1Addr. +* Description : Sets buffer 0/1 address in a double buffer endpoint. +* Input : bEpNum: endpoint number. +* : wBuf0Addr: buffer 0 address. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPDblBuf0Addr(bEpNum,wBuf0Addr) {_SetEPTxAddr(bEpNum, wBuf0Addr);} +#define _SetEPDblBuf1Addr(bEpNum,wBuf1Addr) {_SetEPRxAddr(bEpNum, wBuf1Addr);} + +/******************************************************************************* +* Macro Name : SetEPDblBuffAddr. +* Description : Sets addresses in a double buffer endpoint. +* Input : bEpNum: endpoint number. +* : wBuf0Addr: buffer 0 address. +* : wBuf1Addr = buffer 1 address. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPDblBuffAddr(bEpNum,wBuf0Addr,wBuf1Addr) { \ + _SetEPDblBuf0Addr(bEpNum, wBuf0Addr);\ + _SetEPDblBuf1Addr(bEpNum, wBuf1Addr);\ + } /* _SetEPDblBuffAddr */ + +/******************************************************************************* +* Macro Name : GetEPDblBuf0Addr / GetEPDblBuf1Addr. +* Description : Gets buffer 0/1 address of a double buffer endpoint. +* Input : bEpNum: endpoint number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _GetEPDblBuf0Addr(bEpNum) (_GetEPTxAddr(bEpNum)) +#define _GetEPDblBuf1Addr(bEpNum) (_GetEPRxAddr(bEpNum)) + +/******************************************************************************* +* Macro Name : SetEPDblBuffCount / SetEPDblBuf0Count / SetEPDblBuf1Count. +* Description : Gets buffer 0/1 address of a double buffer endpoint. +* Input : bEpNum: endpoint number. +* : bDir: endpoint dir EP_DBUF_OUT = OUT +* EP_DBUF_IN = IN +* : wCount: Counter value +* Output : None. +* Return : None. +*******************************************************************************/ +#define _SetEPDblBuf0Count(bEpNum, bDir, wCount) { \ + if(bDir == EP_DBUF_OUT)\ + /* OUT endpoint */ \ + {_SetEPRxDblBuf0Count(bEpNum,wCount);} \ + else if(bDir == EP_DBUF_IN)\ + /* IN endpoint */ \ + *_pEPTxCount(bEpNum) = (usb_pma_t)wCount; \ + } /* SetEPDblBuf0Count*/ + +#define _SetEPDblBuf1Count(bEpNum, bDir, wCount) { \ + if(bDir == EP_DBUF_OUT)\ + /* OUT endpoint */ \ + {_SetEPRxCount(bEpNum,wCount);}\ + else if(bDir == EP_DBUF_IN)\ + /* IN endpoint */\ + *_pEPRxCount(bEpNum) = (usb_pma_t)wCount; \ + } /* SetEPDblBuf1Count */ + +#define _SetEPDblBuffCount(bEpNum, bDir, wCount) {\ + _SetEPDblBuf0Count(bEpNum, bDir, wCount); \ + _SetEPDblBuf1Count(bEpNum, bDir, wCount); \ + } /* _SetEPDblBuffCount */ + +/******************************************************************************* +* Macro Name : GetEPDblBuf0Count / GetEPDblBuf1Count. +* Description : Gets buffer 0/1 rx/tx counter for double buffering. +* Input : bEpNum: endpoint number. +* Output : None. +* Return : None. +*******************************************************************************/ +#define _GetEPDblBuf0Count(bEpNum) (_GetEPTxCount(bEpNum)) +#define _GetEPDblBuf1Count(bEpNum) (_GetEPRxCount(bEpNum)) + + +/* External variables --------------------------------------------------------*/ +extern __IO uint16_t wIstr; /* ISTR register last read value */ + +/* Exported functions ------------------------------------------------------- */ +void SetCNTR(uint16_t /*wRegValue*/); +void SetISTR(uint16_t /*wRegValue*/); +void SetDADDR(uint16_t /*wRegValue*/); +void SetBTABLE(uint16_t /*wRegValue*/); +void SetBTABLE(uint16_t /*wRegValue*/); +uint16_t GetCNTR(void); +uint16_t GetISTR(void); +uint16_t GetFNR(void); +uint16_t GetDADDR(void); +uint16_t GetBTABLE(void); +void SetENDPOINT(uint8_t /*bEpNum*/, uint16_t /*wRegValue*/); +uint16_t GetENDPOINT(uint8_t /*bEpNum*/); +void SetEPType(uint8_t /*bEpNum*/, uint16_t /*wType*/); +uint16_t GetEPType(uint8_t /*bEpNum*/); +void SetEPTxStatus(uint8_t /*bEpNum*/, uint16_t /*wState*/); +void SetEPRxStatus(uint8_t /*bEpNum*/, uint16_t /*wState*/); +void SetDouBleBuffEPStall(uint8_t /*bEpNum*/, uint8_t bDir); +uint16_t GetEPTxStatus(uint8_t /*bEpNum*/); +uint16_t GetEPRxStatus(uint8_t /*bEpNum*/); +void SetEPTxValid(uint8_t /*bEpNum*/); +void SetEPRxValid(uint8_t /*bEpNum*/); +uint16_t GetTxStallStatus(uint8_t /*bEpNum*/); +uint16_t GetRxStallStatus(uint8_t /*bEpNum*/); +void SetEP_KIND(uint8_t /*bEpNum*/); +void ClearEP_KIND(uint8_t /*bEpNum*/); +void Set_Status_Out(uint8_t /*bEpNum*/); +void Clear_Status_Out(uint8_t /*bEpNum*/); +void SetEPDoubleBuff(uint8_t /*bEpNum*/); +void ClearEPDoubleBuff(uint8_t /*bEpNum*/); +void ClearEP_CTR_RX(uint8_t /*bEpNum*/); +void ClearEP_CTR_TX(uint8_t /*bEpNum*/); +void ToggleDTOG_RX(uint8_t /*bEpNum*/); +void ToggleDTOG_TX(uint8_t /*bEpNum*/); +void ClearDTOG_RX(uint8_t /*bEpNum*/); +void ClearDTOG_TX(uint8_t /*bEpNum*/); +void SetEPAddress(uint8_t /*bEpNum*/, uint8_t /*bAddr*/); +uint8_t GetEPAddress(uint8_t /*bEpNum*/); +void SetEPTxAddr(uint8_t /*bEpNum*/, uint16_t /*wAddr*/); +void SetEPRxAddr(uint8_t /*bEpNum*/, uint16_t /*wAddr*/); +uint16_t GetEPTxAddr(uint8_t /*bEpNum*/); +uint16_t GetEPRxAddr(uint8_t /*bEpNum*/); +void SetEPCountRxReg(uint32_t * /*pdwReg*/, uint16_t /*wCount*/); +void SetEPTxCount(uint8_t /*bEpNum*/, uint16_t /*wCount*/); +void SetEPRxCount(uint8_t /*bEpNum*/, uint16_t /*wCount*/); +uint16_t GetEPTxCount(uint8_t /*bEpNum*/); +uint16_t GetEPRxCount(uint8_t /*bEpNum*/); +void SetEPDblBuf0Addr(uint8_t /*bEpNum*/, uint16_t /*wBuf0Addr*/); +void SetEPDblBuf1Addr(uint8_t /*bEpNum*/, uint16_t /*wBuf1Addr*/); +void SetEPDblBuffAddr(uint8_t /*bEpNum*/, uint16_t /*wBuf0Addr*/, uint16_t /*wBuf1Addr*/); +uint16_t GetEPDblBuf0Addr(uint8_t /*bEpNum*/); +uint16_t GetEPDblBuf1Addr(uint8_t /*bEpNum*/); +void SetEPDblBuffCount(uint8_t /*bEpNum*/, uint8_t /*bDir*/, uint16_t /*wCount*/); +void SetEPDblBuf0Count(uint8_t /*bEpNum*/, uint8_t /*bDir*/, uint16_t /*wCount*/); +void SetEPDblBuf1Count(uint8_t /*bEpNum*/, uint8_t /*bDir*/, uint16_t /*wCount*/); +uint16_t GetEPDblBuf0Count(uint8_t /*bEpNum*/); +uint16_t GetEPDblBuf1Count(uint8_t /*bEpNum*/); +EP_DBUF_DIR GetEPDblBufDir(uint8_t /*bEpNum*/); +void FreeUserBuffer(uint8_t bEpNum/*bEpNum*/, uint8_t bDir); +uint16_t ToWord(uint8_t, uint8_t); +uint16_t ByteSwap(uint16_t); + +#endif /* __USB_REGS_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_sil.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_sil.h new file mode 100755 index 0000000000..5548175290 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_sil.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @file usb_sil.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Simplified Interface Layer function prototypes. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_SIL_H +#define __USB_SIL_H + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +uint32_t USB_SIL_Init(void); +uint32_t USB_SIL_Write(uint8_t bEpAddr, uint8_t* pBufferPointer, uint32_t wBufferSize); +uint32_t USB_SIL_Read(uint8_t bEpAddr, uint8_t* pBufferPointer); + +/* External variables --------------------------------------------------------*/ + +#endif /* __USB_SIL_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h new file mode 100755 index 0000000000..cdce618704 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/inc/usb_type.h @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * @file usb_type.h + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Type definitions used by the USB Library + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_TYPE_H +#define __USB_TYPE_H + +/* Includes ------------------------------------------------------------------*/ +#include "usb_conf.h" + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +#ifndef NULL +#define NULL ((void *)0) +#endif + +#if !defined(FALSE) && !defined(TRUE) +typedef enum +{ + FALSE = 0, TRUE = !FALSE +} +usb_fs_device_driver_bool; +#endif + +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ +/* External variables --------------------------------------------------------*/ + +#endif /* __USB_TYPE_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_core.c b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_core.c new file mode 100755 index 0000000000..f79a10f36b --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_core.c @@ -0,0 +1,1045 @@ +/** + ****************************************************************************** + * @file usb_core.c + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Standard protocol processing (USB v2.0) + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ +#include "usb_lib.h" +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +#define ValBit(VAR,Place) (VAR & (1 << Place)) +#define SetBit(VAR,Place) (VAR |= (1 << Place)) +#define ClrBit(VAR,Place) (VAR &= ((1 << Place) ^ 255)) +#define Send0LengthData() { _SetEPTxCount(ENDP0, 0); \ + vSetEPTxStatus(EP_TX_VALID); \ + } + +#define vSetEPRxStatus(st) (SaveRState = st) +#define vSetEPTxStatus(st) (SaveTState = st) + +#define USB_StatusIn() Send0LengthData() +#define USB_StatusOut() vSetEPRxStatus(EP_RX_VALID) + +#define StatusInfo0 StatusInfo.bw.bb1 /* Reverse bb0 & bb1 */ +#define StatusInfo1 StatusInfo.bw.bb0 + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +uint16_t_uint8_t StatusInfo; + +uint8_t Data_Mul_MaxPacketSize = FALSE; +/* Private function prototypes -----------------------------------------------*/ +static void DataStageOut(void); +static void DataStageIn(void); +static void NoData_Setup0(void); +static void Data_Setup0(void); +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************* +* Function Name : Standard_GetConfiguration. +* Description : Return the current configuration variable address. +* Input : Length - How many bytes are needed. +* Output : None. +* Return : Return 1 , if the request is invalid when "Length" is 0. +* Return "Buffer" if the "Length" is not 0. +*******************************************************************************/ +const uint8_t *Standard_GetConfiguration(uint16_t Length) +{ + if (Length == 0) + { + pInformation->Ctrl_Info.Usb_wLength = + sizeof(pInformation->Current_Configuration); + return 0; + } + pUser_Standard_Requests->User_GetConfiguration(); + return (uint8_t *)&pInformation->Current_Configuration; +} + +/******************************************************************************* +* Function Name : Standard_SetConfiguration. +* Description : This routine is called to set the configuration value +* Then each class should configure device itself. +* Input : None. +* Output : None. +* Return : Return USB_SUCCESS, if the request is performed. +* Return USB_UNSUPPORT, if the request is invalid. +*******************************************************************************/ +RESULT Standard_SetConfiguration(void) +{ + + if ((pInformation->USBwValue0 <= + Device_Table.Total_Configuration) && (pInformation->USBwValue1 == 0) + && (pInformation->USBwIndex == 0)) /*call Back usb spec 2.0*/ + { + pInformation->Current_Configuration = pInformation->USBwValue0; + pUser_Standard_Requests->User_SetConfiguration(); + return USB_SUCCESS; + } + else + { + return USB_UNSUPPORT; + } +} + +/******************************************************************************* +* Function Name : Standard_GetInterface. +* Description : Return the Alternate Setting of the current interface. +* Input : Length - How many bytes are needed. +* Output : None. +* Return : Return 0, if the request is invalid when "Length" is 0. +* Return "Buffer" if the "Length" is not 0. +*******************************************************************************/ +const uint8_t *Standard_GetInterface(uint16_t Length) +{ + if (Length == 0) + { + pInformation->Ctrl_Info.Usb_wLength = + sizeof(pInformation->Current_AlternateSetting); + return 0; + } + pUser_Standard_Requests->User_GetInterface(); + return (uint8_t *)&pInformation->Current_AlternateSetting; +} + +/******************************************************************************* +* Function Name : Standard_SetInterface. +* Description : This routine is called to set the interface. +* Then each class should configure the interface them self. +* Input : None. +* Output : None. +* Return : - Return USB_SUCCESS, if the request is performed. +* - Return USB_UNSUPPORT, if the request is invalid. +*******************************************************************************/ +RESULT Standard_SetInterface(void) +{ + RESULT Re; + /*Test if the specified Interface and Alternate Setting are supported by + the application Firmware*/ + Re = (*pProperty->Class_Get_Interface_Setting)(pInformation->USBwIndex0, pInformation->USBwValue0); + + if (pInformation->Current_Configuration != 0) + { + if ((Re != USB_SUCCESS) || (pInformation->USBwIndex1 != 0) + || (pInformation->USBwValue1 != 0)) + { + return USB_UNSUPPORT; + } + else if (Re == USB_SUCCESS) + { + pUser_Standard_Requests->User_SetInterface(); + pInformation->Current_Interface = pInformation->USBwIndex0; + pInformation->Current_AlternateSetting = pInformation->USBwValue0; + return USB_SUCCESS; + } + + } + + return USB_UNSUPPORT; +} + +/******************************************************************************* +* Function Name : Standard_GetStatus. +* Description : Copy the device request data to "StatusInfo buffer". +* Input : - Length - How many bytes are needed. +* Output : None. +* Return : Return 0, if the request is at end of data block, +* or is invalid when "Length" is 0. +*******************************************************************************/ +const uint8_t *Standard_GetStatus(uint16_t Length) +{ + if (Length == 0) + { + pInformation->Ctrl_Info.Usb_wLength = 2; + return 0; + } + + /* Reset Status Information */ + StatusInfo.w = 0; + + if (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT)) + { + /*Get Device Status */ + uint8_t Feature = pInformation->Current_Feature; + + /* Remote Wakeup enabled */ + if (ValBit(Feature, 5)) + { + SetBit(StatusInfo0, 1); + } + else + { + ClrBit(StatusInfo0, 1); + } + + /* Bus-powered */ + if (ValBit(Feature, 6)) + { + SetBit(StatusInfo0, 0); + } + else /* Self-powered */ + { + ClrBit(StatusInfo0, 0); + } + } + /*Interface Status*/ + else if (Type_Recipient == (STANDARD_REQUEST | INTERFACE_RECIPIENT)) + { + return (uint8_t *)&StatusInfo; + } + /*Get EndPoint Status*/ + else if (Type_Recipient == (STANDARD_REQUEST | ENDPOINT_RECIPIENT)) + { + uint8_t Related_Endpoint; + uint8_t wIndex0 = pInformation->USBwIndex0; + + Related_Endpoint = (wIndex0 & 0x0f); + if (ValBit(wIndex0, 7)) + { + /* IN endpoint */ + if (_GetTxStallStatus(Related_Endpoint)) + { + SetBit(StatusInfo0, 0); /* IN Endpoint stalled */ + } + } + else + { + /* OUT endpoint */ + if (_GetRxStallStatus(Related_Endpoint)) + { + SetBit(StatusInfo0, 0); /* OUT Endpoint stalled */ + } + } + + } + else + { + return NULL; + } + pUser_Standard_Requests->User_GetStatus(); + return (uint8_t *)&StatusInfo; +} + +/******************************************************************************* +* Function Name : Standard_ClearFeature. +* Description : Clear or disable a specific feature. +* Input : None. +* Output : None. +* Return : - Return USB_SUCCESS, if the request is performed. +* - Return USB_UNSUPPORT, if the request is invalid. +*******************************************************************************/ +RESULT Standard_ClearFeature(void) +{ + uint32_t Type_Rec = Type_Recipient; + uint32_t Status; + + + if (Type_Rec == (STANDARD_REQUEST | DEVICE_RECIPIENT)) + {/*Device Clear Feature*/ + ClrBit(pInformation->Current_Feature, 5); + return USB_SUCCESS; + } + else if (Type_Rec == (STANDARD_REQUEST | ENDPOINT_RECIPIENT)) + {/*EndPoint Clear Feature*/ + DEVICE* pDev; + uint32_t Related_Endpoint; + uint32_t wIndex0; + uint32_t rEP; + + if ((pInformation->USBwValue != ENDPOINT_STALL) + || (pInformation->USBwIndex1 != 0)) + { + return USB_UNSUPPORT; + } + + pDev = &Device_Table; + wIndex0 = pInformation->USBwIndex0; + rEP = wIndex0 & ~0x80; + Related_Endpoint = ENDP0 + rEP; + + if (ValBit(pInformation->USBwIndex0, 7)) + { + /*Get Status of endpoint & stall the request if the related_ENdpoint + is Disabled*/ + Status = _GetEPTxStatus(Related_Endpoint); + } + else + { + Status = _GetEPRxStatus(Related_Endpoint); + } + + if ((rEP >= pDev->Total_Endpoint) || (Status == 0) + || (pInformation->Current_Configuration == 0)) + { + return USB_UNSUPPORT; + } + + + if (wIndex0 & 0x80) + { + /* IN endpoint */ + if (_GetTxStallStatus(Related_Endpoint )) + { + ClearDTOG_TX(Related_Endpoint); + SetEPTxStatus(Related_Endpoint, EP_TX_VALID); + } + } + else + { + /* OUT endpoint */ + if (_GetRxStallStatus(Related_Endpoint)) + { + if (Related_Endpoint == ENDP0) + { + /* After clear the STALL, enable the default endpoint receiver */ + SetEPRxCount(Related_Endpoint, Device_Property.MaxPacketSize); + _SetEPRxStatus(Related_Endpoint, EP_RX_VALID); + } + else + { + ClearDTOG_RX(Related_Endpoint); + _SetEPRxStatus(Related_Endpoint, EP_RX_VALID); + } + } + } + pUser_Standard_Requests->User_ClearFeature(); + return USB_SUCCESS; + } + + return USB_UNSUPPORT; +} + +/******************************************************************************* +* Function Name : Standard_SetEndPointFeature +* Description : Set or enable a specific feature of EndPoint +* Input : None. +* Output : None. +* Return : - Return USB_SUCCESS, if the request is performed. +* - Return USB_UNSUPPORT, if the request is invalid. +*******************************************************************************/ +RESULT Standard_SetEndPointFeature(void) +{ + uint32_t wIndex0; + uint32_t Related_Endpoint; + uint32_t rEP; + uint32_t Status; + + wIndex0 = pInformation->USBwIndex0; + rEP = wIndex0 & ~0x80; + Related_Endpoint = ENDP0 + rEP; + + if (ValBit(pInformation->USBwIndex0, 7)) + { + /* get Status of endpoint & stall the request if the related_ENdpoint + is Disabled*/ + Status = _GetEPTxStatus(Related_Endpoint); + } + else + { + Status = _GetEPRxStatus(Related_Endpoint); + } + + if (Related_Endpoint >= Device_Table.Total_Endpoint + || pInformation->USBwValue != 0 || Status == 0 + || pInformation->Current_Configuration == 0) + { + return USB_UNSUPPORT; + } + else + { + if (wIndex0 & 0x80) + { + /* IN endpoint */ + _SetEPTxStatus(Related_Endpoint, EP_TX_STALL); + } + + else + { + /* OUT endpoint */ + _SetEPRxStatus(Related_Endpoint, EP_RX_STALL); + } + } + pUser_Standard_Requests->User_SetEndPointFeature(); + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : Standard_SetDeviceFeature. +* Description : Set or enable a specific feature of Device. +* Input : None. +* Output : None. +* Return : - Return USB_SUCCESS, if the request is performed. +* - Return USB_UNSUPPORT, if the request is invalid. +*******************************************************************************/ +RESULT Standard_SetDeviceFeature(void) +{ + SetBit(pInformation->Current_Feature, 5); + pUser_Standard_Requests->User_SetDeviceFeature(); + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : Standard_GetDescriptorData. +* Description : Standard_GetDescriptorData is used for descriptors transfer. +* : This routine is used for the descriptors resident in Flash +* or RAM +* pDesc can be in either Flash or RAM +* The purpose of this routine is to have a versatile way to +* response descriptors request. It allows user to generate +* certain descriptors with software or read descriptors from +* external storage part by part. +* Input : - Length - Length of the data in this transfer. +* - pDesc - A pointer points to descriptor struct. +* The structure gives the initial address of the descriptor and +* its original size. +* Output : None. +* Return : Address of a part of the descriptor pointed by the Usb_ +* wOffset The buffer pointed by this address contains at least +* Length bytes. +*******************************************************************************/ +uint8_t *Standard_GetDescriptorData(uint16_t Length, ONE_DESCRIPTOR *pDesc) +{ + uint32_t wOffset; + + wOffset = pInformation->Ctrl_Info.Usb_wOffset; + if (Length == 0) + { + pInformation->Ctrl_Info.Usb_wLength = pDesc->Descriptor_Size - wOffset; + return 0; + } + + return (uint8_t *)(pDesc->Descriptor + wOffset); +} + +/******************************************************************************* +* Function Name : DataStageOut. +* Description : Data stage of a Control Write Transfer. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void DataStageOut(void) +{ + ENDPOINT_INFO *pEPinfo = &pInformation->Ctrl_Info; + uint32_t save_rLength; + + save_rLength = pEPinfo->Usb_rLength; + + if (pEPinfo->CopyData && save_rLength) + { + uint8_t *Buffer; + uint32_t Length; + + Length = pEPinfo->PacketSize; + if (Length > save_rLength) + { + Length = save_rLength; + } + + Buffer = (*pEPinfo->CopyData)(Length); + pEPinfo->Usb_rLength -= Length; + pEPinfo->Usb_rOffset += Length; + PMAToUserBufferCopy(Buffer, GetEPRxAddr(ENDP0), Length); + + } + + if (pEPinfo->Usb_rLength != 0) + { + vSetEPRxStatus(EP_RX_VALID);/* re-enable for next data reception */ + SetEPTxCount(ENDP0, 0); + vSetEPTxStatus(EP_TX_VALID);/* Expect the host to abort the data OUT stage */ + } + /* Set the next State*/ + if (pEPinfo->Usb_rLength >= pEPinfo->PacketSize) + { + pInformation->ControlState = OUT_DATA; + } + else + { + if (pEPinfo->Usb_rLength > 0) + { + pInformation->ControlState = LAST_OUT_DATA; + } + else if (pEPinfo->Usb_rLength == 0) + { + pInformation->ControlState = WAIT_STATUS_IN; + USB_StatusIn(); + } + } +} + +/******************************************************************************* +* Function Name : DataStageIn. +* Description : Data stage of a Control Read Transfer. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void DataStageIn(void) +{ + ENDPOINT_INFO *pEPinfo = &pInformation->Ctrl_Info; + uint32_t save_wLength = pEPinfo->Usb_wLength; + uint32_t ControlState = pInformation->ControlState; + + uint8_t *DataBuffer; + uint32_t Length; + + if ((save_wLength == 0) && (ControlState == LAST_IN_DATA)) + { + if(Data_Mul_MaxPacketSize == TRUE) + { + /* No more data to send and empty packet */ + Send0LengthData(); + ControlState = LAST_IN_DATA; + Data_Mul_MaxPacketSize = FALSE; + } + else + { + /* No more data to send so STALL the TX Status*/ + ControlState = WAIT_STATUS_OUT; + vSetEPTxStatus(EP_TX_STALL); + + } + + goto Expect_Status_Out; + } + + Length = pEPinfo->PacketSize; + ControlState = (save_wLength <= Length) ? LAST_IN_DATA : IN_DATA; + + if (Length > save_wLength) + { + Length = save_wLength; + } + + DataBuffer = (*pEPinfo->CopyData)(Length); + + UserToPMABufferCopy(DataBuffer, GetEPTxAddr(ENDP0), Length); + + SetEPTxCount(ENDP0, Length); + + pEPinfo->Usb_wLength -= Length; + pEPinfo->Usb_wOffset += Length; + vSetEPTxStatus(EP_TX_VALID); + + USB_StatusOut();/* Expect the host to abort the data IN stage */ + +Expect_Status_Out: + pInformation->ControlState = ControlState; +} + +/******************************************************************************* +* Function Name : NoData_Setup0. +* Description : Proceed the processing of setup request without data stage. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void NoData_Setup0(void) +{ + RESULT Result = USB_UNSUPPORT; + uint32_t RequestNo = pInformation->USBbRequest; + uint32_t ControlState; + + if (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT)) + { + /* Device Request*/ + /* SET_CONFIGURATION*/ + if (RequestNo == SET_CONFIGURATION) + { + Result = Standard_SetConfiguration(); + } + + /*SET ADDRESS*/ + else if (RequestNo == SET_ADDRESS) + { + if ((pInformation->USBwValue0 > 127) || (pInformation->USBwValue1 != 0) + || (pInformation->USBwIndex != 0) + || (pInformation->Current_Configuration != 0)) + /* Device Address should be 127 or less*/ + { + ControlState = STALLED; + goto exit_NoData_Setup0; + } + else + { + Result = USB_SUCCESS; + } + } + /*SET FEATURE for Device*/ + else if (RequestNo == SET_FEATURE) + { + if ((pInformation->USBwValue0 == DEVICE_REMOTE_WAKEUP) \ + && (pInformation->USBwIndex == 0)) + { + Result = Standard_SetDeviceFeature(); + } + else + { + Result = USB_UNSUPPORT; + } + } + /*Clear FEATURE for Device */ + else if (RequestNo == CLEAR_FEATURE) + { + if (pInformation->USBwValue0 == DEVICE_REMOTE_WAKEUP + && pInformation->USBwIndex == 0 + && ValBit(pInformation->Current_Feature, 5)) + { + Result = Standard_ClearFeature(); + } + else + { + Result = USB_UNSUPPORT; + } + } + + } + + /* Interface Request*/ + else if (Type_Recipient == (STANDARD_REQUEST | INTERFACE_RECIPIENT)) + { + /*SET INTERFACE*/ + if (RequestNo == SET_INTERFACE) + { + Result = Standard_SetInterface(); + } + } + + /* EndPoint Request*/ + else if (Type_Recipient == (STANDARD_REQUEST | ENDPOINT_RECIPIENT)) + { + /*CLEAR FEATURE for EndPoint*/ + if (RequestNo == CLEAR_FEATURE) + { + Result = Standard_ClearFeature(); + } + /* SET FEATURE for EndPoint*/ + else if (RequestNo == SET_FEATURE) + { + Result = Standard_SetEndPointFeature(); + } + } + else + { + Result = USB_UNSUPPORT; + } + + + if (Result != USB_SUCCESS) + { + Result = (*pProperty->Class_NoData_Setup)(RequestNo); + if (Result == USB_NOT_READY) + { + ControlState = PAUSE; + goto exit_NoData_Setup0; + } + } + + if (Result != USB_SUCCESS) + { + ControlState = STALLED; + goto exit_NoData_Setup0; + } + + ControlState = WAIT_STATUS_IN;/* After no data stage SETUP */ + + USB_StatusIn(); + +exit_NoData_Setup0: + pInformation->ControlState = ControlState; + return; +} + +/******************************************************************************* +* Function Name : Data_Setup0. +* Description : Proceed the processing of setup request with data stage. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void Data_Setup0(void) +{ + const uint8_t *(*CopyRoutine)(uint16_t); + RESULT Result; + uint32_t Request_No = pInformation->USBbRequest; + + uint32_t Related_Endpoint, Reserved; + uint32_t wOffset, Status; + + + + CopyRoutine = NULL; + wOffset = 0; + + /*GET DESCRIPTOR*/ + if (Request_No == GET_DESCRIPTOR) + { + if (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT)) + { + uint8_t wValue1 = pInformation->USBwValue1; + if (wValue1 == DEVICE_DESCRIPTOR) + { + CopyRoutine = pProperty->GetDeviceDescriptor; + } + else if (wValue1 == CONFIG_DESCRIPTOR) + { + CopyRoutine = pProperty->GetConfigDescriptor; + } + else if (wValue1 == STRING_DESCRIPTOR) + { + CopyRoutine = pProperty->GetStringDescriptor; + } /* End of GET_DESCRIPTOR */ + } + } + + /*GET STATUS*/ + else if ((Request_No == GET_STATUS) && (pInformation->USBwValue == 0) + && (pInformation->USBwLength == 0x0002) + && (pInformation->USBwIndex1 == 0)) + { + /* GET STATUS for Device*/ + if ((Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT)) + && (pInformation->USBwIndex == 0)) + { + CopyRoutine = Standard_GetStatus; + } + + /* GET STATUS for Interface*/ + else if (Type_Recipient == (STANDARD_REQUEST | INTERFACE_RECIPIENT)) + { + if (((*pProperty->Class_Get_Interface_Setting)(pInformation->USBwIndex0, 0) == USB_SUCCESS) + && (pInformation->Current_Configuration != 0)) + { + CopyRoutine = Standard_GetStatus; + } + } + + /* GET STATUS for EndPoint*/ + else if (Type_Recipient == (STANDARD_REQUEST | ENDPOINT_RECIPIENT)) + { + Related_Endpoint = (pInformation->USBwIndex0 & 0x0f); + Reserved = pInformation->USBwIndex0 & 0x70; + + if (ValBit(pInformation->USBwIndex0, 7)) + { + /*Get Status of endpoint & stall the request if the related_ENdpoint + is Disabled*/ + Status = _GetEPTxStatus(Related_Endpoint); + } + else + { + Status = _GetEPRxStatus(Related_Endpoint); + } + + if ((Related_Endpoint < Device_Table.Total_Endpoint) && (Reserved == 0) + && (Status != 0)) + { + CopyRoutine = Standard_GetStatus; + } + } + + } + + /*GET CONFIGURATION*/ + else if (Request_No == GET_CONFIGURATION) + { + if (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT)) + { + CopyRoutine = Standard_GetConfiguration; + } + } + /*GET INTERFACE*/ + else if (Request_No == GET_INTERFACE) + { + if ((Type_Recipient == (STANDARD_REQUEST | INTERFACE_RECIPIENT)) + && (pInformation->Current_Configuration != 0) && (pInformation->USBwValue == 0) + && (pInformation->USBwIndex1 == 0) && (pInformation->USBwLength == 0x0001) + && ((*pProperty->Class_Get_Interface_Setting)(pInformation->USBwIndex0, 0) == USB_SUCCESS)) + { + CopyRoutine = Standard_GetInterface; + } + + } + + if (CopyRoutine) + { + pInformation->Ctrl_Info.Usb_wOffset = wOffset; + pInformation->Ctrl_Info.CopyData = (typeof(pInformation->Ctrl_Info.CopyData))CopyRoutine; + /* sb in the original the cast to word was directly */ + /* now the cast is made step by step */ + (*CopyRoutine)(0); + Result = USB_SUCCESS; + } + else + { + Result = (*pProperty->Class_Data_Setup)(pInformation->USBbRequest); + if (Result == USB_NOT_READY) + { + pInformation->ControlState = PAUSE; + return; + } + } + + if (pInformation->Ctrl_Info.Usb_wLength == 0xFFFF) + { + /* Data is not ready, wait it */ + pInformation->ControlState = PAUSE; + return; + } + if ((Result == USB_UNSUPPORT) || (pInformation->Ctrl_Info.Usb_wLength == 0)) + { + /* Unsupported request */ + pInformation->ControlState = STALLED; + return; + } + + + if (ValBit(pInformation->USBbmRequestType, 7)) + { + /* Device ==> Host */ + __IO uint32_t wLength = pInformation->USBwLength; + + /* Restrict the data length to be the one host asks for */ + if (pInformation->Ctrl_Info.Usb_wLength > wLength) + { + pInformation->Ctrl_Info.Usb_wLength = wLength; + } + + else if (pInformation->Ctrl_Info.Usb_wLength < pInformation->USBwLength) + { + if (pInformation->Ctrl_Info.Usb_wLength < pProperty->MaxPacketSize) + { + Data_Mul_MaxPacketSize = FALSE; + } + else if ((pInformation->Ctrl_Info.Usb_wLength % pProperty->MaxPacketSize) == 0) + { + Data_Mul_MaxPacketSize = TRUE; + } + } + + pInformation->Ctrl_Info.PacketSize = pProperty->MaxPacketSize; + DataStageIn(); + } + else + { + pInformation->ControlState = OUT_DATA; + vSetEPRxStatus(EP_RX_VALID); /* enable for next data reception */ + } + + return; +} + +/******************************************************************************* +* Function Name : Setup0_Process +* Description : Get the device request data and dispatch to individual process. +* Input : None. +* Output : None. +* Return : Post0_Process. +*******************************************************************************/ +uint8_t __attribute__((optimize("O0"))) Setup0_Process(void) +{ + + union + { + uint8_t* b; + uint16_t* w; + } pBuf; + +#ifdef STM32F10X_CL + USB_OTG_EP *ep; + uint16_t offset = 0; + + ep = PCD_GetOutEP(ENDP0); + pBuf.b = ep->xfer_buff; +#elif defined(STM32F303xD) || defined(STM32F303xE) + uint16_t offset = 0; + pBuf.b = (uint8_t *)(PMAAddr + _GetEPRxAddr(ENDP0)); +#else + uint16_t offset = 1; + + pBuf.b = PMAAddr + (uint8_t *)(_GetEPRxAddr(ENDP0) * 2); /* *2 for 32 bits addr */ +#endif /* STM32F10X_CL */ + + if (pInformation->ControlState != PAUSE) + { + pInformation->USBbmRequestType = *pBuf.b++; /* bmRequestType */ + pInformation->USBbRequest = *pBuf.b++; /* bRequest */ + pBuf.w += offset; /* word not accessed because of 32 bits addressing */ + pInformation->USBwValue = ByteSwap(*pBuf.w++); /* wValue */ + pBuf.w += offset; /* word not accessed because of 32 bits addressing */ + pInformation->USBwIndex = ByteSwap(*pBuf.w++); /* wIndex */ + pBuf.w += offset; /* word not accessed because of 32 bits addressing */ + pInformation->USBwLength = *pBuf.w; /* wLength */ + } + + pInformation->ControlState = SETTING_UP; + if (pInformation->USBwLength == 0) + { + /* Setup with no data stage */ + NoData_Setup0(); + } + else + { + /* Setup with data stage */ + Data_Setup0(); + } + return Post0_Process(); +} + +/******************************************************************************* +* Function Name : In0_Process +* Description : Process the IN token on all default endpoint. +* Input : None. +* Output : None. +* Return : Post0_Process. +*******************************************************************************/ +uint8_t In0_Process(void) +{ + uint32_t ControlState = pInformation->ControlState; + + if ((ControlState == IN_DATA) || (ControlState == LAST_IN_DATA)) + { + DataStageIn(); + /* ControlState may be changed outside the function */ + ControlState = pInformation->ControlState; + } + + else if (ControlState == WAIT_STATUS_IN) + { + if ((pInformation->USBbRequest == SET_ADDRESS) && + (Type_Recipient == (STANDARD_REQUEST | DEVICE_RECIPIENT))) + { + SetDeviceAddress(pInformation->USBwValue0); + pUser_Standard_Requests->User_SetDeviceAddress(); + } + (*pProperty->Process_Status_IN)(); + ControlState = STALLED; + } + + else + { + ControlState = STALLED; + } + + pInformation->ControlState = ControlState; + + return Post0_Process(); +} + +/******************************************************************************* +* Function Name : Out0_Process +* Description : Process the OUT token on all default endpoint. +* Input : None. +* Output : None. +* Return : Post0_Process. +*******************************************************************************/ +uint8_t Out0_Process(void) +{ + uint32_t ControlState = pInformation->ControlState; + + if ((ControlState == IN_DATA) || (ControlState == LAST_IN_DATA)) + { + /* host aborts the transfer before finish */ + ControlState = STALLED; + } + else if ((ControlState == OUT_DATA) || (ControlState == LAST_OUT_DATA)) + { + DataStageOut(); + ControlState = pInformation->ControlState; /* may be changed outside the function */ + } + + else if (ControlState == WAIT_STATUS_OUT) + { + (*pProperty->Process_Status_OUT)(); + ControlState = STALLED; + } + + + /* Unexpect state, STALL the endpoint */ + else + { + ControlState = STALLED; + } + + pInformation->ControlState = ControlState; + + return Post0_Process(); +} + +/******************************************************************************* +* Function Name : Post0_Process +* Description : Stall the Endpoint 0 in case of error. +* Input : None. +* Output : None. +* Return : - 0 if the control State is in PAUSE +* - 1 if not. +*******************************************************************************/ +uint8_t Post0_Process(void) +{ + + SetEPRxCount(ENDP0, Device_Property.MaxPacketSize); + + if (pInformation->ControlState == STALLED) + { + vSetEPRxStatus(EP_RX_STALL); + vSetEPTxStatus(EP_TX_STALL); + } + + return (pInformation->ControlState == PAUSE); +} + +/******************************************************************************* +* Function Name : SetDeviceAddress. +* Description : Set the device and all the used Endpoints addresses. +* Input : - Val: device address. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetDeviceAddress(uint8_t Val) +{ + uint32_t i; + uint32_t nEP = Device_Table.Total_Endpoint; + + /* set address in every used endpoint */ + for (i = 0; i < nEP; i++) + { + _SetEPAddress((uint8_t)i, (uint8_t)i); + } /* for */ + _SetDADDR(Val | DADDR_EF); /* set device address and enable function */ +} + +/******************************************************************************* +* Function Name : NOP_Process +* Description : No operation function. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void NOP_Process(void) +{ +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_init.c b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_init.c new file mode 100755 index 0000000000..0949620ad5 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_init.c @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file usb_init.c + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Initialization routines & global variables + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ +#include "usb_lib.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* The number of current endpoint, it will be used to specify an endpoint */ + uint8_t EPindex; +/* The number of current device, it is an index to the Device_Table */ +/* uint8_t Device_no; */ +/* Points to the DEVICE_INFO structure of current device */ +/* The purpose of this register is to speed up the execution */ +DEVICE_INFO *pInformation; +/* Points to the DEVICE_PROP structure of current device */ +/* The purpose of this register is to speed up the execution */ +DEVICE_PROP *pProperty; +/* Temporary save the state of Rx & Tx status. */ +/* Whenever the Rx or Tx state is changed, its value is saved */ +/* in this variable first and will be set to the EPRB or EPRA */ +/* at the end of interrupt process */ +uint16_t SaveState ; +uint16_t wInterrupt_Mask; +DEVICE_INFO Device_Info; +USER_STANDARD_REQUESTS *pUser_Standard_Requests; + +/* Extern variables ----------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************* +* Function Name : USB_Init +* Description : USB system initialization +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void USB_Init(void) +{ + pInformation = &Device_Info; + pInformation->ControlState = 2; + pProperty = &Device_Property; + pUser_Standard_Requests = &User_Standard_Requests; + /* Initialize devices one by one */ + pProperty->Init(); +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_int.c b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_int.c new file mode 100755 index 0000000000..dbaa33102d --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_int.c @@ -0,0 +1,195 @@ +/** + ****************************************************************************** + * @file usb_int.c + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Endpoint CTR (Low and High) interrupt's service routines + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usb_lib.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +__IO uint16_t SaveRState; +__IO uint16_t SaveTState; + +/* Extern variables ----------------------------------------------------------*/ +extern void (*pEpInt_IN[7])(void); /* Handles IN interrupts */ +extern void (*pEpInt_OUT[7])(void); /* Handles OUT interrupts */ + +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************* +* Function Name : CTR_LP. +* Description : Low priority Endpoint Correct Transfer interrupt's service +* routine. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void CTR_LP(void) +{ + __IO uint16_t wEPVal = 0; + /* stay in loop while pending interrupts */ + while (((wIstr = _GetISTR()) & ISTR_CTR) != 0) + { + /* extract highest priority endpoint number */ + EPindex = (uint8_t)(wIstr & ISTR_EP_ID); + if (EPindex == 0) + { + /* Decode and service control endpoint interrupt */ + /* calling related service routine */ + /* (Setup0_Process, In0_Process, Out0_Process) */ + + /* save RX & TX status */ + /* and set both to NAK */ + + SaveRState = _GetENDPOINT(ENDP0); + SaveTState = SaveRState & EPTX_STAT; + SaveRState &= EPRX_STAT; + + _SetEPRxTxStatus(ENDP0,EP_RX_NAK,EP_TX_NAK); + + /* DIR bit = origin of the interrupt */ + + if ((wIstr & ISTR_DIR) == 0) + { + /* DIR = 0 */ + + /* DIR = 0 => IN int */ + /* DIR = 0 implies that (EP_CTR_TX = 1) always */ + + _ClearEP_CTR_TX(ENDP0); + In0_Process(); + + /* before terminate set Tx & Rx status */ + + _SetEPRxTxStatus(ENDP0,SaveRState,SaveTState); + return; + } + else + { + /* DIR = 1 */ + + /* DIR = 1 & CTR_RX => SETUP or OUT int */ + /* DIR = 1 & (CTR_TX | CTR_RX) => 2 int pending */ + + wEPVal = _GetENDPOINT(ENDP0); + + if ((wEPVal &EP_SETUP) != 0) + { + _ClearEP_CTR_RX(ENDP0); /* SETUP bit kept frozen while CTR_RX = 1 */ + Setup0_Process(); + /* before terminate set Tx & Rx status */ + + _SetEPRxTxStatus(ENDP0,SaveRState,SaveTState); + return; + } + + else if ((wEPVal & EP_CTR_RX) != 0) + { + _ClearEP_CTR_RX(ENDP0); + Out0_Process(); + /* before terminate set Tx & Rx status */ + + _SetEPRxTxStatus(ENDP0,SaveRState,SaveTState); + return; + } + } + }/* if(EPindex == 0) */ + else + { + /* Decode and service non control endpoints interrupt */ + + /* process related endpoint register */ + wEPVal = _GetENDPOINT(EPindex); + if ((wEPVal & EP_CTR_RX) != 0) + { + /* clear int flag */ + _ClearEP_CTR_RX(EPindex); + + /* call OUT service function */ + (*pEpInt_OUT[EPindex-1])(); + + } /* if((wEPVal & EP_CTR_RX) */ + + if ((wEPVal & EP_CTR_TX) != 0) + { + /* clear int flag */ + _ClearEP_CTR_TX(EPindex); + + /* call IN service function */ + (*pEpInt_IN[EPindex-1])(); + } /* if((wEPVal & EP_CTR_TX) != 0) */ + + }/* if(EPindex == 0) else */ + + }/* while(...) */ +} + +/******************************************************************************* +* Function Name : CTR_HP. +* Description : High Priority Endpoint Correct Transfer interrupt's service +* routine. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void CTR_HP(void) +{ + uint32_t wEPVal = 0; + + while (((wIstr = _GetISTR()) & ISTR_CTR) != 0) + { + _SetISTR((uint16_t)CLR_CTR); /* clear CTR flag */ + /* extract highest priority endpoint number */ + EPindex = (uint8_t)(wIstr & ISTR_EP_ID); + /* process related endpoint register */ + wEPVal = _GetENDPOINT(EPindex); + if ((wEPVal & EP_CTR_RX) != 0) + { + /* clear int flag */ + _ClearEP_CTR_RX(EPindex); + + /* call OUT service function */ + (*pEpInt_OUT[EPindex-1])(); + + } /* if((wEPVal & EP_CTR_RX) */ + else if ((wEPVal & EP_CTR_TX) != 0) + { + /* clear int flag */ + _ClearEP_CTR_TX(EPindex); + + /* call IN service function */ + (*pEpInt_IN[EPindex-1])(); + + + } /* if((wEPVal & EP_CTR_TX) != 0) */ + + }/* while(...) */ +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c new file mode 100755 index 0000000000..0d00951aba --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c @@ -0,0 +1,145 @@ +/** + ****************************************************************************** + * @file usb_mem.c + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Utility functions for memory transfers to/from PMA + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usb_lib.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Extern variables ----------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +#if defined(STM32F303xC) || \ + defined(STM32F303x8) || defined(STM32F334x8) || \ + defined(STM32F301x8) || \ + defined(STM32F373xC) || defined(STM32F378xx) || \ + defined(STM32F302xC) +/******************************************************************************* +* Function Name : UserToPMABufferCopy +* Description : Copy a buffer from user memory area to packet memory area (PMA) +* Input : - pbUsrBuf: pointer to user memory area. +* - wPMABufAddr: address into PMA. +* - wNBytes: no. of bytes to be copied. +* Output : None. +* Return : None . +*******************************************************************************/ +void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) +{ + uint32_t n = (wNBytes + 1) >> 1; /* n = (wNBytes + 1) / 2 */ + uint32_t i, temp1, temp2; + uint16_t *pdwVal; + pdwVal = (uint16_t *)(wPMABufAddr * 2 + PMAAddr); + for (i = n; i != 0; i--) + { + temp1 = (uint16_t) * pbUsrBuf; + pbUsrBuf++; + temp2 = temp1 | (uint16_t) * pbUsrBuf << 8; + *pdwVal++ = temp2; + pdwVal++; + pbUsrBuf++; + } +} + +/******************************************************************************* +* Function Name : PMAToUserBufferCopy +* Description : Copy a buffer from packet memory area (PMA) to user memory area +* Input : - pbUsrBuf = pointer to user memory area. +* - wPMABufAddr = address into PMA. +* - wNBytes = no. of bytes to be copied. +* Output : None. +* Return : None. +*******************************************************************************/ +void PMAToUserBufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) +{ + uint32_t n = (wNBytes + 1) >> 1;/* /2*/ + uint32_t i; + uint32_t *pdwVal; + pdwVal = (uint32_t *)(wPMABufAddr * 2 + PMAAddr); + for (i = n; i != 0; i--) + { + *(uint16_t*)pbUsrBuf++ = *pdwVal++; + pbUsrBuf++; + } +} +#endif /* STM32F303xC || */ + /* STM32F303x8 || STM32F334x8 || */ + /* STM32F301x8 || */ + /* STM32F373xC || STM32F378xx */ +#if defined(STM32F302xE) || defined(STM32F303xD) || defined(STM32F303xE) +/******************************************************************************* +* Function Name : UserToPMABufferCopy +* Description : Copy a buffer from user memory area to packet memory area (PMA) +* Input : - pbUsrBuf: pointer to user memory area. +* - wPMABufAddr: address into PMA. +* - wNBytes: no. of bytes to be copied. +* Output : None. +* Return : None . +*******************************************************************************/ +void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) +{ + uint32_t n = ((uint32_t)((uint32_t)wNBytes + 1U)) >> 1U; + uint32_t i; + uint16_t temp1, temp2; + uint16_t *pdwVal; + pdwVal = (uint16_t *)((uint32_t)(wPMABufAddr + PMAAddr)); + for (i = n; i != 0U; i--) + { + temp1 = (uint16_t) * pbUsrBuf; + pbUsrBuf++; + temp2 = temp1 | ((uint16_t)((uint16_t) * pbUsrBuf << 8U)) ; + *pdwVal++ = temp2; + pbUsrBuf++; + } +} + +/******************************************************************************* +* Function Name : PMAToUserBufferCopy +* Description : Copy a buffer from packet memory area (PMA) to user memory area +* Input : - pbUsrBuf = pointer to user memory area. +* - wPMABufAddr = address into PMA. +* - wNBytes = no. of bytes to be copied. +* Output : None. +* Return : None. +*******************************************************************************/ +void PMAToUserBufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) +{ + uint32_t n = ((uint32_t)((uint32_t)wNBytes + 1U)) >> 1U; + uint32_t i; + uint16_t *pdwVal; + pdwVal = (uint16_t *)((uint32_t)(wPMABufAddr + PMAAddr)); + for (i = n; i != 0U; i--) + { + *(uint16_t*)((uint32_t)pbUsrBuf++) = *pdwVal++; + pbUsrBuf++; + } +} +#endif /* STM32F302xE || STM32F303xD || STM32F303xE */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c new file mode 100755 index 0000000000..b1e22fe6e3 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_regs.c @@ -0,0 +1,760 @@ +/** + ****************************************************************************** + * @file usb_regs.c + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Interface functions to USB cell registers + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "usb_lib.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Extern variables ----------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************* +* Function Name : SetCNTR. +* Description : Set the CNTR register value. +* Input : wRegValue: new register value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetCNTR(uint16_t wRegValue) +{ + _SetCNTR(wRegValue); +} + +/******************************************************************************* +* Function Name : GetCNTR. +* Description : returns the CNTR register value. +* Input : None. +* Output : None. +* Return : CNTR register Value. +*******************************************************************************/ +uint16_t GetCNTR(void) +{ + return(_GetCNTR()); +} + +/******************************************************************************* +* Function Name : SetISTR. +* Description : Set the ISTR register value. +* Input : wRegValue: new register value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetISTR(uint16_t wRegValue) +{ + _SetISTR(wRegValue); +} + +/******************************************************************************* +* Function Name : GetISTR +* Description : Returns the ISTR register value. +* Input : None. +* Output : None. +* Return : ISTR register Value +*******************************************************************************/ +uint16_t GetISTR(void) +{ + return(_GetISTR()); +} + +/******************************************************************************* +* Function Name : GetFNR +* Description : Returns the FNR register value. +* Input : None. +* Output : None. +* Return : FNR register Value +*******************************************************************************/ +uint16_t GetFNR(void) +{ + return(_GetFNR()); +} + +/******************************************************************************* +* Function Name : SetDADDR +* Description : Set the DADDR register value. +* Input : wRegValue: new register value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetDADDR(uint16_t wRegValue) +{ + _SetDADDR(wRegValue); +} + +/******************************************************************************* +* Function Name : GetDADDR +* Description : Returns the DADDR register value. +* Input : None. +* Output : None. +* Return : DADDR register Value +*******************************************************************************/ +uint16_t GetDADDR(void) +{ + return(_GetDADDR()); +} + +/******************************************************************************* +* Function Name : SetBTABLE +* Description : Set the BTABLE. +* Input : wRegValue: New register value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetBTABLE(uint16_t wRegValue) +{ + _SetBTABLE(wRegValue); +} + +/******************************************************************************* +* Function Name : GetBTABLE. +* Description : Returns the BTABLE register value. +* Input : None. +* Output : None. +* Return : BTABLE address. +*******************************************************************************/ +uint16_t GetBTABLE(void) +{ + return(_GetBTABLE()); +} + +/******************************************************************************* +* Function Name : SetENDPOINT +* Description : Set the Endpoint register value. +* Input : bEpNum: Endpoint Number. +* wRegValue. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetENDPOINT(uint8_t bEpNum, uint16_t wRegValue) +{ + _SetENDPOINT(bEpNum, wRegValue); +} + +/******************************************************************************* +* Function Name : GetENDPOINT +* Description : Return the Endpoint register value. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint register value. +*******************************************************************************/ +uint16_t GetENDPOINT(uint8_t bEpNum) +{ + return(_GetENDPOINT(bEpNum)); +} + +/******************************************************************************* +* Function Name : SetEPType +* Description : sets the type in the endpoint register. +* Input : bEpNum: Endpoint Number. +* wType: type definition. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPType(uint8_t bEpNum, uint16_t wType) +{ + _SetEPType(bEpNum, wType); +} + +/******************************************************************************* +* Function Name : GetEPType +* Description : Returns the endpoint type. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint Type +*******************************************************************************/ +uint16_t GetEPType(uint8_t bEpNum) +{ + return(_GetEPType(bEpNum)); +} + +/******************************************************************************* +* Function Name : SetEPTxStatus +* Description : Set the status of Tx endpoint. +* Input : bEpNum: Endpoint Number. +* wState: new state. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPTxStatus(uint8_t bEpNum, uint16_t wState) +{ + _SetEPTxStatus(bEpNum, wState); +} + +/******************************************************************************* +* Function Name : SetEPRxStatus +* Description : Set the status of Rx endpoint. +* Input : bEpNum: Endpoint Number. +* wState: new state. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPRxStatus(uint8_t bEpNum, uint16_t wState) +{ + _SetEPRxStatus(bEpNum, wState); +} + +/******************************************************************************* +* Function Name : SetDouBleBuffEPStall +* Description : sets the status for Double Buffer Endpoint to STALL +* Input : bEpNum: Endpoint Number. +* bDir: Endpoint direction. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetDouBleBuffEPStall(uint8_t bEpNum, uint8_t bDir) +{ + uint16_t Endpoint_DTOG_Status; + Endpoint_DTOG_Status = GetENDPOINT(bEpNum); + if (bDir == EP_DBUF_OUT) + { /* OUT double buffered endpoint */ + _SetENDPOINT(bEpNum, Endpoint_DTOG_Status & ~EPRX_DTOG1); + } + else if (bDir == EP_DBUF_IN) + { /* IN double buffered endpoint */ + _SetENDPOINT(bEpNum, Endpoint_DTOG_Status & ~EPTX_DTOG1); + } +} + +/******************************************************************************* +* Function Name : GetEPTxStatus +* Description : Returns the endpoint Tx status. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint TX Status +*******************************************************************************/ +uint16_t GetEPTxStatus(uint8_t bEpNum) +{ + return(_GetEPTxStatus(bEpNum)); +} + +/******************************************************************************* +* Function Name : GetEPRxStatus +* Description : Returns the endpoint Rx status. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint RX Status +*******************************************************************************/ +uint16_t GetEPRxStatus(uint8_t bEpNum) +{ + return(_GetEPRxStatus(bEpNum)); +} + +/******************************************************************************* +* Function Name : SetEPTxValid +* Description : Valid the endpoint Tx Status. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPTxValid(uint8_t bEpNum) +{ + _SetEPTxStatus(bEpNum, EP_TX_VALID); +} + +/******************************************************************************* +* Function Name : SetEPRxValid +* Description : Valid the endpoint Rx Status. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPRxValid(uint8_t bEpNum) +{ + _SetEPRxStatus(bEpNum, EP_RX_VALID); +} + +/******************************************************************************* +* Function Name : SetEP_KIND +* Description : Clear the EP_KIND bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEP_KIND(uint8_t bEpNum) +{ + _SetEP_KIND(bEpNum); +} + +/******************************************************************************* +* Function Name : ClearEP_KIND +* Description : set the EP_KIND bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ClearEP_KIND(uint8_t bEpNum) +{ + _ClearEP_KIND(bEpNum); +} +/******************************************************************************* +* Function Name : Clear_Status_Out +* Description : Clear the Status Out of the related Endpoint +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void Clear_Status_Out(uint8_t bEpNum) +{ + _ClearEP_KIND(bEpNum); +} +/******************************************************************************* +* Function Name : Set_Status_Out +* Description : Set the Status Out of the related Endpoint +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void Set_Status_Out(uint8_t bEpNum) +{ + _SetEP_KIND(bEpNum); +} +/******************************************************************************* +* Function Name : SetEPDoubleBuff +* Description : Enable the double buffer feature for the endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDoubleBuff(uint8_t bEpNum) +{ + _SetEP_KIND(bEpNum); +} +/******************************************************************************* +* Function Name : ClearEPDoubleBuff +* Description : Disable the double buffer feature for the endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ClearEPDoubleBuff(uint8_t bEpNum) +{ + _ClearEP_KIND(bEpNum); +} +/******************************************************************************* +* Function Name : GetTxStallStatus +* Description : Returns the Stall status of the Tx endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Tx Stall status. +*******************************************************************************/ +uint16_t GetTxStallStatus(uint8_t bEpNum) +{ + return(_GetTxStallStatus(bEpNum)); +} +/******************************************************************************* +* Function Name : GetRxStallStatus +* Description : Returns the Stall status of the Rx endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Rx Stall status. +*******************************************************************************/ +uint16_t GetRxStallStatus(uint8_t bEpNum) +{ + return(_GetRxStallStatus(bEpNum)); +} +/******************************************************************************* +* Function Name : ClearEP_CTR_RX +* Description : Clear the CTR_RX bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ClearEP_CTR_RX(uint8_t bEpNum) +{ + _ClearEP_CTR_RX(bEpNum); +} +/******************************************************************************* +* Function Name : ClearEP_CTR_TX +* Description : Clear the CTR_TX bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ClearEP_CTR_TX(uint8_t bEpNum) +{ + _ClearEP_CTR_TX(bEpNum); +} +/******************************************************************************* +* Function Name : ToggleDTOG_RX +* Description : Toggle the DTOG_RX bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ToggleDTOG_RX(uint8_t bEpNum) +{ + _ToggleDTOG_RX(bEpNum); +} +/******************************************************************************* +* Function Name : ToggleDTOG_TX +* Description : Toggle the DTOG_TX bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ToggleDTOG_TX(uint8_t bEpNum) +{ + _ToggleDTOG_TX(bEpNum); +} +/******************************************************************************* +* Function Name : ClearDTOG_RX. +* Description : Clear the DTOG_RX bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ClearDTOG_RX(uint8_t bEpNum) +{ + _ClearDTOG_RX(bEpNum); +} +/******************************************************************************* +* Function Name : ClearDTOG_TX. +* Description : Clear the DTOG_TX bit. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +void ClearDTOG_TX(uint8_t bEpNum) +{ + _ClearDTOG_TX(bEpNum); +} +/******************************************************************************* +* Function Name : SetEPAddress +* Description : Set the endpoint address. +* Input : bEpNum: Endpoint Number. +* bAddr: New endpoint address. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPAddress(uint8_t bEpNum, uint8_t bAddr) +{ + _SetEPAddress(bEpNum, bAddr); +} +/******************************************************************************* +* Function Name : GetEPAddress +* Description : Get the endpoint address. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint address. +*******************************************************************************/ +uint8_t GetEPAddress(uint8_t bEpNum) +{ + return(_GetEPAddress(bEpNum)); +} +/******************************************************************************* +* Function Name : SetEPTxAddr +* Description : Set the endpoint Tx buffer address. +* Input : bEpNum: Endpoint Number. +* wAddr: new address. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPTxAddr(uint8_t bEpNum, uint16_t wAddr) +{ + _SetEPTxAddr(bEpNum, wAddr); +} +/******************************************************************************* +* Function Name : SetEPRxAddr +* Description : Set the endpoint Rx buffer address. +* Input : bEpNum: Endpoint Number. +* wAddr: new address. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPRxAddr(uint8_t bEpNum, uint16_t wAddr) +{ + _SetEPRxAddr(bEpNum, wAddr); +} +/******************************************************************************* +* Function Name : GetEPTxAddr +* Description : Returns the endpoint Tx buffer address. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Rx buffer address. +*******************************************************************************/ +uint16_t GetEPTxAddr(uint8_t bEpNum) +{ + return(_GetEPTxAddr(bEpNum)); +} +/******************************************************************************* +* Function Name : GetEPRxAddr. +* Description : Returns the endpoint Rx buffer address. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Rx buffer address. +*******************************************************************************/ +uint16_t GetEPRxAddr(uint8_t bEpNum) +{ + return(_GetEPRxAddr(bEpNum)); +} +/******************************************************************************* +* Function Name : SetEPTxCount. +* Description : Set the Tx count. +* Input : bEpNum: Endpoint Number. +* wCount: new count value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPTxCount(uint8_t bEpNum, uint16_t wCount) +{ + _SetEPTxCount(bEpNum, wCount); +} +/******************************************************************************* +* Function Name : SetEPCountRxReg. +* Description : Set the Count Rx Register value. +* Input : *pdwReg: point to the register. +* wCount: the new register value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPCountRxReg(uint32_t *pdwReg, uint16_t wCount) +{ + _SetEPCountRxReg(dwReg, wCount); +} +/******************************************************************************* +* Function Name : SetEPRxCount +* Description : Set the Rx count. +* Input : bEpNum: Endpoint Number. +* wCount: the new count value. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPRxCount(uint8_t bEpNum, uint16_t wCount) +{ + _SetEPRxCount(bEpNum, wCount); +} +/******************************************************************************* +* Function Name : GetEPTxCount +* Description : Get the Tx count. +* Input : bEpNum: Endpoint Number. +* Output : None +* Return : Tx count value. +*******************************************************************************/ +uint16_t GetEPTxCount(uint8_t bEpNum) +{ + return(_GetEPTxCount(bEpNum)); +} +/******************************************************************************* +* Function Name : GetEPRxCount +* Description : Get the Rx count. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Rx count value. +*******************************************************************************/ +uint16_t GetEPRxCount(uint8_t bEpNum) +{ + return(_GetEPRxCount(bEpNum)); +} +/******************************************************************************* +* Function Name : SetEPDblBuffAddr +* Description : Set the addresses of the buffer 0 and 1. +* Input : bEpNum: Endpoint Number. +* wBuf0Addr: new address of buffer 0. +* wBuf1Addr: new address of buffer 1. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDblBuffAddr(uint8_t bEpNum, uint16_t wBuf0Addr, uint16_t wBuf1Addr) +{ + _SetEPDblBuffAddr(bEpNum, wBuf0Addr, wBuf1Addr); +} +/******************************************************************************* +* Function Name : SetEPDblBuf0Addr +* Description : Set the Buffer 1 address. +* Input : bEpNum: Endpoint Number +* wBuf0Addr: new address. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDblBuf0Addr(uint8_t bEpNum, uint16_t wBuf0Addr) +{ + _SetEPDblBuf0Addr(bEpNum, wBuf0Addr); +} +/******************************************************************************* +* Function Name : SetEPDblBuf1Addr +* Description : Set the Buffer 1 address. +* Input : bEpNum: Endpoint Number +* wBuf1Addr: new address. +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDblBuf1Addr(uint8_t bEpNum, uint16_t wBuf1Addr) +{ + _SetEPDblBuf1Addr(bEpNum, wBuf1Addr); +} +/******************************************************************************* +* Function Name : GetEPDblBuf0Addr +* Description : Returns the address of the Buffer 0. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : None. +*******************************************************************************/ +uint16_t GetEPDblBuf0Addr(uint8_t bEpNum) +{ + return(_GetEPDblBuf0Addr(bEpNum)); +} +/******************************************************************************* +* Function Name : GetEPDblBuf1Addr +* Description : Returns the address of the Buffer 1. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Address of the Buffer 1. +*******************************************************************************/ +uint16_t GetEPDblBuf1Addr(uint8_t bEpNum) +{ + return(_GetEPDblBuf1Addr(bEpNum)); +} +/******************************************************************************* +* Function Name : SetEPDblBuffCount +* Description : Set the number of bytes for a double Buffer +* endpoint. +* Input : bEpNum,bDir, wCount +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDblBuffCount(uint8_t bEpNum, uint8_t bDir, uint16_t wCount) +{ + _SetEPDblBuffCount(bEpNum, bDir, wCount); +} +/******************************************************************************* +* Function Name : SetEPDblBuf0Count +* Description : Set the number of bytes in the buffer 0 of a double Buffer +* endpoint. +* Input : bEpNum, bDir, wCount +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDblBuf0Count(uint8_t bEpNum, uint8_t bDir, uint16_t wCount) +{ + _SetEPDblBuf0Count(bEpNum, bDir, wCount); +} +/******************************************************************************* +* Function Name : SetEPDblBuf1Count +* Description : Set the number of bytes in the buffer 0 of a double Buffer +* endpoint. +* Input : bEpNum, bDir, wCount +* Output : None. +* Return : None. +*******************************************************************************/ +void SetEPDblBuf1Count(uint8_t bEpNum, uint8_t bDir, uint16_t wCount) +{ + _SetEPDblBuf1Count(bEpNum, bDir, wCount); +} +/******************************************************************************* +* Function Name : GetEPDblBuf0Count +* Description : Returns the number of byte received in the buffer 0 of a double +* Buffer endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint Buffer 0 count +*******************************************************************************/ +uint16_t GetEPDblBuf0Count(uint8_t bEpNum) +{ + return(_GetEPDblBuf0Count(bEpNum)); +} +/******************************************************************************* +* Function Name : GetEPDblBuf1Count +* Description : Returns the number of data received in the buffer 1 of a double +* Buffer endpoint. +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : Endpoint Buffer 1 count. +*******************************************************************************/ +uint16_t GetEPDblBuf1Count(uint8_t bEpNum) +{ + return(_GetEPDblBuf1Count(bEpNum)); +} +/******************************************************************************* +* Function Name : GetEPDblBufDir +* Description : gets direction of the double buffered endpoint +* Input : bEpNum: Endpoint Number. +* Output : None. +* Return : EP_DBUF_OUT, EP_DBUF_IN, +* EP_DBUF_ERR if the endpoint counter not yet programmed. +*******************************************************************************/ +EP_DBUF_DIR GetEPDblBufDir(uint8_t bEpNum) +{ + if ((uint16_t)(*_pEPRxCount(bEpNum) & 0xFC00) != 0) + return(EP_DBUF_OUT); + else if (((uint16_t)(*_pEPTxCount(bEpNum)) & 0x03FF) != 0) + return(EP_DBUF_IN); + else + return(EP_DBUF_ERR); +} +/******************************************************************************* +* Function Name : FreeUserBuffer +* Description : free buffer used from the application realizing it to the line + toggles bit SW_BUF in the double buffered endpoint register +* Input : bEpNum, bDir +* Output : None. +* Return : None. +*******************************************************************************/ +void FreeUserBuffer(uint8_t bEpNum, uint8_t bDir) +{ + if (bDir == EP_DBUF_OUT) + { /* OUT double buffered endpoint */ + _ToggleDTOG_TX(bEpNum); + } + else if (bDir == EP_DBUF_IN) + { /* IN double buffered endpoint */ + _ToggleDTOG_RX(bEpNum); + } +} + +/******************************************************************************* +* Function Name : ToWord +* Description : merge two byte in a word. +* Input : bh: byte high, bl: bytes low. +* Output : None. +* Return : resulted word. +*******************************************************************************/ +uint16_t ToWord(uint8_t bh, uint8_t bl) +{ + uint16_t wRet; + wRet = (uint16_t)bl | ((uint16_t)bh << 8); + return(wRet); +} +/******************************************************************************* +* Function Name : ByteSwap +* Description : Swap two byte in a word. +* Input : wSwW: word to Swap. +* Output : None. +* Return : resulted word. +*******************************************************************************/ +uint16_t ByteSwap(uint16_t wSwW) +{ + uint8_t bTemp; + uint16_t wRet; + bTemp = (uint8_t)(wSwW & 0xff); + wRet = (wSwW >> 8) | ((uint16_t)bTemp << 8); + return(wRet); +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_sil.c b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_sil.c new file mode 100755 index 0000000000..5f2e4db007 --- /dev/null +++ b/flight/pios/stm32f30x/libraries/STM32_USB-FS-Device_Driver/src/usb_sil.c @@ -0,0 +1,103 @@ +/** + ****************************************************************************** + * @file usb_sil.c + * @author MCD Application Team + * @version V4.0.0 + * @date 28-August-2012 + * @brief Simplified Interface Layer for Global Initialization and Endpoint + * Rea/Write operations. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ +#include "usb_lib.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Extern variables ----------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************* +* Function Name : USB_SIL_Init +* Description : Initialize the USB Device IP and the Endpoint 0. +* Input : None. +* Output : None. +* Return : Status. +*******************************************************************************/ +uint32_t USB_SIL_Init(void) +{ + /* USB interrupts initialization */ + /* clear pending interrupts */ + _SetISTR(0); + wInterrupt_Mask = IMR_MSK; + /* set interrupts mask */ + _SetCNTR(wInterrupt_Mask); + return 0; +} + +/******************************************************************************* +* Function Name : USB_SIL_Write +* Description : Write a buffer of data to a selected endpoint. +* Input : - bEpAddr: The address of the non control endpoint. +* - pBufferPointer: The pointer to the buffer of data to be written +* to the endpoint. +* - wBufferSize: Number of data to be written (in bytes). +* Output : None. +* Return : Status. +*******************************************************************************/ +uint32_t USB_SIL_Write(uint8_t bEpAddr, uint8_t* pBufferPointer, uint32_t wBufferSize) +{ + /* Use the memory interface function to write to the selected endpoint */ + UserToPMABufferCopy(pBufferPointer, GetEPTxAddr(bEpAddr & 0x7F), wBufferSize); + + /* Update the data length in the control register */ + SetEPTxCount((bEpAddr & 0x7F), wBufferSize); + + return 0; +} + +/******************************************************************************* +* Function Name : USB_SIL_Read +* Description : Write a buffer of data to a selected endpoint. +* Input : - bEpAddr: The address of the non control endpoint. +* - pBufferPointer: The pointer to which will be saved the +* received data buffer. +* Output : None. +* Return : Number of received data (in Bytes). +*******************************************************************************/ +uint32_t USB_SIL_Read(uint8_t bEpAddr, uint8_t* pBufferPointer) +{ + uint32_t DataLength = 0; + + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(bEpAddr & 0x7F); + + /* Use the memory interface function to write to the selected endpoint */ + PMAToUserBufferCopy(pBufferPointer, GetEPRxAddr(bEpAddr & 0x7F), DataLength); + + /* Return the number of received data */ + return DataLength; +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/library.mk b/flight/pios/stm32f30x/library.mk new file mode 100644 index 0000000000..eea30af0b1 --- /dev/null +++ b/flight/pios/stm32f30x/library.mk @@ -0,0 +1,63 @@ +# +# Rules to (help) build the F30x device support. +# + +# Directory containing this makefile +PIOS_DEVLIB := $(dir $(lastword $(MAKEFILE_LIST))) + +MCU := cortex-m4 + +USE_USB ?= YES + +LDSRC = $(PIOS_DEVLIB)link_memory.lds $(PIOS_DEVLIB)link_sections.lds + +CDEFS += -D$(CHIP) -DSTM32F3 +CDEFS += -DHSE_VALUE=$(OSCILLATOR_FREQ) +CDEFS += -DUSE_STDPERIPH_DRIVER +CDEFS += -DARM_MATH_CM4 -D__FPU_PRESENT=1 +CDEFS += -DPIOS_TARGET_PROVIDES_FAST_HEAP + +ARCHFLAGS += -mcpu=cortex-m4 -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard + +# PIOS device library source and includes +PIOS_DEVLIB_SRC = $(sort $(wildcard $(PIOS_DEVLIB)*.c)) + +ifeq ($(PIOS_OMITS_USB),YES) + SRC += $(filter-out $(wildcard $(PIOS_DEVLIB)pios_usb*.c), $(PIOS_DEVLIB_SRC)) +else + SRC += $(PIOS_DEVLIB_SRC) +endif + +EXTRAINCDIRS += $(PIOS_DEVLIB)inc + +# CMSIS for the F3 +include $(PIOSCOMMON)/libraries/CMSIS/library.mk +CMSIS_DEVICEDIR := $(PIOS_DEVLIB)libraries/CMSIS3/Device/ST/STM32F30x +EXTRAINCDIRS += $(CMSIS_DEVICEDIR)/Include + +# ST Peripheral library +PERIPHLIB = $(PIOS_DEVLIB)libraries/STM32F30x_StdPeriph_Driver +ALL_SPL_SOURCES = $(sort $(wildcard $(PERIPHLIB)/src/*.c)) +SPL_SOURCES = $(filter-out %stm32f30x_can.c, $(ALL_SPL_SOURCES)) +SRC += $(SPL_SOURCES) +EXTRAINCDIRS += $(PERIPHLIB)/inc + +ifneq ($(PIOS_OMITS_USB),YES) + # ST USB Device library + USBDEVLIB = $(PIOS_DEVLIB)/libraries/STM32_USB-FS-Device_Driver + SRC += $(sort $(wildcard $(USBDEVLIB)/src/*.c)) + EXTRAINCDIRS += $(USBDEVLIB)/inc +endif + +# +# FreeRTOS +# +# If the application has included the generic FreeRTOS support, then add in +# the device-specific pieces of the code. +# +ifneq ($(FREERTOS_DIR),) + FREERTOS_PORTDIR := $(FREERTOS_DIR) + SRC += $(sort $(wildcard $(FREERTOS_PORTDIR)/portable/GCC/ARM_CM4F/*.c)) + EXTRAINCDIRS += $(FREERTOS_PORTDIR)/portable/GCC/ARM_CM4F + include $(PIOSCOMMON)/libraries/msheap/library.mk +endif diff --git a/flight/pios/stm32f30x/link_memory.lds b/flight/pios/stm32f30x/link_memory.lds new file mode 100644 index 0000000000..de5d4df64a --- /dev/null +++ b/flight/pios/stm32f30x/link_memory.lds @@ -0,0 +1,26 @@ +#define SRAM_BANK_BASE 0x20000000 +#define CCSRAM_BANK_BASE 0x10000000 + +#ifdef STM32F303xC +# define SRAM_BANK_SIZE 0x00A000 +# define CCSRAM_BANK_SIZE 0x002000 +#endif +#ifdef STM32F303xE +# define SRAM_BANK_SIZE 0x010000 +# define CCSRAM_BANK_SIZE 0x004000 +#endif + +MEMORY +{ + BD_INFO (r) : ORIGIN = (BL_BANK_BASE + BL_BANK_SIZE) - 0x80, LENGTH = 0x000080 +#ifdef EE_BANK_BASE + EE_BANK (rx) : ORIGIN = EE_BANK_BASE, LENGTH = EE_BANK_SIZE +#endif /* EE_BANK_BASE */ +#ifdef BOOTLOADER + FLASH (rx) : ORIGIN = BL_BANK_BASE, LENGTH = BL_BANK_SIZE +#else + FLASH (rx) : ORIGIN = FW_BANK_BASE, LENGTH = FW_BANK_SIZE +#endif + SRAM (rwx) : ORIGIN = SRAM_BANK_BASE, LENGTH = SRAM_BANK_SIZE + CCSRAM (rw) : ORIGIN = CCSRAM_BANK_BASE, LENGTH = CCSRAM_BANK_SIZE +} diff --git a/flight/pios/stm32f30x/link_sections.lds b/flight/pios/stm32f30x/link_sections.lds new file mode 100644 index 0000000000..50480fe37f --- /dev/null +++ b/flight/pios/stm32f30x/link_sections.lds @@ -0,0 +1,206 @@ +/* Section Definitions */ +SECTIONS +{ + /* + * Vectors, code and constant data. + */ + .text : + { + PROVIDE (pios_isr_vector_table_base = .); + KEEP(*(.cpu_vectors)) /* CPU exception vectors */ + KEEP(*(.io_vectors)) /* I/O interrupt vectors */ + *(.text .text.* .gnu.linkonce.t.*) + *(.glue_7t) *(.glue_7) + *(.rodata .rodata* .gnu.linkonce.r.*) + KEEP(*(.keep)) + } > FLASH + + /* + * Init section for UAVObjects. + */ + .initcalluavobj.init : + { + . = ALIGN(4); + __uavobj_initcall_start = .; + KEEP(*(.initcalluavobj.init)) + . = ALIGN(4); + __uavobj_initcall_end = .; + } >FLASH + + /* + * Module init section section + */ + .initcallmodule.init : + { + . = ALIGN(4); + __module_initcall_start = .; + KEEP(*(.initcallmodule.init)) + . = ALIGN(4); + __module_initcall_end = .; + } >FLASH + + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + + /* + * C++ exception handling. + */ + .ARM.extab : + { + *(.ARM.extab* .gnu.linkonce.armextab.*) + } > FLASH + .ARM.exidx : + { + __exidx_start = .; + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + __exidx_end = .; + } > FLASH + + /* + * Markers for the end of the 'text' section and the in-flash start of + * non-constant data + */ + . = ALIGN(4); + _etext = .; + _sidata = .; + + /* + * Board info structure, normally only generated by the bootloader but can + * be read by the application. + */ + PROVIDE(pios_board_info_blob = ORIGIN(BD_INFO)); + .boardinfo : + { + . = ALIGN(4); + KEEP(*(.boardinfo)) + . = ALIGN(ORIGIN(BD_INFO)+LENGTH(BD_INFO)); + } > BD_INFO + + /* + * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow + * results in a hard fault. + */ + .istack (NOLOAD) : + { + . = ALIGN(4); + _irq_stack_end = . ; + *(.irqstack) + _irq_stack_top = . ; + } > CCSRAM + + + /* + * Non-const initialised data. + */ + .data : AT (_sidata) + { + . = ALIGN(4); + _sdata = .; + *(.data .data.*) + . = ALIGN(4); + _edata = . ; + } > SRAM + + /* + * Uninitialised data (BSS + commons). + */ + .bss (NOLOAD) : + { + _sbss = . ; + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + _ebss = . ; + PROVIDE ( _end = _ebss ) ; + } > SRAM + + /* + * The heap consumes the remainder of the SRAM. + */ + .heap (NOLOAD) : + { + . = ALIGN(4); + _sheap = . ; + + /* + * This allows us to declare an object or objects up to the minimum acceptable + * heap size and receive a linker error if the space available for the heap is + * not sufficient. + */ + *(.heap) + + /* extend the heap up to the top of SRAM */ + . = ORIGIN(SRAM) + LENGTH(SRAM) - ABSOLUTE(_sheap); + _eheap = .; + } > SRAM + + /* + * 'Fast' memory goes in the CCM SRAM + */ + .fast (NOLOAD) : + { + _sfast = . ; + *(.fast) + _efast = . ; + } > CCSRAM + + /* + * The fastheap consumes the remainder of the CCSRAM. + */ + .fastheap (NOLOAD) : + { + . = ALIGN(4); + _sfastheap = . ; + + /* + * This allows us to declare an object or objects up to the minimum acceptable + * heap size and receive a linker error if the space available for the heap is + * not sufficient. + */ + *(.fastheap) + + /* extend the heap up to the top of SRAM */ + . = ORIGIN(CCSRAM) + LENGTH(CCSRAM) - ABSOLUTE(_sfastheap); + _efastheap = .; + } > CCSRAM + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + /* DWARF debug sections. + Symbols in the DWARF debugging sections are relative to the beginning + of the section so we begin them at 0. */ + /* DWARF 1 */ + .debug 0 : { *(.debug) } + .line 0 : { *(.line) } + /* GNU DWARF 1 extensions */ + .debug_srcinfo 0 : { *(.debug_srcinfo) } + .debug_sfnames 0 : { *(.debug_sfnames) } + /* DWARF 1.1 and DWARF 2 */ + .debug_aranges 0 : { *(.debug_aranges) } + .debug_pubnames 0 : { *(.debug_pubnames) } + /* DWARF 2 */ + .debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_line 0 : { *(.debug_line) } + .debug_frame 0 : { *(.debug_frame) } + .debug_str 0 : { *(.debug_str) } + .debug_loc 0 : { *(.debug_loc) } + .debug_macinfo 0 : { *(.debug_macinfo) } + /* SGI/MIPS DWARF 2 extensions */ + .debug_weaknames 0 : { *(.debug_weaknames) } + .debug_funcnames 0 : { *(.debug_funcnames) } + .debug_typenames 0 : { *(.debug_typenames) } + .debug_varnames 0 : { *(.debug_varnames) } +} diff --git a/flight/pios/stm32f30x/pios_adc.c b/flight/pios/stm32f30x/pios_adc.c new file mode 100644 index 0000000000..66b852f9e7 --- /dev/null +++ b/flight/pios/stm32f30x/pios_adc.c @@ -0,0 +1,450 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ADC ADC Functions + * @brief STM32F30x ADC PIOS interface + * @{ + * + * @file pios_adc.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief Analog to Digital conversion routines + * @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_ADC + +#include +#include + +#if !defined(PIOS_ADC_MAX_SAMPLES) +#define PIOS_ADC_MAX_SAMPLES 0 +#endif + +#if !defined(PIOS_ADC_MAX_OVERSAMPLING) +#define PIOS_ADC_MAX_OVERSAMPLING 0 +#endif + +#if !defined(PIOS_ADC_USE_ADC2) +#define PIOS_ADC_USE_ADC2 0 +#endif + +#if !defined(PIOS_ADC_NUM_CHANNELS) +#define PIOS_ADC_NUM_CHANNELS 0 +#endif + +struct pios_adc_pin_config { + GPIO_TypeDef *port; + uint32_t pin; + uint32_t channel; + bool initialize; +}; + +static const struct pios_adc_pin_config config[] = PIOS_DMA_PIN_CONFIG; +#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0])) + +#define PIOS_ADC_DMA_BUFFER_SIZE (PIOS_ADC_MAX_SAMPLES * PIOS_ADC_NUM_PINS) + +// Private types +enum pios_adc_dev_magic { + PIOS_ADC_DEV_MAGIC = 0x58375124, +}; + +struct adc_accumulator { + uint32_t accumulator; + uint32_t count; +}; + +struct pios_adc_dev { + const struct pios_adc_cfg *cfg; + ADCCallback callback_function; +#if defined(PIOS_INCLUDE_FREERTOS) + xQueueHandle data_queue; +#endif + enum pios_adc_dev_magic magic; + volatile uint16_t raw_data_buffer[PIOS_ADC_DMA_BUFFER_SIZE] __attribute__((aligned(4))); // Double buffer that DMA just used + struct adc_accumulator accumulator[PIOS_ADC_NUM_PINS]; +}; + +struct pios_adc_dev *pios_adc_dev; + +// Private functions +void PIOS_ADC_downsample_data(); +static struct pios_adc_dev *PIOS_ADC_Allocate(); +static bool PIOS_ADC_validate(struct pios_adc_dev *); + +static void init_pins(struct pios_adc_dev *adc_dev); +static void init_dma(struct pios_adc_dev *adc_dev); +static void init_adc(struct pios_adc_dev *adc_dev); + +static void init_pins(__attribute__((unused)) struct pios_adc_dev *adc_dev) +{ + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { + if (!config[i].initialize) { + continue; + } + PIOS_ADC_PinSetup(i); + } +} + +static void init_dma(struct pios_adc_dev *adc_dev) +{ + /* Disable interrupts */ + DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, pios_adc_dev->cfg->dma.irq.flags, DISABLE); + + /* Configure DMA channel */ + DMA_DeInit(adc_dev->cfg->dma.rx.channel); + DMA_InitTypeDef DMAInit = adc_dev->cfg->dma.rx.init; + DMAInit.DMA_PeripheralBaseAddr = (uint32_t)&adc_dev->cfg->adc_dev->DR; + + DMAInit.DMA_MemoryBaseAddr = (uint32_t)&pios_adc_dev->raw_data_buffer[0]; + DMAInit.DMA_BufferSize = PIOS_ADC_DMA_BUFFER_SIZE; + DMAInit.DMA_DIR = DMA_DIR_PeripheralSRC; + DMAInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMAInit.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMAInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMAInit.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMAInit.DMA_Mode = DMA_Mode_Circular; + DMAInit.DMA_M2M = DMA_M2M_Disable; + + DMA_Init(adc_dev->cfg->dma.rx.channel, &DMAInit); /* channel is actually stream ... */ + + /* enable DMA */ + DMA_Cmd(adc_dev->cfg->dma.rx.channel, ENABLE); + + /* Trigger interrupt when for half conversions too to indicate double buffer */ + DMA_ITConfig(adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE); + DMA_ITConfig(adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE); + + /* Configure DMA interrupt */ + NVIC_InitTypeDef NVICInit = adc_dev->cfg->dma.irq.init; + NVIC_Init(&NVICInit); +} + +static void init_adc(struct pios_adc_dev *adc_dev) +{ + ADC_DeInit(adc_dev->cfg->adc_dev); + + if (adc_dev->cfg->adc_dev == ADC1 || adc_dev->cfg->adc_dev == ADC2) { + RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div32); + } else { + RCC_ADCCLKConfig(RCC_ADC34PLLCLK_Div32); + } + + ADC_VoltageRegulatorCmd(adc_dev->cfg->adc_dev, ENABLE); + PIOS_DELAY_WaituS(10); + ADC_SelectCalibrationMode(adc_dev->cfg->adc_dev, ADC_CalibrationMode_Single); + ADC_StartCalibration(adc_dev->cfg->adc_dev); + while (ADC_GetCalibrationStatus(adc_dev->cfg->adc_dev) != RESET) { + ; + } + + /* Do common ADC init */ + ADC_CommonInitTypeDef ADC_CommonInitStructure; + ADC_CommonStructInit(&ADC_CommonInitStructure); + + ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + + ADC_CommonInitStructure.ADC_Clock = ADC_Clock_AsynClkMode; + ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular; + ADC_CommonInitStructure.ADC_TwoSamplingDelay = 0; + ADC_DMAConfig(adc_dev->cfg->adc_dev, ADC_DMAMode_Circular); + ADC_CommonInit(adc_dev->cfg->adc_dev, &ADC_CommonInitStructure); + + ADC_InitTypeDef ADC_InitStructure; + ADC_StructInit(&ADC_InitStructure); + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ContinuousConvMode = ADC_ContinuousConvMode_Enable; + ADC_InitStructure.ADC_ExternalTrigConvEvent = ADC_ExternalTrigConvEvent_0; + ADC_InitStructure.ADC_ExternalTrigEventEdge = ADC_ExternalTrigEventEdge_None; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + + ADC_InitStructure.ADC_NbrOfRegChannel = ((PIOS_ADC_NUM_PINS) /* >> 1*/); + + ADC_Init(adc_dev->cfg->adc_dev, &ADC_InitStructure); + + /* Enable DMA request */ + ADC_DMACmd(adc_dev->cfg->adc_dev, ENABLE); + + /* Configure input scan */ + + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) { + ADC_RegularChannelConfig(adc_dev->cfg->adc_dev, + config[i].channel, + i + 1, + ADC_SampleTime_61Cycles5); /* XXX this is totally arbitrary... */ + } + + ADC_Cmd(adc_dev->cfg->adc_dev, ENABLE); + + while (!ADC_GetFlagStatus(adc_dev->cfg->adc_dev, ADC_FLAG_RDY)) { + ; + } + + ADC_StartConversion(adc_dev->cfg->adc_dev); +} + +static bool PIOS_ADC_validate(struct pios_adc_dev *dev) +{ + if (dev == NULL) { + return false; + } + + return dev->magic == PIOS_ADC_DEV_MAGIC; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_adc_dev *PIOS_ADC_Allocate() +{ + struct pios_adc_dev *adc_dev; + + adc_dev = (struct pios_adc_dev *)pios_malloc(sizeof(*adc_dev)); + if (!adc_dev) { + return NULL; + } + + memset(adc_dev, 0, sizeof(*adc_dev)); + + adc_dev->magic = PIOS_ADC_DEV_MAGIC; + return adc_dev; +} +#else +#error Not implemented +static struct pios_adc_dev *PIOS_ADC_Allocate() +{ + return (struct pios_adc_dev *)NULL; +} +#endif + +/** + * @brief Init the ADC. + */ +int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg) +{ + PIOS_Assert(cfg); + + pios_adc_dev = PIOS_ADC_Allocate(); + if (pios_adc_dev == NULL) { + return -1; + } + + pios_adc_dev->cfg = cfg; + pios_adc_dev->callback_function = NULL; + +#if defined(PIOS_INCLUDE_FREERTOS) + pios_adc_dev->data_queue = NULL; +#endif + + init_pins(pios_adc_dev); + init_dma(pios_adc_dev); + init_adc(pios_adc_dev); + + return 0; +} + +/** + * @brief Configure the ADC to run at a fixed oversampling + * @param[in] oversampling the amount of oversampling to run at + */ +void PIOS_ADC_Config(__attribute__((unused)) uint32_t oversampling) +{ + /* we ignore this */ +} + +/** + * Returns value of an ADC Pin + * @param[in] pin number + * @return ADC pin value averaged over the set of samples since the last reading. + * @return -1 if pin doesn't exist + * @return -2 if no data acquired since last read + */ +int32_t last_conv_value; +int32_t PIOS_ADC_PinGet(uint32_t pin) +{ + int32_t result; + + /* Check if pin exists */ + if (pin >= PIOS_ADC_NUM_PINS) { + return -1; + } + + if (pios_adc_dev->accumulator[pin].accumulator <= 0) { + return -2; + } + + /* return accumulated result and clear accumulator */ + result = pios_adc_dev->accumulator[pin].accumulator / (pios_adc_dev->accumulator[pin].count ? : 1); + pios_adc_dev->accumulator[pin].accumulator = result; + pios_adc_dev->accumulator[pin].count = 1; + + return result; +} + +float PIOS_ADC_PinGetVolt(uint32_t pin) +{ + return ((float)PIOS_ADC_PinGet(pin)) * PIOS_ADC_VOLTAGE_SCALE; +} + +/** + * @brief Set a callback function that is executed whenever + * the ADC double buffer swaps + * @note Not currently supported. + */ +void PIOS_ADC_SetCallback(ADCCallback new_function) +{ + pios_adc_dev->callback_function = new_function; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +/** + * @brief Register a queue to add data to when downsampled + * @note Not currently supported. + */ +void PIOS_ADC_SetQueue(xQueueHandle data_queue) +{ + pios_adc_dev->data_queue = data_queue; +} +#endif + +/** + * @brief Return the address of the downsampled data buffer + * @note Not currently supported. + */ +float *PIOS_ADC_GetBuffer(void) +{ + return NULL; +} + +/** + * @brief Return the address of the raw data data buffer + * @note Not currently supported. + */ +int16_t *PIOS_ADC_GetRawBuffer(void) +{ + return NULL; +} + +/** + * @brief Return the amount of over sampling + * @note Not currently supported (always returns 1) + */ +uint8_t PIOS_ADC_GetOverSampling(void) +{ + return 1; +} + +/** + * @brief Set the fir coefficients. Takes as many samples as the + * current filter order plus one (normalization) + * + * @param new_filter Array of adc_oversampling floats plus one for the + * filter coefficients + * @note Not currently supported. + */ +void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float *new_filter) +{ + // not implemented +} + +/** + * @brief accumulate the data for each of the channels. + */ +void accumulate(struct pios_adc_dev *dev, volatile uint16_t *buffer) +{ + volatile uint16_t *sp = buffer; + + /* + * Accumulate sampled values. + */ + + int count = (PIOS_ADC_MAX_SAMPLES / 2); + + while (count--) { + for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) { + dev->accumulator[i].accumulator += *sp++; + dev->accumulator[i].count++; + /* + * If the accumulator reaches half-full, rescale in order to + * make more space. + */ + if (dev->accumulator[i].accumulator >= (((uint32_t)1) << 31)) { + dev->accumulator[i].accumulator /= 2; + dev->accumulator[i].count /= 2; + } + } + } + +#if defined(PIOS_INCLUDE_FREERTOS) + // XXX should do something with this + if (pios_adc_dev->data_queue) { + static portBASE_TYPE xHigherPriorityTaskWoken; +// xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken); + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); + } + +#endif + +// if(pios_adc_dev->callback_function) +// pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer); +} + +/** + * @brief Interrupt on buffer flip. + * + * The hardware is done with the 'other' buffer, so we can pass it to the accumulator. + */ +void PIOS_ADC_DMA_Handler(void) +{ + if (!PIOS_ADC_validate(pios_adc_dev)) { + return; + } + + if (DMA_GetFlagStatus(pios_adc_dev->cfg->full_flag)) { // whole double buffer filled + DMA_ClearFlag(pios_adc_dev->cfg->full_flag); + accumulate(pios_adc_dev, &pios_adc_dev->raw_data_buffer[PIOS_ADC_DMA_BUFFER_SIZE / 2]); + } else if (DMA_GetFlagStatus(pios_adc_dev->cfg->half_flag)) { + DMA_ClearFlag(pios_adc_dev->cfg->half_flag); + accumulate(pios_adc_dev, &pios_adc_dev->raw_data_buffer[0]); + } else { + // This should not happen, probably due to transfer errors + DMA_ClearFlag(pios_adc_dev->cfg->dma.irq.flags); + } +} + +void PIOS_ADC_PinSetup(uint32_t pin) +{ + if (config[pin].port != NULL && pin < PIOS_ADC_NUM_PINS) { + /* Setup analog pin */ + GPIO_InitTypeDef GPIO_InitStructure; + + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; + GPIO_InitStructure.GPIO_Pin = config[pin].pin; + GPIO_Init(config[pin].port, &GPIO_InitStructure); + } +} +#endif /* PIOS_INCLUDE_ADC */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_bkp.c b/flight/pios/stm32f30x/pios_bkp.c new file mode 100644 index 0000000000..4475445a2c --- /dev/null +++ b/flight/pios/stm32f30x/pios_bkp.c @@ -0,0 +1,111 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BKP Backup SRAM functions + * @brief Hardware abstraction layer for backup sram + * @{ + * + * @file pios_bkp.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief IAP 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 + */ + +#include +#include +#include + +/**************************************************************************************** +* Header files +****************************************************************************************/ + +/***************************************************************************************** + * Public Definitions/Macros + ****************************************************************************************/ + +/**************************************************************************************** +* Public Functions +****************************************************************************************/ +const uint32_t pios_bkp_registers_map[] = { + RTC_BKP_DR0, + RTC_BKP_DR1, + RTC_BKP_DR2, + RTC_BKP_DR3, + RTC_BKP_DR4, + RTC_BKP_DR5, + RTC_BKP_DR6, + RTC_BKP_DR7, + RTC_BKP_DR8, + RTC_BKP_DR9, + RTC_BKP_DR10, + RTC_BKP_DR11, + RTC_BKP_DR12, + RTC_BKP_DR13, + RTC_BKP_DR14, + RTC_BKP_DR15, +}; +#define PIOS_BKP_REGISTERS_COUNT NELEMENTS(pios_bkp_registers_map) + +void PIOS_BKP_Init(void) +{ + /* Enable CRC clock */ + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); + + /* Enable PWR and BKP clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); + + /* Clear Tamper pin Event(TE) pending flag */ + RTC_ClearFlag(RTC_FLAG_TAMP1F); +} + +uint16_t PIOS_BKP_ReadRegister(uint32_t regnumber) +{ + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + return (uint16_t)RTC_ReadBackupRegister(pios_bkp_registers_map[regnumber]); + } +} + +void PIOS_BKP_WriteRegister(uint32_t regnumber, uint16_t data) +{ + if (PIOS_BKP_REGISTERS_COUNT < regnumber) { + PIOS_Assert(0); + } else { + RTC_WriteBackupRegister(pios_bkp_registers_map[regnumber], (uint32_t)data); + } +} + +void PIOS_BKP_EnableWrite(void) +{ + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(ENABLE); +} + +void PIOS_BKP_DisableWrite(void) +{ + /* Enable write access to Backup domain */ + PWR_BackupAccessCmd(DISABLE); +} + + +/**************************************************************************************** +* Public Data +****************************************************************************************/ diff --git a/flight/pios/stm32f30x/pios_bl_helper.c b/flight/pios/stm32f30x/pios_bl_helper.c new file mode 100644 index 0000000000..40bb489e05 --- /dev/null +++ b/flight/pios/stm32f30x/pios_bl_helper.c @@ -0,0 +1,127 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BOOTLOADER Functions + * @brief Bootloader Helper Functions + * + * + * @file pios_bl_helper.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright(C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @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 + +#ifdef PIOS_INCLUDE_BL_HELPER + +#include +#include +#include + +uint8_t *PIOS_BL_HELPER_FLASH_If_Read(uint32_t SectorAddress) +{ + return (uint8_t *)(SectorAddress); +} + +#if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) + +static bool erase_flash(uint32_t startAddress, uint32_t endAddress); + +uint8_t PIOS_BL_HELPER_FLASH_Ini() +{ + FLASH_Unlock(); + return 1; +} + +uint8_t PIOS_BL_HELPER_FLASH_Start() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint32_t startAddress = bdinfo->fw_base; + uint32_t endAddress = bdinfo->fw_base + bdinfo->fw_size + bdinfo->desc_size; + + bool success = erase_flash(startAddress, endAddress); + + return (success) ? 1 : 0; +} + +uint8_t PIOS_BL_HELPER_FLASH_Erase_Bootloader() +{ +/// Bootloader memory space erase + uint32_t startAddress = BL_BANK_BASE; + uint32_t endAddress = BL_BANK_BASE + BL_BANK_SIZE; + + bool success = erase_flash(startAddress, endAddress); + + return (success) ? 1 : 0; +} + +static bool erase_flash(uint32_t startAddress, uint32_t endAddress) +{ + uint32_t pageAddress = startAddress; + uint8_t fail = false; + + while ((pageAddress < endAddress) && (fail == false)) { + for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) { + if (FLASH_ErasePage(pageAddress) == FLASH_COMPLETE) { + fail = false; + break; + } else { + fail = true; + } + } + + pageAddress += 2048; + } + return !fail; +} + +#endif /* if defined(PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT) */ + +uint32_t PIOS_BL_HELPER_CRC_Memory_Calc() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + PIOS_BL_HELPER_CRC_Ini(); + CRC_ResetDR(); + CRC_CalcBlockCRC((uint32_t *)bdinfo->fw_base, (bdinfo->fw_size) >> 2); + return CRC_GetCRC(); +} + +void PIOS_BL_HELPER_FLASH_Read_Description(uint8_t *array, uint8_t size) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + uint8_t x = 0; + + if (size > bdinfo->desc_size) { + size = bdinfo->desc_size; + } + for (uint32_t i = bdinfo->fw_base + bdinfo->fw_size; i < bdinfo->fw_base + bdinfo->fw_size + size; ++i) { + array[x] = *PIOS_BL_HELPER_FLASH_If_Read(i); + ++x; + } +} + +void PIOS_BL_HELPER_CRC_Ini() +{ + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); +} + +#endif /* PIOS_INCLUDE_BL_HELPER */ diff --git a/flight/pios/stm32f30x/pios_debug.c b/flight/pios/stm32f30x/pios_debug.c new file mode 100644 index 0000000000..a7c9bf53a3 --- /dev/null +++ b/flight/pios/stm32f30x/pios_debug.c @@ -0,0 +1,178 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @defgroup PIOS_DEBUG Debugging Functions + * @brief Debugging functionality + * @{ + * + * @file pios_debug.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Debugging 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 + */ + +#include "pios.h" + +// Global variables +const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; + +#ifdef PIOS_ENABLE_DEBUG_PINS +static const struct pios_tim_channel *debug_channels; +static uint8_t debug_num_channels; +#endif /* PIOS_ENABLE_DEBUG_PINS */ + +/** + * Initialise Debug-features + */ +void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel *channels, __attribute__((unused)) uint8_t num_channels) +{ +#ifdef PIOS_ENABLE_DEBUG_PINS + PIOS_Assert(channels); + PIOS_Assert(num_channels); + + /* Store away the GPIOs we've been given */ + debug_channels = channels; + debug_num_channels = num_channels; + + /* Configure the GPIOs we've been given */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &channels[i]; + + // Initialise pins as standard output pins + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin; + + /* Initialize the GPIO */ + GPIO_Init(chan->pin.gpio, &GPIO_InitStructure); + + /* Set the pin low */ + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + } +#endif // PIOS_ENABLE_DEBUG_PINS +} + +/** + * Set debug-pin high + * \param pin 0 for S1 output + */ +void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin) +{ +#ifdef PIOS_ENABLE_DEBUG_PINS + if (!debug_channels || pin >= debug_num_channels) { + return; + } + + const struct pios_tim_channel *chan = &debug_channels[pin]; + + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET); + +#endif // PIOS_ENABLE_DEBUG_PINS +} + +/** + * Set debug-pin low + * \param pin 0 for S1 output + */ +void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin) +{ +#ifdef PIOS_ENABLE_DEBUG_PINS + if (!debug_channels || pin >= debug_num_channels) { + return; + } + + const struct pios_tim_channel *chan = &debug_channels[pin]; + + GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET); + +#endif // PIOS_ENABLE_DEBUG_PINS +} + + +void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value) +{ +#ifdef PIOS_ENABLE_DEBUG_PINS + if (!debug_channels) { + return; + } + +#pragma message("This code is not portable and should be revised") + PIOS_Assert(0); + + uint32_t bsrr_l = (((~value) & 0x0F) << (16 + 6)) | ((value & 0x0F) << 6); + uint32_t bsrr_h = (((~value) & 0xF0) << (16 + 6 - 4)) | ((value & 0xF0) << (6 - 4)); + + PIOS_IRQ_Disable(); + + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + // debug_channels[0].pin.gpio->BSRR = bsrr_l; + // debug_channels[4].pin.gpio->BSRR = bsrr_h; + + PIOS_IRQ_Enable(); +#endif // PIOS_ENABLE_DEBUG_PINS +} + +void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value) +{ +#ifdef PIOS_ENABLE_DEBUG_PINS + if (!debug_channels) { + return; + } + +#pragma message("This code is not portable and should be revised") + PIOS_Assert(0); + + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + uint32_t bsrr_l = ((~(value & 0x0F) << (16 + 6))) | ((value & 0x0F) << 6); + // debug_channels[0].pin.gpio->BSRR = bsrr_l; +#endif // PIOS_ENABLE_DEBUG_PINS +} + + +/** + * Report a serious error and halt + */ +void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg) +{ +#ifdef PIOS_INCLUDE_DEBUG_CONSOLE + register int *lr asm ("lr"); // Link-register holds the PC of the caller + DEBUG_PRINTF(0, "\r%s @0x%x\r", msg, lr); +#endif + + // Stay put + while (1) { + ; + } +} + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_delay.c b/flight/pios/stm32f30x/pios_delay.c new file mode 100644 index 0000000000..272afc3f2b --- /dev/null +++ b/flight/pios/stm32f30x/pios_delay.c @@ -0,0 +1,184 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELAY Delay Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_delay.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * Michael Smith Copyright (C) 2012 + * @brief Delay Functions + * - Provides a micro-second granular delay using the CPU + * cycle counter. + * @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 + +#ifdef PIOS_INCLUDE_DELAY + +/* cycles per microsecond */ +static uint32_t us_ticks; +static uint32_t raw_hz; + +/** + * Initialises the Timer used by PIOS_DELAY functions. + * + * \return always zero (success) + */ + +int32_t PIOS_DELAY_Init(void) +{ + RCC_ClocksTypeDef clocks; + + /* compute the number of system clocks per microsecond */ + RCC_GetClocksFreq(&clocks); + us_ticks = clocks.SYSCLK_Frequency / 1000000; + PIOS_DEBUG_Assert(us_ticks > 1); + raw_hz = clocks.SYSCLK_Frequency; + + /* turn on access to the DWT registers */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + /* enable the CPU cycle counter */ + DWT_CTRL |= CYCCNTENA; + + return 0; +} + +/** + * Waits for a specific number of uS + * + * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay + * \return < 0 on errors + */ +int32_t PIOS_DELAY_WaituS(uint32_t uS) +{ + uint32_t elapsed = 0; + uint32_t last_count = PIOS_DELAY_GetRaw(); + + for (;;) { + uint32_t current_count = PIOS_DELAY_GetRaw(); + uint32_t elapsed_uS; + + /* measure the time elapsed since the last time we checked */ + elapsed += current_count - last_count; + last_count = current_count; + + /* convert to microseconds */ + elapsed_uS = elapsed / us_ticks; + if (elapsed_uS >= uS) { + break; + } + + /* reduce the delay by the elapsed time */ + uS -= elapsed_uS; + + /* keep fractional microseconds for the next iteration */ + elapsed %= us_ticks; + } + + /* No error */ + return 0; +} + +/** + * Waits for a specific number of mS + * + * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay (1..65535 milliseconds) + * \return < 0 on errors + */ +int32_t PIOS_DELAY_WaitmS(uint32_t mS) +{ + while (mS--) { + PIOS_DELAY_WaituS(1000); + } + + /* No error */ + return 0; +} + +/** + * @brief Query the Delay timer for the current uS + * @return A microsecond value + */ +uint32_t PIOS_DELAY_GetuS() +{ + return PIOS_DELAY_GetRaw() / us_ticks; +} + +/** + * @brief Calculate time in microseconds since a previous time + * @param[in] t previous time + * @return time in us since previous time t. + */ +uint32_t PIOS_DELAY_GetuSSince(uint32_t t) +{ + return PIOS_DELAY_GetuS() - t; +} + +/** + * @brief Get the raw delay timer frequency + * @return raw delay timer frequency in Hz + */ +uint32_t PIOS_DELAY_GetRawHz() +{ + return raw_hz; +} + +/** + * @brief Compare to raw times to and convert to us + * @return A microsecond value + */ +uint32_t PIOS_DELAY_DiffuS(uint32_t raw) +{ + uint32_t diff = PIOS_DELAY_GetRaw() - raw; + + return diff / us_ticks; +} + +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) +/** + * @brief Subtract two raw times and convert to us. + * @return Interval between raw times in microseconds + */ +uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) +{ + return (later - raw) / us_ticks; +} +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ + +#endif /* PIOS_INCLUDE_DELAY */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_exti.c b/flight/pios/stm32f30x/pios_exti.c new file mode 100644 index 0000000000..8f2d785402 --- /dev/null +++ b/flight/pios/stm32f30x/pios_exti.c @@ -0,0 +1,326 @@ +/** + ****************************************************************************** + * + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EXTI External Interrupt Handlers + * @brief External interrupt handler functions + * @{ + * + * @file pios_exti.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief External Interrupt Handlers + * @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_EXTI + +#define EXTI_MAX_LINES 16 + +static pios_exti_vector_t pios_exti_vector[EXTI_MAX_LINES]; + +static uint8_t PIOS_EXTI_line_to_index(uint32_t line) +{ + switch (line) { + case EXTI_Line0: return 0; + + case EXTI_Line1: return 1; + + case EXTI_Line2: return 2; + + case EXTI_Line3: return 3; + + case EXTI_Line4: return 4; + + case EXTI_Line5: return 5; + + case EXTI_Line6: return 6; + + case EXTI_Line7: return 7; + + case EXTI_Line8: return 8; + + case EXTI_Line9: return 9; + + case EXTI_Line10: return 10; + + case EXTI_Line11: return 11; + + case EXTI_Line12: return 12; + + case EXTI_Line13: return 13; + + case EXTI_Line14: return 14; + + case EXTI_Line15: return 15; + } + + PIOS_Assert(0); + return 0xFF; +} + +uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef *gpio_port) +{ + switch ((uint32_t)gpio_port) { + case (uint32_t)GPIOA: return EXTI_PortSourceGPIOA; + + case (uint32_t)GPIOB: return EXTI_PortSourceGPIOB; + + case (uint32_t)GPIOC: return EXTI_PortSourceGPIOC; + + case (uint32_t)GPIOD: return EXTI_PortSourceGPIOD; + + case (uint32_t)GPIOE: return EXTI_PortSourceGPIOE; + + case (uint32_t)GPIOF: return EXTI_PortSourceGPIOF; + } + + PIOS_Assert(0); + return 0xFF; +} + +uint8_t PIOS_EXTI_gpio_pin_to_exti_source_pin(uint32_t gpio_pin) +{ + switch ((uint32_t)gpio_pin) { + case GPIO_Pin_0: return GPIO_PinSource0; + + case GPIO_Pin_1: return GPIO_PinSource1; + + case GPIO_Pin_2: return GPIO_PinSource2; + + case GPIO_Pin_3: return GPIO_PinSource3; + + case GPIO_Pin_4: return GPIO_PinSource4; + + case GPIO_Pin_5: return GPIO_PinSource5; + + case GPIO_Pin_6: return GPIO_PinSource6; + + case GPIO_Pin_7: return GPIO_PinSource7; + + case GPIO_Pin_8: return GPIO_PinSource8; + + case GPIO_Pin_9: return GPIO_PinSource9; + + case GPIO_Pin_10: return GPIO_PinSource10; + + case GPIO_Pin_11: return GPIO_PinSource11; + + case GPIO_Pin_12: return GPIO_PinSource12; + + case GPIO_Pin_13: return GPIO_PinSource13; + + case GPIO_Pin_14: return GPIO_PinSource14; + + case GPIO_Pin_15: return GPIO_PinSource15; + } + + PIOS_Assert(0); + return 0xFF; +} + +int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg) +{ + PIOS_Assert(cfg); + + /* Connect this config to the requested vector */ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + + if (pios_exti_vector[line_index]) { + /* Someone else already has this mapped */ + return -1; + } + + /* Bind the vector to the exti line */ + pios_exti_vector[line_index] = cfg->vector; + + /* Initialize the GPIO pin */ + GPIO_Init(cfg->pin.gpio, (GPIO_InitTypeDef *)&cfg->pin.init); + + /* Set up the EXTI interrupt source */ + uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); + /* Following is not entirely correct! There is cfg->pin.pin_source to serve this purpose, and GPIO_Pin can also contain more than one bit set */ + uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); + SYSCFG_EXTILineConfig(exti_source_port, exti_source_pin); + EXTI_Init((EXTI_InitTypeDef *)&cfg->exti.init); + + /* Enable the interrupt channel */ + NVIC_Init((NVIC_InitTypeDef *)&cfg->irq.init); + + return 0; +} + +int32_t PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg) +{ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + + if (pios_exti_vector[line_index] == cfg->vector) { + EXTI_InitTypeDef disable = cfg->exti.init; + disable.EXTI_LineCmd = DISABLE; + + EXTI_Init(&disable); + pios_exti_vector[line_index] = 0; + + return 0; + } + + return -1; +} + +static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) +{ + if (pios_exti_vector[line_index]) { + return pios_exti_vector[line_index](); + } + + /* Unconfigured interrupt just fired! */ + return false; +} + +/* Bind Interrupt Handlers */ + +#ifdef PIOS_INCLUDE_FREERTOS +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } +#else +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + PIOS_EXTI_generic_irq_handler(line); \ + } +#endif + +static void PIOS_EXTI_0_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI0_IRQHandler(void) __attribute__((alias("PIOS_EXTI_0_irq_handler"))); + +static void PIOS_EXTI_1_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI1_IRQHandler(void) __attribute__((alias("PIOS_EXTI_1_irq_handler"))); + +static void PIOS_EXTI_2_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI2_IRQHandler(void) __attribute__((alias("PIOS_EXTI_2_irq_handler"))); + +static void PIOS_EXTI_3_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI3_IRQHandler(void) __attribute__((alias("PIOS_EXTI_3_irq_handler"))); + +static void PIOS_EXTI_4_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI4_IRQHandler(void) __attribute__((alias("PIOS_EXTI_4_irq_handler"))); + +static void PIOS_EXTI_9_5_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI9_5_IRQHandler(void) __attribute__((alias("PIOS_EXTI_9_5_irq_handler"))); + +static void PIOS_EXTI_15_10_irq_handler(void) +{ +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} +void EXTI15_10_IRQHandler(void) __attribute__((alias("PIOS_EXTI_15_10_irq_handler"))); + +#endif /* PIOS_INCLUDE_EXTI */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_flash_internal.c b/flight/pios/stm32f30x/pios_flash_internal.c new file mode 100644 index 0000000000..9c1132664a --- /dev/null +++ b/flight/pios/stm32f30x/pios_flash_internal.c @@ -0,0 +1,304 @@ +/** + ****************************************************************************** + * + * @file pios_flash_internal.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief brief goes here. + * -- + * @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_FLASH_INTERNAL + +#include "stm32f30x_flash.h" +#include "pios_flash_internal_priv.h" +#include "pios_flash.h" +#include + +struct device_flash_sector { + uint32_t start; + uint32_t size; + uint16_t st_sector; +}; + +static bool PIOS_Flash_Internal_GetSectorInfo(uint32_t address, uint8_t *sector_number, uint32_t *sector_start, uint32_t *sector_size) +{ + uint16_t sector = (address - 0x08000000) / 2048; + + if (sector <= 127) { + /* address lies within this sector */ + *sector_number = sector; + *sector_start = sector * 2048 + 0x08000000; + *sector_size = 2048; + return true; + } + + return false; +} + +enum pios_internal_flash_dev_magic { + PIOS_INTERNAL_FLASH_DEV_MAGIC = 0x33445902, +}; + +struct pios_internal_flash_dev { + enum pios_internal_flash_dev_magic magic; + +#if defined(PIOS_INCLUDE_FREERTOS) + xSemaphoreHandle transaction_lock; +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ +}; + +static bool PIOS_Flash_Internal_Validate(struct pios_internal_flash_dev *flash_dev) +{ + return flash_dev && (flash_dev->magic == PIOS_INTERNAL_FLASH_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) +{ + struct pios_internal_flash_dev *flash_dev; + + flash_dev = (struct pios_internal_flash_dev *)pios_malloc(sizeof(*flash_dev)); + if (!flash_dev) { + return NULL; + } + + flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; + + return flash_dev; +} +#else +static struct pios_internal_flash_dev pios_internal_flash_devs[PIOS_INTERNAL_FLASH_MAX_DEVS]; +static uint8_t pios_internal_flash_num_devs; +static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void) +{ + struct pios_internal_flash_dev *flash_dev; + + if (pios_internal_flash_num_devs >= PIOS_INTERNAL_FLASH_MAX_DEVS) { + return NULL; + } + + flash_dev = &pios_internal_flash_devs[pios_internal_flash_num_devs++]; + flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC; + + return flash_dev; +} + +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + +int32_t PIOS_Flash_Internal_Init(uintptr_t *flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg *cfg) +{ + struct pios_internal_flash_dev *flash_dev; + + flash_dev = PIOS_Flash_Internal_alloc(); + if (flash_dev == NULL) { + return -1; + } + +#if defined(PIOS_INCLUDE_FREERTOS) + flash_dev->transaction_lock = xSemaphoreCreateMutex(); +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + + *flash_id = (uintptr_t)flash_dev; + + return 0; +} + +/********************************** + * + * Provide a PIOS flash driver API + * + *********************************/ +#include "pios_flash.h" + +static int32_t PIOS_Flash_Internal_StartTransaction(uintptr_t flash_id) +{ + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; + + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } + +#if defined(PIOS_INCLUDE_FREERTOS) + if (xSemaphoreTake(flash_dev->transaction_lock, portMAX_DELAY) != pdTRUE) { + return -2; + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + + /* Unlock the internal flash so we can write to it */ + FLASH_Unlock(); + return 0; +} + +static int32_t PIOS_Flash_Internal_EndTransaction(uintptr_t flash_id) +{ + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; + + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } + +#if defined(PIOS_INCLUDE_FREERTOS) + if (xSemaphoreGive(flash_dev->transaction_lock) != pdTRUE) { + return -2; + } +#endif /* defined(PIOS_INCLUDE_FREERTOS) */ + + /* Lock the internal flash again so we can no longer write to it */ + FLASH_Lock(); + + return 0; +} + +static int32_t PIOS_Flash_Internal_EraseSector(uintptr_t flash_id, uint32_t addr) +{ + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; + + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } + + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } + + if (FLASH_ErasePage(sector_start) != FLASH_COMPLETE) { + return -3; + } + + return 0; +} + +static int32_t PIOS_Flash_Internal_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) +{ + PIOS_Assert(data); + + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; + + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } + + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; + uint32_t hword_data; + uint32_t offset; + + /* Ensure that the base address is in a valid sector */ + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } + + /* Ensure that the entire write occurs within the same sector */ + if ((uintptr_t)addr + len > sector_start + sector_size) { + /* Write crosses the end of the sector */ + return -3; + } + + /* Write the data */ + uint32_t temp_addr = addr; + uint16_t numberOfhWords = len / 2; + uint16_t x = 0; + FLASH_Status status; + for (x = 0; x < numberOfhWords; ++x) { + offset = 2 * x; + hword_data = (data[offset + 1] << 8) | data[offset]; + + if (hword_data != *(uint16_t *)(temp_addr + offset)) { + status = FLASH_ProgramHalfWord(temp_addr + offset, hword_data); + } else { + status = FLASH_COMPLETE; + } + PIOS_Assert(status == FLASH_COMPLETE); + } + + uint16_t mod = len % 2; + if (mod == 1) { + offset = 2 * x; + hword_data = 0xFF00 | data[offset]; + if (hword_data != *(uint16_t *)(temp_addr + offset)) { + status = FLASH_ProgramHalfWord(temp_addr + offset, hword_data); + } else { + status = FLASH_COMPLETE; + } + PIOS_Assert(status == FLASH_COMPLETE); + } + + return 0; +} + +static int32_t PIOS_Flash_Internal_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t *data, uint16_t len) +{ + PIOS_Assert(data); + + struct pios_internal_flash_dev *flash_dev = (struct pios_internal_flash_dev *)flash_id; + + if (!PIOS_Flash_Internal_Validate(flash_dev)) { + return -1; + } + + uint8_t sector_number; + uint32_t sector_start; + uint32_t sector_size; + + /* Ensure that the base address is in a valid sector */ + if (!PIOS_Flash_Internal_GetSectorInfo(addr, + §or_number, + §or_start, + §or_size)) { + /* We're asking for an invalid flash address */ + return -2; + } + + /* Ensure that the entire read occurs within the same sector */ + if ((uintptr_t)addr + len > sector_start + sector_size) { + /* Read crosses the end of the sector */ + return -3; + } + + /* Read the data into the buffer directly */ + memcpy(data, (void *)addr, len); + + return 0; +} + +/* Provide a flash driver to external drivers */ +const struct pios_flash_driver pios_internal_flash_driver = { + .start_transaction = PIOS_Flash_Internal_StartTransaction, + .end_transaction = PIOS_Flash_Internal_EndTransaction, + .erase_sector = PIOS_Flash_Internal_EraseSector, + .write_data = PIOS_Flash_Internal_WriteData, + .read_data = PIOS_Flash_Internal_ReadData, +}; + +#endif /* PIOS_INCLUDE_FLASH_INTERNAL */ diff --git a/flight/pios/stm32f30x/pios_gpio.c b/flight/pios/stm32f30x/pios_gpio.c new file mode 100644 index 0000000000..b87ed6e0a4 --- /dev/null +++ b/flight/pios/stm32f30x/pios_gpio.c @@ -0,0 +1,170 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_GPIO GPIO Functions + * @brief STM32 Hardware GPIO handling code + * @{ + * + * @file pios_gpio.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief GPIO functions, init, toggle, on & off. + * @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_GPIO + +#include + +/** + * Initialises all the GPIO's + */ +int32_t PIOS_GPIO_Init(uint32_t *gpios_dev_id, const struct pios_gpio_cfg *cfg) +{ + PIOS_Assert(cfg); + *gpios_dev_id = (uint32_t)cfg; + + for (uint8_t i = 0; i < cfg->num_gpios; i++) { + const struct pios_gpio *gpio = &(cfg->gpios[i]); + + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)gpio->pin.gpio) { + case (uint32_t)GPIOA: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); + break; + case (uint32_t)GPIOB: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); + break; + case (uint32_t)GPIOC: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE); + break; + case (uint32_t)GPIOD: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOD, ENABLE); + break; + case (uint32_t)GPIOE: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOE, ENABLE); + break; + case (uint32_t)GPIOF: + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOF, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + + if (gpio->remap) { + GPIO_PinAFConfig(gpio->pin.gpio, gpio->pin.init.GPIO_Pin, gpio->remap); + } + + GPIO_Init(gpio->pin.gpio, &((struct pios_gpio *)gpio)->pin.init); + + PIOS_GPIO_Off(*gpios_dev_id, i); + } + + return 0; +} + +/** + * Turn on GPIO + * \param[in] GPIO GPIO id + */ +void PIOS_GPIO_On(uint32_t gpios_dev_id, uint8_t gpio_id) +{ + const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id; + + PIOS_Assert(gpio_cfg); + + if (gpio_id >= gpio_cfg->num_gpios) { + /* GPIO index out of range */ + return; + } + + const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]); + + if (gpio->active_low) { + GPIO_ResetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin); + } else { + GPIO_SetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin); + } +} + +/** + * Turn off GPIO + * \param[in] GPIO GPIO id + */ +void PIOS_GPIO_Off(uint32_t gpios_dev_id, uint8_t gpio_id) +{ + const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id; + + PIOS_Assert(gpio_cfg); + + if (gpio_id >= gpio_cfg->num_gpios) { + /* GPIO index out of range */ + return; + } + + const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]); + + if (gpio->active_low) { + GPIO_SetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin); + } else { + GPIO_ResetBits(gpio->pin.gpio, gpio->pin.init.GPIO_Pin); + } +} + +/** + * Toggle GPIO on/off + * \param[in] GPIO GPIO id + */ +void PIOS_GPIO_Toggle(uint32_t gpios_dev_id, uint8_t gpio_id) +{ + const struct pios_gpio_cfg *gpio_cfg = (const struct pios_gpio_cfg *)gpios_dev_id; + + PIOS_Assert(gpio_cfg); + + if (gpio_id >= gpio_cfg->num_gpios) { + /* GPIO index out of range */ + return; + } + + const struct pios_gpio *gpio = &(gpio_cfg->gpios[gpio_id]); + + if (GPIO_ReadOutputDataBit(gpio->pin.gpio, gpio->pin.init.GPIO_Pin) == Bit_SET) { + if (gpio->active_low) { + PIOS_GPIO_On(gpios_dev_id, gpio_id); + } else { + PIOS_GPIO_Off(gpios_dev_id, gpio_id); + } + } else { + if (gpio->active_low) { + PIOS_GPIO_Off(gpios_dev_id, gpio_id); + } else { + PIOS_GPIO_On(gpios_dev_id, gpio_id); + } + } +} + +#endif /* PIOS_INCLUDE_GPIO */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_i2c.c b/flight/pios/stm32f30x/pios_i2c.c new file mode 100644 index 0000000000..f269f96bb3 --- /dev/null +++ b/flight/pios/stm32f30x/pios_i2c.c @@ -0,0 +1,915 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_I2C I2C Functions + * @brief STM32F30x Hardware dependent I2C functionality + * @{ + * + * @file pios_i2c.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * dRonin, http://dronin.org, Copyright (C) 2016 + * Tau Labs, http://taulabs.org, Copyright (C) 2012-2014 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief I2C Enable/Disable routines + * @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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_I2C) + +#include + +enum i2c_adapter_state { + I2C_STATE_FSM_FAULT = 0, /* Must be zero so undefined transitions land here */ + + I2C_STATE_BUS_ERROR, + + I2C_STATE_STOPPED, + I2C_STATE_STOPPING, + I2C_STATE_STARTING, + + I2C_STATE_R_MORE_TXN_ADDR, + I2C_STATE_R_MORE_TXN_PRE_ONE, + I2C_STATE_R_MORE_TXN_PRE_FIRST, + I2C_STATE_R_MORE_TXN_PRE_MIDDLE, + I2C_STATE_R_MORE_TXN_PRE_LAST, + I2C_STATE_R_MORE_TXN_POST_LAST, + + I2C_STATE_R_LAST_TXN_ADDR, + I2C_STATE_R_LAST_TXN_PRE_ONE, + I2C_STATE_R_LAST_TXN_PRE_FIRST, + I2C_STATE_R_LAST_TXN_PRE_MIDDLE, + I2C_STATE_R_LAST_TXN_PRE_LAST, + I2C_STATE_R_LAST_TXN_POST_LAST, + + I2C_STATE_W_MORE_TXN_ADDR, + I2C_STATE_W_MORE_TXN_MIDDLE, + I2C_STATE_W_MORE_TXN_LAST, + + I2C_STATE_W_LAST_TXN_ADDR, + I2C_STATE_W_LAST_TXN_MIDDLE, + I2C_STATE_W_LAST_TXN_LAST, + + I2C_STATE_NACK, + + I2C_STATE_NUM_STATES /* Must be last */ +}; + +enum i2c_adapter_event { + I2C_EVENT_START, + I2C_EVENT_RECEIVER_BUFFER_NOT_EMPTY, + I2C_EVENT_TRANSMIT_BUFFER_EMPTY, + I2C_EVENT_TRANSFER_COMPLETE, + I2C_EVENT_STOP, + I2C_EVENT_NACK, + I2C_EVENT_BUS_ERROR, + I2C_EVENT_STOPPED, + I2C_EVENT_AUTO, + + I2C_EVENT_NUM_EVENTS /* Must be last */ +}; + +/* dirty hack to keep compatible with enum in pios_i2c_priv.h */ +#define I2C_STATE_WRITE_BYTE I2C_STATE_W_MORE_TXN_ADDR +#define I2C_STATE_READ_BYTE I2C_STATE_W_MORE_TXN_MIDDLE +#define I2C_STATE_TRANSFER_COMPLETE I2C_STATE_W_MORE_TXN_LAST + +// #define I2C_HALT_ON_ERRORS + +#if defined(PIOS_I2C_DIAGNOSTICS) +static struct pios_i2c_fault_history i2c_adapter_fault_history; + +static volatile uint32_t i2c_evirq_history[I2C_LOG_DEPTH]; +static volatile uint8_t i2c_evirq_history_pointer; + +static volatile uint32_t i2c_erirq_history[I2C_LOG_DEPTH]; +static volatile uint8_t i2c_erirq_history_pointer; + +static volatile uint8_t i2c_state_history[I2C_LOG_DEPTH]; +static volatile uint8_t i2c_state_history_pointer; + +static volatile uint8_t i2c_state_event_history[I2C_LOG_DEPTH]; +static volatile uint8_t i2c_state_event_history_pointer; + +static uint32_t i2c_fsm_fault_count; +static uint32_t i2c_bad_event_counter; +static uint32_t i2c_error_interrupt_counter; +static uint32_t i2c_nack_counter; +static uint32_t i2c_timeout_counter; +#endif + +static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_bus_error(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_starting(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_write_byte(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_read_byte(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_transfer_complete(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void go_nack(struct pios_i2c_adapter *i2c_adapter, bool *woken); + +struct i2c_adapter_transition { + void (*entry_fn)(struct pios_i2c_adapter *i2c_adapter, bool *woken); + enum i2c_adapter_state next_state[I2C_EVENT_NUM_EVENTS]; +}; + +static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter, bool *woken); +static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event, bool *woken); +static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter); +static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter); + +#if defined(PIOS_I2C_DIAGNOSTICS) +static void i2c_adapter_log_fault(struct pios_i2c_adapter *i2c_adapter, enum pios_i2c_error_type type); +#endif + +static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { + [I2C_STATE_FSM_FAULT] = { + .entry_fn = go_fsm_fault, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPED, + }, + }, + [I2C_STATE_BUS_ERROR] = { + .entry_fn = go_bus_error, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPED, + }, + }, + + [I2C_STATE_STOPPED] = { + .entry_fn = go_stopped, + .next_state = { + [I2C_EVENT_START] = I2C_STATE_STARTING, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + + [I2C_STATE_STARTING] = { + .entry_fn = go_starting, + .next_state = { + [I2C_EVENT_TRANSMIT_BUFFER_EMPTY] = I2C_STATE_WRITE_BYTE, + [I2C_EVENT_RECEIVER_BUFFER_NOT_EMPTY] = I2C_STATE_READ_BYTE, + [I2C_EVENT_NACK] = I2C_STATE_NACK, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + + [I2C_STATE_WRITE_BYTE] = { + .entry_fn = go_write_byte, + .next_state = { + [I2C_EVENT_TRANSMIT_BUFFER_EMPTY] = I2C_STATE_WRITE_BYTE, + [I2C_EVENT_RECEIVER_BUFFER_NOT_EMPTY] = I2C_STATE_READ_BYTE, + [I2C_EVENT_TRANSFER_COMPLETE] = I2C_STATE_TRANSFER_COMPLETE, + [I2C_EVENT_STOP] = I2C_STATE_STOPPED, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + + [I2C_STATE_READ_BYTE] = { + .entry_fn = go_read_byte, + .next_state = { + [I2C_EVENT_TRANSMIT_BUFFER_EMPTY] = I2C_STATE_WRITE_BYTE, + [I2C_EVENT_RECEIVER_BUFFER_NOT_EMPTY] = I2C_STATE_READ_BYTE, + [I2C_EVENT_TRANSFER_COMPLETE] = I2C_STATE_TRANSFER_COMPLETE, + [I2C_EVENT_STOP] = I2C_STATE_STOPPED, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + + [I2C_STATE_TRANSFER_COMPLETE] = { + .entry_fn = go_transfer_complete, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STARTING, + [I2C_EVENT_BUS_ERROR] = I2C_STATE_BUS_ERROR, + }, + }, + + [I2C_STATE_NACK] = { + .entry_fn = go_nack, + .next_state = { + [I2C_EVENT_AUTO] = I2C_STATE_STOPPED, + }, + }, +}; + +static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ +#if defined(I2C_HALT_ON_ERRORS) + PIOS_DEBUG_Assert(0); +#endif + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; + + i2c_adapter_reset_bus(i2c_adapter); +} + +static void go_bus_error(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ + /* Note that this transfer has hit a bus error */ + i2c_adapter->bus_error = true; + + i2c_adapter_reset_bus(i2c_adapter); +} + +static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken) +{ + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_NACKI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_TXI, DISABLE); + + enum pios_i2c_transfer_result result = + i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR : + i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK : + PIOS_I2C_TRANSFER_OK; + + if (i2c_adapter->transfer_result) { + *(i2c_adapter->transfer_result) = result; + } + + pios_i2c_callback cb = i2c_adapter->callback; + + if (!cb) { /* No callback? Signal that we are done */ +#ifdef PIOS_INCLUDE_FREERTOS + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { +#if defined(I2C_HALT_ON_ERRORS) + PIOS_DEBUG_Assert(0); +#endif + } + if (pxHigherPriorityTaskWoken == pdTRUE) { + *woken = true; + } +#endif /* PIOS_INCLUDE_FREERTOS */ + } + +#ifdef PIOS_INCLUDE_FREERTOS + /* Unlock the bus */ + signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken); + if (xHigherPriorityTaskWoken == pdTRUE) { + *woken = true; + } +#else + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); +#endif /* PIOS_INCLUDE_FREERTOS */ + + if (cb && cb(result)) { /* User provided callback? Do it, but with bus unlocked */ + *woken = true; + } +} + +static void go_starting(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn->len <= 255); // FIXME: implement this using TCR + + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + + I2C_ClearITPendingBit(i2c_adapter->cfg->regs, I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI); + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_NACKI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_TXI, ENABLE); + + I2C_TransferHandling( + i2c_adapter->cfg->regs, + (i2c_adapter->active_txn->addr << 1), + i2c_adapter->active_txn->len, + i2c_adapter->active_txn == i2c_adapter->last_txn ? I2C_AutoEnd_Mode : I2C_SoftEnd_Mode, + i2c_adapter->active_txn->rw == PIOS_I2C_TXN_WRITE ? I2C_Generate_Start_Write : I2C_Generate_Start_Read); +} + +static void go_write_byte(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + + I2C_SendData(i2c_adapter->cfg->regs, *(i2c_adapter->active_byte)); + + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); +} + +static void go_read_byte(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ + PIOS_DEBUG_Assert(i2c_adapter->active_txn); + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); + + *(i2c_adapter->active_byte) = I2C_ReceiveData(i2c_adapter->cfg->regs); + + /* Move to the next byte */ + i2c_adapter->active_byte++; + PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte); +} + +static void go_transfer_complete(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ + /* Move to the next transaction */ + i2c_adapter->active_txn++; + PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); +} + +static void go_nack(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) +{ + i2c_adapter->nack = true; + I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_NACKI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_TXI, DISABLE); + /* It seems that we don't need this with F3 i2c peripheral */ + // I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + // I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); +} + +static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event, bool *woken) +{ +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_state_event_history[i2c_state_event_history_pointer] = event; + i2c_state_event_history_pointer = (i2c_state_event_history_pointer + 1) % I2C_LOG_DEPTH; + + i2c_state_history[i2c_state_history_pointer] = i2c_adapter->curr_state; + i2c_state_history_pointer = (i2c_state_history_pointer + 1) % I2C_LOG_DEPTH; + + if (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event] == I2C_STATE_FSM_FAULT) { + i2c_adapter_log_fault(i2c_adapter, PIOS_I2C_ERROR_FSM); + } +#endif + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + enum i2c_adapter_state prev_state = i2c_adapter->curr_state; + if (prev_state) { + ; + } + + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event]; + + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter, woken); + } + + /* Process any AUTO transitions in the FSM */ + i2c_adapter_process_auto(i2c_adapter, woken); +} + +static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter, bool *woken) +{ + enum i2c_adapter_state prev_state = i2c_adapter->curr_state; + + if (prev_state) { + ; + } + + while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) { + i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]; + + /* Call the entry function (if any) for the next state. */ + if (i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn) { + i2c_adapter_transitions[i2c_adapter->curr_state].entry_fn(i2c_adapter, woken); + } + } +} + +static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter) +{ + i2c_adapter_reset_bus(i2c_adapter); + i2c_adapter->curr_state = I2C_STATE_STOPPED; +} + +static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) +{ + uint8_t retry_count = 0; + uint8_t retry_count_clk = 0; + static const uint8_t MAX_I2C_RETRY_COUNT = 10; + + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); + + /* Make sure the bus is free by clocking it until any slaves release the bus. */ + GPIO_InitTypeDef scl_gpio_init; + scl_gpio_init = i2c_adapter->cfg->scl.init; + scl_gpio_init.GPIO_Mode = GPIO_Mode_OUT; + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->scl.gpio, &scl_gpio_init); + + GPIO_InitTypeDef sda_gpio_init; + sda_gpio_init = i2c_adapter->cfg->sda.init; + sda_gpio_init.GPIO_Mode = GPIO_Mode_OUT; + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_Init(i2c_adapter->cfg->sda.gpio, &sda_gpio_init); + + /* Check SDA line to determine if slave is asserting bus and clock out if so, this may */ + /* have to be repeated (due to further bus errors) but better than clocking 0xFF into an */ + /* ESC */ + + retry_count_clk = 0; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET && (retry_count_clk++ < MAX_I2C_RETRY_COUNT)) { + retry_count = 0; + /* Set clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET && (retry_count++ < MAX_I2C_RETRY_COUNT)) { + PIOS_DELAY_WaituS(1); + } + + PIOS_DELAY_WaituS(2); + + /* Set clock low */ + GPIO_ResetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + + /* Clock high again */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + } + + /* Generate a start then stop condition */ + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + GPIO_ResetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + PIOS_DELAY_WaituS(2); + + /* Set data and clock high and wait for any clock stretching to finish. */ + GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); + GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); + + retry_count = 0; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET && (retry_count++ < MAX_I2C_RETRY_COUNT)) { + PIOS_DELAY_WaituS(1); + } + + /* Wait for data to be high */ + retry_count = 0; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET && (retry_count++ < MAX_I2C_RETRY_COUNT)) { + PIOS_DELAY_WaituS(1); + } + + /* Bus signals are guaranteed to be high (ie. free) after this point */ + /* Initialize the GPIO pins to the peripheral function */ + if (i2c_adapter->cfg->remapSCL) { + GPIO_PinAFConfig(i2c_adapter->cfg->scl.gpio, + __builtin_ctz(i2c_adapter->cfg->scl.init.GPIO_Pin), + i2c_adapter->cfg->remapSCL); + } + if (i2c_adapter->cfg->remapSDA) { + GPIO_PinAFConfig(i2c_adapter->cfg->sda.gpio, + __builtin_ctz(i2c_adapter->cfg->sda.init.GPIO_Pin), + i2c_adapter->cfg->remapSDA); + } + GPIO_Init(i2c_adapter->cfg->scl.gpio, (GPIO_InitTypeDef *)&(i2c_adapter->cfg->scl.init)); // Struct is const, function signature not + GPIO_Init(i2c_adapter->cfg->sda.gpio, (GPIO_InitTypeDef *)&(i2c_adapter->cfg->sda.init)); + + RCC_I2CCLKConfig(i2c_adapter->cfg->regs == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); + + /* Reset the I2C block */ + I2C_DeInit(i2c_adapter->cfg->regs); + + /* Initialize the I2C block */ + I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef *)&(i2c_adapter->cfg->init)); + + /* Enable the I2C block */ + I2C_Cmd(i2c_adapter->cfg->regs, ENABLE); + + if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_BUSY)) { + I2C_SoftwareResetCmd(i2c_adapter->cfg->regs); + } +} + +/** + * Logs the last N state transitions and N IRQ events due to + * an error condition + * \param[in] i2c the adapter number to log an event for + */ +#if defined(PIOS_I2C_DIAGNOSTICS) +void i2c_adapter_log_fault(__attribute__((unused)) struct pios_i2c_adapter *i2c_adapter, enum pios_i2c_error_type type) +{ + i2c_adapter_fault_history.type = type; + for (uint8_t i = 0; i < I2C_LOG_DEPTH; i++) { + i2c_adapter_fault_history.evirq[i] = + i2c_evirq_history[(I2C_LOG_DEPTH + i2c_evirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.erirq[i] = + i2c_erirq_history[(I2C_LOG_DEPTH + i2c_erirq_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.event[i] = + i2c_state_event_history[(I2C_LOG_DEPTH + i2c_state_event_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + i2c_adapter_fault_history.state[i] = + i2c_state_history[(I2C_LOG_DEPTH + i2c_state_history_pointer - 1 - i) % I2C_LOG_DEPTH]; + } + switch (type) { + case PIOS_I2C_ERROR_EVENT: + i2c_bad_event_counter++; + break; + case PIOS_I2C_ERROR_FSM: + i2c_fsm_fault_count++; + break; + case PIOS_I2C_ERROR_INTERRUPT: + i2c_error_interrupt_counter++; + break; + } +} +#endif /* if defined(PIOS_I2C_DIAGNOSTICS) */ + +/** + * Logs the last N state transitions and N IRQ events due to + * an error condition + * \param[out] data address where to copy the pios_i2c_fault_history structure to + * \param[out] counts three uint16 that receive the bad event, fsm, and error irq + * counts + */ +void PIOS_I2C_GetDiagnostics(struct pios_i2c_fault_history *data, uint8_t *counts) +{ +#if defined(PIOS_I2C_DIAGNOSTICS) + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + counts[PIOS_I2C_BAD_EVENT_COUNTER] = i2c_bad_event_counter; + counts[PIOS_I2C_FSM_FAULT_COUNT] = i2c_fsm_fault_count; + counts[PIOS_I2C_ERROR_INTERRUPT_COUNTER] = i2c_error_interrupt_counter; + counts[PIOS_I2C_NACK_COUNTER] = i2c_nack_counter; + counts[PIOS_I2C_TIMEOUT_COUNTER] = i2c_timeout_counter; +#else + struct pios_i2c_fault_history i2c_adapter_fault_history; + i2c_adapter_fault_history.type = PIOS_I2C_ERROR_EVENT; + + memcpy(data, &i2c_adapter_fault_history, sizeof(i2c_adapter_fault_history)); + memset(counts, 0, sizeof(*counts) * PIOS_I2C_ERROR_COUNT_NUMELEM); +#endif +} + +static bool PIOS_I2C_validate(struct pios_i2c_adapter *i2c_adapter) +{ + return i2c_adapter->magic == PIOS_I2C_DEV_MAGIC; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) +{ + struct pios_i2c_adapter *i2c_adapter; + + i2c_adapter = (struct pios_i2c_adapter *)pios_malloc(sizeof(*i2c_adapter)); + if (!i2c_adapter) { + return NULL; + } + + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + return i2c_adapter; +} +#else +static struct pios_i2c_adapter pios_i2c_adapters[PIOS_I2C_MAX_DEVS]; +static uint8_t pios_i2c_num_adapters; +static struct pios_i2c_adapter *PIOS_I2C_alloc(void) +{ + struct pios_i2c_adapter *i2c_adapter; + + if (pios_i2c_num_adapters >= PIOS_I2C_MAX_DEVS) { + return NULL; + } + + i2c_adapter = &pios_i2c_adapters[pios_i2c_num_adapters++]; + i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; + + return i2c_adapter; +} +#endif /* if defined(PIOS_INCLUDE_CHIBIOS) && 0 */ + + +/** + * Initializes IIC driver + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) +{ + PIOS_DEBUG_Assert(i2c_id); + PIOS_DEBUG_Assert(cfg); + + struct pios_i2c_adapter *i2c_adapter; + + i2c_adapter = (struct pios_i2c_adapter *)PIOS_I2C_alloc(); + if (!i2c_adapter) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + i2c_adapter->cfg = cfg; + + vSemaphoreCreateBinary(i2c_adapter->sem_ready); + i2c_adapter->sem_busy = xSemaphoreCreateMutex(); + + /* Initialize the state machine */ + i2c_adapter_fsm_init(i2c_adapter); + + *i2c_id = (uint32_t)i2c_adapter; + + /* Configure and enable I2C interrupts */ + NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->event.init)); + NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->error.init)); + + /* No error */ + return 0; + +out_fail: + return -1; +} + +int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns) +{ + // FIXME: only supports transfer sizes up to 255 bytes + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + enum pios_i2c_transfer_result result = PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + + bool semaphore_success = true; + +#ifdef PIOS_INCLUDE_FREERTOS + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return PIOS_I2C_TRANSFER_BUSY; + } +#else + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return PIOS_I2C_TRANSFER_BUSY; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); +#endif /* PIOS_INCLUDE_FREERTOS */ + + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + + i2c_adapter->transfer_result = &result; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = &txn_list[0]; + i2c_adapter->bus_error = false; + i2c_adapter->nack = false; + +#ifdef PIOS_INCLUDE_FREERTOS + /* Make sure the done/ready semaphore is consumed before we start */ + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); +#endif + + i2c_adapter->callback = NULL; + + bool dummy = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START, &dummy); + + /* Wait for the transfer to complete */ +#ifdef PIOS_INCLUDE_FREERTOS + semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); + xSemaphoreGive(i2c_adapter->sem_ready); +#endif /* PIOS_INCLUDE_FREERTOS */ + + /* After this point, the bus is already unlocked from ISR + * Touching i2c_adapter is not allowed */ +#if defined(PIOS_I2C_DIAGNOSTICS) + if (!semaphore_success) { + i2c_timeout_counter++; + result = PIOS_I2C_TRANSFER_TIMEOUT; + } +#endif + return result; +} + +static int32_t PIOS_I2C_Transfer_Callback_Internal(struct pios_i2c_adapter *i2c_adapter, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback) +{ + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + + i2c_adapter->transfer_result = 0; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = &txn_list[0]; + i2c_adapter->bus_error = false; + i2c_adapter->nack = false; + i2c_adapter->callback = callback; + + // Estimate bytes of transmission. Per txns: 1 adress byte + length + i2c_adapter->transfer_timeout_ticks = num_txns; + for (uint32_t i = 0; i < num_txns; i++) { + i2c_adapter->transfer_timeout_ticks += txn_list[i].len; + } + // timeout if it takes eight times the expected time + i2c_adapter->transfer_timeout_ticks <<= 3; + + bool dummy = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START, &dummy); + + return 0; +} + +int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback) +{ + // FIXME: only supports transfer sizes up to 255 bytes + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + +#ifdef PIOS_INCLUDE_FREERTOS + /* Lock the bus */ + portTickType timeout; + timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; + if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { + return -2; + } +#else + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return -2; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); +#endif /* PIOS_INCLUDE_FREERTOS */ + + return PIOS_I2C_Transfer_Callback_Internal(i2c_adapter, txn_list, num_txns, callback); +} + +int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback, bool *woken) +{ + // FIXME: only supports transfer sizes up to 255 bytes + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); + +#ifdef PIOS_INCLUDE_FREERTOS + signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + + /* Lock the bus */ + bool locked = xSemaphoreTakeFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken) == pdTRUE; + + if (xHigherPriorityTaskWoken == pdTRUE) { + *woken = true; + } + + if (!locked) { + return -2; + } +#else + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return -2; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); +#endif /* PIOS_INCLUDE_FREERTOS */ + + return PIOS_I2C_Transfer_Callback_Internal(i2c_adapter, txn_list, num_txns, callback); +} + +void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) +{ + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + + bool woken = false; + + if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_RXNE)) { + // flag will be cleared by event +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_evirq_history[i2c_evirq_history_pointer] = I2C_FLAG_RXNE; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_RECEIVER_BUFFER_NOT_EMPTY, &woken); + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_TXIS)) { + // flag will be cleared by event +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_evirq_history[i2c_evirq_history_pointer] = I2C_FLAG_TXIS; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSMIT_BUFFER_EMPTY, &woken); + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_NACKF)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_NACKF); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_evirq_history[i2c_evirq_history_pointer] = I2C_FLAG_NACKF; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; + ++i2c_nack_counter; +#endif + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_NACK, &woken); + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_TC)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_TC); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_evirq_history[i2c_evirq_history_pointer] = I2C_FLAG_TC; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_TRANSFER_COMPLETE, &woken); + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_STOPF)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_STOPF); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_evirq_history[i2c_evirq_history_pointer] = I2C_FLAG_STOPF; + i2c_evirq_history_pointer = (i2c_evirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOP, &woken); + } + + portEND_SWITCHING_ISR(woken ? pdTRUE : pdFALSE); +} + + +void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id) +{ + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + + else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_BERR)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_BERR); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_BERR; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_ARLO)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_ARLO); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_ARLO; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_OVR)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_OVR); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_OVR; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_PECERR)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_PECERR); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_PECERR; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_TIMEOUT)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_TIMEOUT); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_TIMEOUT; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_ALERT)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_ALERT); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_ALERT; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } else if (I2C_GetFlagStatus(i2c_adapter->cfg->regs, I2C_FLAG_BUSY)) { + I2C_ClearFlag(i2c_adapter->cfg->regs, I2C_FLAG_BUSY); +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_erirq_history[i2c_erirq_history_pointer] = I2C_FLAG_BUSY; + i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % I2C_LOG_DEPTH; +#endif + } + +#if defined(PIOS_I2C_DIAGNOSTICS) + i2c_adapter_log_fault(i2c_adapter, PIOS_I2C_ERROR_INTERRUPT); +#endif + + /* Fail hard on any errors for now */ + bool woken = false; + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_BUS_ERROR, &woken); + + portEND_SWITCHING_ISR(woken ? pdTRUE : pdFALSE); +} + +#endif /* if defined(PIOS_INCLUDE_I2C) */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_irq.c b/flight/pios/stm32f30x/pios_irq.c new file mode 100644 index 0000000000..3a2a79b5d5 --- /dev/null +++ b/flight/pios/stm32f30x/pios_irq.c @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_IRQ IRQ Setup Functions + * @brief STM32 Hardware code to enable and disable interrupts + * @{ + * + * @file pios_irq.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief IRQ Enable/Disable routines + * @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_IRQ + +/* Private Function Prototypes */ + +/* Local Variables */ +/* The nesting counter ensures, that interrupts won't be enabled so long nested functions disable them */ +static uint32_t nested_ctr; + +/* Stored priority level before IRQ has been disabled (important for co-existence with vPortEnterCritical) */ +static uint32_t prev_primask; + +/** + * Disables all interrupts (nested) + * \return < 0 On errors + */ +int32_t PIOS_IRQ_Disable(void) +{ + /* Get current priority if nested level == 0 */ + if (!nested_ctr) { + __asm volatile (" mrs %0, primask\n" : "=r" (prev_primask) + ); + } + + /* Disable interrupts */ + __asm volatile (" mov r0, #1 \n" " msr primask, r0\n" ::: "r0"); + + ++nested_ctr; + + /* No error */ + return 0; +} + +/** + * Enables all interrupts (nested) + * \return < 0 on errors + * \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) + */ +int32_t PIOS_IRQ_Enable(void) +{ + /* Check for nesting error */ + if (nested_ctr == 0) { + /* Nesting error */ + return -1; + } + + /* Decrease nesting level */ + --nested_ctr; + + /* Set back previous priority once nested level reached 0 again */ + if (nested_ctr == 0) { + __asm volatile (" msr primask, %0\n" ::"r" (prev_primask) + ); + } + + /* No error */ + return 0; +} + +#endif /* PIOS_INCLUDE_IRQ */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_ppm.c b/flight/pios/stm32f30x/pios_ppm.c new file mode 100644 index 0000000000..9f1875a02c --- /dev/null +++ b/flight/pios/stm32f30x/pios_ppm.c @@ -0,0 +1,401 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_PPM PPM Input Functions + * @brief Code to measure PPM input and seperate into channels + * @{ + * + * @file pios_ppm.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief PPM Input functions (STM32 dependent) + * @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_PPM + +#include "pios_ppm_priv.h" + +/* Provide a RCVR driver */ +static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel); + +const struct pios_rcvr_driver pios_ppm_rcvr_driver = { + .read = PIOS_PPM_Get, +#if defined(PIOS_INCLUDE_FREERTOS) + .get_semaphore = PIOS_PPM_Get_Semaphore +#endif +}; + +#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 +#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS +#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames +#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds +#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds + +static void PIOS_PPM_Supervisor(uint32_t ppm_id); + +enum pios_ppm_dev_magic { + PIOS_PPM_DEV_MAGIC = 0xee014d8b, +}; + +struct pios_ppm_dev { + enum pios_ppm_dev_magic magic; + const struct pios_ppm_cfg *cfg; + + uint8_t PulseIndex; + uint32_t PreviousTime; + uint32_t CurrentTime; + uint32_t DeltaTime; + uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t LargeCounter; + int8_t NumChannels; + int8_t NumChannelsPrevFrame; + uint8_t NumChannelCounter; + + uint8_t supv_timer; + bool Tracking; + bool Fresh; + +#ifdef PIOS_INCLUDE_FREERTOS + xSemaphoreHandle new_sample_semaphores[PIOS_PPM_IN_MIN_NUM_CHANNELS]; +#endif /* PIOS_INCLUDE_FREERTOS */ +}; + +static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) +{ + return ppm_dev->magic == PIOS_PPM_DEV_MAGIC; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_ppm_dev *PIOS_PPM_alloc(void) +{ + struct pios_ppm_dev *ppm_dev; + + ppm_dev = (struct pios_ppm_dev *)pios_malloc(sizeof(*ppm_dev)); + if (!ppm_dev) { + return NULL; + } + + // Initialize the semaphores to 0. + for (uint8_t i = 0; i < PIOS_PPM_IN_MIN_NUM_CHANNELS; ++i) { + ppm_dev->new_sample_semaphores[i] = 0; + } + + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + return ppm_dev; +} +#else +static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS]; +static uint8_t pios_ppm_num_devs; +static struct pios_ppm_dev *PIOS_PPM_alloc(void) +{ + struct pios_ppm_dev *ppm_dev; + + if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { + return NULL; + } + + ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + + return ppm_dev; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + +static void PIOS_PPM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static const struct pios_tim_callbacks tim_callbacks = { + .overflow = PIOS_PPM_tim_overflow_cb, + .edge = PIOS_PPM_tim_edge_cb, +}; + +extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg) +{ + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); + + struct pios_ppm_dev *ppm_dev; + + ppm_dev = (struct pios_ppm_dev *)PIOS_PPM_alloc(); + if (!ppm_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + ppm_dev->cfg = cfg; + + /* Set up the state variables */ + ppm_dev->PulseIndex = 0; + ppm_dev->PreviousTime = 0; + ppm_dev->CurrentTime = 0; + ppm_dev->DeltaTime = 0; + ppm_dev->LargeCounter = 0; + ppm_dev->NumChannels = -1; + ppm_dev->NumChannelsPrevFrame = -1; + ppm_dev->NumChannelCounter = 0; + ppm_dev->Tracking = false; + ppm_dev->Fresh = false; + + for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + /* Flush counter variables */ + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { + return -1; + } + + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; + + /* Configure timer for input capture */ + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); + break; + } + } + + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { + PIOS_DEBUG_Assert(0); + } + + *ppm_id = (uint32_t)ppm_dev; + + return 0; + +out_fail: + return -1; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return 0; + } + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return 0; + } + + if (ppm_dev->new_sample_semaphores[channel] == 0) { + vSemaphoreCreateBinary(ppm_dev->new_sample_semaphores[channel]); + } + return ppm_dev->new_sample_semaphores[channel]; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + +/** + * 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_PPM_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return ppm_dev->CaptureValue[channel]; +} + +static void PIOS_PPM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, + __attribute__((unused)) uint8_t channel, uint16_t count) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } + + ppm_dev->LargeCounter += count; +} + + +static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +{ + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } + + if (chan_idx >= ppm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + /* Shift the last measurement out */ + ppm_dev->PreviousTime = ppm_dev->CurrentTime; + + /* Grab the new count */ + ppm_dev->CurrentTime = count; + + /* Convert to 32-bit timer result */ + ppm_dev->CurrentTime += ppm_dev->LargeCounter; + + /* Capture computation */ + ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; + + ppm_dev->PreviousTime = ppm_dev->CurrentTime; + + /* Sync pulse detection */ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame + && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) { + ppm_dev->NumChannelCounter++; + } else { + ppm_dev->NumChannels = ppm_dev->PulseIndex; + } + } else { + ppm_dev->NumChannelCounter = 0; + } + + /* Check if the last frame was well formed */ + if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { + /* The last frame was well formed */ + for (int32_t i = 0; i < ppm_dev->NumChannels; i++) { + ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; + } + for (uint32_t i = ppm_dev->NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } +#if defined(PIOS_INCLUDE_FREERTOS) + /* Signal that a new sample is ready on this channel. */ + if (ppm_dev->new_sample_semaphores[chan_idx] != 0) { + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + if (xSemaphoreGiveFromISR(ppm_dev->new_sample_semaphores[chan_idx], &pxHigherPriorityTaskWoken) == pdTRUE) { + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for his? */ + } + } +#endif /* USE_FREERTOS */ + } else { + for (uint32_t i = 0; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } + } + + ppm_dev->Fresh = true; + ppm_dev->Tracking = true; + ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; + ppm_dev->PulseIndex = 0; + + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ + } else if (ppm_dev->Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; + ppm_dev->PulseIndex++; + } else { + /* Not a valid pulse duration */ + ppm_dev->Tracking = false; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } + } +} + +static void PIOS_PPM_Supervisor(uint32_t ppm_id) +{ + /* Recover our device context */ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)ppm_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } + + /* + * RTC runs at 625Hz so divide down the base rate so + * that this loop runs at 25Hz. + */ + if (++(ppm_dev->supv_timer) < 25) { + return; + } + ppm_dev->supv_timer = 0; + + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = false; + + for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; + } + } + + ppm_dev->Fresh = false; +} + +#endif /* PIOS_INCLUDE_PPM */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_pwm.c b/flight/pios/stm32f30x/pios_pwm.c new file mode 100644 index 0000000000..d0a6fcdda8 --- /dev/null +++ b/flight/pios/stm32f30x/pios_pwm.c @@ -0,0 +1,283 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_PWM PWM Input Functions + * @brief Code to measure with PWM input + * @{ + * + * @file pios_pwm.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief PWM Input functions (STM32 dependent) + * @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_PWM + +#include "pios_pwm_priv.h" + +/* Provide a RCVR driver */ +static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel); + +const struct pios_rcvr_driver pios_pwm_rcvr_driver = { + .read = PIOS_PWM_Get, +}; + +/* Local Variables */ +/* 100 ms timeout without updates on channels */ +static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; + +enum pios_pwm_dev_magic { + PIOS_PWM_DEV_MAGIC = 0xab30293c, +}; + +struct pios_pwm_dev { + enum pios_pwm_dev_magic magic; + const struct pios_pwm_cfg *cfg; + + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; +}; + +static bool PIOS_PWM_validate(struct pios_pwm_dev *pwm_dev) +{ + return pwm_dev->magic == PIOS_PWM_DEV_MAGIC; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_pwm_dev *PIOS_PWM_alloc(void) +{ + struct pios_pwm_dev *pwm_dev; + + pwm_dev = (struct pios_pwm_dev *)pios_malloc(sizeof(*pwm_dev)); + if (!pwm_dev) { + return NULL; + } + + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + return pwm_dev; +} +#else +static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS]; +static uint8_t pios_pwm_num_devs; +static struct pios_pwm_dev *PIOS_PWM_alloc(void) +{ + struct pios_pwm_dev *pwm_dev; + + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return NULL; + } + + pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + + return pwm_dev; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + +static void PIOS_PWM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static const struct pios_tim_callbacks tim_callbacks = { + .overflow = PIOS_PWM_tim_overflow_cb, + .edge = PIOS_PWM_tim_edge_cb, +}; + +/** + * Initialises all the pins + */ +int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg) +{ + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); + + struct pios_pwm_dev *pwm_dev; + + pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc(); + if (!pwm_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + pwm_dev->cfg = cfg; + + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + pwm_dev->CaptureState[i] = 0; + pwm_dev->RiseValue[i] = 0; + pwm_dev->FallValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + } + + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { + return -1; + } + + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel *chan = &cfg->channels[i]; + + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } + + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } + + *pwm_id = (uint32_t)pwm_dev; + + return 0; + +out_fail: + return -1; +} + +/** + * 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_PWM_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)rcvr_id; + + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return PIOS_RCVR_INVALID; + } + + if (channel >= PIOS_PWM_NUM_INPUTS) { + /* Channel out of range */ + return PIOS_RCVR_INVALID; + } + return pwm_dev->CaptureValue[channel]; +} + +static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) +{ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; + + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } + + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + pwm_dev->us_since_update[channel] += count; + if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } +} + +static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +{ + /* Recover our device context */ + struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context; + + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } + + if (chan_idx >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + const struct pios_tim_channel *chan = &pwm_dev->cfg->channels[chan_idx]; + + if (pwm_dev->CaptureState[chan_idx] == 0) { + pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; + } else { + pwm_dev->FallValue[chan_idx] = count; + } + + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; + if (pwm_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 1; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { + pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); + } else { + pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); + } + + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 0; + + /* Increase supervisor counter */ + pwm_dev->CapCounter[chan_idx]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } +} + +#endif /* PIOS_INCLUDE_PWM */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_rtc.c b/flight/pios/stm32f30x/pios_rtc.c new file mode 100644 index 0000000000..559fde9956 --- /dev/null +++ b/flight/pios/stm32f30x/pios_rtc.c @@ -0,0 +1,141 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RTC RTC Functions + * @brief Code to manage initialization and interrups of RTC peripheral + * @{ + * + * @file pios_rtc.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief RTC Functions (STM32 dependent) + * @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_RTC + +#include + +#ifndef PIOS_RTC_PRESCALER +#define PIOS_RTC_PRESCALER 100 +#endif + +struct rtc_callback_entry { + void (*fn)(uint32_t); + uint32_t data; +}; + +#define PIOS_RTC_MAX_CALLBACKS 3 +struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS]; +static uint8_t rtc_callback_next = 0; + +void PIOS_RTC_Init(const struct pios_rtc_cfg *cfg) +{ + RCC_BackupResetCmd(ENABLE); + RCC_BackupResetCmd(DISABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); + PWR_BackupAccessCmd(ENABLE); + // Divide external 8 MHz clock to 1 MHz + RCC_RTCCLKConfig(cfg->clksrc); + RCC_RTCCLKCmd(ENABLE); + + RTC_WakeUpCmd(DISABLE); + // Divide 1 Mhz clock by 16 -> 62.5 khz + RTC_WakeUpClockConfig(RTC_WakeUpClock_RTCCLK_Div16); + // Divide 62.5 khz by 200 to get 625 Hz + RTC_SetWakeUpCounter(cfg->prescaler); // cfg->prescaler); + RTC_WakeUpCmd(ENABLE); + + /* Configure and enable the RTC Second interrupt */ + EXTI_InitTypeDef ExtiInit = { + .EXTI_Line = EXTI_Line20, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }; + EXTI_Init((EXTI_InitTypeDef *)&ExtiInit); + NVIC_Init((NVIC_InitTypeDef *)&cfg->irq.init); + RTC_ITConfig(RTC_IT_WUT, ENABLE); + + RTC_ClearFlag(RTC_FLAG_WUTF); +} + +uint32_t PIOS_RTC_Counter() +{ + return RTC_GetWakeUpCounter(); +} + +/* FIXME: This shouldn't use hard-coded clock rates, dividers or prescalers. + * Should get these from the cfg struct passed to init. + */ +float PIOS_RTC_Rate() +{ + return (float)(8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER); +} + +float PIOS_RTC_MsPerTick() +{ + return 1000.0f / PIOS_RTC_Rate(); +} + +/* TODO: This needs a mutex around rtc_callbacks[] */ +bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data) +{ + struct rtc_callback_entry *cb; + + if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) { + return false; + } + + cb = &rtc_callback_list[rtc_callback_next++]; + + cb->fn = fn; + cb->data = data; + return true; +} + +void PIOS_RTC_irq_handler(void) +{ + if (RTC_GetITStatus(RTC_IT_WUT)) { + /* Call all registered callbacks */ + for (uint8_t i = 0; i < rtc_callback_next; i++) { + struct rtc_callback_entry *cb = &rtc_callback_list[i]; + if (cb->fn) { + (cb->fn)(cb->data); + } + } + + /* Clear the RTC Second interrupt */ + RTC_ClearITPendingBit(RTC_IT_WUT); + } + + if (EXTI_GetITStatus(EXTI_Line20) != RESET) { + EXTI_ClearITPendingBit(EXTI_Line20); + } +} + +#endif /* PIOS_INCLUDE_RTC */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_spi.c b/flight/pios/stm32f30x/pios_spi.c new file mode 100644 index 0000000000..b105836655 --- /dev/null +++ b/flight/pios/stm32f30x/pios_spi.c @@ -0,0 +1,751 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SPI SPI Functions + * @brief PIOS interface to read and write from SPI ports + * @{ + * + * @file pios_spi.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Hardware Abstraction Layer for SPI ports of STM32 + * @see The GNU Public License (GPL) Version 3 + * @notes + * + * Note that additional chip select lines can be easily added by using + * the remaining free GPIOs of the core module. Shared SPI ports should be + * arbitrated with (FreeRTOS based) Mutexes to avoid collisions! + * + *****************************************************************************/ +/* + * 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 + +#ifdef PIOS_INCLUDE_SPI + +#include + +#define SPI_MAX_BLOCK_PIO 12800 + +static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_dev) +{ + /* Should check device magic here */ + return true; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_spi_dev *PIOS_SPI_alloc(void) +{ + return pios_malloc(sizeof(struct pios_spi_dev)); +} +#else +static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS]; +static uint8_t pios_spi_num_devs; +static struct pios_spi_dev *PIOS_SPI_alloc(void) +{ + if (pios_spi_num_devs >= PIOS_SPI_MAX_DEVS) { + return NULL; + } + + return &pios_spi_devs[pios_spi_num_devs++]; +} +#endif + +/** + * Initialises SPI pins + * \param[in] mode currently only mode 0 supported + * \return < 0 if initialisation failed + */ +int32_t PIOS_SPI_Init(uint32_t *spi_id, const struct pios_spi_cfg *cfg) +{ + uint32_t init_ssel = 0; + + PIOS_Assert(spi_id); + PIOS_Assert(cfg); + + struct pios_spi_dev *spi_dev; + + spi_dev = (struct pios_spi_dev *)PIOS_SPI_alloc(); + if (!spi_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + spi_dev->cfg = cfg; + +#if defined(PIOS_INCLUDE_FREERTOS) + vSemaphoreCreateBinary(spi_dev->busy); + xSemaphoreGive(spi_dev->busy); +#else + spi_dev->busy = 0; +#endif + + /* Disable callback function */ + spi_dev->callback = NULL; + + /* Set rx/tx dummy bytes to a known value */ + spi_dev->rx_dummy_byte = 0xFF; + spi_dev->tx_dummy_byte = 0xFF; + + switch (spi_dev->cfg->init.SPI_NSS) { + case SPI_NSS_Soft: + if (spi_dev->cfg->init.SPI_Mode == SPI_Mode_Master) { + /* We're a master in soft NSS mode, make sure we see NSS high at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Set); + /* Init as many slave selects as the config advertises. */ + init_ssel = spi_dev->cfg->slave_count; + } else { + /* We're a slave in soft NSS mode, make sure we see NSS low at all times. */ + SPI_NSSInternalSoftwareConfig(spi_dev->cfg->regs, SPI_NSSInternalSoft_Reset); + } + break; + case SPI_NSS_Hard: + /* only legal for single-slave config */ + PIOS_Assert(spi_dev->cfg->slave_count == 1); + init_ssel = 1; + /* FIXME: Should this also call SPI_SSOutputCmd()? */ + break; + + default: + PIOS_Assert(0); + } + + /* Initialize the GPIO pins */ + /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ + if (spi_dev->cfg->remap) { + GPIO_PinAFConfig(spi_dev->cfg->sclk.gpio, + __builtin_ctz(spi_dev->cfg->sclk.init.GPIO_Pin), + spi_dev->cfg->remap); + GPIO_PinAFConfig(spi_dev->cfg->mosi.gpio, + __builtin_ctz(spi_dev->cfg->mosi.init.GPIO_Pin), + spi_dev->cfg->remap); + GPIO_PinAFConfig(spi_dev->cfg->miso.gpio, + __builtin_ctz(spi_dev->cfg->miso.init.GPIO_Pin), + spi_dev->cfg->remap); + for (uint32_t i = 0; i < init_ssel; i++) { + GPIO_PinAFConfig(spi_dev->cfg->ssel[i].gpio, + __builtin_ctz(spi_dev->cfg->ssel[i].init.GPIO_Pin), + spi_dev->cfg->remap); + } + } + + /* Initialize the GPIO pins */ + GPIO_Init(spi_dev->cfg->sclk.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->sclk.init)); + GPIO_Init(spi_dev->cfg->mosi.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->mosi.init)); + GPIO_Init(spi_dev->cfg->miso.gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->miso.init)); + for (uint32_t i = 0; i < init_ssel; i++) { + /* Since we're driving the SSEL pin in software, ensure that the slave is deselected */ + /* XXX multi-slave support - maybe have another SPI_NSS_ mode? */ + GPIO_SetBits(spi_dev->cfg->ssel[i].gpio, spi_dev->cfg->ssel[i].init.GPIO_Pin); + GPIO_Init(spi_dev->cfg->ssel[i].gpio, (GPIO_InitTypeDef *)&(spi_dev->cfg->ssel[i].init)); + } + + /* Enable the associated peripheral clock */ + switch ((uint32_t)spi_dev->cfg->regs) { + case (uint32_t)SPI1: + /* Enable SPI peripheral clock (APB2 == high speed) */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); + break; + case (uint32_t)SPI2: + /* Enable SPI peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); + break; + case (uint32_t)SPI3: + /* Enable SPI peripheral clock (APB1 == slow speed) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); + break; + } + + bool use_dma = spi_dev->cfg->dma.rx.channel && spi_dev->cfg->dma.tx.channel; + + /* Enable DMA clock */ + if (use_dma) { + RCC_AHBPeriphClockCmd(spi_dev->cfg->dma.ahb_clk, ENABLE); + + /* Configure DMA for SPI Rx */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.rx.channel, (DMA_InitTypeDef *)&(spi_dev->cfg->dma.rx.init)); + + /* Configure DMA for SPI Tx */ + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + DMA_Init(spi_dev->cfg->dma.tx.channel, (DMA_InitTypeDef *)&(spi_dev->cfg->dma.tx.init)); + } + + /* Initialize the SPI block */ + SPI_I2S_DeInit(spi_dev->cfg->regs); + SPI_Init(spi_dev->cfg->regs, (SPI_InitTypeDef *)&(spi_dev->cfg->init)); + + /* Configure CRC calculation */ + if (spi_dev->cfg->use_crc) { + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } else { + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + } + + /* Configure the RX FIFO Threshold -- 8 bits */ + SPI_RxFIFOThresholdConfig(spi_dev->cfg->regs, SPI_RxFIFOThreshold_QF); + + /* Enable SPI */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); + + /* Enable SPI interrupts to DMA */ + if (use_dma) { + SPI_I2S_DMACmd(spi_dev->cfg->regs, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE); + } + + /* Configure DMA interrupt */ + NVIC_Init((NVIC_InitTypeDef *)&(spi_dev->cfg->dma.irq.init)); + + *spi_id = (uint32_t)spi_dev; + return 0; + +out_fail: + return -1; +} + +/** + * (Re-)initialises SPI peripheral clock rate + * + * \param[in] spi SPI number (0 or 1) + * \param[in] spi_prescaler configures the SPI speed: + *
    + *
  • PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead) + *
  • PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s) + *
  • PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s) + *
  • PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s) + *
  • PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s) + *
  • PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s) + *
  • PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s) + *
  • PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s) + *
+ * \return 0 if no error + * \return -1 if disabled SPI port selected + * \return -3 if invalid spi_prescaler selected + */ +int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + SPI_InitTypeDef SPI_InitStructure; + + if (spi_prescaler >= 8) { + /* Invalid prescaler selected */ + return -3; + } + + /* Start with a copy of the default configuration for the peripheral */ + SPI_InitStructure = spi_dev->cfg->init; + + /* Adjust the prescaler for the peripheral's clock */ + SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t)spi_prescaler & 7) << 3; + + /* Write back the new configuration */ + SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure); + + PIOS_SPI_TransferByte(spi_id, 0xFF); + return 0; +} + +/** + * Claim the SPI bus semaphore. Calling the SPI functions does not require this + * \param[in] spi SPI number (0 or 1) + * \return 0 if no error + * \return -1 if timeout before claiming semaphore + */ +int32_t PIOS_SPI_ClaimBus(uint32_t spi_id) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE) { + return -1; + } +#else + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + uint32_t timeout = 0xffff; + while ((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout) { + ; + } + if (timeout == 0) { // timed out + return -1; + } + + PIOS_IRQ_Disable(); + if (spi_dev->busy) { + return -1; + } + spi_dev->busy = 1; + PIOS_IRQ_Enable(); +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + return 0; +} + +/** + * Claim the SPI bus semaphore from an ISR. Has no timeout. + * \param[in] spi SPI number (0 or 1) + * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + * \return 0 if no error + * \return -1 if timeout before claiming semaphore + */ +int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE) { + return -1; + } + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; + +#else + if (woken) { + *woken = false; + } + return PIOS_SPI_ClaimBus(spi_id); + +#endif +} + +/** + * Release the SPI bus semaphore. Calling the SPI functions does not require this + * \param[in] spi SPI number (0 or 1) + * \return 0 if no error + */ +int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + xSemaphoreGive(spi_dev->busy); +#else + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + PIOS_IRQ_Disable(); + spi_dev->busy = 0; + PIOS_IRQ_Enable(); +#endif + return 0; +} + +/** + * Release the SPI bus semaphore from ISR. Calling the SPI functions does not require this + * \param[in] spi SPI number (0 or 1) + * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority + * task has is now eligible to run, else unchanged + * \return 0 if no error + */ +int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE; + + bool valid = PIOS_SPI_validate(spi_dev); + PIOS_Assert(valid) + + xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken); + if (woken) { + *woken = *woken || (higherPriorityTaskWoken == pdTRUE); + } + return 0; + +#else + if (woken) { + *woken = false; + } + return PIOS_SPI_ReleaseBus(spi_id); + +#endif +} + + +/** + * Controls the RC (Register Clock alias Chip Select) pin of a SPI port + * \param[in] spi SPI number (0 or 1) + * \param[in] pin_value 0 or 1 + * \return 0 if no error + */ +int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint32_t slave_id, uint8_t pin_value) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + PIOS_Assert(slave_id <= spi_dev->cfg->slave_count) + + /* XXX multi-slave support? */ + if (pin_value) { + GPIO_SetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } else { + GPIO_ResetBits(spi_dev->cfg->ssel[slave_id].gpio, spi_dev->cfg->ssel[slave_id].init.GPIO_Pin); + } + + return 0; +} + +/** + * Transfers a byte to SPI output and reads back the return value from SPI input + * \param[in] spi SPI number (0 or 1) + * \param[in] b the byte which should be transfered + */ +int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + uint8_t rx_byte; + + /* Make sure the RXNE flag is cleared by reading the DR register */ + SPI_ReceiveData8(spi_dev->cfg->regs); + + /* Wait until the TXE goes high */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) == RESET) { + ; + } + + /* Start the transfer */ + SPI_SendData8(spi_dev->cfg->regs, b); + + /* Wait until there is a byte to read */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_RXNE) == RESET) { + ; + } + + /* Read the rx'd byte */ + rx_byte = SPI_ReceiveData8(spi_dev->cfg->regs); + + /* Wait until the TXE goes high */ + while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) { + ; + } + + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + /* Return received byte */ + return rx_byte; +} + + +/** + * Transfers a block of bytes via PIO. + * + * \param[in] spi_id SPI device handle + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + */ +static int32_t PIOS_SPI_TransferBlock_PIO(struct pios_spi_dev *spi_dev, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, __attribute__((unused)) void *callback) +{ + uint8_t b; + + while (len--) { + /* get the byte to send */ + b = send_buffer ? *(send_buffer++) : 0xff; + + /* Wait until the TXE goes high */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) == RESET) { + ; + } + + /* Start the transfer */ + SPI_SendData8(spi_dev->cfg->regs, b); + + /* Wait until there is a byte to read */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_RXNE) == RESET) { + ; + } + + /* Read the rx'd byte */ + b = SPI_ReceiveData8(spi_dev->cfg->regs); + + /* save the received byte */ + if (receive_buffer) { + *(receive_buffer++) = b; + } + } + + /* Wait for SPI transfer to have fully completed */ + while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) { + ; + } + + return 0; +} + + +/** + * Transfers a block of bytes via DMA. + * \param[in] spi SPI number (0 or 1) + * \param[in] send_buffer pointer to buffer which should be sent.
+ * If NULL, 0xff (all-one) will be sent. + * \param[in] receive_buffer pointer to buffer which should get the received values.
+ * If NULL, received bytes will be discarded. + * \param[in] len number of bytes which should be transfered + * \param[in] callback pointer to callback function which will be executed + * from DMA channel interrupt once the transfer is finished. + * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will + * block until the transfer is finished. + * \return >= 0 if no error during transfer + * \return -1 if disabled SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ +static int32_t PIOS_SPI_TransferBlock_DMA(struct pios_spi_dev *spi_dev, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) +{ + DMA_InitTypeDef dma_init; + + /* Exit if ongoing transfer */ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + return -3; + } + + /* Disable the SPI peripheral */ + SPI_Cmd(spi_dev->cfg->regs, DISABLE); + + /* Disable the DMA channels */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, DISABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, DISABLE); + + /* Set callback function */ + spi_dev->callback = callback; + + /* + * Configure Rx channel + */ + + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.rx.init; + + if (receive_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_MemoryBaseAddr = (uint32_t)receive_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->rx_dummy_byte = 0xFF; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&spi_dev->rx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } + if (spi_dev->cfg->use_crc) { + /* Make sure the CRC error flag is cleared before we start */ + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } + + dma_init.DMA_BufferSize = len; + DMA_Init(spi_dev->cfg->dma.rx.channel, &(dma_init)); + + /* + * Configure Tx channel + */ + + /* Start with the default configuration for this peripheral */ + dma_init = spi_dev->cfg->dma.tx.init; + + if (send_buffer != NULL) { + /* Enable memory addr. increment - bytes written into receive buffer */ + dma_init.DMA_MemoryBaseAddr = (uint32_t)send_buffer; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Enable; + } else { + /* Disable memory addr. increment - bytes written into dummy buffer */ + spi_dev->tx_dummy_byte = 0xFF; + dma_init.DMA_MemoryBaseAddr = (uint32_t)&spi_dev->tx_dummy_byte; + dma_init.DMA_MemoryInc = DMA_MemoryInc_Disable; + } + + if (spi_dev->cfg->use_crc) { + /* The last byte of the payload will be replaced with the CRC8 */ + dma_init.DMA_BufferSize = len - 1; + } else { + dma_init.DMA_BufferSize = len; + } + + DMA_Init(spi_dev->cfg->dma.tx.channel, &(dma_init)); + + /* Enable DMA interrupt if callback function active */ + DMA_ITConfig(spi_dev->cfg->dma.rx.channel, DMA_IT_TC, (callback != NULL) ? ENABLE : DISABLE); + + /* Flush out the CRC registers */ + SPI_CalculateCRC(spi_dev->cfg->regs, DISABLE); + (void)SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + + /* Make sure to flush out the receive buffer */ + (void)SPI_I2S_ReceiveData16(spi_dev->cfg->regs); + + if (spi_dev->cfg->use_crc) { + /* Need a 0->1 transition to reset the CRC logic */ + SPI_CalculateCRC(spi_dev->cfg->regs, ENABLE); + } + + /* Start DMA transfers */ + DMA_Cmd(spi_dev->cfg->dma.rx.channel, ENABLE); + DMA_Cmd(spi_dev->cfg->dma.tx.channel, ENABLE); + + /* Reenable the SPI device */ + SPI_Cmd(spi_dev->cfg->regs, ENABLE); + + if (callback) { + /* User has requested a callback, don't wait for the transfer to complete. */ + return 0; + } + + /* Wait until all bytes have been transmitted/received */ + while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { + ; + } + + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } + + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } + + /* Check the CRC on the transfer if enabled. */ + if (spi_dev->cfg->use_crc) { + /* Check the SPI CRC error flag */ + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + return -4; + } + } + + /* No error */ + return 0; +} + +int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + if ((len > SPI_MAX_BLOCK_PIO) && spi_dev->cfg->dma.rx.channel && spi_dev->cfg->dma.tx.channel) { + return PIOS_SPI_TransferBlock_DMA(spi_dev, send_buffer, receive_buffer, len, callback); + } + + return PIOS_SPI_TransferBlock_PIO(spi_dev, send_buffer, receive_buffer, len, callback); +} + +/** + * Check if a transfer is in progress + * \param[in] spi SPI number (0 or 1) + * \return >= 0 if no transfer is in progress + * \return -1 if disabled SPI port selected + * \return -2 if unsupported SPI port selected + * \return -3 if function has been called during an ongoing DMA transfer + */ +int32_t PIOS_SPI_Busy(uint32_t spi_id) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + /* DMA buffer has data or SPI transmit register not empty or SPI is busy*/ + if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel) || + !SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE) || + SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + return -3; + } + + return 0; +} + +void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescaler) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid); + PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler)); + + spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler; +} + +void PIOS_SPI_IRQ_Handler(uint32_t spi_id) +{ + struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; + + bool valid = PIOS_SPI_validate(spi_dev); + + PIOS_Assert(valid) + + DMA_ClearFlag(spi_dev->cfg->dma.irq.flags); + + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (!(SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_TXE))) { + ; + } + + /* Wait for the final bytes of the transfer to complete, including CRC byte(s). */ + while (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_I2S_FLAG_BSY)) { + ; + } + + if (spi_dev->callback != NULL) { + bool crc_ok = true; + uint8_t crc_val; + + if (SPI_I2S_GetFlagStatus(spi_dev->cfg->regs, SPI_FLAG_CRCERR)) { + crc_ok = false; + SPI_I2S_ClearFlag(spi_dev->cfg->regs, SPI_FLAG_CRCERR); + } + crc_val = SPI_GetCRC(spi_dev->cfg->regs, SPI_CRC_Rx); + spi_dev->callback(crc_ok, crc_val); + } +} + +#endif /* PIOS_INCLUDE_SPI */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_sys.c b/flight/pios/stm32f30x/pios_sys.c new file mode 100644 index 0000000000..6d849bc147 --- /dev/null +++ b/flight/pios/stm32f30x/pios_sys.c @@ -0,0 +1,287 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SYS System Functions + * @brief PIOS System Initialization code + * @{ + * + * @file pios_sys.c + * @author Michael Smith Copyright (C) 2011 + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author Tau Labs, http://taulabs.org, Copyright (C) 2012-2014 + * @brief Sets up basic STM32 system hardware, functions are called from Main. + * @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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_SYS) + +#define MEM8(addr) (*((volatile uint8_t *)(addr))) + +/* Private Function Prototypes */ +static void NVIC_Configuration(void); + +/** + * Initialises all system peripherals + */ +void PIOS_SYS_Init(void) +{ + /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */ + SystemInit(); + SystemCoreClockUpdate(); /* update SystemCoreClock for use elsewhere */ + + /* + * @todo might make sense to fetch the bus clocks and save them somewhere to avoid + * having to use the clunky get-all-clocks API everytime we need one. + */ + + /* Initialise Basic NVIC */ + /* do this early to ensure that we take exceptions in the right place */ + NVIC_Configuration(); + + /* Init the delay system */ + PIOS_DELAY_Init(); + + /* + * 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_GPIOA | + RCC_AHBPeriph_GPIOB | + RCC_AHBPeriph_GPIOC | + RCC_AHBPeriph_GPIOD | + RCC_AHBPeriph_GPIOE | + RCC_AHBPeriph_GPIOF | + RCC_AHBPeriph_TS | + RCC_AHBPeriph_CRC | + RCC_AHBPeriph_FLITF | + RCC_AHBPeriph_SRAM | + RCC_AHBPeriph_DMA2 | + RCC_AHBPeriph_DMA1 | + RCC_AHBPeriph_ADC34 | + RCC_AHBPeriph_ADC12 | + 0, ENABLE); + + RCC_APB1PeriphClockCmd( + RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_TIM4 | + RCC_APB1Periph_TIM6 | + RCC_APB1Periph_TIM7 | + 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_PWR | + RCC_APB1Periph_DAC | + 0, ENABLE); + + RCC_APB2PeriphClockCmd( + RCC_APB2Periph_TIM1 | + RCC_APB2Periph_TIM8 | + RCC_APB2Periph_TIM15 | + RCC_APB2Periph_TIM16 | + RCC_APB2Periph_TIM17 | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_SYSCFG | + 0, ENABLE); + + /* + * Configure all pins as input / pullup to avoid issues with + * uncommitted pins, excepting special-function pins that we need to + * remain as-is. + */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // default is un-pulled input + + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; +#if (PIOS_USB_ENABLED) + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_11 | GPIO_Pin_12); // leave USB D+/D- alone +#endif + GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); // leave JTAG pins alone + GPIO_Init(GPIOA, &GPIO_InitStructure); + + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All; + GPIO_Init(GPIOB, &GPIO_InitStructure); + GPIO_Init(GPIOC, &GPIO_InitStructure); + GPIO_Init(GPIOD, &GPIO_InitStructure); + GPIO_Init(GPIOE, &GPIO_InitStructure); + GPIO_Init(GPIOF, &GPIO_InitStructure); +} + +/** + * Shutdown PIOS and reset the microcontroller:
+ *
    + *
  • Disable all RTOS tasks + *
  • Disable all interrupts + *
  • Turn off all board LEDs + *
  • Reset STM32 + *
+ * \return < 0 if reset failed + */ +int32_t PIOS_SYS_Reset(void) +{ + // disable all interrupts + PIOS_IRQ_Disable(); + + // turn off all board LEDs +#ifdef PIOS_INCLUDE_LED +# ifdef PIOS_LED_HEARTBEAT + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +# endif /* PIOS_LED_HEARTBEAT */ +# ifdef PIOS_LED_ALARM + PIOS_LED_Off(PIOS_LED_ALARM); +# endif /* PIOS_LED_ALARM */ +# ifdef PIOS_BUZZER_ALARM + PIOS_LED_Off(PIOS_BUZZER_ALARM); +# endif /* PIOS_BUZZER_ALARM */ +#endif /* PIOS_INCLUDE_LED */ + /* XXX F10x port resets most (but not all) peripherals ... do we care? */ + + /* Reset STM32 */ + NVIC_SystemReset(); + + while (1) { + ; + } + + /* We will never reach this point */ + return -1; +} + +/** + * Returns the serial number as byte array + * param[out] pointer to a byte array which can store at least 12 bytes + * (12 bytes returned for STM32) + * return < 0 if feature not supported + */ +int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array) +{ + int i; + + /* Stored in the so called "electronic signature" */ + for (i = 0; i < PIOS_SYS_SERIAL_NUM_BINARY_LEN; ++i) { + uint8_t b = MEM8(0x1ffff7ac + i); + + array[i] = b; + } + + /* No error */ + return 0; +} + +/** + * Returns the serial number as a string + * param[out] str pointer to a string which can store at least 32 digits + zero terminator! + * (24 digits returned for STM32) + * return < 0 if feature not supported + */ +int32_t PIOS_SYS_SerialNumberGet(char *str) +{ + int i; + + /* Stored in the so called "electronic signature" */ + for (i = 0; i < PIOS_SYS_SERIAL_NUM_ASCII_LEN; ++i) { + uint8_t b = MEM8(0x1ffff7ac + (i / 2)); + if (!(i & 1)) { + b >>= 4; + } + b &= 0x0f; + + str[i] = ((b > 9) ? ('A' - 10) : '0') + b; + } + str[i] = '\0'; + + /* No error */ + return 0; +} + +/** + * Configures Vector Table base location and SysTick + */ +static void NVIC_Configuration(void) +{ + /* Set the Vector Table base address as specified in .ld file */ + extern void *pios_isr_vector_table_base; + + NVIC_SetVectorTable((uint32_t)&pios_isr_vector_table_base, 0x0); + + /* 4 bits for Interrupt priorities so no sub priorities */ + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + /* Configure HCLK clock as SysTick clock source. */ + SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK); +} + +#ifdef USE_FULL_ASSERT +/** + * Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * \param[in] file pointer to the source file name + * \param[in] line assert_param error line source number + * \retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* When serial debugging is implemented, use something like this. */ + /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ + + /* Setup the LEDs to Alternate */ +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_On(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ + + /* Infinite loop */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ + for (int i = 0; i < 1000000; i++) { + ; + } + } +} +#endif /* ifdef USE_FULL_ASSERT */ + +#endif /* if defined(PIOS_INCLUDE_SYS) */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_tim.c b/flight/pios/stm32f30x/pios_tim.c new file mode 100644 index 0000000000..3c6f9e22fa --- /dev/null +++ b/flight/pios/stm32f30x/pios_tim.c @@ -0,0 +1,401 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_TIM Timer Functions + * @brief PIOS Timer control code + * @{ + * + * @file pios_tim.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sets up timers and ways to register callbacks on them + * @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_TIM + +#include "pios_tim_priv.h" + +enum pios_tim_dev_magic { + PIOS_TIM_DEV_MAGIC = 0x87654098, +}; + +struct pios_tim_dev { + enum pios_tim_dev_magic magic; + + const struct pios_tim_channel *channels; + uint8_t num_channels; + + const struct pios_tim_callbacks *callbacks; + uint32_t context; +}; + +#if 0 +static bool PIOS_TIM_validate(struct pios_tim_dev *tim_dev) +{ + return tim_dev->magic == PIOS_TIM_DEV_MAGIC; +} +#endif + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_tim_dev *PIOS_TIM_alloc(void) +{ + struct pios_tim_dev *tim_dev; + + tim_dev = (struct pios_tim_dev *)pios_malloc(sizeof(*tim_dev)); + if (!tim_dev) { + return NULL; + } + + tim_dev->magic = PIOS_TIM_DEV_MAGIC; + return tim_dev; +} +#else +static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS]; +static uint8_t pios_tim_num_devs; +static struct pios_tim_dev *PIOS_TIM_alloc(void) +{ + struct pios_tim_dev *tim_dev; + + if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { + return NULL; + } + + tim_dev = &pios_tim_devs[pios_tim_num_devs++]; + tim_dev->magic = PIOS_TIM_DEV_MAGIC; + + return tim_dev; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) && 0 */ + + +int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) +{ + PIOS_DEBUG_Assert(cfg); + + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, (TIM_TimeBaseInitTypeDef *)cfg->time_base_init); + + /* Configure internal timer clocks */ + TIM_InternalClockConfig(cfg->timer); + + /* Enable timers */ + TIM_Cmd(cfg->timer, ENABLE); + + /* Enable Interrupts */ + NVIC_Init((NVIC_InitTypeDef *)&cfg->irq.init); + + // Advanced timers TIM1 & TIM8 need special handling: + // There are up to 4 separate interrupts handlers for each advanced timer, but + // pios_tim_clock_cfg has provision for only one irq init, so we take care here + // to enable additional irq channels that we intend to use. + + if (cfg->timer == TIM1) { + NVIC_InitTypeDef init = cfg->irq.init; + init.NVIC_IRQChannel = TIM1_UP_TIM16_IRQn; + NVIC_Init(&init); + } else if (cfg->timer == TIM8) { + NVIC_InitTypeDef init = cfg->irq.init; + init.NVIC_IRQChannel = TIM8_UP_IRQn; + NVIC_Init(&init); + } + + return 0; +} + +int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *channels, uint8_t num_channels, const struct pios_tim_callbacks *callbacks, uint32_t context) +{ + PIOS_Assert(channels); + PIOS_Assert(num_channels); + + struct pios_tim_dev *tim_dev; + tim_dev = (struct pios_tim_dev *)PIOS_TIM_alloc(); + if (!tim_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + tim_dev->channels = channels; + tim_dev->num_channels = num_channels; + tim_dev->callbacks = callbacks; + tim_dev->context = context; + + /* Configure the pins */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel *chan = &(channels[i]); + + GPIO_Init(chan->pin.gpio, (GPIO_InitTypeDef *)&chan->pin.init); + + if (chan->remap) { + GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap); + } + } + + *tim_id = (uint32_t)tim_dev; + + return 0; + +out_fail: + return -1; +} + +static void PIOS_TIM_generic_irq_handler(TIM_TypeDef *timer) +{ + /* Check for an overflow event on this timer */ + bool overflow_event = 0; + uint16_t overflow_count = false; + + if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { + TIM_ClearITPendingBit(timer, TIM_IT_Update); + overflow_count = timer->ARR; + overflow_event = true; + } + + /* Iterate over all registered clients of the TIM layer to find channels on this timer */ + for (uint8_t i = 0; i < pios_tim_num_devs; i++) { + const struct pios_tim_dev *tim_dev = &pios_tim_devs[i]; + + if (!tim_dev->channels || tim_dev->num_channels == 0) { + /* No channels to process on this client */ + continue; + } + + for (uint8_t j = 0; j < tim_dev->num_channels; j++) { + const struct pios_tim_channel *chan = &tim_dev->channels[j]; + + if (chan->timer != timer) { + /* channel is not on this timer */ + continue; + } + + /* Figure out which interrupt bit we should be looking at */ + uint16_t timer_it; + switch (chan->timer_chan) { + case TIM_Channel_1: + timer_it = TIM_IT_CC1; + break; + case TIM_Channel_2: + timer_it = TIM_IT_CC2; + break; + case TIM_Channel_3: + timer_it = TIM_IT_CC3; + break; + case TIM_Channel_4: + timer_it = TIM_IT_CC4; + break; + default: + PIOS_Assert(0); + break; + } + + bool edge_event; + uint16_t edge_count; + if (TIM_GetITStatus(chan->timer, timer_it) == SET) { + TIM_ClearITPendingBit(chan->timer, timer_it); + + /* Read the current counter */ + switch (chan->timer_chan) { + case TIM_Channel_1: + edge_count = TIM_GetCapture1(chan->timer); + break; + case TIM_Channel_2: + edge_count = TIM_GetCapture2(chan->timer); + break; + case TIM_Channel_3: + edge_count = TIM_GetCapture3(chan->timer); + break; + case TIM_Channel_4: + edge_count = TIM_GetCapture4(chan->timer); + break; + default: + PIOS_Assert(0); + break; + } + edge_event = true; + } else { + edge_event = false; + edge_count = 0; + } + + if (!tim_dev->callbacks) { + /* No callbacks registered, we're done with this channel */ + continue; + } + + /* Generate the appropriate callbacks */ + if (overflow_event & edge_event) { + /* + * When both edge and overflow happen in the same interrupt, we + * need a heuristic to determine the order of the edge and overflow + * events so that the callbacks happen in the right order. If we + * get the order wrong, our pulse width calculations could be off by up + * to ARR ticks. That could be bad. + * + * Heuristic: If the edge_count is < 32 ticks above zero then we assume the + * edge happened just after the overflow. + */ + + if (edge_count < 32) { + /* Call the overflow callback first */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + /* Call the edge callback second */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } else { + /* Call the edge callback first */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + /* Call the overflow callback second */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + } + } else if (overflow_event && tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } else if (edge_event && tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } + } +} + +/* Bind Interrupt Handlers + * + * Map all valid TIM IRQs to the common interrupt handler + * and give it enough context to properly demux the various timers + */ +void TIM1_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_CC_irq_handler"))); +static void PIOS_TIM_1_CC_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM1); +} + +// The rest of TIM1 interrupts are overlapped +void TIM1_BRK_TIM15_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_BRK_TIM_15_irq_handler"))); +static void PIOS_TIM_1_BRK_TIM_15_irq_handler(void) +{ + if (TIM_GetITStatus(TIM1, TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM15, TIM_IT_Update | TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_COM | TIM_IT_Trigger | TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM15); + } +} + +void TIM1_UP_TIM16_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_UP_TIM_16_irq_handler"))); +static void PIOS_TIM_1_UP_TIM_16_irq_handler(void) +{ + if (TIM_GetITStatus(TIM1, TIM_IT_Update)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM16, TIM_IT_Update | TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_COM | TIM_IT_Trigger | TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM16); + } +} +void TIM1_TRG_COM_TIM17_IRQHandler(void) __attribute__((alias("PIOS_TIM_1_TRG_COM_TIM_17_irq_handler"))); +static void PIOS_TIM_1_TRG_COM_TIM_17_irq_handler(void) +{ + if (TIM_GetITStatus(TIM1, TIM_IT_Trigger | TIM_IT_COM)) { + PIOS_TIM_generic_irq_handler(TIM1); + } else if (TIM_GetITStatus(TIM17, TIM_IT_Update | TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_COM | TIM_IT_Trigger | TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM17); + } +} + +void TIM2_IRQHandler(void) __attribute__((alias("PIOS_TIM_2_irq_handler"))); +static void PIOS_TIM_2_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM2); +} + +void TIM3_IRQHandler(void) __attribute__((alias("PIOS_TIM_3_irq_handler"))); +static void PIOS_TIM_3_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM3); +} + +void TIM4_IRQHandler(void) __attribute__((alias("PIOS_TIM_4_irq_handler"))); +static void PIOS_TIM_4_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM4); +} + +void TIM6_DAC_IRQHandler(void) __attribute__((alias("PIOS_TIM_6_DAC_irq_handler"))); +static void PIOS_TIM_6_DAC_irq_handler(void) +{ + // What about DAC irq? + if (TIM_GetITStatus(TIM6, TIM_IT_Update | TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_COM | TIM_IT_Trigger | TIM_IT_Break)) { + PIOS_TIM_generic_irq_handler(TIM6); + } +} + +void TIM7_IRQHandler(void) __attribute__((alias("PIOS_TIM_7_irq_handler"))); +static void PIOS_TIM_7_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM7); +} + +void TIM8_CC_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_CC_irq_handler"))); +static void PIOS_TIM_8_CC_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM8); +} + +void TIM8_BRK_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_BRK_irq_handler"))); +static void PIOS_TIM_8_BRK_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM8); +} + +void TIM8_UP_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_UP_irq_handler"))); +static void PIOS_TIM_8_UP_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM8); +} + +void TIM8_TRG_COM_IRQHandler(void) __attribute__((alias("PIOS_TIM_8_TRG_COM_irq_handler"))); +static void PIOS_TIM_8_TRG_COM_irq_handler(void) +{ + PIOS_TIM_generic_irq_handler(TIM8); +} + + +#endif /* PIOS_INCLUDE_TIM */ diff --git a/flight/pios/stm32f30x/pios_usart.c b/flight/pios/stm32f30x/pios_usart.c new file mode 100644 index 0000000000..554b092e9e --- /dev/null +++ b/flight/pios/stm32f30x/pios_usart.c @@ -0,0 +1,601 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USART USART Functions + * @brief PIOS interface for USART port + * @{ + * + * @file pios_usart.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) + * @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_USART + +#include + +/* Provide a COM driver */ +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_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); +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); +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_config = PIOS_USART_ChangeConfig, + .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 { + PIOS_USART_DEV_MAGIC = 0x11223344, +}; + +struct pios_usart_dev { + enum pios_usart_dev_magic magic; + const struct pios_usart_cfg *cfg; + USART_InitTypeDef init; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + bool config_locked; + uint32_t rx_dropped; + uint8_t irq_channel; +}; + +static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) +{ + return usart_dev->magic == PIOS_USART_DEV_MAGIC; +} + +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) +{ + struct pios_usart_dev *usart_dev; + + usart_dev = (struct pios_usart_dev *)pios_malloc(sizeof(struct pios_usart_dev)); + if (!usart_dev) { + return NULL; + } + + memset(usart_dev, 0, sizeof(struct pios_usart_dev)); + usart_dev->magic = PIOS_USART_DEV_MAGIC; + return usart_dev; +} +#else +static struct pios_usart_dev pios_usart_devs[PIOS_USART_MAX_DEVS]; +static uint8_t pios_usart_num_devs; +static struct pios_usart_dev *PIOS_USART_alloc(void) +{ + struct pios_usart_dev *usart_dev; + + if (pios_usart_num_devs >= PIOS_USART_MAX_DEVS) { + return NULL; + } + + usart_dev = &pios_usart_devs[pios_usart_num_devs++]; + + memset(usart_dev, 0, sizeof(struct pios_usart_dev)); + usart_dev->magic = PIOS_USART_DEV_MAGIC; + + return usart_dev; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + +/* Bind Interrupt Handlers + * + * Map all valid USART IRQs to the common interrupt handler + * and provide storage for a 32-bit device id IRQ to map + * each physical IRQ to a specific registered device instance. + */ +static void PIOS_USART_generic_irq_handler(uint32_t usart_id); + +static uint32_t PIOS_USART_1_id; +void USART1_EXTI25_IRQHandler(void) __attribute__((alias("PIOS_USART_1_irq_handler"))); +static void PIOS_USART_1_irq_handler(void) +{ + PIOS_USART_generic_irq_handler(PIOS_USART_1_id); +} + +static uint32_t PIOS_USART_2_id; +void USART2_EXTI26_IRQHandler(void) __attribute__((alias("PIOS_USART_2_irq_handler"))); +static void PIOS_USART_2_irq_handler(void) +{ + PIOS_USART_generic_irq_handler(PIOS_USART_2_id); +} + +static uint32_t PIOS_USART_3_id; +void USART3_EXTI28_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); +} + + +static uint32_t PIOS_UART_4_id; +void UART4_EXTI34_IRQHandler(void) __attribute__((alias("PIOS_UART_4_irq_handler"))); +static void PIOS_UART_4_irq_handler(void) +{ + PIOS_USART_generic_irq_handler(PIOS_UART_4_id); +} + +static uint32_t PIOS_UART_5_id; +void UART5_EXTI35_IRQHandler(void) __attribute__((alias("PIOS_UART_5_irq_handler"))); +static void PIOS_UART_5_irq_handler(void) +{ + PIOS_USART_generic_irq_handler(PIOS_UART_5_id); +} + +/** + * Initialise a single USART device + */ +int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) +{ + PIOS_DEBUG_Assert(usart_id); + PIOS_DEBUG_Assert(cfg); + + uint32_t *local_id; + uint8_t irq_channel; + + switch ((uint32_t)cfg->regs) { + case (uint32_t)USART1: + local_id = &PIOS_USART_1_id; + irq_channel = USART1_IRQn; + break; + case (uint32_t)USART2: + local_id = &PIOS_USART_2_id; + irq_channel = USART2_IRQn; + break; + case (uint32_t)USART3: + local_id = &PIOS_USART_3_id; + irq_channel = USART3_IRQn; + break; + case (uint32_t)UART4: + local_id = &PIOS_UART_4_id; + irq_channel = UART4_IRQn; + break; + case (uint32_t)UART5: + local_id = &PIOS_UART_5_id; + irq_channel = UART5_IRQn; + break; + default: + goto out_fail; + } + + if (*local_id) { + /* this port is already open */ + *usart_id = *local_id; + return 0; + } + + struct pios_usart_dev *usart_dev; + + usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); + if (!usart_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + usart_dev->cfg = cfg; + usart_dev->irq_channel = irq_channel; + + /* Set some sane defaults */ + USART_StructInit(&usart_dev->init); + + /* We will set modes later, depending on installed callbacks */ + usart_dev->init.USART_Mode = 0; + + /* DTR handling? */ + + *usart_id = (uint32_t)usart_dev; + *local_id = (uint32_t)usart_dev; + + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); + + /* Disable overrun detection */ + USART_OverrunDetectionConfig(usart_dev->cfg->regs, USART_OVRDetection_Disable); + + 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_PinAFConfig(usart_dev->cfg->rx.gpio, + usart_dev->cfg->rx.pin_source, + 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, + usart_dev->cfg->tx.pin_source, + usart_dev->cfg->remap); + } + + GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); + } + + /* Write new configuration */ +#if 0 + const char *dbg_parity = "?"; + switch (usart_dev->init.USART_Parity) { + case USART_Parity_No: dbg_parity = "N"; break; + case USART_Parity_Even: dbg_parity = "E"; break; + case USART_Parity_Odd: dbg_parity = "O"; break; + } + const char *dbg_mode = "???"; + switch (usart_dev->init.USART_Mode) { + case USART_Mode_Rx: dbg_mode = "rx"; break; + case USART_Mode_Tx: dbg_mode = "tx"; break; + case USART_Mode_Rx | USART_Mode_Tx: dbg_mode = "rx_tx"; break; + } + const char *dbg_flow_control = "???"; + switch (usart_dev->init.USART_HardwareFlowControl) { + case USART_HardwareFlowControl_None: dbg_flow_control = "none"; break; + case USART_HardwareFlowControl_RTS: dbg_flow_control = "rts"; break; + case USART_HardwareFlowControl_CTS: dbg_flow_control = "cts"; break; + case USART_HardwareFlowControl_RTS_CTS: dbg_flow_control = "rts_cts"; break; + } + const char *dbg_stop_bits = "???"; + switch (usart_dev->init.USART_StopBits) { + case USART_StopBits_1: dbg_stop_bits = "1"; break; + case USART_StopBits_2: dbg_stop_bits = "2"; break; + case USART_StopBits_1_5: dbg_stop_bits = "1.5"; break; + } + + DEBUG_PRINTF(0, "PIOS_USART_Setup: 0x%08x %u %u%s%s mode=%s flow_control=%s\r\n", + (uint32_t)usart_dev, + usart_dev->init.USART_BaudRate, + usart_dev->init.USART_WordLength == USART_WordLength_8b ? 8 : 9, + dbg_parity, + dbg_stop_bits, + dbg_mode, + dbg_flow_control); +#endif /* if 0 */ + { // fix parity stuff + USART_InitTypeDef init = usart_dev->init; + + if ((init.USART_Parity != USART_Parity_No) && (init.USART_WordLength == USART_WordLength_8b)) { + init.USART_WordLength = USART_WordLength_9b; + } + + USART_Init(usart_dev->cfg->regs, &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; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); +} +static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); +} + +/** + * Changes the baud rate of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ +static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + if (usart_dev->config_locked) { + return; + } + + usart_dev->init.USART_BaudRate = baud; + + PIOS_USART_Setup(usart_dev); +} + +/** + * Changes configuration of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] word_len Requested word length + * \param[in] stop_bits Requested stop bits + * \param[in] parity Requested parity + * + */ +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) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + if (usart_dev->config_locked) { + return; + } + + switch (word_len) { + case PIOS_COM_Word_length_8b: + usart_dev->init.USART_WordLength = USART_WordLength_8b; + break; + case PIOS_COM_Word_length_9b: + usart_dev->init.USART_WordLength = USART_WordLength_9b; + break; + default: + break; + } + + switch (stop_bits) { + case PIOS_COM_StopBits_1: + usart_dev->init.USART_StopBits = USART_StopBits_1; + break; + case PIOS_COM_StopBits_1_5: + usart_dev->init.USART_StopBits = USART_StopBits_1_5; + break; + case PIOS_COM_StopBits_2: + usart_dev->init.USART_StopBits = USART_StopBits_2; + break; + default: + break; + } + + switch (parity) { + case PIOS_COM_Parity_No: + usart_dev->init.USART_Parity = USART_Parity_No; + break; + case PIOS_COM_Parity_Even: + usart_dev->init.USART_Parity = USART_Parity_Even; + break; + case PIOS_COM_Parity_Odd: + usart_dev->init.USART_Parity = USART_Parity_Odd; + break; + default: + break; + } + + if (baud_rate) { + usart_dev->init.USART_BaudRate = baud_rate; + } + + PIOS_USART_Setup(usart_dev); +} + + +static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* + * 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_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) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* + * 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_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) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + /* Force read of dr after sr to make sure to clear error flags */ + volatile uint16_t sr = usart_dev->cfg->regs->ISR; + volatile uint8_t dr = usart_dev->cfg->regs->RDR; + + /* Check if RXNE flag is set */ + bool rx_need_yield = false; + if (sr & USART_ISR_RXNE) { + uint8_t byte = dr; + 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); + if (rc < 1) { + /* Lost bytes on rx */ + usart_dev->rx_dropped += 1; + } + } + } + + /* Check if TXE flag is set */ + bool tx_need_yield = false; + if (sr & USART_ISR_TXE) { + if (usart_dev->tx_out_cb) { + uint8_t b; + uint16_t bytes_to_send; + + bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + + if (bytes_to_send > 0) { + /* Send the byte we've been given */ + usart_dev->cfg->regs->TDR = b; + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } else { + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); + } + } + +#if defined(PIOS_INCLUDE_FREERTOS) + if (rx_need_yield || tx_need_yield) { + vPortYield(); + } +#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); + + /* some sort of locking would be great to have */ + + uint32_t cr1_ue = usart_dev->cfg->regs->CR1 & USART_CR1_UE; + + switch (ctl) { + case PIOS_IOCTL_USART_SET_IRQ_PRIO: + return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); + + case PIOS_IOCTL_USART_SET_INVERTED: + { + enum PIOS_USART_Inverted inverted = *(enum PIOS_USART_Inverted *)param; + + usart_dev->cfg->regs->CR1 &= ~((uint32_t)USART_CR1_UE); + + if (usart_dev->cfg->rx.gpio != 0) { + GPIO_InitTypeDef gpioInit = usart_dev->cfg->rx.init; + + gpioInit.GPIO_PuPd = (inverted & PIOS_USART_Inverted_Rx) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP; + + GPIO_Init(usart_dev->cfg->rx.gpio, &gpioInit); + + USART_InvPinCmd(usart_dev->cfg->regs, USART_InvPin_Rx, (inverted & PIOS_USART_Inverted_Rx) ? ENABLE : DISABLE); + } + + if (usart_dev->cfg->tx.gpio != 0) { + USART_InvPinCmd(usart_dev->cfg->regs, USART_InvPin_Tx, (inverted & PIOS_USART_Inverted_Tx) ? ENABLE : DISABLE); + } + } + break; + case PIOS_IOCTL_USART_SET_SWAPPIN: + usart_dev->cfg->regs->CR1 &= ~((uint32_t)USART_CR1_UE); + USART_SWAPPinCmd(usart_dev->cfg->regs, *(bool *)param ? ENABLE : DISABLE); + break; + case PIOS_IOCTL_USART_SET_HALFDUPLEX: + usart_dev->cfg->regs->CR1 &= ~((uint32_t)USART_CR1_UE); + USART_HalfDuplexCmd(usart_dev->cfg->regs, *(bool *)param ? ENABLE : DISABLE); + break; + case PIOS_IOCTL_USART_GET_DSMBIND: + 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; + case PIOS_IOCTL_USART_LOCK_CONFIG: + usart_dev->config_locked = *(bool *)param; + break; + default: + return -1; + } + + usart_dev->cfg->regs->CR1 |= cr1_ue; + + return 0; +} + +#endif /* PIOS_INCLUDE_USART */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_usb.c b/flight/pios/stm32f30x/pios_usb.c new file mode 100644 index 0000000000..f7a36e8251 --- /dev/null +++ b/flight/pios/stm32f30x/pios_usb.c @@ -0,0 +1,335 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB USB Setup Functions + * @brief PIOS USB device implementation + * @{ + * + * @file pios_usb.c + * @author The LibrePilot Project, http://www.librepilot.org (C) 2017. + * Tau Labs, http://taulabs.org, Copyright (C) 2012-2013 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief USB device functions (STM32 dependent code) + * @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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_USB) + +#include "usb_lib.h" +#include "pios_usb_board_data.h" + +#include "pios_usb.h" +#include "pios_usb_priv.h" + + +/* Rx/Tx status */ +static bool transfer_possible = false; + +#ifdef PIOS_INCLUDE_FREERTOS +struct { + void (*callback)(bool connected, uint32_t context); + uint32_t context; +} connectionState_cb_list[3]; +#endif + +/* USB activity detection */ +static volatile bool sof_seen_since_reset = false; + +enum pios_usb_dev_magic { + PIOS_USB_DEV_MAGIC = 0x17365904, +}; + +struct pios_usb_dev { + enum pios_usb_dev_magic magic; + const struct pios_usb_cfg *cfg; +}; +#ifdef PIOS_INCLUDE_FREERTOS +static void raiseConnectionStateCallback(bool connected); +#endif + +/** + * @brief Validate the usb device structure + * @returns 0 if valid device or -1 otherwise + */ +static int32_t PIOS_USB_validate(struct pios_usb_dev *usb_dev) +{ + if (usb_dev == NULL) { + return -1; + } + + if (usb_dev->magic != PIOS_USB_DEV_MAGIC) { + return -1; + } + + return 0; +} + +#ifdef PIOS_INCLUDE_FREERTOS +static struct pios_usb_dev *PIOS_USB_alloc(void) +{ + struct pios_usb_dev *usb_dev; + + usb_dev = (struct pios_usb_dev *)pios_malloc(sizeof(*usb_dev)); + if (!usb_dev) { + return NULL; + } + + usb_dev->magic = PIOS_USB_DEV_MAGIC; + return usb_dev; +} +#else +static struct pios_usb_dev pios_usb_devs[PIOS_USB_MAX_DEVS]; +static uint8_t pios_usb_num_devs; +static struct pios_usb_dev *PIOS_USB_alloc(void) +{ + struct pios_usb_dev *usb_dev; + + if (pios_usb_num_devs >= PIOS_USB_MAX_DEVS) { + return NULL; + } + + usb_dev = &pios_usb_devs[pios_usb_num_devs++]; + usb_dev->magic = PIOS_USB_DEV_MAGIC; + + return usb_dev; +} +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ + +/** + * Initialises USB COM layer + * \return < 0 if initialisation failed + * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions + */ +static uint32_t pios_usb_com_id; +int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg) +{ + PIOS_Assert(usb_id); + PIOS_Assert(cfg); + + struct pios_usb_dev *usb_dev; + + usb_dev = (struct pios_usb_dev *)PIOS_USB_alloc(); + if (!usb_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + usb_dev->cfg = cfg; + + PIOS_USB_Reenumerate(); + + /* + * This is a horrible hack to make this available to + * the interrupt callbacks. This should go away ASAP. + */ + pios_usb_com_id = (uintptr_t)usb_dev; + + /* Enable the USB Interrupts */ + NVIC_Init((NVIC_InitTypeDef *)&usb_dev->cfg->irq.init); + + /* Configure USB D-/D+ (DM/DP) pins */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(GPIOA, &GPIO_InitStructure); + + GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_14); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_14); + + /* Configure VBUS sense pin */ + if (usb_dev->cfg->vsense.gpio) { + GPIO_Init(usb_dev->cfg->vsense.gpio, (GPIO_InitTypeDef *)&usb_dev->cfg->vsense.init); + } + + /* Select USBCLK source */ + RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); + /* Enable the USB clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); + + USB_Init(); + USB_SIL_Init(); + + *usb_id = (uintptr_t)usb_dev; + + return 0; /* No error */ + +out_fail: + return -1; +} + +/** + * This function is called by the USB driver on cable connection/disconnection + * \param[in] connected connection status (1 if connected) + * \return < 0 on errors + * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions + */ +int32_t PIOS_USB_ChangeConnectionState(bool Connected) +{ + // In all cases: re-initialise USB HID driver + if (Connected) { + transfer_possible = true; + + // TODO: Check SetEPRxValid(ENDP1); + +#if defined(USB_LED_ON) + USB_LED_ON; // turn the USB led on +#endif + } else { + // Cable disconnected: disable transfers + transfer_possible = false; + +#if defined(USB_LED_OFF) + USB_LED_OFF; // turn the USB led off +#endif + } + +#ifdef PIOS_INCLUDE_FREERTOS + raiseConnectionStateCallback(Connected); +#endif + + return 0; +} + +int32_t PIOS_USB_Reenumerate() +{ + /* Force USB reset and power-down (this will also release the USB pins for direct GPIO control) */ + _SetCNTR(CNTR_FRES | CNTR_PDWN); + + /* Using a "dirty" method to force a re-enumeration: */ + /* Force DPM (Pin PA12) low for ca. 10 mS before USB Tranceiver will be enabled */ + /* This overrules the external Pull-Up at PA12, and at least Windows & MacOS will enumerate again */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(GPIOA, &GPIO_InitStructure); + + PIOS_DELAY_WaitmS(50); + + /* Release power-down, still hold reset */ + _SetCNTR(CNTR_PDWN); + PIOS_DELAY_WaituS(5); + + /* CNTR_FRES = 0 */ + _SetCNTR(0); + + /* Clear pending interrupts */ + _SetISTR(0); + + /* set back to alternate function */ + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_Init(GPIOA, &GPIO_InitStructure); + + /* Configure USB clock */ + /* USBCLK = PLLCLK / 1.5 */ + RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); + /* Enable USB clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); + + return 0; +} + +bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id) +{ + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_com_id; + + if (PIOS_USB_validate(usb_dev) != 0) { + return false; + } + + // If board is configured to have a VSENSE pin, use that + if (usb_dev->cfg->vsense.gpio != NULL) { + return GPIO_ReadInputDataBit(usb_dev->cfg->vsense.gpio, usb_dev->cfg->vsense.init.GPIO_Pin) == Bit_SET; + } + + return sof_seen_since_reset; +} + +/** + * This function returns the connection status of the USB HID interface + * \return 1: interface available + * \return 0: interface not available + * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions + */ +bool PIOS_USB_CheckAvailable(uint32_t id) +{ + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_com_id; + + if (PIOS_USB_validate(usb_dev) != 0) { + return false; + } + + return PIOS_USB_CableConnected(id) && transfer_possible; +} + +void SOF_Callback(void) +{ + sof_seen_since_reset = true; +} + +void SUSP_Callback(void) +{ + sof_seen_since_reset = false; +} + +#ifdef PIOS_INCLUDE_FREERTOS +void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context) +{ + PIOS_Assert(connectionStateCallback); + + for (uint32_t i = 0; i < NELEMENTS(connectionState_cb_list); i++) { + if (connectionState_cb_list[i].callback == NULL) { + connectionState_cb_list[i].callback = connectionStateCallback; + connectionState_cb_list[i].context = context; + return; + } + } + + PIOS_Assert(0); +} + +static void raiseConnectionStateCallback(bool connected) +{ + uint32_t i = 0; + + while (i < NELEMENTS(connectionState_cb_list) && connectionState_cb_list[i].callback != NULL) { + connectionState_cb_list[i].callback(connected, connectionState_cb_list[i].context); + i++; + } +} +#else /* PIOS_INCLUDE_FREERTOS */ +void PIOS_USB_RegisterConnectionStateCallback(__attribute__((unused)) void (*connectionStateCallback)(bool connected, uint32_t context), __attribute__((unused)) uint32_t context) +{} +#endif /* PIOS_INCLUDE_FREERTOS */ + +#endif /* if defined(PIOS_INCLUDE_USB) */ + +/** + * @} + * @} + */ diff --git a/flight/pios/stm32f30x/pios_usb_cdc.c b/flight/pios/stm32f30x/pios_usb_cdc.c new file mode 100644 index 0000000000..186fb95b48 --- /dev/null +++ b/flight/pios/stm32f30x/pios_usb_cdc.c @@ -0,0 +1,526 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_COM USB COM Functions + * @brief PIOS USB COM implementation for CDC interfaces + * @notes This implements a CDC Serial Port + * @{ + * + * @file pios_usb_cdc.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief USB COM functions (STM32 dependent code) + * @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_USB_CDC + +#include "pios_usb_cdc_priv.h" +#include "pios_usb_board_data.h" /* PIOS_BOARD_*_DATA_LENGTH */ + +/* STM32 USB Library Definitions */ +#include "usb_lib.h" + +static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_USB_CDC_RegisterBaudRateCallback(uint32_t usbcdc_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context); +static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available baud_rate_cb, uint32_t context); +static void PIOS_USB_CDC_ChangeConnectionState(bool connected, uint32_t usbcdc_id); +static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); +static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); +static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id); + +const struct pios_com_driver pios_usb_cdc_com_driver = { + .tx_start = PIOS_USB_CDC_TxStart, + .rx_start = PIOS_USB_CDC_RxStart, + .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .bind_baud_rate_cb = PIOS_USB_CDC_RegisterBaudRateCallback, + .available = PIOS_USB_CDC_Available, + .bind_available_cb = PIOS_USB_CDC_RegisterAvailableCallback, +}; + +enum pios_usb_cdc_dev_magic { + PIOS_USB_CDC_DEV_MAGIC = 0xAABBCCDD, +}; + +struct pios_usb_cdc_dev { + enum pios_usb_cdc_dev_magic magic; + const struct pios_usb_cdc_cfg *cfg; + + uint32_t lower_id; + + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + + pios_com_callback_baud_rate baud_rate_cb; + uint32_t baud_rate_context; + pios_com_callback_available available_cb; + uint32_t available_context; + + uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH]; + /* + * NOTE: This is -1 as somewhat of a hack. It ensures that we always send packets + * that are strictly < maxPacketSize for this interface which means we never have + * to bother with zero length packets (ZLP). + */ + uint8_t tx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH - 1]; + + uint32_t rx_dropped; + uint32_t rx_oversize; +}; + +static bool PIOS_USB_CDC_validate(struct pios_usb_cdc_dev *usb_cdc_dev) +{ + return usb_cdc_dev->magic == PIOS_USB_CDC_DEV_MAGIC; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) +{ + struct pios_usb_cdc_dev *usb_cdc_dev; + + usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_fastheapmalloc(sizeof(struct pios_usb_cdc_dev)); + if (!usb_cdc_dev) { + return NULL; + } + + memset(usb_cdc_dev, 0, sizeof(struct pios_usb_cdc_dev)); + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + return usb_cdc_dev; +} +#else +static struct pios_usb_cdc_dev pios_usb_cdc_devs[PIOS_USB_CDC_MAX_DEVS]; +static uint8_t pios_usb_cdc_num_devs; +static struct pios_usb_cdc_dev *PIOS_USB_CDC_alloc(void) +{ + struct pios_usb_cdc_dev *usb_cdc_dev; + + if (pios_usb_cdc_num_devs >= PIOS_USB_CDC_MAX_DEVS) { + return NULL; + } + + usb_cdc_dev = &pios_usb_cdc_devs[pios_usb_cdc_num_devs++]; + + memset(usb_cdc_dev, 0, sizeof(struct pios_usb_cdc_dev)); + usb_cdc_dev->magic = PIOS_USB_CDC_DEV_MAGIC; + + return usb_cdc_dev; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + +static void PIOS_USB_CDC_DATA_EP_IN_Callback(void); +static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void); +static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void); + +static uint32_t pios_usb_cdc_id; + +/* Need a better way to pull these in */ +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); + +int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cfg, uint32_t lower_id) +{ + PIOS_Assert(usbcdc_id); + PIOS_Assert(cfg); + + struct pios_usb_cdc_dev *usb_cdc_dev; + + usb_cdc_dev = (struct pios_usb_cdc_dev *)PIOS_USB_CDC_alloc(); + if (!usb_cdc_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + usb_cdc_dev->cfg = cfg; + usb_cdc_dev->lower_id = lower_id; + + pios_usb_cdc_id = (uint32_t)usb_cdc_dev; + + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_OUT[cfg->ctrl_tx_ep - 1] = PIOS_USB_CDC_CTRL_EP_IN_Callback; + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_CDC_DATA_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback; + + PIOS_USB_RegisterConnectionStateCallback(PIOS_USB_CDC_ChangeConnectionState, (uint32_t)usb_cdc_dev); + + *usbcdc_id = (uint32_t)usb_cdc_dev; + + return 0; + +out_fail: + return -1; +} + + +static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->rx_in_context = context; + usb_cdc_dev->rx_in_cb = rx_in_cb; +} + +static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->tx_out_context = context; + usb_cdc_dev->tx_out_cb = tx_out_cb; +} + +static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= sizeof(usb_cdc_dev->rx_packet_buffer))) { + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); +} + +static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev *usb_cdc_dev) +{ + uint16_t bytes_to_tx; + + if (!usb_cdc_dev->tx_out_cb) { + return; + } + + bool need_yield = false; + bytes_to_tx = (usb_cdc_dev->tx_out_cb)(usb_cdc_dev->tx_out_context, + usb_cdc_dev->tx_packet_buffer, + sizeof(usb_cdc_dev->tx_packet_buffer), + NULL, + &need_yield); + if (bytes_to_tx == 0) { + return; + } + + UserToPMABufferCopy(usb_cdc_dev->tx_packet_buffer, + GetEPTxAddr(usb_cdc_dev->cfg->data_tx_ep), + bytes_to_tx); + SetEPTxCount(usb_cdc_dev->cfg->data_tx_ep, bytes_to_tx); + SetEPTxValid(usb_cdc_dev->cfg->data_tx_ep); + +#if defined(PIOS_INCLUDE_FREERTOS) + if (need_yield) { + vPortYield(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ +} + +static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, __attribute__((unused)) uint16_t tx_bytes_avail) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + if (!PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id)) { + return; + } + + if (GetEPTxStatus(usb_cdc_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_CDC_SendData(usb_cdc_dev); +} + +static void PIOS_USB_CDC_DATA_EP_IN_Callback(void) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + PIOS_USB_CDC_SendData(usb_cdc_dev); +} + +static void PIOS_USB_CDC_DATA_EP_OUT_Callback(void) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + uint32_t DataLength; + + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_cdc_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_cdc_dev->rx_packet_buffer)) { + usb_cdc_dev->rx_oversize++; + DataLength = sizeof(usb_cdc_dev->rx_packet_buffer); + } + + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_cdc_dev->rx_packet_buffer, + GetEPRxAddr(usb_cdc_dev->cfg->data_rx_ep), + DataLength); + + if (!usb_cdc_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } + + uint16_t headroom; + bool need_yield = false; + uint16_t rc; + rc = (usb_cdc_dev->rx_in_cb)(usb_cdc_dev->rx_in_context, + usb_cdc_dev->rx_packet_buffer, + DataLength, + &headroom, + &need_yield); + + if (rc < DataLength) { + /* Lost bytes on rx */ + usb_cdc_dev->rx_dropped += (DataLength - rc); + } + + if (headroom >= sizeof(usb_cdc_dev->rx_packet_buffer)) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_cdc_dev->cfg->data_rx_ep, EP_RX_NAK); + } + +#if defined(PIOS_INCLUDE_FREERTOS) + if (need_yield) { + vPortYield(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ +} + +static uint16_t control_line_state; +RESULT PIOS_USB_CDC_SetControlLineState(void) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + if (!valid) { + /* No CDC interface is configured */ + return USB_UNSUPPORT; + } + + uint8_t wValue0 = pInformation->USBwValue0; + uint8_t wValue1 = pInformation->USBwValue1; + + control_line_state = wValue1 << 8 | wValue0; + + return USB_SUCCESS; +} + +static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + return PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE; +} + +static struct usb_cdc_line_coding line_coding = { + .dwDTERate = htousbl(57600), + .bCharFormat = USB_CDC_LINE_CODING_STOP_1, + .bParityType = USB_CDC_LINE_CODING_PARITY_NONE, + .bDataBits = 8, +}; + +uint8_t *PIOS_USB_CDC_SetLineCoding(uint16_t Length) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + if (!valid) { + /* No CDC interface is configured */ + return NULL; + } + + if (Length == 0) { + /* Report the number of bytes we're prepared to consume */ + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + pInformation->Ctrl_Info.Usb_rLength = sizeof(line_coding); + return NULL; + } else { + /* Give out a pointer to the data struct */ + return (uint8_t *)&line_coding; + } +} + +const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + if (!valid) { + /* No CDC interface is configured */ + return NULL; + } + + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = sizeof(line_coding); + return NULL; + } else { + return (uint8_t *)&line_coding; + } +} + +struct usb_cdc_serial_state_report uart_state = { + .bmRequestType = 0xA1, + .bNotification = USB_CDC_NOTIFICATION_SERIAL_STATE, + .wValue = 0, + .wIndex = htousbs(1), + .wLength = htousbs(2), + .bmUartState = htousbs(0), +}; + +static void PIOS_USB_CDC_CTRL_EP_IN_Callback(void) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* Give back UART State Bitmap */ + /* UART State Bitmap + * 15-7: reserved + * 6: bOverRun overrun error + * 5: bParity parity error + * 4: bFraming framing error + * 3: bRingSignal RI + * 2: bBreak break reception + * 1: bTxCarrier DSR + * 0: bRxCarrier DCD + */ + uart_state.bmUartState = htousbs(0x0003); + + UserToPMABufferCopy((uint8_t *)&uart_state, + GetEPTxAddr(usb_cdc_dev->cfg->ctrl_tx_ep), + sizeof(uart_state)); + SetEPTxCount(usb_cdc_dev->cfg->ctrl_tx_ep, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPTxValid(usb_cdc_dev->cfg->ctrl_tx_ep); +} + +static void PIOS_USB_CDC_RegisterBaudRateCallback(uint32_t usbcdc_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->baud_rate_context = context; + usb_cdc_dev->baud_rate_cb = baud_rate_cb; +} + +void PIOS_USB_CDC_SetLineCoding_Completed() +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + if (!valid) { + /* No CDC interface is configured */ + return; + } + + if (usb_cdc_dev->baud_rate_cb) { + usb_cdc_dev->baud_rate_cb(usb_cdc_dev->baud_rate_context, usbltoh(line_coding.dwDTERate)); + } +} + +static void PIOS_USB_CDC_ChangeConnectionState(__attribute__((unused)) bool connected, uint32_t usbcdc_id) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + if (usb_cdc_dev->available_cb) { + (usb_cdc_dev->available_cb)(usb_cdc_dev->available_context, PIOS_USB_CDC_Available(usbcdc_id)); + } +} + +static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available available_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->available_context = context; + usb_cdc_dev->available_cb = available_cb; +} + +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f30x/pios_usb_hid.c b/flight/pios/stm32f30x/pios_usb_hid.c new file mode 100644 index 0000000000..4b819cb15a --- /dev/null +++ b/flight/pios/stm32f30x/pios_usb_hid.c @@ -0,0 +1,380 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_HID USB COM Functions + * @brief PIOS USB COM implementation for HID interfaces + * @notes This implements serial emulation over HID reports + * @{ + * + * @file pios_usb_hid.c + * @author The LibrePilot Project, htpp://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief USB COM functions (STM32 dependent code) + * @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_USB_HID + +#include "pios_usb_hid_priv.h" +#include "pios_usb_board_data.h" /* PIOS_BOARD_*_DATA_LENGTH */ + +/* STM32 USB Library Definitions */ +#include "usb_lib.h" + +static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail); +static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail); +static uint32_t PIOS_USB_HID_Available(uint32_t usbhid_id); + +const struct pios_com_driver pios_usb_hid_com_driver = { + .tx_start = PIOS_USB_HID_TxStart, + .rx_start = PIOS_USB_HID_RxStart, + .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_HID_Available, +}; + +enum pios_usb_hid_dev_magic { + PIOS_USB_HID_DEV_MAGIC = 0xAA00BB00, +}; + +struct pios_usb_hid_dev { + enum pios_usb_hid_dev_magic magic; + const struct pios_usb_hid_cfg *cfg; + + uint32_t lower_id; + + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + + uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH]; + + uint32_t rx_dropped; + uint32_t rx_oversize; +}; + +static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev *usb_hid_dev) +{ + return usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC; +} + +#ifdef PIOS_INCLUDE_FREERTOS +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) +{ + struct pios_usb_hid_dev *usb_hid_dev; + + usb_hid_dev = (struct pios_usb_hid_dev *)pios_fastheapmalloc(sizeof(struct pios_usb_hid_dev)); + if (!usb_hid_dev) { + return NULL; + } + + memset(usb_hid_dev, 0, sizeof(struct pios_usb_hid_dev)); + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + return usb_hid_dev; +} +#else +static struct pios_usb_hid_dev pios_usb_hid_devs[PIOS_USB_HID_MAX_DEVS]; +static uint8_t pios_usb_hid_num_devs; +static struct pios_usb_hid_dev *PIOS_USB_HID_alloc(void) +{ + struct pios_usb_hid_dev *usb_hid_dev; + + if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { + return NULL; + } + + usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; + + memset(usb_hid_dev, 0, sizeof(struct pios_usb_hid_dev)); + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + + return usb_hid_dev; +} +#endif /* ifdef PIOS_INCLUDE_FREERTOS */ + +static void PIOS_USB_HID_EP_IN_Callback(void); +static void PIOS_USB_HID_EP_OUT_Callback(void); + +static uint32_t pios_usb_hid_id; + +/* Need a better way to pull these in */ +extern void(*pEpInt_IN[7]) (void); +extern void(*pEpInt_OUT[7]) (void); + +int32_t PIOS_USB_HID_Init(uint32_t *usbhid_id, const struct pios_usb_hid_cfg *cfg, uint32_t lower_id) +{ + PIOS_Assert(usbhid_id); + PIOS_Assert(cfg); + + struct pios_usb_hid_dev *usb_hid_dev; + + usb_hid_dev = (struct pios_usb_hid_dev *)PIOS_USB_HID_alloc(); + if (!usb_hid_dev) { + goto out_fail; + } + + /* Bind the configuration to the device instance */ + usb_hid_dev->cfg = cfg; + usb_hid_dev->lower_id = lower_id; + + pios_usb_hid_id = (uint32_t)usb_hid_dev; + + /* Bind lower level callbacks into the USB infrastructure */ + pEpInt_IN[cfg->data_tx_ep - 1] = PIOS_USB_HID_EP_IN_Callback; + pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_HID_EP_OUT_Callback; + + *usbhid_id = (uint32_t)usb_hid_dev; + + return 0; + +out_fail: + return -1; +} + + +static void PIOS_USB_HID_SendReport(struct pios_usb_hid_dev *usb_hid_dev) +{ + uint16_t bytes_to_tx; + + if (!usb_hid_dev->tx_out_cb) { + return; + } + + bool need_yield = false; +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[1], + sizeof(usb_hid_dev->tx_packet_buffer) - 1, + NULL, + &need_yield); +#else + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[2], + sizeof(usb_hid_dev->tx_packet_buffer) - 2, + NULL, + &need_yield); +#endif + if (bytes_to_tx == 0) { + return; + } + + /* Always set type as report ID */ + usb_hid_dev->tx_packet_buffer[0] = 1; + +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, + GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), + bytes_to_tx + 1); +#else + usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, + GetEPTxAddr(usb_hid_dev->cfg->data_tx_ep), + bytes_to_tx + 2); +#endif + /* Is this correct? Why do we always send the whole buffer? */ + SetEPTxCount(usb_hid_dev->cfg->data_tx_ep, sizeof(usb_hid_dev->tx_packet_buffer)); + SetEPTxValid(usb_hid_dev->cfg->data_tx_ep); + +#ifdef PIOS_INCLUDE_FREERTOS + if (need_yield) { + vPortYield(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ +} + +static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + + PIOS_Assert(valid); + + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + // If endpoint was stalled and there is now space make it valid +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; +#else + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; +#endif + + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(usb_hid_dev->cfg->data_rx_ep) != EP_RX_VALID) && + (rx_bytes_avail >= max_payload_length)) { + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); + } + PIOS_IRQ_Enable(); +} + +static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + + PIOS_Assert(valid); + + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + if (GetEPTxStatus(usb_hid_dev->cfg->data_tx_ep) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); +} + +static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->rx_in_context = context; + usb_hid_dev->rx_in_cb = rx_in_cb; +} + +static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->tx_out_context = context; + usb_hid_dev->tx_out_cb = tx_out_cb; +} + +/** + * @brief Callback used to indicate a transmission from device INto host completed + * Checks if any data remains, pads it into HID packet and sends. + */ +static void PIOS_USB_HID_EP_IN_Callback(void) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + + PIOS_Assert(valid); + + if (!PIOS_USB_CheckAvailable(usb_hid_dev->lower_id)) { + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); +} + +/** + * EP1 OUT Callback Routine + */ +static void PIOS_USB_HID_EP_OUT_Callback(void) +{ + struct pios_usb_hid_dev *usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + + PIOS_Assert(valid); + + uint32_t DataLength; + + /* Read received data (63 bytes) */ + /* Get the number of received data on the selected Endpoint */ + DataLength = GetEPRxCount(usb_hid_dev->cfg->data_rx_ep); + if (DataLength > sizeof(usb_hid_dev->rx_packet_buffer)) { + DataLength = sizeof(usb_hid_dev->rx_packet_buffer); + } + + /* Use the memory interface function to read from the selected endpoint */ + PMAToUserBufferCopy((uint8_t *)usb_hid_dev->rx_packet_buffer, + GetEPRxAddr(usb_hid_dev->cfg->data_rx_ep), + DataLength); + + if (!usb_hid_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); + return; + } + + /* The first byte is report ID (not checked), the second byte is the valid data length */ + uint16_t headroom; + bool need_yield = false; +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[1], + sizeof(usb_hid_dev->rx_packet_buffer) - 1, + &headroom, + &need_yield); +#else + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[2], + usb_hid_dev->rx_packet_buffer[1], + &headroom, + &need_yield); +#endif + +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; +#else + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; +#endif + + if (headroom >= max_payload_length) { + /* We have room for a maximum length message */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); + } else { + /* Not enough room left for a message, apply backpressure */ + SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_NAK); + } + +#ifdef PIOS_INCLUDE_FREERTOS + if (need_yield) { + vPortYield(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ +} + +static uint32_t PIOS_USB_HID_Available(uint32_t usbhid_id) +{ + return PIOS_USB_CheckAvailable(usbhid_id) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE; +} + +#endif /* PIOS_INCLUDE_USB_HID */ diff --git a/flight/pios/stm32f30x/pios_usb_hid_istr.c b/flight/pios/stm32f30x/pios_usb_hid_istr.c new file mode 100644 index 0000000000..99965333ff --- /dev/null +++ b/flight/pios/stm32f30x/pios_usb_hid_istr.c @@ -0,0 +1,336 @@ +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** +* File Name : usb_istr.c +* Author : MCD Application Team +* Version : V3.2.1 +* Date : 07/05/2010 +* Description : ISTR events interrupt service routines +******************************************************************************** +* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS +* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. +* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, +* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE +* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING +* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +*******************************************************************************/ + +/* Includes ------------------------------------------------------------------*/ +#include "pios.h" + +#ifdef PIOS_INCLUDE_USB + +#include "usb_lib.h" +#include "pios_usb_hid_pwr.h" +#include "pios_usb_hid_istr.h" +#include "pios_usb_hid.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +__IO uint16_t wIstr; /* ISTR register last read value */ +__IO uint8_t bIntPackSOF = 0; /* SOFs received between 2 consecutive packets */ + +/* Extern variables ----------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ +/* function pointers to non-control endpoints service routines */ +void(*pEpInt_IN[7]) (void) = { + EP1_IN_Callback, EP2_IN_Callback, EP3_IN_Callback, EP4_IN_Callback, EP5_IN_Callback, EP6_IN_Callback, EP7_IN_Callback, +}; + +void(*pEpInt_OUT[7]) (void) = { + EP1_OUT_Callback, EP2_OUT_Callback, EP3_OUT_Callback, EP4_OUT_Callback, EP5_OUT_Callback, EP6_OUT_Callback, EP7_OUT_Callback, +}; + +#ifndef STM32F10X_CL + +/******************************************************************************* +* Function Name : USB_Istr +* Description : STR events interrupt service routine +* Input : +* Output : +* Return : +*******************************************************************************/ +void USB_LP_CAN1_RX0_IRQHandler(void) // USB_Istr(void) +{ + wIstr = _GetISTR(); + +#if (IMR_MSK & ISTR_CTR) + if (wIstr & ISTR_CTR & wInterrupt_Mask) { + /* servicing of the endpoint correct transfer interrupt */ + /* clear of the CTR flag into the sub */ + CTR_LP(); +#ifdef CTR_CALLBACK + CTR_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_RESET) + if (wIstr & ISTR_RESET & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_RESET); + Device_Property.Reset(); +#ifdef RESET_CALLBACK + RESET_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_DOVR) + if (wIstr & ISTR_DOVR & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_DOVR); +#ifdef DOVR_CALLBACK + DOVR_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_ERR) + if (wIstr & ISTR_ERR & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_ERR); +#ifdef ERR_CALLBACK + ERR_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_WKUP) + if (wIstr & ISTR_WKUP & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_WKUP); + Resume(RESUME_EXTERNAL); +#ifdef WKUP_CALLBACK + WKUP_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_SUSP) + if (wIstr & ISTR_SUSP & wInterrupt_Mask) { + /* check if SUSPEND is possible */ + if (fSuspendEnabled) { + Suspend(); + } else { + /* if not possible then resume after xx ms */ + Resume(RESUME_LATER); + } + /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */ + _SetISTR((uint16_t)CLR_SUSP); +#ifdef SUSP_CALLBACK + SUSP_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_SOF) + if (wIstr & ISTR_SOF & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_SOF); + bIntPackSOF++; + +#ifdef SOF_CALLBACK + SOF_Callback(); +#endif + } +#endif + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#if (IMR_MSK & ISTR_ESOF) + if (wIstr & ISTR_ESOF & wInterrupt_Mask) { + _SetISTR((uint16_t)CLR_ESOF); + /* resume handling timing is made with ESOFs */ + Resume(RESUME_ESOF); /* request without change of the machine state */ + +#ifdef ESOF_CALLBACK + ESOF_Callback(); +#endif + } +#endif +} /* USB_Istr */ + +/*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ +#else /* STM32F10X_CL */ + +/******************************************************************************* +* Function Name : STM32_PCD_OTG_ISR_Handler +* Description : Handles all USB Device Interrupts +* Input : None +* Output : None +* Return : status +*******************************************************************************/ +u32 STM32_PCD_OTG_ISR_Handler(void) +{ + USB_OTG_GINTSTS_TypeDef gintr_status; + u32 retval = 0; + + if (USBD_FS_IsDeviceMode()) { /* ensure that we are in device mode */ + gintr_status.d32 = OTGD_FS_ReadCoreItr(); + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + + /* If there is no interrupt pending exit the interrupt routine */ + if (!gintr_status.d32) { + return 0; + } + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Early Suspend interrupt */ +#ifdef INTR_ERLYSUSPEND + if (gintr_status.b.erlysuspend) { + retval |= OTGD_FS_Handle_EarlySuspend_ISR(); + } +#endif /* INTR_ERLYSUSPEND */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* End of Periodic Frame interrupt */ +#ifdef INTR_EOPFRAME + if (gintr_status.b.eopframe) { + retval |= OTGD_FS_Handle_EOPF_ISR(); + } +#endif /* INTR_EOPFRAME */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Non Periodic Tx FIFO Emty interrupt */ +#ifdef INTR_NPTXFEMPTY + if (gintr_status.b.nptxfempty) { + retval |= OTGD_FS_Handle_NPTxFE_ISR(); + } +#endif /* INTR_NPTXFEMPTY */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Wakeup or RemoteWakeup interrupt */ +#ifdef INTR_WKUPINTR + if (gintr_status.b.wkupintr) { + retval |= OTGD_FS_Handle_Wakeup_ISR(); + } +#endif /* INTR_WKUPINTR */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Suspend interrupt */ +#ifdef INTR_USBSUSPEND + if (gintr_status.b.usbsuspend) { + /* check if SUSPEND is possible */ + if (fSuspendEnabled) { + Suspend(); + } else { + /* if not possible then resume after xx ms */ + Resume(RESUME_LATER); /* This case shouldn't happen in OTG Device mode because + there's no ESOF interrupt to increment the ResumeS.bESOFcnt in the Resume state machine */ + } + + retval |= OTGD_FS_Handle_USBSuspend_ISR(); + } +#endif /* INTR_USBSUSPEND */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Start of Frame interrupt */ +#ifdef INTR_SOFINTR + if (gintr_status.b.sofintr) { + /* Update the frame number variable */ + bIntPackSOF++; + + retval |= OTGD_FS_Handle_Sof_ISR(); + } +#endif /* INTR_SOFINTR */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Receive FIFO Queue Status Level interrupt */ +#ifdef INTR_RXSTSQLVL + if (gintr_status.b.rxstsqlvl) { + retval |= OTGD_FS_Handle_RxStatusQueueLevel_ISR(); + } +#endif /* INTR_RXSTSQLVL */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Enumeration Done interrupt */ +#ifdef INTR_ENUMDONE + if (gintr_status.b.enumdone) { + retval |= OTGD_FS_Handle_EnumDone_ISR(); + } +#endif /* INTR_ENUMDONE */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Reset interrutp */ +#ifdef INTR_USBRESET + if (gintr_status.b.usbreset) { + retval |= OTGD_FS_Handle_UsbReset_ISR(); + } +#endif /* INTR_USBRESET */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* IN Endpoint interrupt */ +#ifdef INTR_INEPINTR + if (gintr_status.b.inepint) { + retval |= OTGD_FS_Handle_InEP_ISR(); + } +#endif /* INTR_INEPINTR */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* OUT Endpoint interrupt */ +#ifdef INTR_OUTEPINTR + if (gintr_status.b.outepintr) { + retval |= OTGD_FS_Handle_OutEP_ISR(); + } +#endif /* INTR_OUTEPINTR */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Mode Mismatch interrupt */ +#ifdef INTR_MODEMISMATCH + if (gintr_status.b.modemismatch) { + retval |= OTGD_FS_Handle_ModeMismatch_ISR(); + } +#endif /* INTR_MODEMISMATCH */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Global IN Endpoints NAK Effective interrupt */ +#ifdef INTR_GINNAKEFF + if (gintr_status.b.ginnakeff) { + retval |= OTGD_FS_Handle_GInNakEff_ISR(); + } +#endif /* INTR_GINNAKEFF */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Global OUT Endpoints NAK effective interrupt */ +#ifdef INTR_GOUTNAKEFF + if (gintr_status.b.goutnakeff) { + retval |= OTGD_FS_Handle_GOutNakEff_ISR(); + } +#endif /* INTR_GOUTNAKEFF */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Isochrounous Out packet Dropped interrupt */ +#ifdef INTR_ISOOUTDROP + if (gintr_status.b.isooutdrop) { + retval |= OTGD_FS_Handle_IsoOutDrop_ISR(); + } +#endif /* INTR_ISOOUTDROP */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Endpoint Mismatch error interrupt */ +#ifdef INTR_EPMISMATCH + if (gintr_status.b.epmismatch) { + retval |= OTGD_FS_Handle_EPMismatch_ISR(); + } +#endif /* INTR_EPMISMATCH */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Incomplete Isochrous IN tranfer error interrupt */ +#ifdef INTR_INCOMPLISOIN + if (gintr_status.b.incomplisoin) { + retval |= OTGD_FS_Handle_IncomplIsoIn_ISR(); + } +#endif /* INTR_INCOMPLISOIN */ + + /*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*/ + /* Incomplete Isochrous OUT tranfer error interrupt */ +#ifdef INTR_INCOMPLISOOUT + if (gintr_status.b.outepintr) { + retval |= OTGD_FS_Handle_IncomplIsoOut_ISR(); + } +#endif /* INTR_INCOMPLISOOUT */ + } + return retval; +} + +#endif /* STM32F10X_CL */ + +#endif /* PIOS_INCLUDE_USB */ + +/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/pios_usb_hid_pwr.c b/flight/pios/stm32f30x/pios_usb_hid_pwr.c new file mode 100644 index 0000000000..7a028a91de --- /dev/null +++ b/flight/pios/stm32f30x/pios_usb_hid_pwr.c @@ -0,0 +1,292 @@ +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** +* File Name : usb_pwr.c +* Author : MCD Application Team +* Version : V3.2.1 +* Date : 07/05/2010 +* Description : Connection/disconnection & power management +******************************************************************************** +* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS +* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. +* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, +* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE +* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING +* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +*******************************************************************************/ + +/* Includes ------------------------------------------------------------------*/ +#include "pios.h" +#include "stm32f30x.h" +#include "usb_lib.h" +#include "usb_conf.h" +#include "pios_usb_hid_pwr.h" +#include "pios_usb_hid.h" + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +__IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ +__IO bool fSuspendEnabled = true; /* true when suspend is possible */ + +struct { + __IO RESUME_STATE eState; + __IO uint8_t bESOFcnt; +} ResumeS; + +/* Extern variables ----------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Extern function prototypes ------------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************* +* Function Name : USB_Cable_Config. +* Description : Software Connection/Disconnection of USB Cable. +* Input : NewState: new state. +* Output : None. +* Return : None +*******************************************************************************/ +void USB_Cable_Config(FunctionalState __attribute__((unused)) NewState) +{} + +/******************************************************************************* +* Function Name : PowerOn +* Description : +* Input : None. +* Output : None. +* Return : USB_SUCCESS. +*******************************************************************************/ +RESULT PowerOn(void) +{ +#ifndef STM32F10X_CL + uint16_t wRegVal; + + /*** cable plugged-in ? ***/ + USB_Cable_Config(ENABLE); + + /*** CNTR_PWDN = 0 ***/ + wRegVal = CNTR_FRES; + _SetCNTR(wRegVal); + + /*** CNTR_FRES = 0 ***/ + wInterrupt_Mask = 0; + _SetCNTR(wInterrupt_Mask); + /*** Clear pending interrupts ***/ + _SetISTR(0); + /*** Set interrupt mask ***/ + wInterrupt_Mask = CNTR_RESETM | CNTR_SUSPM | CNTR_WKUPM; + _SetCNTR(wInterrupt_Mask); +#endif /* STM32F10X_CL */ + + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : PowerOff +* Description : handles switch-off conditions +* Input : None. +* Output : None. +* Return : USB_SUCCESS. +*******************************************************************************/ +RESULT PowerOff() +{ +#ifndef STM32F10X_CL + /* disable all ints and force USB reset */ + _SetCNTR(CNTR_FRES); + /* clear interrupt status register */ + _SetISTR(0); + /* Disable the Pull-Up */ + USB_Cable_Config(DISABLE); + /* switch-off device */ + _SetCNTR(CNTR_FRES + CNTR_PDWN); +#endif /* STM32F10X_CL */ + + /* sw variables reset */ + /* ... */ + + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : Enter_LowPowerMode. +* Description : Power-off system clocks and power while entering suspend mode. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void Enter_LowPowerMode(void) +{ + /* Set the device state to suspend */ + bDeviceState = SUSPENDED; +} + +/******************************************************************************* +* Function Name : Suspend +* Description : sets suspend mode operating conditions +* Input : None. +* Output : None. +* Return : USB_SUCCESS. +*******************************************************************************/ +void Suspend(void) +{ +#ifndef STM32F10X_CL + uint16_t wCNTR; + /* suspend preparation */ + /* ... */ + + /* macrocell enters suspend mode */ + wCNTR = _GetCNTR(); + wCNTR |= CNTR_FSUSP; + _SetCNTR(wCNTR); +#endif /* STM32F10X_CL */ + + /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ + /* power reduction */ + /* ... on connected devices */ + +#ifndef STM32F10X_CL + /* force low-power mode in the macrocell */ + wCNTR = _GetCNTR(); + wCNTR |= CNTR_LPMODE; + _SetCNTR(wCNTR); +#endif /* STM32F10X_CL */ + + /* switch-off the clocks */ + /* ... */ + Enter_LowPowerMode(); +} + +/******************************************************************************* +* Function Name : Leave_LowPowerMode. +* Description : Restores system clocks and power while exiting suspend mode. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +void Leave_LowPowerMode(void) +{ + DEVICE_INFO *pInfo = &Device_Info; + + /* Set the device state to the correct state */ + if (pInfo->Current_Configuration != 0) { + /* Device configured */ + bDeviceState = CONFIGURED; + } else { + bDeviceState = ATTACHED; + } +} + +/******************************************************************************* +* Function Name : Resume_Init +* Description : Handles wake-up restoring normal operations +* Input : None. +* Output : None. +* Return : USB_SUCCESS. +*******************************************************************************/ +void Resume_Init(void) +{ +#ifndef STM32F10X_CL + uint16_t wCNTR; +#endif /* STM32F10X_CL */ + + /* ------------------ ONLY WITH BUS-POWERED DEVICES ---------------------- */ + /* restart the clocks */ + /* ... */ + +#ifndef STM32F10X_CL + /* CNTR_LPMODE = 0 */ + wCNTR = _GetCNTR(); + wCNTR &= (~CNTR_LPMODE); + _SetCNTR(wCNTR); +#endif /* STM32F10X_CL */ + + /* restore full power */ + /* ... on connected devices */ + Leave_LowPowerMode(); + +#ifndef STM32F10X_CL + /* reset FSUSP bit */ + _SetCNTR(IMR_MSK); +#endif /* STM32F10X_CL */ + + /* reverse suspend preparation */ + /* ... */ +} + +/******************************************************************************* +* Function Name : Resume +* Description : This is the state machine handling resume operations and +* timing sequence. The control is based on the Resume structure +* variables and on the ESOF interrupt calling this subroutine +* without changing machine state. +* Input : a state machine value (RESUME_STATE) +* RESUME_ESOF doesn't change ResumeS.eState allowing +* decrementing of the ESOF counter in different states. +* Output : None. +* Return : None. +*******************************************************************************/ +void Resume(RESUME_STATE eResumeSetVal) +{ +#ifndef STM32F10X_CL + uint16_t wCNTR; +#endif /* STM32F10X_CL */ + + if (eResumeSetVal != RESUME_ESOF) { + ResumeS.eState = eResumeSetVal; + } + + switch (ResumeS.eState) { + case RESUME_EXTERNAL: + Resume_Init(); + ResumeS.eState = RESUME_OFF; + break; + case RESUME_INTERNAL: + Resume_Init(); + ResumeS.eState = RESUME_START; + break; + case RESUME_LATER: + ResumeS.bESOFcnt = 2; + ResumeS.eState = RESUME_WAIT; + break; + case RESUME_WAIT: + ResumeS.bESOFcnt--; + if (ResumeS.bESOFcnt == 0) { + ResumeS.eState = RESUME_START; + } + break; + case RESUME_START: +#ifdef STM32F10X_CL + OTGD_FS_SetRemoteWakeup(); +#else + wCNTR = _GetCNTR(); + wCNTR |= CNTR_RESUME; + _SetCNTR(wCNTR); +#endif /* STM32F10X_CL */ + ResumeS.eState = RESUME_ON; + ResumeS.bESOFcnt = 10; + break; + case RESUME_ON: +#ifndef STM32F10X_CL + ResumeS.bESOFcnt--; + if (ResumeS.bESOFcnt == 0) { +#endif /* STM32F10X_CL */ +#ifdef STM32F10X_CL + OTGD_FS_ResetRemoteWakeup(); +#else + wCNTR = _GetCNTR(); + wCNTR &= (~CNTR_RESUME); + _SetCNTR(wCNTR); +#endif /* STM32F10X_CL */ + ResumeS.eState = RESUME_OFF; +#ifndef STM32F10X_CL + } +#endif /* STM32F10X_CL */ + break; + case RESUME_OFF: + case RESUME_ESOF: + default: + ResumeS.eState = RESUME_OFF; + break; + } +} + +/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/pios_usbhook.c b/flight/pios/stm32f30x/pios_usbhook.c new file mode 100644 index 0000000000..738eacd91f --- /dev/null +++ b/flight/pios/stm32f30x/pios_usbhook.c @@ -0,0 +1,572 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USBHOOK USB glue code + * @brief Glue between PiOS and STM32 libs + * @{ + * + * @file pios_usbhook.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Glue between PiOS and STM32 libs + * @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_USB + +#include "pios_usb.h" /* PIOS_USB_* */ +#include "pios_usbhook.h" +#include "pios_usb_defs.h" /* struct usb_* */ +#include "pios_usb_hid_pwr.h" +#include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ + +/* STM32 USB Library Definitions */ +#include "usb_lib.h" + +static void (*ep0_rxready_cb)(void) = 0; + +static ONE_DESCRIPTOR Device_Descriptor; + +void PIOS_USBHOOK_RegisterDevice(const uint8_t *desc, uint16_t desc_size) +{ + Device_Descriptor.Descriptor = desc; + Device_Descriptor.Descriptor_Size = desc_size; +} + +static ONE_DESCRIPTOR Config_Descriptor; + +void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t *desc, uint16_t desc_size) +{ + Config_Descriptor.Descriptor = desc; + Config_Descriptor.Descriptor_Size = desc_size; +} + +static ONE_DESCRIPTOR String_Descriptor[4]; + +void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *desc, uint16_t desc_size) +{ + if (string_id < NELEMENTS(String_Descriptor)) { + String_Descriptor[string_id].Descriptor = desc; + String_Descriptor[string_id].Descriptor_Size = desc_size; + } +} + +static ONE_DESCRIPTOR Hid_Descriptor; + +void PIOS_USB_HID_RegisterHidDescriptor(const uint8_t *desc, uint16_t desc_size) +{ + Hid_Descriptor.Descriptor = desc; + Hid_Descriptor.Descriptor_Size = desc_size; +} + +static ONE_DESCRIPTOR Hid_Report_Descriptor; + +void PIOS_USB_HID_RegisterHidReport(const uint8_t *desc, uint16_t desc_size) +{ + Hid_Report_Descriptor.Descriptor = desc; + Hid_Report_Descriptor.Descriptor_Size = desc_size; +} + +void PIOS_USBHOOK_Deactivate(void) +{} + +#include "stm32f30x.h" /* __IO */ +__IO uint8_t EXTI_Enable; + +uint32_t ProtocolValue; + +DEVICE Device_Table = { + PIOS_USB_BOARD_EP_NUM, + 1 +}; + +static void PIOS_USBHOOK_Init(void); +static void PIOS_USBHOOK_Reset(void); +static void PIOS_USBHOOK_Status_In(void); +static void PIOS_USBHOOK_Status_Out(void); +static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo); +static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo); +static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t AlternateSetting); +static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length); + +DEVICE_PROP Device_Property = { + .Init = PIOS_USBHOOK_Init, + .Reset = PIOS_USBHOOK_Reset, + .Process_Status_IN = PIOS_USBHOOK_Status_In, + .Process_Status_OUT = PIOS_USBHOOK_Status_Out, + .Class_Data_Setup = PIOS_USBHOOK_Data_Setup, + .Class_NoData_Setup = PIOS_USBHOOK_NoData_Setup, + .Class_Get_Interface_Setting = PIOS_USBHOOK_Get_Interface_Setting, + .GetDeviceDescriptor = PIOS_USBHOOK_GetDeviceDescriptor, + .GetConfigDescriptor = PIOS_USBHOOK_GetConfigDescriptor, + .GetStringDescriptor = PIOS_USBHOOK_GetStringDescriptor, + .RxEP_buffer = 0, + .MaxPacketSize = 0x40, +}; + +static void PIOS_USBHOOK_SetConfiguration(void); +static void PIOS_USBHOOK_SetDeviceAddress(void); + +USER_STANDARD_REQUESTS User_Standard_Requests = { + .User_GetConfiguration = NOP_Process, + .User_SetConfiguration = PIOS_USBHOOK_SetConfiguration, + .User_GetInterface = NOP_Process, + .User_SetInterface = NOP_Process, + .User_GetStatus = NOP_Process, + .User_ClearFeature = NOP_Process, + .User_SetEndPointFeature = NOP_Process, + .User_SetDeviceFeature = NOP_Process, + .User_SetDeviceAddress = PIOS_USBHOOK_SetDeviceAddress +}; + +static RESULT PIOS_USBHOOK_SetProtocol(void); +static const uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length); + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_Init. +* Description : Custom HID init routine. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static void PIOS_USBHOOK_Init(void) +{ + pInformation->Current_Configuration = 0; + + /* Connect the device */ + PowerOn(); + + /* Perform basic device initialization operations */ + USB_SIL_Init(); + + bDeviceState = UNCONNECTED; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_Reset. +* Description : Custom HID reset routine. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static void PIOS_USBHOOK_Reset(void) +{ + /* Set DEVICE as not configured */ + pInformation->Current_Configuration = 0; + pInformation->Current_Interface = 0; /*the default Interface */ + + /* Current Feature initialization */ + pInformation->Current_Feature = 0; + +#ifdef STM32F10X_CL + /* EP0 is already configured in DFU_Init() by USB_SIL_Init() function */ + + /* Init EP1 IN as Interrupt endpoint */ + OTG_DEV_EP_Init(EP1_IN, OTG_DEV_EP_TYPE_INT, 2); + + /* Init EP1 OUT as Interrupt endpoint */ + OTG_DEV_EP_Init(EP1_OUT, OTG_DEV_EP_TYPE_INT, 2); +#else + SetBTABLE(BTABLE_ADDRESS); + + /* Initialize Endpoint 0 (Control) */ + SetEPType(ENDP0, EP_CONTROL); + SetEPTxAddr(ENDP0, ENDP0_TXADDR); + SetEPTxStatus(ENDP0, EP_TX_STALL); + Clear_Status_Out(ENDP0); + + SetEPRxAddr(ENDP0, ENDP0_RXADDR); + SetEPRxCount(ENDP0, Device_Property.MaxPacketSize); + SetEPRxValid(ENDP0); + +#if defined(PIOS_INCLUDE_USB_HID) + /* Initialize Endpoint 1 (HID) */ + SetEPType(ENDP1, EP_INTERRUPT); + SetEPTxAddr(ENDP1, ENDP1_TXADDR); + SetEPTxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); + SetEPTxStatus(ENDP1, EP_TX_NAK); + + SetEPRxAddr(ENDP1, ENDP1_RXADDR); + SetEPRxCount(ENDP1, PIOS_USB_BOARD_HID_DATA_LENGTH); + SetEPRxStatus(ENDP1, EP_RX_VALID); +#endif /* PIOS_INCLUDE_USB_HID */ + +#if defined(PIOS_INCLUDE_USB_CDC) + /* Initialize Endpoint 2 (CDC Call Control) */ + SetEPType(ENDP2, EP_INTERRUPT); + SetEPTxAddr(ENDP2, ENDP2_TXADDR); + SetEPTxStatus(ENDP2, EP_TX_NAK); + + SetEPRxAddr(ENDP2, ENDP2_RXADDR); + SetEPRxCount(ENDP2, PIOS_USB_BOARD_CDC_MGMT_LENGTH); + SetEPRxStatus(ENDP2, EP_RX_DIS); + + /* Initialize Endpoint 3 (CDC Data) */ + SetEPType(ENDP3, EP_BULK); + SetEPTxAddr(ENDP3, ENDP3_TXADDR); + SetEPTxStatus(ENDP3, EP_TX_NAK); + + SetEPRxAddr(ENDP3, ENDP3_RXADDR); + SetEPRxCount(ENDP3, PIOS_USB_BOARD_CDC_DATA_LENGTH); + SetEPRxStatus(ENDP3, EP_RX_VALID); + +#endif /* PIOS_INCLUDE_USB_CDC */ + + /* Set this device to response on default address */ + SetDeviceAddress(0); +#endif /* STM32F10X_CL */ + + bDeviceState = ATTACHED; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_SetConfiguration. +* Description : Update the device state to configured +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static void PIOS_USBHOOK_SetConfiguration(void) +{ + if (pInformation->Current_Configuration != 0) { + /* Device configured */ + bDeviceState = CONFIGURED; + } + + /* Enable transfers */ + PIOS_USB_ChangeConnectionState(pInformation->Current_Configuration != 0); +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_SetConfiguration. +* Description : Update the device state to addressed. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static void PIOS_USBHOOK_SetDeviceAddress(void) +{ + bDeviceState = ADDRESSED; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_Status_In. +* Description : status IN routine. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static void PIOS_USBHOOK_Status_In(void) +{ + /* this callback gets executed after sending ZLP and doing In0_Process(), to ack */ + if (ep0_rxready_cb) { + ep0_rxready_cb(); + ep0_rxready_cb = 0; + } +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_Status_Out +* Description : status OUT routine. +* Input : None. +* Output : None. +* Return : None. +*******************************************************************************/ +static void PIOS_USBHOOK_Status_Out(void) +{} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_Data_Setup +* Description : Handle the data class specific requests. +* Input : Request Nb. +* Output : None. +* Return : USB_UNSUPPORT or USB_SUCCESS. +*******************************************************************************/ +extern uint8_t *PIOS_USB_CDC_SetLineCoding(uint16_t Length); +extern const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length); +extern void PIOS_USB_CDC_SetLineCoding_Completed(); + +static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo) +{ + uint8_t *(*CopyOutRoutine)(uint16_t); + const uint8_t *(*CopyInRoutine)(uint16_t); + + CopyInRoutine = NULL; + CopyOutRoutine = NULL; + + switch (Type_Recipient) { + case (STANDARD_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { +#if defined(PIOS_INCLUDE_USB_CDC) + case 2: /* HID Interface */ +#else + case 0: /* HID Interface */ +#endif + switch (RequestNo) { + case GET_DESCRIPTOR: + switch (pInformation->USBwValue1) { + case USB_DESC_TYPE_REPORT: + CopyInRoutine = PIOS_USBHOOK_GetReportDescriptor; + break; + case USB_DESC_TYPE_HID: + CopyInRoutine = PIOS_USBHOOK_GetHIDDescriptor; + break; + } + } + } + break; + + case (CLASS_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { +#if defined(PIOS_INCLUDE_USB_CDC) + case 2: /* HID Interface */ +#else + case 0: /* HID Interface */ +#endif + switch (RequestNo) { + case USB_HID_REQ_GET_PROTOCOL: + CopyInRoutine = PIOS_USBHOOK_GetProtocolValue; + break; + } + + break; +#if defined(PIOS_INCLUDE_USB_CDC) + case 0: /* CDC Call Control Interface */ + switch (RequestNo) { + case USB_CDC_REQ_SET_LINE_CODING: + CopyOutRoutine = PIOS_USB_CDC_SetLineCoding; + ep0_rxready_cb = PIOS_USB_CDC_SetLineCoding_Completed; + break; + case USB_CDC_REQ_GET_LINE_CODING: + CopyInRoutine = PIOS_USB_CDC_GetLineCoding; + break; + } + + break; + + case 1: /* CDC Data Interface */ + switch (RequestNo) { + case 0: + break; + } + + break; +#endif /* PIOS_INCLUDE_USB_CDC */ + } + break; + } + + /* No registered copy routine */ + if ((CopyInRoutine == NULL) && (CopyOutRoutine == NULL)) { + return USB_UNSUPPORT; + } + + /* Registered copy in AND copy out routine */ + if ((CopyInRoutine != NULL) && (CopyOutRoutine != NULL)) { + /* This should never happen */ + return USB_UNSUPPORT; + } + + if (CopyInRoutine != NULL) { + pInformation->Ctrl_Info.CopyData = (typeof(pInformation->Ctrl_Info.CopyData))CopyInRoutine; + pInformation->Ctrl_Info.Usb_wOffset = 0; + (*CopyInRoutine)(0); + } else if (CopyOutRoutine != NULL) { + pInformation->Ctrl_Info.CopyData = CopyOutRoutine; + pInformation->Ctrl_Info.Usb_rOffset = 0; + (*CopyOutRoutine)(0); + } + + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_NoData_Setup +* Description : handle the no data class specific requests +* Input : Request Nb. +* Output : None. +* Return : USB_UNSUPPORT or USB_SUCCESS. +*******************************************************************************/ +extern RESULT PIOS_USB_CDC_SetControlLineState(void); + +static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo) +{ + switch (Type_Recipient) { + case (CLASS_REQUEST | INTERFACE_RECIPIENT): + switch (pInformation->USBwIndex0) { +#if defined(PIOS_INCLUDE_USB_CDC) + case 2: /* HID */ +#else + case 0: /* HID */ +#endif + switch (RequestNo) { + case USB_HID_REQ_SET_PROTOCOL: + return PIOS_USBHOOK_SetProtocol(); + + break; + } + + break; + +#if defined(PIOS_INCLUDE_USB_CDC) + case 0: /* CDC Call Control Interface */ + switch (RequestNo) { + case USB_CDC_REQ_SET_CONTROL_LINE_STATE: + return PIOS_USB_CDC_SetControlLineState(); + + break; + } + + break; +#endif /* PIOS_INCLUDE_USB_CDC */ + } + + break; + } + + return USB_UNSUPPORT; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_GetDeviceDescriptor. +* Description : Gets the device descriptor. +* Input : Length +* Output : None. +* Return : The address of the device descriptor. +*******************************************************************************/ +static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) +{ + return Standard_GetDescriptorData(Length, &Device_Descriptor); +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_GetConfigDescriptor. +* Description : Gets the configuration descriptor. +* Input : Length +* Output : None. +* Return : The address of the configuration descriptor. +*******************************************************************************/ +static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) +{ + return Standard_GetDescriptorData(Length, &Config_Descriptor); +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_GetStringDescriptor +* Description : Gets the string descriptors according to the needed index +* Input : Length +* Output : None. +* Return : The address of the string descriptors. +*******************************************************************************/ +static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) +{ + uint8_t wValue0 = pInformation->USBwValue0; + + if (wValue0 > 4) { + return NULL; + } else { + return Standard_GetDescriptorData(Length, &String_Descriptor[wValue0]); + } +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_GetReportDescriptor. +* Description : Gets the HID report descriptor. +* Input : Length +* Output : None. +* Return : The address of the configuration descriptor. +*******************************************************************************/ +static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) +{ + return Standard_GetDescriptorData(Length, &Hid_Report_Descriptor); +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_GetHIDDescriptor. +* Description : Gets the HID descriptor. +* Input : Length +* Output : None. +* Return : The address of the configuration descriptor. +*******************************************************************************/ +static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) +{ + return Standard_GetDescriptorData(Length, &Hid_Descriptor); +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_Get_Interface_Setting. +* Description : tests the interface and the alternate setting according to the +* supported one. +* Input : - Interface : interface number. +* - AlternateSetting : Alternate Setting number. +* Output : None. +* Return : USB_SUCCESS or USB_UNSUPPORT. +*******************************************************************************/ +static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t AlternateSetting) +{ + if (AlternateSetting > 0) { + return USB_UNSUPPORT; + } else if (Interface > 0) { + return USB_UNSUPPORT; + } + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_SetProtocol +* Description : Set Protocol request routine. +* Input : None. +* Output : None. +* Return : USB SUCCESS. +*******************************************************************************/ +static RESULT PIOS_USBHOOK_SetProtocol(void) +{ + uint8_t wValue0 = pInformation->USBwValue0; + + ProtocolValue = wValue0; + return USB_SUCCESS; +} + +/******************************************************************************* +* Function Name : PIOS_USBHOOK_GetProtocolValue +* Description : get the protocol value +* Input : Length. +* Output : None. +* Return : address of the protcol value. +*******************************************************************************/ +static const uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length) +{ + if (Length == 0) { + pInformation->Ctrl_Info.Usb_wLength = 1; + return NULL; + } else { + return (uint8_t *)(&ProtocolValue); + } +} + +#endif /* PIOS_INCLUDE_USB */ + +/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/pios_wdg.c b/flight/pios/stm32f30x/pios_wdg.c new file mode 100644 index 0000000000..9cf3c35be4 --- /dev/null +++ b/flight/pios/stm32f30x/pios_wdg.c @@ -0,0 +1,186 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_WDG Watchdog Functions + * @brief PIOS Comamnds to initialize and clear watchdog timer + * @{ + * + * @file pios_wdg.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief Hardware Abstraction Layer for SPI ports of STM32 + * @see The GNU Public License (GPL) Version 3 + * @notes + * + * The PIOS Watchdog provides a HAL to initialize a watchdog + * + *****************************************************************************/ +/* + * 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 + */ + +/* + * @todo This is virtually identical to the F1xx code and should be merged. + */ + +#include "pios.h" + +#ifdef PIOS_INCLUDE_WDG + +#include "stm32f30x_iwdg.h" +#include "stm32f30x_dbgmcu.h" +#include "stm32f30x_rtc.h" + +static struct wdg_configuration { + uint16_t used_flags; + uint16_t bootup_flags; +} wdg_configuration; + +/** + * @brief Initialize the watchdog timer for a specified timeout + * + * It is important to note that this function returns the achieved timeout + * for this hardware. For hardware independence this should be checked when + * scheduling updates. Other hardware dependent details may need to be + * considered such as a window time which sets a minimum update time, + * and this function should return a recommended delay for clearing. + * + * For the STM32 nominal clock rate is 32 khz, but for the maximum clock rate of + * 60 khz and a prescaler of 4 yields a clock rate of 15 khz. The delay that is + * set in the watchdog assumes the nominal clock rate, but the delay for FreeRTOS + * to use is 75% of the minimal delay. + * + * @returns Maximum recommended delay between updates based on PIOS_WATCHDOG_TIMEOUT constant + */ +uint16_t PIOS_WDG_Init() +{ + uint16_t delay = ((uint32_t)PIOS_WATCHDOG_TIMEOUT * 60) / 16; + + if (delay > 0x0fff) { + delay = 0x0fff; + } +#if defined(PIOS_INCLUDE_WDG) + DBGMCU_APB1PeriphConfig(DBGMCU_IWDG_STOP, ENABLE); // OP-1272 : write in APB1 register + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + IWDG_SetPrescaler(IWDG_Prescaler_16); + IWDG_SetReload(delay); + IWDG_ReloadCounter(); + IWDG_Enable(); + + // watchdog flags now stored in backup registers + PWR_BackupAccessCmd(ENABLE); + + wdg_configuration.bootup_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); +#endif + return delay; +} + +/** + * @brief Register a module against the watchdog + * + * There are two ways to use PIOS WDG: this is for when + * multiple modules must be monitored. In this case they + * must first register against the watchdog system and + * only when all of the modules have been updated with the + * watchdog be cleared. Each module must have its own + * bit in the 16 bit + * + * @param[in] flag the bit this module wants to use + * @returns True if that bit is unregistered + */ +bool PIOS_WDG_RegisterFlag(uint16_t flag_requested) +{ + // flag are being registered so we are in module initialization phase + // clear the WDG to prevent timeout while initializing modules. (OP-815) + PIOS_WDG_Clear(); + + /* Fail if flag already registered */ + if (wdg_configuration.used_flags & flag_requested) { + return false; + } + + // FIXME: Protect with semaphore + wdg_configuration.used_flags |= flag_requested; + + return true; +} + +/** + * @brief Function called by modules to indicate they are still running + * + * This function will set this flag in the active flags register (which is + * a backup regsiter) and if all the registered flags are set will clear + * the watchdog and set only this flag in the backup register + * + * @param[in] flag the flag to set + * @return true if the watchdog cleared, false if flags are pending + */ +bool PIOS_WDG_UpdateFlag(uint16_t flag) +{ + // we can probably avoid using a semaphore here which will be good for + // efficiency and not blocking critical tasks. race condition could + // overwrite their flag update, but unlikely to block _all_ of them + // for the timeout window + uint16_t cur_flags = RTC_ReadBackupRegister(PIOS_WDG_REGISTER); + + if ((cur_flags | flag) == wdg_configuration.used_flags) { + PIOS_WDG_Clear(); + RTC_WriteBackupRegister(PIOS_WDG_REGISTER, flag); + return true; + } else { + RTC_WriteBackupRegister(PIOS_WDG_REGISTER, cur_flags | flag); + return false; + } +} + +/** + * @brief Returns the flags that were set at bootup + * + * This is used for diagnostics, if only one flag not set this + * was likely the module that wasn't running before reset + * + * @return The active flags register from bootup + */ +uint16_t PIOS_WDG_GetBootupFlags() +{ + return wdg_configuration.bootup_flags; +} + +/** + * @brief Returns the currently active flags + * + * For external monitoring + * + * @return The active flags register + */ +uint16_t PIOS_WDG_GetActiveFlags() +{ + return RTC_ReadBackupRegister(PIOS_WDG_REGISTER); +} + +/** + * @brief Clear the watchdog timer + * + * This function must be called at the appropriate delay to prevent a reset event occuring + */ +void PIOS_WDG_Clear(void) +{ +#if defined(PIOS_INCLUDE_WDG) + IWDG_ReloadCounter(); +#endif +} + +#endif /* PIOS_INCLUDE_WDG */ diff --git a/flight/pios/stm32f30x/pios_ws2811.c b/flight/pios/stm32f30x/pios_ws2811.c new file mode 100644 index 0000000000..3cb23fe31e --- /dev/null +++ b/flight/pios/stm32f30x/pios_ws2811.c @@ -0,0 +1,208 @@ +/** + ****************************************************************************** + * + * @file pios_ws2811.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief A driver for ws2811 rgb led controller. + * this is a port of the CleanFlight/BetaFlight implementation. + * @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" +#include "pios_ws2811.h" +#include "pios_ws2811_cfg.h" + +#define WS2811_BITS_PER_LED 24 +// for 50us delay +#define WS2811_DELAY_BUFFER_LENGTH 42 + +#define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * PIOS_WS2811_NUMLEDS) +// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) + +#define WS2811_TIMER_HZ 24000000 +#define WS2811_TIMER_PERIOD 29 +// timer compare value for logical 1 +#define BIT_COMPARE_1 17 +// timer compare value for logical 0 +#define BIT_COMPARE_0 9 + +#define PIOS_WS2811_MAGIC 0x00281100 + +struct pios_ws2811_dev { + uint32_t magic; + const struct pios_ws2811_cfg *config; + uint8_t dma_buffer[WS2811_DMA_BUFFER_SIZE]; + bool dma_active; +}; + +struct pios_ws2811_dev *ws2811_dev; + +void PIOS_WS2811_Init(uint32_t *dev_id, const struct pios_ws2811_cfg *ws2811_cfg) +{ + if (ws2811_dev) { + return; + } + + ws2811_dev = (struct pios_ws2811_dev *)pios_malloc(sizeof(*ws2811_dev)); + + PIOS_Assert(ws2811_dev); + + memset(ws2811_dev, 0, sizeof(*ws2811_dev)); + + ws2811_dev->magic = PIOS_WS2811_MAGIC; + ws2811_dev->config = ws2811_cfg; + + + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_OCInitTypeDef TIM_OCInitStructure; + DMA_InitTypeDef DMA_InitStructure; + + GPIO_Init(ws2811_cfg->pin.gpio, (GPIO_InitTypeDef *)&ws2811_cfg->pin.init); + + GPIO_PinAFConfig(ws2811_cfg->pin.gpio, ws2811_cfg->pin.pin_source, ws2811_cfg->remap); + + /* Compute the prescaler value */ + uint16_t prescalerValue = (uint16_t)(SystemCoreClock / WS2811_TIMER_HZ) - 1; + /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz + TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(ws2811_cfg->timer, &TIM_TimeBaseStructure); + + /* PWM1 Mode configuration: Channel1 */ + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_Pulse = 0; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; + + switch (ws2811_cfg->timer_chan) { + case TIM_Channel_1: + TIM_OC1Init(ws2811_cfg->timer, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(ws2811_cfg->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(ws2811_cfg->timer, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(ws2811_cfg->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(ws2811_cfg->timer, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(ws2811_cfg->timer, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(ws2811_cfg->timer, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(ws2811_cfg->timer, TIM_OCPreload_Enable); + break; + } + + + TIM_CtrlPWMOutputs(ws2811_cfg->timer, ENABLE); + + /* configure DMA */ + // NVIC setup here + NVIC_InitTypeDef NVIC_InitStructure; + + NVIC_InitStructure.NVIC_IRQChannel = ws2811_cfg->dma_irqn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + + DMA_DeInit(ws2811_cfg->dma_chan); + + DMA_StructInit(&DMA_InitStructure); + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ws2811_cfg->timer->CCR1 + ws2811_cfg->timer_chan; + DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ws2811_dev->dma_buffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; + DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; + + DMA_Init(ws2811_cfg->dma_chan, &DMA_InitStructure); + + TIM_DMACmd(ws2811_cfg->timer, ws2811_cfg->timer_dma_source, ENABLE); + + DMA_ITConfig(ws2811_cfg->dma_chan, DMA_IT_TC, ENABLE); + + *dev_id = (uint32_t)ws2811_dev; +} + +void PIOS_WS2811_DMA_irq_handler() +{ + if (!ws2811_dev) { + return; + } + + if (DMA_GetITStatus(ws2811_dev->config->dma_tcif)) { + ws2811_dev->dma_active = false; + DMA_Cmd(ws2811_dev->config->dma_chan, DISABLE); + + DMA_ClearITPendingBit(ws2811_dev->config->dma_tcif); + } +} + + +/** + * Set a led color + * @param c color + * @param led led number + * @param update Perform an update after changing led color + */ +void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update) +{ + if (!ws2811_dev || (led >= PIOS_WS2811_NUMLEDS)) { + return; + } + + int offset = led * WS2811_BITS_PER_LED; + + uint32_t grb = (c.G << 16) | (c.R << 8) | (c.B); + + for (int bit = (WS2811_BITS_PER_LED - 1); bit >= 0; --bit) { + ws2811_dev->dma_buffer[offset++] = (grb & (1 << bit)) ? BIT_COMPARE_1 : BIT_COMPARE_0; + } + + if (update) { + PIOS_WS2811_Update(); + } +} + +void PIOS_WS2811_Update() +{ + if (!ws2811_dev || ws2811_dev->dma_active) { + return; + } + + ws2811_dev->dma_active = true; + + DMA_SetCurrDataCounter(ws2811_dev->config->dma_chan, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(ws2811_dev->config->timer, 0); + TIM_Cmd(ws2811_dev->config->timer, ENABLE); + + DMA_Cmd(ws2811_dev->config->dma_chan, ENABLE); +} diff --git a/flight/pios/stm32f30x/startup.c b/flight/pios/stm32f30x/startup.c new file mode 100644 index 0000000000..e4299b60f8 --- /dev/null +++ b/flight/pios/stm32f30x/startup.c @@ -0,0 +1,189 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * + * @file startup.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author Tau Labs, http://taulabs.org, Copyright (C) 2012-2013 + * @brief C based startup of F3 + * @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 + +/* prototype for main() that tells us not to worry about it possibly returning */ +extern int main(void) __attribute__((noreturn)); + +/* prototype our _main() to avoid prolog/epilog insertion and other related junk */ +void _main(void) __attribute__((noreturn, naked, no_instrument_function)); + +/** default handler for CPU exceptions */ +static void default_cpu_handler(void) __attribute__((noreturn, naked, no_instrument_function)); + +/** BSS symbols XXX should have a header that defines all of these */ +extern char _sbss, _ebss; + +/** DATA symbols XXX should have a header that defines all of these */ +extern char _sidata, _sdata, _edata, _sfast, _efast; + +/** The bootstrap/IRQ stack XXX should define size somewhere else */ +char irq_stack[1024] __attribute__((section(".irqstack"))); + +/** exception handler */ +typedef void (vector)(void); + +/** CortexM3 CPU vectors */ +struct cm3_vectors { + void *initial_stack; + vector *entry; + vector *vectors[14]; +}; + +/** + * Initial startup code. + */ +void _main(void) +{ + // load the stack base for the current stack before we attempt to branch to any function + // that might bounds-check the stack + asm volatile ("mov r10, %0" : : "r" (&irq_stack[0]) :); + + /* enable usage, bus and memory faults */ + SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk | SCB_SHCSR_MEMFAULTENA_Msk; + + /* configure FP state save behaviour - automatic, lazy save */ + FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk | FPU_FPCCR_LSPEN_Msk; + + /* configure default FPU state */ + FPU->FPDSCR |= FPU_FPDSCR_DN_Msk; /* enable Default NaN */ + FPU->FPDSCR |= FPU_FPDSCR_FZ_Msk; /* Use flush to zero for very + * small values. */ + + /* enable the FPU */ + SCB->CPACR |= (0xf << 20); // turn on CP10/11 for FP support on cores that implement it + + /* copy initialised data from flash to RAM */ + memcpy(&_sdata, &_sidata, &_edata - &_sdata); + + /* zero the BSS */ + memset(&_sbss, 0, &_ebss - &_sbss); + + /* zero any 'fast' RAM that's been used */ + memset(&_sfast, 0, &_efast - &_sfast); + + /* fill most of the IRQ/bootstrap stack with a watermark pattern so we can measure how much is used */ + /* leave a little space at the top in case memset() isn't a leaf with no locals */ + memset(&irq_stack, 0xa5, sizeof(irq_stack) - 64); + + /* call main */ + (void)main(); +} + +/** + * Default handler for CPU exceptions. + */ +static void default_cpu_handler(void) +{ + for (;;) { + ; + } +} + +static void default_NMI_Handler() +{ + default_cpu_handler(); +} +static void default_HardFault_Handler() +{ + default_cpu_handler(); +} +static void default_MemManage_Handler() +{ + default_cpu_handler(); +} +static void default_BusFault_Handler() +{ + default_cpu_handler(); +} +static void default_UsageFault_Handler() +{ + default_cpu_handler(); +} +static void default_DebugMon_Handler() +{ + default_cpu_handler(); +} +static void default_vPortSVCHandler() +{ + default_cpu_handler(); +} +static void default_xPortPendSVHandler() +{ + default_cpu_handler(); +} +static void default_xPortSysTickHandler() +{ + default_cpu_handler(); +} + + +/** Prototype for optional exception vector handlers */ +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_" #_name))) + +/* standard CMSIS vector names */ +HANDLER(NMI_Handler); +HANDLER(HardFault_Handler); +HANDLER(MemManage_Handler); +HANDLER(BusFault_Handler); +HANDLER(UsageFault_Handler); +HANDLER(DebugMon_Handler); +/* these vectors point directly to the relevant FreeRTOS functions if they are defined */ +HANDLER(vPortSVCHandler); +HANDLER(xPortPendSVHandler); +HANDLER(xPortSysTickHandler); + +/** CortexM3 vector table */ +struct cm3_vectors cpu_vectors __attribute((section(".cpu_vectors"))) = +{ + .initial_stack = &irq_stack[sizeof(irq_stack)], + .entry = (vector *)_main, + .vectors = { + NMI_Handler, + HardFault_Handler, + MemManage_Handler, + BusFault_Handler, + UsageFault_Handler, + 0, + 0, + 0, + 0, + vPortSVCHandler, + DebugMon_Handler, // Maybe + 0, + xPortPendSVHandler, + xPortSysTickHandler, + } +}; + +/** + * @} + */ diff --git a/flight/pios/stm32f30x/system.c b/flight/pios/stm32f30x/system.c new file mode 100644 index 0000000000..3cd82f47f7 --- /dev/null +++ b/flight/pios/stm32f30x/system.c @@ -0,0 +1,370 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.0.0 + * @date 04-September-2012 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f30x.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | DISABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2012 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Defines + * @{ + */ +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + +uint32_t SystemCoreClock = 72000000; + +__I uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 }; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +static void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + SetSysClock(); + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate(void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = (pllmull >> 18) + 2; + + if (pllsource == 0x00) { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } else { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +static void SetSysClock(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* Enable HSE */ +#ifdef PIOS_RCC_HSE_BYPASS + RCC->CR |= ((uint32_t)RCC_CR_HSEON | RCC_CR_HSEBYP); +#else + RCC->CR |= ((uint32_t)RCC_CR_HSEON); +#endif /* PIOS_HSE_BYPASS */ + + /* Wait till HSE is ready and if Time out is reached exit */ + do { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) { + HSEStatus = (uint32_t)0x01; + } else { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) { + /* Enable Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t) ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while ((RCC->CR & RCC_CR_PLLRDY) == 0) {} + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t) ~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) {} + } else { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/flight/pios/stm32f30x/vectors_stm32f30x.c b/flight/pios/stm32f30x/vectors_stm32f30x.c new file mode 100644 index 0000000000..2dc18c359b --- /dev/null +++ b/flight/pios/stm32f30x/vectors_stm32f30x.c @@ -0,0 +1,233 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * + * @file vectors_stm32f30x.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * Tau Labs, http://taulabs.org, Copyright (C) 2012-2013 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief C based vectors for F3 + * @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 + */ + +/** interrupt handler */ +typedef void (vector)(void); + +/** default interrupt handler */ +static void default_io_handler(void) +{ + asm volatile ("BKPT #01"); + for (;;) { + ; + } +} + +/** prototypes an interrupt handler */ +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_io_handler"))) + +HANDLER(Reserved_IRQHandler); // Reserved +HANDLER(WWDG_IRQHandler); // Window WatchDog +HANDLER(PVD_IRQHandler); // PVD through EXTI Line detection +HANDLER(TAMP_STAMP_IRQHandler); // Tamper and TimeStamps through the EXTI line +HANDLER(RTC_WKUP_IRQHandler); // RTC Wakeup through the EXTI line +HANDLER(FLASH_IRQHandler); // FLASH +HANDLER(RCC_IRQHandler); // RCC +HANDLER(EXTI0_IRQHandler); // EXTI Line0 +HANDLER(EXTI1_IRQHandler); // EXTI Line1 +HANDLER(EXTI2_TS_IRQHandler); // EXTI Line2 and Touch Sense Interrupt +HANDLER(EXTI3_IRQHandler); // EXTI Line3 +HANDLER(EXTI4_IRQHandler); // EXTI Line4 +HANDLER(DMA1_Channel1_IRQHandler); // DMA1 Channel 1 +HANDLER(DMA1_Channel2_IRQHandler); // DMA1 Channel 2 +HANDLER(DMA1_Channel3_IRQHandler); // DMA1 Channel 3 +HANDLER(DMA1_Channel4_IRQHandler); // DMA1 Channel 4 +HANDLER(DMA1_Channel5_IRQHandler); // DMA1 Channel 5 +HANDLER(DMA1_Channel6_IRQHandler); // DMA1 Channel 6 +HANDLER(DMA1_Channel7_IRQHandler); // DMA1 Channel 7 +HANDLER(ADC1_2_IRQHandler); // ADC1 and ADC2 +HANDLER(USB_HP_CAN1_TX_IRQHandler); // USB Device High Priority or CAN1 TX +HANDLER(USB_LP_CAN1_RX0_IRQHandler); // USB Device Low Priority or CAN1 RX0 +HANDLER(CAN1_RX1_IRQHandler); // CAN1 RX1 +HANDLER(CAN1_SCE_IRQHandler); // CAN1 SCE +HANDLER(EXTI9_5_IRQHandler); // External Line[9:5]s +HANDLER(TIM1_BRK_TIM15_IRQHandler); // TIM1 Break and TIM15 +HANDLER(TIM1_UP_TIM16_IRQHandler); // TIM1 Update and TIM16 +HANDLER(TIM1_TRG_COM_TIM17_IRQHandler); // TIM1 Trigger and Commutation and TIM17 +HANDLER(TIM1_CC_IRQHandler); // TIM1 Capture Compare +HANDLER(TIM2_IRQHandler); // TIM2 +HANDLER(TIM3_IRQHandler); // TIM3 +HANDLER(TIM4_IRQHandler); // TIM4 +HANDLER(I2C1_EV_EXTI23_IRQHandler); // I2C1 Event and EXTI23 +HANDLER(I2C1_ER_IRQHandler); // I2C1 Error +HANDLER(I2C2_EV_EXTI24_IRQHandler); // I2C2 Event and EXTI24 +HANDLER(I2C2_ER_IRQHandler); // I2C2 Error +HANDLER(SPI1_IRQHandler); // SPI1 +HANDLER(SPI2_IRQHandler); // SPI2 +HANDLER(USART1_EXTI25_IRQHandler); // USART1 and EXTI25 +HANDLER(USART2_EXTI26_IRQHandler); // USART2 and EXTI26 +HANDLER(USART3_EXTI28_IRQHandler); // USART3 and EXTI28 +HANDLER(EXTI15_10_IRQHandler); // External Line[15:10]s +HANDLER(RTC_Alarm_IRQHandler); // RTC Alarm (A and B) through EXTI Line +HANDLER(USB_WKUP_IRQHandler); // USB FS Wakeup through EXTI line +HANDLER(TIM8_BRK_IRQHandler); // TIM8 Break +HANDLER(TIM8_UP_IRQHandler); // TIM8 Update +HANDLER(TIM8_TRG_COM_IRQHandler); // TIM8 Trigger and Commutation +HANDLER(TIM8_CC_IRQHandler); // TIM8 Capture Compare +HANDLER(ADC3_IRQHandler); // ADC3 +HANDLER(SPI3_IRQHandler); // SPI3 +HANDLER(UART4_EXTI34_IRQHandler); // UART4 and EXTI34 +HANDLER(UART5_EXTI35_IRQHandler); // UART5 and EXTI35 +HANDLER(TIM6_DAC_IRQHandler); // TIM6 and DAC1&2 underrun errors +HANDLER(TIM7_IRQHandler); // TIM7 +HANDLER(DMA2_Channel1_IRQHandler); // DMA2 Channel 1 +HANDLER(DMA2_Channel2_IRQHandler); // DMA2 Channel 2 +HANDLER(DMA2_Channel3_IRQHandler); // DMA2 Channel 3 +HANDLER(DMA2_Channel4_IRQHandler); // DMA2 Channel 4 +HANDLER(DMA2_Channel5_IRQHandler); // DMA2 Channel 5 +HANDLER(ADC4_IRQHandler); // ADC4 +HANDLER(COMP1_2_3_IRQHandler); // COMP1, COMP2 and COMP3 +HANDLER(COMP4_5_6_IRQHandler); // COMP4, COMP5 and COMP6 +HANDLER(COMP7_IRQHandler); // COMP7 +HANDLER(USB_HP_IRQHandler); // USB High Priority remap +HANDLER(USB_LP_IRQHandler); // USB Low Priority remap +HANDLER(USB_WKUP_RMP_IRQHandler); // USB Wakup remap +HANDLER(FPU_IRQHandler); // FPU + +#if defined(STM32F303xD) || defined(STM32F303xE) +HANDLER(FMC_IRQHandler); +HANDLER(I2C3_EV_IRQHandler); +HANDLER(I2C3_ER_IRQHandler); +HANDLER(TIM20_BRK_IRQHandler); +HANDLER(TIM20_UP_IRQHandler); +HANDLER(TIM20_TRG_COM_IRQHandler); +HANDLER(TIM20_CC_IRQHandler); +HANDLER(SPI4_IRQHandler); +#endif /* defined(STM32F303xD) || defined(STM32F303xE) */ + +/** stm32f30x interrupt vector table */ +vector *io_vectors[] __attribute__((section(".io_vectors"))) = { + WWDG_IRQHandler, // Window WatchDog + PVD_IRQHandler, // PVD through EXTI Line detection + TAMP_STAMP_IRQHandler, // Tamper and TimeStamps through the EXTI line + RTC_WKUP_IRQHandler, // RTC Wakeup through the EXTI line + FLASH_IRQHandler, // FLASH + RCC_IRQHandler, // RCC + EXTI0_IRQHandler, // EXTI Line0 + EXTI1_IRQHandler, // EXTI Line1 + EXTI2_TS_IRQHandler, // EXTI Line2 and Touch Sense Interrupt + EXTI3_IRQHandler, // EXTI Line3 + EXTI4_IRQHandler, // EXTI Line4 + DMA1_Channel1_IRQHandler, // DMA1 Channel 1 + DMA1_Channel2_IRQHandler, // DMA1 Channel 2 + DMA1_Channel3_IRQHandler, // DMA1 Channel 3 + DMA1_Channel4_IRQHandler, // DMA1 Channel 4 + DMA1_Channel5_IRQHandler, // DMA1 Channel 5 + DMA1_Channel6_IRQHandler, // DMA1 Channel 6 + DMA1_Channel7_IRQHandler, // DMA1 Channel 7 + ADC1_2_IRQHandler, // ADC1 and ADC2 + USB_HP_CAN1_TX_IRQHandler, // USB Device High Priority or CAN1 TX + USB_LP_CAN1_RX0_IRQHandler, // USB Device Low Priority or CAN1 RX0 + CAN1_RX1_IRQHandler, // CAN1 RX1 + CAN1_SCE_IRQHandler, // CAN1 SCE + EXTI9_5_IRQHandler, // External Line[9:5]s + TIM1_BRK_TIM15_IRQHandler, // TIM1 Break and TIM15 + TIM1_UP_TIM16_IRQHandler, // TIM1 Update and TIM16 + TIM1_TRG_COM_TIM17_IRQHandler, // TIM1 Trigger and Commutation and TIM17 + TIM1_CC_IRQHandler, // TIM1 Capture Compare + TIM2_IRQHandler, // TIM2 + TIM3_IRQHandler, // TIM3 + TIM4_IRQHandler, // TIM4 + I2C1_EV_EXTI23_IRQHandler, // I2C1 Event and EXTI23 + I2C1_ER_IRQHandler, // I2C1 Error + I2C2_EV_EXTI24_IRQHandler, // I2C2 Event and EXTI24 + I2C2_ER_IRQHandler, // I2C2 Error + SPI1_IRQHandler, // SPI1 + SPI2_IRQHandler, // SPI2 + USART1_EXTI25_IRQHandler, // USART1 and EXTI25 + USART2_EXTI26_IRQHandler, // USART2 and EXTI26 + USART3_EXTI28_IRQHandler, // USART3 and EXTI28 + EXTI15_10_IRQHandler, // External Line[15:10]s + RTC_Alarm_IRQHandler, // RTC Alarm (A and B) through EXTI Line + USB_WKUP_IRQHandler, // USB FS Wakeup through EXTI line + TIM8_BRK_IRQHandler, // TIM8 Break + TIM8_UP_IRQHandler, // TIM8 Update + TIM8_TRG_COM_IRQHandler, // TIM8 Trigger and Commutation + TIM8_CC_IRQHandler, // TIM8 Capture Compare + ADC3_IRQHandler, // ADC3 +#if defined(STM32F303xD) || defined(STM32F303xE) + FMC_IRQHandler, // FMC_IRQHandler (Available in STM32F303xD/E only) +#else + Reserved_IRQHandler, // reserved +#endif + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + SPI3_IRQHandler, // SPI3 + UART4_EXTI34_IRQHandler, // UART4 and EXTI34 + UART5_EXTI35_IRQHandler, // UART5 and EXTI35 + TIM6_DAC_IRQHandler, // TIM6 and DAC1&2 underrun errors + TIM7_IRQHandler, // TIM7 + DMA2_Channel1_IRQHandler, // DMA2 Channel 1 + DMA2_Channel2_IRQHandler, // DMA2 Channel 2 + DMA2_Channel3_IRQHandler, // DMA2 Channel 3 + DMA2_Channel4_IRQHandler, // DMA2 Channel 4 + DMA2_Channel5_IRQHandler, // DMA2 Channel 5 + ADC4_IRQHandler, // ADC4 + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + COMP1_2_3_IRQHandler, // COMP1, COMP2 and COMP3 + COMP4_5_6_IRQHandler, // COMP4, COMP5 and COMP6 + COMP7_IRQHandler, // COMP7 + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved +#if defined(STM32F303xD) || defined(STM32F303xE) + I2C3_EV_IRQHandler, /*!< I2C3 event interrupt */ + I2C3_ER_IRQHandler, /*!< I2C3 error interrupt */ +#else + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved +#endif + USB_HP_IRQHandler, // USB High Priority remap + USB_LP_IRQHandler, // USB Low Priority remap + USB_WKUP_RMP_IRQHandler, // USB Wakup remap +#if defined(STM32F303xD) || defined(STM32F303xE) + TIM20_BRK_IRQHandler, /*!< TIM20 Break Interrupt */ + TIM20_UP_IRQHandler, /*!< TIM20 Update Interrupt */ + TIM20_TRG_COM_IRQHandler, /*!< TIM20 Trigger and Commutation Interrupt */ + TIM20_CC_IRQHandler, /*!< TIM20 Capture Compare Interrupt */ +#else + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved + Reserved_IRQHandler, // reserved +#endif + FPU_IRQHandler, // FPU +#if defined(STM32F303xD) || defined(STM32F303xE) + Reserved_IRQHandler, + Reserved_IRQHandler, + SPI4_IRQHandler /*!< SPI4 global Interrupt */ +#endif +}; + +/** + * @} + */ diff --git a/flight/pios/stm32f4xx/inc/pios_delay_raw.h b/flight/pios/stm32f4xx/inc/pios_delay_raw.h new file mode 100644 index 0000000000..6a49415633 --- /dev/null +++ b/flight/pios/stm32f4xx/inc/pios_delay_raw.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELAY Delay Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_delay_raw.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @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_DELAY_RAW_H +#define PIOS_DELAY_RAW_H + +/* these should be defined by CMSIS, but they aren't */ +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1 << 0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) + +#define PIOS_DELAY_GetRaw() (DWT_CYCCNT) + +extern uint32_t PIOS_DELAY_GetRawHz(); + +#endif /* PIOS_DELAY_RAW_H */ diff --git a/flight/pios/stm32f4xx/inc/pios_ws2811_cfg.h b/flight/pios/stm32f4xx/inc/pios_ws2811_cfg.h new file mode 100644 index 0000000000..af3876519d --- /dev/null +++ b/flight/pios/stm32f4xx/inc/pios_ws2811_cfg.h @@ -0,0 +1,136 @@ +/** + ****************************************************************************** + * + * @file pios_ws2811_cfg.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief A driver for ws2811 rgb led controller. + * this is a plain PiOS port of the very clever solution + * implemented by Omri Iluz in the chibios driver here: + * https://github.com/omriiluz/WS2812B-LED-Driver-ChibiOS + * @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_WS2811_CFG_H_ +#define PIOS_WS2811_CFG_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24)) +#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord +#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord +#define PIOS_WS2811_TIM_PERIOD 20 + +// Following times are keept on the lower side to accounts +// for bus contentions and irq response time +#define PIOS_WS2811_T0_HIGH_PERIOD 32 // .35us +/- 150nS +#define PIOS_WS2811_T1_HIGH_PERIOD 70 // .70us +/- 150nS + +#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \ + { \ + .DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \ + .DMA_Channel = channel, \ + .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ + .DMA_FIFOMode = DMA_FIFOMode_Enable, \ + .DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \ + .DMA_Memory0BaseAddr = 0, \ + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ + .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ + .DMA_MemoryInc = DMA_MemoryInc_Enable, \ + .DMA_Mode = DMA_Mode_Circular, \ + .DMA_PeripheralBaseAddr = 0, \ + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ + .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ + .DMA_Priority = DMA_Priority_VeryHigh, } + +#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \ + { \ + .DMA_BufferSize = 4, \ + .DMA_Channel = channel, \ + .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ + .DMA_FIFOMode = DMA_FIFOMode_Enable, \ + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ + .DMA_Memory0BaseAddr = 0, \ + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ + .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ + .DMA_MemoryInc = DMA_MemoryInc_Enable, \ + .DMA_Mode = DMA_Mode_Circular, \ + .DMA_PeripheralBaseAddr = 0, \ + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ + .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ + .DMA_Priority = DMA_Priority_VeryHigh, } + +#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \ + { \ + .DMA_BufferSize = 4, \ + .DMA_Channel = channel, \ + .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ + .DMA_FIFOMode = DMA_FIFOMode_Enable, \ + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ + .DMA_Memory0BaseAddr = 0, \ + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ + .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ + .DMA_MemoryInc = DMA_MemoryInc_Enable, \ + .DMA_Mode = DMA_Mode_Circular, \ + .DMA_PeripheralBaseAddr = 0, \ + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ + .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ + .DMA_Priority = DMA_Priority_High } + + +typedef uint16_t ledbuf_t; + +struct pios_ws2811_pin_cfg { + GPIO_TypeDef *gpio; + GPIO_InitTypeDef gpioInit; +}; +struct pios_ws2811_cfg { + TIM_TimeBaseInitTypeDef timerInit; + TIM_TypeDef *timer; + uint8_t timerCh1; + uint8_t timerCh2; + + DMA_InitTypeDef dmaInitCh1; + DMA_Stream_TypeDef *streamCh1; + uint32_t dmaItCh1; + + DMA_InitTypeDef dmaInitCh2; + DMA_Stream_TypeDef *streamCh2; + uint32_t dmaItCh2; + + DMA_InitTypeDef dmaInitUpdate; + DMA_Stream_TypeDef *streamUpdate; + uint32_t dmaItUpdate; + uint16_t dmaSource; + struct stm32_irq irq; +}; + +void PIOS_WS2811_Init(uint32_t *dev_id, const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg); + +void PIOS_WS2811_DMA_irq_handler(); +#endif /* PIOS_WS2811_H_ */ diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c index d6e52c6045..b572b95507 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c @@ -681,7 +681,7 @@ static uint32_t DCD_WriteEmptyTxFifo(USB_OTG_CORE_HANDLE *pdev, uint32_t epnum) } // --- start fix uint32_t fifoemptymsk; - if (len < ep->maxpacket) + if (len < ep->maxpacket || ep->xfer_count==0) { // FIFO empty fifoemptymsk = 0x1 << epnum; diff --git a/flight/pios/stm32f4xx/library.mk b/flight/pios/stm32f4xx/library.mk index 669c8c08a4..bd515456ca 100644 --- a/flight/pios/stm32f4xx/library.mk +++ b/flight/pios/stm32f4xx/library.mk @@ -15,7 +15,7 @@ LINKER_SCRIPTS_COMPAT = $(PIOS_DEVLIB)link_$(BOARD)_fw_memory.ld \ $(PIOS_DEVLIB)link_$(BOARD)_sections_compat.ld # Compiler options implied by the F4xx -CDEFS += -DSTM32F4XX +CDEFS += -DSTM32F4XX -DSTM32F4 ifeq ($(CHIPFAMILY),STM32F427_437xx) CDEFS += -DPIOS_TARGET_PROVIDES_FAST_HEAP #large heap support must be enabled if SRAM > 128K diff --git a/flight/pios/stm32f4xx/link_STM32F4xx_DI_sections.ld b/flight/pios/stm32f4xx/link_STM32F4xx_DI_sections.ld index 99a3f509f5..6cd23f430b 100644 --- a/flight/pios/stm32f4xx/link_STM32F4xx_DI_sections.ld +++ b/flight/pios/stm32f4xx/link_STM32F4xx_DI_sections.ld @@ -39,6 +39,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + /* * C++ exception handling. */ diff --git a/flight/pios/stm32f4xx/link_STM32F4xx_OSD_sections.ld b/flight/pios/stm32f4xx/link_STM32F4xx_OSD_sections.ld index 99a3f509f5..b46c691c84 100644 --- a/flight/pios/stm32f4xx/link_STM32F4xx_OSD_sections.ld +++ b/flight/pios/stm32f4xx/link_STM32F4xx_OSD_sections.ld @@ -39,6 +39,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + /* * C++ exception handling. */ diff --git a/flight/pios/stm32f4xx/link_STM32F4xx_RM_sections.ld b/flight/pios/stm32f4xx/link_STM32F4xx_RM_sections.ld index 99a3f509f5..b46c691c84 100644 --- a/flight/pios/stm32f4xx/link_STM32F4xx_RM_sections.ld +++ b/flight/pios/stm32f4xx/link_STM32F4xx_RM_sections.ld @@ -39,6 +39,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + /* * C++ exception handling. */ diff --git a/flight/pios/stm32f4xx/link_STM32F4xx_RN_sections.ld b/flight/pios/stm32f4xx/link_STM32F4xx_RN_sections.ld index b728c80f19..2462187f7f 100644 --- a/flight/pios/stm32f4xx/link_STM32F4xx_RN_sections.ld +++ b/flight/pios/stm32f4xx/link_STM32F4xx_RN_sections.ld @@ -39,6 +39,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + /* * C++ exception handling. */ diff --git a/flight/pios/stm32f4xx/link_STM32F4xx_RP_sections.ld b/flight/pios/stm32f4xx/link_STM32F4xx_RP_sections.ld index 99a3f509f5..6cd23f430b 100644 --- a/flight/pios/stm32f4xx/link_STM32F4xx_RP_sections.ld +++ b/flight/pios/stm32f4xx/link_STM32F4xx_RP_sections.ld @@ -39,6 +39,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + /* * C++ exception handling. */ diff --git a/flight/pios/stm32f4xx/link_STM32F4xx_RQ_sections.ld b/flight/pios/stm32f4xx/link_STM32F4xx_RQ_sections.ld index 41644bacb6..e6baf2d503 100644 --- a/flight/pios/stm32f4xx/link_STM32F4xx_RQ_sections.ld +++ b/flight/pios/stm32f4xx/link_STM32F4xx_RQ_sections.ld @@ -39,6 +39,16 @@ SECTIONS __module_initcall_end = .; } >FLASH + /* settings init sections */ + .initcallsettings.init : + { + . = ALIGN(4); + __settings_initcall_start = .; + KEEP(*(.initcallsettings.init)) + . = ALIGN(4); + __settings_initcall_end = .; + } >FLASH + /* * C++ exception handling. */ diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index f9c5d62b1c..272afc3f2b 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -35,14 +35,9 @@ #ifdef PIOS_INCLUDE_DELAY -/* these should be defined by CMSIS, but they aren't */ -#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) -#define CYCCNTENA (1 << 0) -#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) - - /* cycles per microsecond */ static uint32_t us_ticks; +static uint32_t raw_hz; /** * Initialises the Timer used by PIOS_DELAY functions. @@ -58,6 +53,7 @@ int32_t PIOS_DELAY_Init(void) RCC_GetClocksFreq(&clocks); us_ticks = clocks.SYSCLK_Frequency / 1000000; PIOS_DEBUG_Assert(us_ticks > 1); + raw_hz = clocks.SYSCLK_Frequency; /* turn on access to the DWT registers */ CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; @@ -82,10 +78,10 @@ int32_t PIOS_DELAY_Init(void) int32_t PIOS_DELAY_WaituS(uint32_t uS) { uint32_t elapsed = 0; - uint32_t last_count = DWT_CYCCNT; + uint32_t last_count = PIOS_DELAY_GetRaw(); for (;;) { - uint32_t current_count = DWT_CYCCNT; + uint32_t current_count = PIOS_DELAY_GetRaw(); uint32_t elapsed_uS; /* measure the time elapsed since the last time we checked */ @@ -136,7 +132,7 @@ int32_t PIOS_DELAY_WaitmS(uint32_t mS) */ uint32_t PIOS_DELAY_GetuS() { - return DWT_CYCCNT / us_ticks; + return PIOS_DELAY_GetRaw() / us_ticks; } /** @@ -150,12 +146,12 @@ uint32_t PIOS_DELAY_GetuSSince(uint32_t t) } /** - * @brief Get the raw delay timer, useful for timing - * @return Unitless value (uint32 wrap around) + * @brief Get the raw delay timer frequency + * @return raw delay timer frequency in Hz */ -uint32_t PIOS_DELAY_GetRaw() +uint32_t PIOS_DELAY_GetRawHz() { - return DWT_CYCCNT; + return raw_hz; } /** @@ -164,7 +160,7 @@ uint32_t PIOS_DELAY_GetRaw() */ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) { - uint32_t diff = DWT_CYCCNT - raw; + uint32_t diff = PIOS_DELAY_GetRaw() - raw; return diff / us_ticks; } diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c deleted file mode 100644 index bdd8244e13..0000000000 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ /dev/null @@ -1,442 +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) 2012. - * @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 - -#ifndef PIOS_INCLUDE_RTC -#error PIOS_INCLUDE_RTC must be used to use DSM -#endif - -/* 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 = 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); - - /* RX line, set high */ - GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); - - /* Wait until the bind window opens. */ - while (PIOS_DELAY_GetuS() < DSM_BIND_MIN_DELAY_US) { - ; - } - - 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_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 2821f79bee..92bb65bc59 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -159,7 +159,7 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter); static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter); static void i2c_adapter_log_fault(enum pios_i2c_error_type type); -static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter); +static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, signed portBASE_TYPE *pxHigherPriorityTaskWoken); static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = { [I2C_STATE_FSM_FAULT] = { @@ -420,18 +420,19 @@ static void go_stopping(struct pios_i2c_adapter *i2c_adapter) I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + if (i2c_adapter->callback) { + i2c_adapter_callback_handler(i2c_adapter, &pxHigherPriorityTaskWoken); + } else { #ifdef USE_FREERTOS - if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { + if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { #if defined(I2C_HALT_ON_ERRORS) - PIOS_DEBUG_Assert(0); + PIOS_DEBUG_Assert(0); #endif - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } #endif /* USE_FREERTOS */ - - if (i2c_adapter->callback) { - i2c_adapter_callback_handler(i2c_adapter); } + + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ } @@ -853,18 +854,8 @@ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) uint32_t i2c_cb_count = 0; -static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter) +static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, signed portBASE_TYPE *pxHigherPriorityTaskWoken) { - bool semaphore_success = true; - - /* Wait for the transfer to complete */ -#ifdef USE_FREERTOS - portTickType timeout; - timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); - xSemaphoreGive(i2c_adapter->sem_ready); -#endif /* USE_FREERTOS */ - /* transfer_timeout_ticks is set by PIOS_I2C_Transfer_Callback */ /* Spin waiting for the transfer to finish */ while (!i2c_adapter_fsm_terminated(i2c_adapter)) { @@ -885,17 +876,10 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter) i2c_adapter_fsm_init(i2c_adapter); } - // Execute user supplied function - i2c_adapter->callback(); - - i2c_cb_count++; - #ifdef USE_FREERTOS /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); - if (!semaphore_success) { - i2c_timeout_counter++; - } + xSemaphoreGiveFromISR(i2c_adapter->sem_busy, pxHigherPriorityTaskWoken); + #else PIOS_IRQ_Disable(); i2c_adapter->busy = 0; @@ -903,7 +887,14 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter) #endif /* USE_FREERTOS */ - return (!i2c_adapter->bus_error) && semaphore_success; + // Execute user supplied function + if (i2c_adapter->callback(PIOS_I2C_TRANSFER_OK)) { + *pxHigherPriorityTaskWoken = pdTRUE; + } + + i2c_cb_count++; + + return !i2c_adapter->bus_error; } @@ -1057,7 +1048,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; if (!PIOS_I2C_validate(i2c_adapter)) { - return -1; + return PIOS_I2C_TRANSFER_DEVICE_ERROR; } PIOS_DEBUG_Assert(txn_list); @@ -1070,13 +1061,13 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else PIOS_IRQ_Disable(); if (i2c_adapter->busy) { PIOS_IRQ_Enable(); - return -2; + return PIOS_I2C_TRANSFER_BUSY; } i2c_adapter->busy = 1; PIOS_IRQ_Enable(); @@ -1143,13 +1134,37 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - i2c_adapter->nack ? -3 : - 0; + return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT : + i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR : + i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK : + PIOS_I2C_TRANSFER_OK; +} + +static int32_t PIOS_I2C_Transfer_Callback_Internal(struct pios_i2c_adapter *i2c_adapter, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback) +{ + PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + + i2c_adapter->first_txn = &txn_list[0]; + i2c_adapter->last_txn = &txn_list[num_txns - 1]; + i2c_adapter->active_txn = i2c_adapter->first_txn; + i2c_adapter->bus_error = false; + i2c_adapter->nack = false; + i2c_adapter->callback = callback; + + // Estimate bytes of transmission. Per txns: 1 adress byte + length + i2c_adapter->transfer_timeout_ticks = num_txns; + for (uint32_t i = 0; i < num_txns; i++) { + i2c_adapter->transfer_timeout_ticks += txn_list[i].len; + } + // timeout if it takes eight times the expected time + i2c_adapter->transfer_timeout_ticks <<= 3; + + i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + + return 0; } -int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback) +int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback) { struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; @@ -1162,47 +1177,62 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx PIOS_DEBUG_Assert(txn_list); PIOS_DEBUG_Assert(num_txns); - bool semaphore_success = true; - #ifdef USE_FREERTOS /* Lock the bus */ portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else + PIOS_IRQ_Disable(); if (i2c_adapter->busy) { PIOS_IRQ_Enable(); - return -2; + return PIOS_I2C_TRANSFER_BUSY; } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + return PIOS_I2C_Transfer_Callback_Internal(i2c_adapter, txn_list, num_txns, callback); +} - i2c_adapter->first_txn = &txn_list[0]; - i2c_adapter->last_txn = &txn_list[num_txns - 1]; - i2c_adapter->active_txn = i2c_adapter->first_txn; +int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback, bool *woken) +{ + // FIXME: only supports transfer sizes up to 255 bytes + struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; + + bool valid = PIOS_I2C_validate(i2c_adapter); + + PIOS_Assert(valid) + + PIOS_DEBUG_Assert(txn_list); + PIOS_DEBUG_Assert(num_txns); #ifdef USE_FREERTOS - /* Make sure the done/ready semaphore is consumed before we start */ - semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); -#endif + signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - // used in the i2c_adapter_callback_handler function - // Estimate bytes of transmission. Per txns: 1 adress byte + length - i2c_adapter->transfer_timeout_ticks = num_txns; - for (uint32_t i = 0; i < num_txns; i++) { - i2c_adapter->transfer_timeout_ticks += txn_list[i].len; + /* Lock the bus */ + bool locked = xSemaphoreTakeFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken) == pdTRUE; + + if (xHigherPriorityTaskWoken == pdTRUE) { + *woken = true; } - // timeout if it takes eight times the expected time - i2c_adapter->transfer_timeout_ticks <<= 3; - i2c_adapter->callback = callback; - i2c_adapter->bus_error = false; - i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); + if (!locked) { + return PIOS_I2C_TRANSFER_BUSY; + } +#else + PIOS_IRQ_Disable(); + if (i2c_adapter->busy) { + PIOS_IRQ_Enable(); + return PIOS_I2C_TRANSFER_BUSY; + } + i2c_adapter->busy = 1; + PIOS_IRQ_Enable(); +#endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : 0; + return PIOS_I2C_Transfer_Callback_Internal(i2c_adapter, txn_list, num_txns, callback); } void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) diff --git a/flight/pios/stm32f4xx/pios_servo.c b/flight/pios/stm32f4xx/pios_servo.c deleted file mode 100644 index 84a450b7fd..0000000000 --- a/flight/pios/stm32f4xx/pios_servo.c +++ /dev/null @@ -1,276 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_SERVO RC Servo Functions - * @brief Code to do set RC servo output - * @{ - * - * @file pios_servo.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief RC Servo routines (STM32 dependent) - * @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_SERVO - -#include "pios_servo_priv.h" -#include "pios_tim_priv.h" - -/* Private Function Prototypes */ - -static const struct pios_servo_cfg *servo_cfg; - -// determine if the related timer will work in synchronous (or OneShot/OneShot125) One Pulse mode. -static uint8_t pios_servo_bank_mode[PIOS_SERVO_BANKS] = { 0 }; -// used to skip updates when pulse length is higher than update cycle -static uint16_t pios_servo_bank_next_update[PIOS_SERVO_BANKS] = { 0 }; -static uint16_t pios_servo_bank_max_pulse[PIOS_SERVO_BANKS] = { 0 }; -// timer associated to each bank -static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 }; - -// index of bank used for each pin -static uint8_t *pios_servo_pin_bank; - -#define PIOS_SERVO_TIMER_CLOCK 1000000 -#define PIOS_SERVO_SAFE_MARGIN 50 -/** - * Initialise Servos - */ -int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg) -{ - uint32_t tim_id; - - if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { - return -1; - } - - /* Store away the requested configuration */ - servo_cfg = cfg; - pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels); - - uint8_t bank = 0; - for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { - const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - bool new = true; - /* See if any previous channels use that same timer */ - for (uint8_t j = 0; (j < i) && new; j++) { - new &= chan->timer != servo_cfg->channels[j].timer; - } - - if (new) { - PIOS_Assert(bank < PIOS_SERVO_BANKS); - for (uint8_t j = i; j < servo_cfg->num_channels; j++) { - if (servo_cfg->channels[j].timer == chan->timer) { - pios_servo_pin_bank[j] = bank; - } - } - pios_servo_bank_timer[bank] = chan->timer; - - TIM_ARRPreloadConfig(chan->timer, ENABLE); - TIM_CtrlPWMOutputs(chan->timer, ENABLE); - TIM_Cmd(chan->timer, DISABLE); - - bank++; - } - - /* Set up for output compare function */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init); - TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init); - TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init); - TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init); - TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); - break; - } - } - - return 0; -} - -void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode) -{ - PIOS_Assert(bank < PIOS_SERVO_BANKS); - pios_servo_bank_mode[bank] = mode; - - if (pios_servo_bank_timer[bank]) { - for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { - if (pios_servo_pin_bank[i] == bank) { - const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - /* Set up for output compare function */ - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - case TIM_Channel_2: - TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - case TIM_Channel_3: - TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - case TIM_Channel_4: - TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High); - break; - } - } - } - - // Setup the timer accordingly - TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive); - TIM_Cmd(pios_servo_bank_timer[bank], ENABLE); - } -} - - -void PIOS_Servo_Update() -{ - for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) { - const TIM_TypeDef *timer = pios_servo_bank_timer[i]; - if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) { - // a pulse to be generated is longer than cycle period. skip this update. - if (TIM_GetCounter((TIM_TypeDef *)timer) > (uint32_t)(pios_servo_bank_next_update[i] + PIOS_SERVO_SAFE_MARGIN)) { - TIM_GenerateEvent((TIM_TypeDef *)timer, TIM_EventSource_Update); - pios_servo_bank_next_update[i] = pios_servo_bank_max_pulse[i]; - } - } - pios_servo_bank_max_pulse[i] = 0; - } - for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) { - uint8_t bank = pios_servo_pin_bank[i]; - uint8_t mode = pios_servo_bank_mode[bank]; - if (mode == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) { - /* Update the position */ - const struct pios_tim_channel *chan = &servo_cfg->channels[i]; - - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, 0); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, 0); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, 0); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, 0); - break; - } - } - } -} -/** - * Set the servo update rate (Max 500Hz) - * \param[in] array of rates in Hz - * \param[in] array of timer clocks in Hz - * \param[in] maximum number of banks - */ -void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks) -{ - PIOS_Assert(banks <= PIOS_SERVO_BANKS); - if (!servo_cfg) { - return; - } - - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - - for (uint8_t i = 0; i < banks && i < PIOS_SERVO_BANKS; i++) { - const TIM_TypeDef *timer = pios_servo_bank_timer[i]; - if (timer) { - uint32_t new_clock = PIOS_SERVO_TIMER_CLOCK; - if (clock[i]) { - new_clock = clock[i]; - } - // Choose the correct prescaler value for the APB the timer is attached - if (timer == TIM1 || timer == TIM8 || timer == TIM9 || timer == TIM10 || timer == TIM11) { - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / new_clock) - 1; - } else { - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / new_clock) - 1; - } - TIM_TimeBaseStructure.TIM_Period = ((new_clock / speeds[i]) - 1); - TIM_TimeBaseInit((TIM_TypeDef *)timer, &TIM_TimeBaseStructure); - } - } -} - -/** - * Set servo position - * \param[in] Servo Servo number (0-7) - * \param[in] Position Servo position in microseconds - */ -void PIOS_Servo_Set(uint8_t servo, uint16_t position) -{ - /* Make sure servo exists */ - if (!servo_cfg || servo >= servo_cfg->num_channels) { - return; - } - - - /* Update the position */ - const struct pios_tim_channel *chan = &servo_cfg->channels[servo]; - uint16_t val = position; - uint16_t margin = chan->timer->ARR / 50; // Leave 2% of period as margin to prevent overlaps - if (val > (chan->timer->ARR - margin)) { - val = chan->timer->ARR - margin; - } - - uint8_t bank = pios_servo_pin_bank[servo]; - if (pios_servo_bank_max_pulse[bank] < val) { - pios_servo_bank_max_pulse[bank] = val; - } - switch (chan->timer_chan) { - case TIM_Channel_1: - TIM_SetCompare1(chan->timer, val); - break; - case TIM_Channel_2: - TIM_SetCompare2(chan->timer, val); - break; - case TIM_Channel_3: - TIM_SetCompare3(chan->timer, val); - break; - case TIM_Channel_4: - TIM_SetCompare4(chan->timer, val); - break; - } -} - -uint8_t PIOS_Servo_GetPinBank(uint8_t pin) -{ - if (pin < servo_cfg->num_channels) { - return pios_servo_pin_bank[pin]; - } else { - return 0; - } -} - -#endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/pios/stm32f4xx/pios_sys.c b/flight/pios/stm32f4xx/pios_sys.c index ab6759fe9e..c03297871c 100644 --- a/flight/pios/stm32f4xx/pios_sys.c +++ b/flight/pios/stm32f4xx/pios_sys.c @@ -204,12 +204,17 @@ int32_t PIOS_SYS_Reset(void) PIOS_IRQ_Disable(); // turn off all board LEDs -#if defined(PIOS_LED_HEARTBEAT) +#ifdef PIOS_INCLUDE_LED +# ifdef PIOS_LED_HEARTBEAT PIOS_LED_Off(PIOS_LED_HEARTBEAT); -#endif /* PIOS_LED_HEARTBEAT */ -#if defined(PIOS_LED_ALARM) +# endif /* PIOS_LED_HEARTBEAT */ +# ifdef PIOS_LED_ALARM PIOS_LED_Off(PIOS_LED_ALARM); -#endif /* PIOS_LED_ALARM */ +# endif /* PIOS_LED_ALARM */ +# ifdef PIOS_BUZZER_ALARM + PIOS_LED_Off(PIOS_BUZZER_ALARM); +# endif /* PIOS_BUZZER_ALARM */ +#endif /* PIOS_INCLUDE_LED */ /* XXX F10x port resets most (but not all) peripherals ... do we care? */ diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index bcc26cacf7..fef3165ddf 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -7,7 +7,8 @@ * @{ * * @file pios_usart.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2016-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief USART commands. Inits USARTs, controls USARTs & Interupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * @@ -40,19 +41,23 @@ /* Provide a COM driver */ 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_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); 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, + .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 { @@ -62,11 +67,13 @@ enum pios_usart_dev_magic { struct pios_usart_dev { enum pios_usart_dev_magic magic; const struct pios_usart_cfg *cfg; - + USART_InitTypeDef init; pios_com_callback rx_in_cb; uint32_t rx_in_context; pios_com_callback tx_out_cb; uint32_t tx_out_context; + bool config_locked; + uint8_t irq_channel; }; static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) @@ -74,6 +81,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) { @@ -166,6 +197,46 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) PIOS_DEBUG_Assert(usart_id); PIOS_DEBUG_Assert(cfg); + uint32_t *local_id; + uint8_t irq_channel; + + switch ((uint32_t)cfg->regs) { + case (uint32_t)USART1: + local_id = &PIOS_USART_1_id; + irq_channel = USART1_IRQn; + break; + case (uint32_t)USART2: + local_id = &PIOS_USART_2_id; + irq_channel = USART2_IRQn; + break; +#if !defined(STM32F411xE) + case (uint32_t)USART3: + local_id = &PIOS_USART_3_id; + irq_channel = USART3_IRQn; + break; + case (uint32_t)UART4: + local_id = &PIOS_USART_4_id; + irq_channel = UART4_IRQn; + break; + case (uint32_t)UART5: + local_id = &PIOS_USART_5_id; + irq_channel = UART5_IRQn; + break; +#endif /* STM32F411xE */ + case (uint32_t)USART6: + local_id = &PIOS_USART_6_id; + irq_channel = USART6_IRQn; + break; + default: + goto out_fail; + } + + if (*local_id) { + /* this port is already open */ + *usart_id = *local_id; + return 0; + } + struct pios_usart_dev *usart_dev; usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc(); @@ -175,62 +246,42 @@ 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->irq_channel = irq_channel; - /* 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); - } + /* Initialize the comm parameter structure */ + USART_StructInit(&usart_dev->init); // 9600 8n1 + + /* 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) { 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); } - - /* Configure the USART */ - USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); - - *usart_id = (uint32_t)usart_dev; - - /* Configure USART Interrupts */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - PIOS_USART_1_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART2: - PIOS_USART_2_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART3: - PIOS_USART_3_id = (uint32_t)usart_dev; - break; - case (uint32_t)UART4: - PIOS_USART_4_id = (uint32_t)usart_dev; - break; - case (uint32_t)UART5: - PIOS_USART_5_id = (uint32_t)usart_dev; - break; - case (uint32_t)USART6: - PIOS_USART_6_id = (uint32_t)usart_dev; - break; +#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); + + GPIO_WriteBit(PIOS_USART_INVERTER_GPIO, + PIOS_USART_INVERTER_PIN, + PIOS_USART_INVERTER_DISABLE); } - NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); +#endif - // FIXME XXX Clear / reset uart here - sends NUL char else + *usart_id = (uint32_t)usart_dev; + *local_id = (uint32_t)usart_dev; - /* Enable USART */ - USART_Cmd(usart_dev->cfg->regs, ENABLE); + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); return 0; @@ -238,6 +289,50 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) 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 */ + { // fix parity stuff + USART_InitTypeDef init = usart_dev->init; + + if ((init.USART_Parity != USART_Parity_No) && (init.USART_WordLength == USART_WordLength_8b)) { + init.USART_WordLength = USART_WordLength_9b; + } + + USART_Init(usart_dev->cfg->regs, &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; @@ -272,16 +367,89 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) PIOS_Assert(valid); - USART_InitTypeDef USART_InitStructure; + if (usart_dev->config_locked) { + return; + } + + /* Use our working copy of the usart init structure */ + usart_dev->init.USART_BaudRate = baud; + + PIOS_USART_Setup(usart_dev); +} - /* Start with a copy of the default configuration for the peripheral */ - USART_InitStructure = usart_dev->cfg->init; +/** + * Changes configuration of the USART peripheral without re-initialising. + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] word_len Requested word length + * \param[in] stop_bits Requested stop bits + * \param[in] parity Requested parity + * \param[in] baud_rate Requested baud rate + * \param[in] mode Requested 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) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; - /* Adjust the baud rate */ - USART_InitStructure.USART_BaudRate = baud; + bool valid = PIOS_USART_validate(usart_dev); - /* Write back the new configuration */ - USART_Init(usart_dev->cfg->regs, &USART_InitStructure); + PIOS_Assert(valid); + + if (usart_dev->config_locked) { + return; + } + + switch (word_len) { + case PIOS_COM_Word_length_8b: + usart_dev->init.USART_WordLength = USART_WordLength_8b; + break; + case PIOS_COM_Word_length_9b: + usart_dev->init.USART_WordLength = USART_WordLength_9b; + break; + default: + break; + } + + switch (stop_bits) { + case PIOS_COM_StopBits_0_5: + usart_dev->init.USART_StopBits = USART_StopBits_0_5; + break; + case PIOS_COM_StopBits_1: + usart_dev->init.USART_StopBits = USART_StopBits_1; + break; + case PIOS_COM_StopBits_1_5: + usart_dev->init.USART_StopBits = USART_StopBits_1_5; + break; + case PIOS_COM_StopBits_2: + usart_dev->init.USART_StopBits = USART_StopBits_2; + break; + default: + break; + } + + switch (parity) { + case PIOS_COM_Parity_No: + usart_dev->init.USART_Parity = USART_Parity_No; + break; + case PIOS_COM_Parity_Even: + usart_dev->init.USART_Parity = USART_Parity_Even; + break; + case PIOS_COM_Parity_Odd: + usart_dev->init.USART_Parity = USART_Parity_Odd; + break; + default: + break; + } + + if (baud_rate) { + usart_dev->init.USART_BaudRate = baud_rate; + } + + PIOS_USART_Setup(usart_dev); } static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state) @@ -312,8 +480,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) @@ -328,8 +500,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) @@ -382,6 +558,62 @@ 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); + + /* 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; + 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; + case PIOS_IOCTL_USART_LOCK_CONFIG: + usart_dev->config_locked = *(bool *)param; + break; + default: + return COM_IOCTL_ENOSYS; /* unknown ioctl */ + } + + return 0; +} + #endif /* PIOS_INCLUDE_USART */ /** diff --git a/flight/pios/stm32f4xx/pios_usb.c b/flight/pios/stm32f4xx/pios_usb.c index b0f59abc42..158fd8128a 100644 --- a/flight/pios/stm32f4xx/pios_usb.c +++ b/flight/pios/stm32f4xx/pios_usb.c @@ -38,10 +38,17 @@ #include /* Rx/Tx status */ -static uint8_t transfer_possible = 0; +static volatile uint8_t transfer_possible = 0; +#ifdef PIOS_INCLUDE_FREERTOS static void(*disconnection_cb_list[3]) (void); +struct { + void (*callback)(bool connected, uint32_t context); + uint32_t context; +} connectionState_cb_list[3]; +#endif + enum pios_usb_dev_magic { PIOS_USB_DEV_MAGIC = 0x17365904, }; @@ -55,6 +62,7 @@ struct pios_usb_dev { }; #ifdef PIOS_INCLUDE_FREERTOS static void raiseDisconnectionCallbacks(void); +static void raiseConnectionStateCallback(bool connected); #endif /** * @brief Validate the usb device structure @@ -140,6 +148,9 @@ int32_t PIOS_USB_Init(uint32_t *usb_id, const struct pios_usb_cfg *cfg) */ int32_t PIOS_USB_ChangeConnectionState(bool connected) { +#ifdef PIOS_INCLUDE_FREERTOS + static volatile uint8_t lastStatus = 2; // 2 is "no last status" +#endif // In all cases: re-initialise USB HID driver if (connected) { transfer_possible = 1; @@ -158,6 +169,16 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected) #endif } +#ifdef PIOS_INCLUDE_FREERTOS + raiseConnectionStateCallback(connected); + if (lastStatus != transfer_possible) { + if (lastStatus == 1) { + raiseDisconnectionCallbacks(); + } + lastStatus = transfer_possible; + } +#endif + return 0; } @@ -175,28 +196,19 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id) return false; } - usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; -// Please note that checks of transfer_possible and the reconnection handling is -// suppressed for non freertos mode (aka bootloader) as this is causing problems detecting connection and -// broken communications. -#ifdef PIOS_INCLUDE_FREERTOS - static bool lastStatus = false; - bool status = usb_found != 0 && transfer_possible ? 1 : 0; - bool reconnect = false; - if (xSemaphoreTakeFromISR(usb_dev->statusCheckSemaphore, NULL) == pdTRUE) { - reconnect = (lastStatus && !status); - lastStatus = status; - xSemaphoreGiveFromISR(usb_dev->statusCheckSemaphore, NULL); - } - if (reconnect) { - raiseDisconnectionCallbacks(); - } - return status; + return transfer_possible; +} -#else - return usb_found; +/** + * This function returns wether a USB cable (5V pin) has been detected + * \return true: cable connected + * \return false: cable not detected (no cable or cable with no power) + */ +bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id) +{ + struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id; -#endif + return ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low; } /* @@ -204,6 +216,7 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id) * Register a physical disconnection callback * */ +#ifdef PIOS_INCLUDE_FREERTOS void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void)) { PIOS_Assert(disconnectionCB); @@ -215,7 +228,6 @@ void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void)) } PIOS_Assert(0); } -#ifdef PIOS_INCLUDE_FREERTOS static void raiseDisconnectionCallbacks(void) { uint32_t i = 0; @@ -224,7 +236,38 @@ static void raiseDisconnectionCallbacks(void) (disconnection_cb_list[i++])(); } } -#endif + +void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context) +{ + PIOS_Assert(connectionStateCallback); + + for (uint32_t i = 0; i < NELEMENTS(connectionState_cb_list); i++) { + if (connectionState_cb_list[i].callback == NULL) { + connectionState_cb_list[i].callback = connectionStateCallback; + connectionState_cb_list[i].context = context; + return; + } + } + + PIOS_Assert(0); +} + +static void raiseConnectionStateCallback(bool connected) +{ + uint32_t i = 0; + + while (i < NELEMENTS(connectionState_cb_list) && connectionState_cb_list[i].callback != NULL) { + connectionState_cb_list[i].callback(connected, connectionState_cb_list[i].context); + i++; + } +} +#else /* PIOS_INCLUDE_FREERTOS */ +void PIOS_USB_RegisterDisconnectionCallback(__attribute__((unused)) void (*disconnectionCB)(void)) +{} +void PIOS_USB_RegisterConnectionStateCallback(__attribute__((unused)) void (*connectionStateCallback)(bool connected, uint32_t context), __attribute__((unused)) uint32_t context) +{} +#endif /* PIOS_INCLUDE_FREERTOS */ + /* * * Provide STM32 USB OTG BSP layer API diff --git a/flight/pios/stm32f4xx/pios_usb_cdc.c b/flight/pios/stm32f4xx/pios_usb_cdc.c index 62349b1d92..e8e8470656 100644 --- a/flight/pios/stm32f4xx/pios_usb_cdc.c +++ b/flight/pios/stm32f4xx/pios_usb_cdc.c @@ -36,12 +36,15 @@ #include "pios_usb_cdc_priv.h" #include "pios_usb_board_data.h" /* PIOS_BOARD_*_DATA_LENGTH */ #include "pios_usbhook.h" /* PIOS_USBHOOK_* */ +#include "usb_core.h" /* USB_OTG_EP_RX_VALID */ /* Implement COM layer driver API */ static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context); static void PIOS_USB_CDC_RegisterCtrlLineCallback(uint32_t usbcdc_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); static void PIOS_USB_CDC_RegisterBaudRateCallback(uint32_t usbcdc_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context); +static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available baud_rate_cb, uint32_t context); +static void PIOS_USB_CDC_ChangeConnectionState(bool connected, uint32_t usbcdc_id); static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail); static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail); static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id); @@ -54,6 +57,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = { .bind_ctrl_line_cb = PIOS_USB_CDC_RegisterCtrlLineCallback, .bind_baud_rate_cb = PIOS_USB_CDC_RegisterBaudRateCallback, .available = PIOS_USB_CDC_Available, + .bind_available_cb = PIOS_USB_CDC_RegisterAvailableCallback, }; enum pios_usb_cdc_dev_magic { @@ -74,12 +78,13 @@ struct pios_usb_cdc_dev { uint32_t ctrl_line_context; pios_com_callback_baud_rate baud_rate_cb; uint32_t baud_rate_context; + pios_com_callback_available available_cb; + uint32_t available_context; bool usb_ctrl_if_enabled; bool usb_data_if_enabled; uint8_t rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH] __attribute__((aligned(4))); - volatile bool rx_active; /* * NOTE: This is -1 as somewhat of a hack. It ensures that we always send packets @@ -186,8 +191,7 @@ int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cf pios_usb_cdc_id = (uint32_t)usb_cdc_dev; - /* Rx and Tx are not active yet */ - usb_cdc_dev->rx_active = false; + /* Tx and Rx are not active yet */ usb_cdc_dev->tx_active = false; /* Clear stats */ @@ -205,6 +209,8 @@ int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cf usb_cdc_dev->usb_data_if_enabled = false; PIOS_USBHOOK_RegisterIfOps(cfg->data_if, &usb_cdc_data_ifops, (uint32_t)usb_cdc_dev); + PIOS_USB_RegisterConnectionStateCallback(PIOS_USB_CDC_ChangeConnectionState, (uint32_t)usb_cdc_dev); + *usbcdc_id = (uint32_t)usb_cdc_dev; return 0; @@ -268,11 +274,11 @@ static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail) } // If endpoint was stalled and there is now space make it valid - if (!usb_cdc_dev->rx_active && (rx_bytes_avail >= PIOS_USB_BOARD_CDC_DATA_LENGTH)) { + if ((PIOS_USBHOOK_EndpointGetStatus(usb_cdc_dev->cfg->data_rx_ep) != USB_OTG_EP_RX_VALID) + && (rx_bytes_avail >= PIOS_USB_BOARD_CDC_DATA_LENGTH)) { PIOS_USBHOOK_EndpointRx(usb_cdc_dev->cfg->data_rx_ep, usb_cdc_dev->rx_packet_buffer, sizeof(usb_cdc_dev->rx_packet_buffer)); - usb_cdc_dev->rx_active = true; } } @@ -390,7 +396,7 @@ static void PIOS_USB_CDC_CTRL_IF_DeInit(uint32_t usb_cdc_id) } /* DeRegister endpoint specific callbacks with the USBHOOK layer */ - usb_cdc_dev->usb_data_if_enabled = false; + usb_cdc_dev->usb_ctrl_if_enabled = false; } static uint8_t cdc_altset; @@ -472,8 +478,7 @@ static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id) PIOS_Assert(valid); - return (PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) && - (control_line_state & USB_CDC_CONTROL_LINE_STATE_DTE_PRESENT)) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE; + return PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE; } /** @@ -602,6 +607,11 @@ static void PIOS_USB_CDC_DATA_IF_Init(uint32_t usb_cdc_id) PIOS_USB_CDC_DATA_EP_OUT_Callback, (uint32_t)usb_cdc_dev); usb_cdc_dev->usb_data_if_enabled = true; + usb_cdc_dev->tx_active = false; + /* Activate rx prophylactically */ + PIOS_USBHOOK_EndpointRx(usb_cdc_dev->cfg->data_rx_ep, + usb_cdc_dev->rx_packet_buffer, + sizeof(usb_cdc_dev->rx_packet_buffer)); } static void PIOS_USB_CDC_DATA_IF_DeInit(uint32_t usb_cdc_id) @@ -681,7 +691,6 @@ static bool PIOS_USB_CDC_DATA_EP_OUT_Callback( if (!usb_cdc_dev->rx_in_cb) { /* No Rx call back registered, disable the receiver */ - usb_cdc_dev->rx_active = false; return false; } @@ -708,7 +717,6 @@ static bool PIOS_USB_CDC_DATA_EP_OUT_Callback( rc = true; } else { /* Not enough room left for a message, apply backpressure */ - usb_cdc_dev->rx_active = false; rc = false; } @@ -721,4 +729,34 @@ static bool PIOS_USB_CDC_DATA_EP_OUT_Callback( return rc; } +static void PIOS_USB_CDC_ChangeConnectionState(__attribute__((unused)) bool connected, uint32_t usbcdc_id) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + if (usb_cdc_dev->available_cb) { + (usb_cdc_dev->available_cb)(usb_cdc_dev->available_context, PIOS_USB_CDC_Available(usbcdc_id)); + } +} + + +static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available available_cb, uint32_t context) +{ + struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id; + + bool valid = PIOS_USB_CDC_validate(usb_cdc_dev); + + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_cdc_dev->available_context = context; + usb_cdc_dev->available_cb = available_cb; +} + #endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/pios/stm32f4xx/pios_usbhook.c b/flight/pios/stm32f4xx/pios_usbhook.c index dc276c4915..d587905dba 100644 --- a/flight/pios/stm32f4xx/pios_usbhook.c +++ b/flight/pios/stm32f4xx/pios_usbhook.c @@ -218,6 +218,11 @@ void PIOS_USBHOOK_EndpointRx(uint8_t epnum, uint8_t *buf, uint16_t len) DCD_EP_PrepareRx(&pios_usb_otg_core_handle, epnum, buf, len); } +uint32_t PIOS_USBHOOK_EndpointGetStatus(uint8_t epnum) +{ + return DCD_GetEPStatus(&pios_usb_otg_core_handle, epnum); +} + /* * Device level hooks into STM USB library */ @@ -275,7 +280,9 @@ static USBD_DEVICE device_callbacks = { static void PIOS_USBHOOK_USR_Init(void) { PIOS_USB_ChangeConnectionState(false); - reconnect(); + // reconnect dev logically on init (previously a call to reconnect()) + DCD_DevDisconnect(&pios_usb_otg_core_handle); + DCD_DevConnect(&pios_usb_otg_core_handle); } static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed) @@ -486,9 +493,24 @@ static USBD_Class_cb_TypeDef class_callbacks = { static void reconnect(void) { - /* Force a physical disconnect/reconnect */ - DCD_DevDisconnect(&pios_usb_otg_core_handle); - DCD_DevConnect(&pios_usb_otg_core_handle); + static volatile bool in_reconnect = false; + + /* Force a complete device reset. This can trigger a call to reconnect() so prevent recursion */ + if (!in_reconnect) { + in_reconnect = true; // save since volatile and STM32F4 is single core + // disable USB device + DCD_DevDisconnect(&pios_usb_otg_core_handle); + USBD_DeInit(&pios_usb_otg_core_handle); + USB_OTG_StopDevice(&pios_usb_otg_core_handle); + // enable USB device + USBD_Init(&pios_usb_otg_core_handle, + USB_OTG_FS_CORE_ID, + &device_callbacks, + &class_callbacks, + &user_callbacks); + + in_reconnect = false; + } } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/stm32f4xx/pios_ws2811.c b/flight/pios/stm32f4xx/pios_ws2811.c index f0a9ddf298..5a3a8a1111 100644 --- a/flight/pios/stm32f4xx/pios_ws2811.c +++ b/flight/pios/stm32f4xx/pios_ws2811.c @@ -163,7 +163,9 @@ static void genericTIM_OCxPreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreloa * */ -void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg) +#define PIOS_WS2811_MAGIC 0x00281100 + +void PIOS_WS2811_Init(uint32_t *dev_id, const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg) { assert_param(ws2811_cfg); assert_param(ws2811_pin_cfg); @@ -184,6 +186,10 @@ void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pio // Setup timers setupTimer(); setupDMA(); + + // This is required so client (for example Notify module) + // can test for != 0 to know if device is configured or not. + *dev_id = PIOS_WS2811_MAGIC; } void setupTimer() diff --git a/flight/targets/boards/ccf3d/board-info.mk b/flight/targets/boards/ccf3d/board-info.mk new file mode 100644 index 0000000000..e5d65450dd --- /dev/null +++ b/flight/targets/boards/ccf3d/board-info.mk @@ -0,0 +1,19 @@ +BOARD_TYPE := 0x04 +BOARD_REVISION := 0x03 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +CHIP := STM32F303xC + +OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg +OPENOCD_CONFIG := stm32f3x.cfg + +# Note: These must match the values in link_$(BOARD)_memory.ld +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region +FW_BANK_BASE := 0x0800C000 # Start of firmware flash +FW_BANK_SIZE := 0x00034000 # Should include FW_DESC_SIZE (208kB) + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/ccf3d/board_hw_defs.c b/flight/targets/boards/ccf3d/board_hw_defs.c new file mode 100644 index 0000000000..4061f9cdd1 --- /dev/null +++ b/flight/targets/boards/ccf3d/board_hw_defs.c @@ -0,0 +1,860 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the CCF3D board. + *****************************************************************************/ +/* + * 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 + */ + +#if defined(PIOS_INCLUDE_LED) + +#include + +static const struct pios_gpio pios_leds_cc3d[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, +// .remap = GPIO_Remap_SWJ_JTAGDisable, + .active_low = true + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg_cc3d = { + .gpios = pios_leds_cc3d, + .num_gpios = NELEMENTS(pios_leds_cc3d), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg_cc3d; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Gyro interface */ +void PIOS_SPI_gyro_irq_handler(void); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); +static const struct pios_spi_cfg pios_spi_gyro_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi_gyro_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); +} + + +/* Flash/Accel Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_flash_accel_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, /* NOTE: was GPIO_Mode_IN_FLOATING */ + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + } + },{ + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + } + }, +}; + +static uint32_t pios_spi_flash_accel_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); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" + +static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ +}; + +#include "pios_flash.h" + +#endif /* PIOS_INCLUDE_FLASH */ + +/* + * ADC system + */ +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +extern void PIOS_ADC_handler(void); +void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); +// Remap the ADC DMA handler to this one +static const struct pios_adc_cfg pios_adc_cfg = { + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, +}; + +void PIOS_ADC_handler() +{ + PIOS_ADC_DMA_Handler(); +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 + +static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM4, 1, B, 6), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, B, 5), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1), +}; + +static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM4, 4, B, 9), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 2, B, 7), + TIM_SERVO_CHANNEL_CONFIG(TIM1, 1, A, 8), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, B, 4), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), + + // Receiver port pins + // S3-S6 inputs are used as outputs in this case + + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1), +}; + +static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11); + + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_main_cfg = USART_CONFIG(USART1, A, 10, A, 9); +static const struct pios_usart_cfg pios_usart_flexi_cfg = USART_CONFIG(USART3, B, 11, B, 10); +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_rcvrport_pins, + .num_channels = 6, +}; + +const struct pios_servo_cfg pios_servo_rcvr_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_rcvrport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, +}; + +const struct pios_ppm_cfg pios_ppm_pin8_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[5], + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM */ + +#if defined(PIOS_INCLUDE_PPM_FLEXI) +#include + +const struct pios_ppm_cfg pios_ppm_flexi_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_flexi_port, + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + +/* + * PWM Inputs + */ +#if defined(PIOS_INCLUDE_PWM) +#include + +const struct pios_pwm_cfg pios_pwm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), +}; + +const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Leave the first channel for PPM use and use the rest for PWM */ + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, +}; + +#endif /* if defined(PIOS_INCLUDE_PWM) */ + + +/* + * SONAR Inputs + */ +#if defined(PIOS_INCLUDE_HCSR04) +#include + +static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { + { + .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, + }, +}; + +const struct pios_hcsr04_cfg pios_hcsr04_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_flexi_adapter_ev_irq_handler(void); +void PIOS_I2C_flexi_adapter_er_irq_handler(void); +void I2C2_EV_EXTI24_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = { + .regs = I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DigitalFilter = 0x00, + .I2C_AnalogFilter = I2C_AnalogFilter_Enable, + .I2C_Timing = 0x70310309, // 50kHz I2C @ 8MHz input -> PRESC=0x7, SCLDEL=0x3, SDADEL=0x1, SCLH=0x03, SCLL=0x09 + }, + .remapSDA = GPIO_AF_4, /* I2C1, I2C2 */ + .remapSCL = GPIO_AF_4, /* I2C1, I2C2 */ + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_flexi_adapter_id; +void PIOS_I2C_flexi_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_flexi_adapter_id); +} + +void PIOS_I2C_flexi_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_flexi_adapter_id); +} + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_DOWN, + }, + }, + .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_cc3d; +} + +#endif /* PIOS_INCLUDE_USB */ + +/** + * Configuration for 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_Line3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .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 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 +}; + +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/ccf3d/bootloader/Makefile b/flight/targets/boards/ccf3d/bootloader/Makefile new file mode 100644 index 0000000000..152ac0a87c --- /dev/null +++ b/flight/targets/boards/ccf3d/bootloader/Makefile @@ -0,0 +1,29 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for CCF3D, board revision $(BOARD_REVISION)) diff --git a/flight/targets/boards/ccf3d/bootloader/inc/pios_config.h b/flight/targets/boards/ccf3d/bootloader/inc/pios_config.h new file mode 100644 index 0000000000..3586da276e --- /dev/null +++ b/flight/targets/boards/ccf3d/bootloader/inc/pios_config.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * @brief These files contain the code to the CCF3D Bootloader. + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/ccf3d/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/ccf3d/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..84019b78e3 --- /dev/null +++ b/flight/targets/boards/ccf3d/bootloader/inc/pios_usb_board_data.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 2 + +#include /* struct usb_* */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_CCF3D, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" + +/* + * The bootloader uses a simplified report structure + * BL: ... + * FW: ... + * This define changes the behaviour in pios_usb_hid.c + */ +#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/ccf3d/bootloader/main.c b/flight/targets/boards/ccf3d/bootloader/main.c new file mode 100644 index 0000000000..db1158517a --- /dev/null +++ b/flight/targets/boards/ccf3d/bootloader/main.c @@ -0,0 +1,229 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This is the file with the main function of the LibrePilot BootLoader + * @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 +#include +#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) + +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + + +//////////////////////////////////////// +uint8_t tempcount = 0; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; +uint8_t User_DFU_request = FALSE; +static uint8_t mReceive_Buffer[63]; +/* Private function prototypes -----------------------------------------------*/ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); +uint8_t processRX(); +void jump_to_app(); + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + USB_connected = PIOS_USB_CableConnected(0); + + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } + + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } + + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; + + if (JumpToApp == TRUE) { + jump_to_app(); + } + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } + + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } +} + +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } +} +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} + +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); +} diff --git a/flight/targets/boards/ccf3d/bootloader/pios_board.c b/flight/targets/boards/ccf3d/bootloader/pios_board.c new file mode 100644 index 0000000000..bf8f41714e --- /dev/null +++ b/flight/targets/boards/ccf3d/bootloader/pios_board.c @@ -0,0 +1,103 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the CCF3D board. + * @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 "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 + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uint32_t pios_com_telem_usb_id; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +static bool board_init_complete = false; +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(ENABLE); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; + +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + +#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_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)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + +#endif /* PIOS_INCLUDE_USB */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar + + board_init_complete = true; +} + +void PIOS_ADC_DMA_Handler() +{} diff --git a/flight/targets/boards/ccf3d/firmware/Makefile b/flight/targets/boards/ccf3d/firmware/Makefile new file mode 100644 index 0000000000..f6d69f6073 --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/Makefile @@ -0,0 +1,113 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +#MODULES += Notify + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += Battery +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/ccf3d/firmware/UAVObjects.inc b/flight/targets/boards/ccf3d/firmware/UAVObjects.inc new file mode 100644 index 0000000000..21dcd09362 --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/UAVObjects.inc @@ -0,0 +1,133 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/ccf3d/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/ccf3d/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 0000000000..5fea013d7a --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/ccf3d/firmware/inc/openpilot.h b/flight/targets/boards/ccf3d/firmware/inc/openpilot.h new file mode 100644 index 0000000000..5576baca66 --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @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 OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/ccf3d/firmware/inc/pios_config.h b/flight/targets/boards/ccf3d/firmware/inc/pios_config.h new file mode 100644 index 0000000000..6645c1340d --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/inc/pios_config.h @@ -0,0 +1,189 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2015 + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +/* #define PIOS_INCLUDE_ADC */ +#define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +#define PIOS_INCLUDE_MPU6000 +#define PIOS_MPU6000_ACCEL +/* #define PIOS_INCLUDE_HMC5843 */ +/* #define PIOS_INCLUDE_HMC5X83 */ +/* #define PIOS_HMC5883_HAS_GPIOS */ +/* #define PIOS_INCLUDE_BMP085 */ +/* #define PIOS_INCLUDE_MS56XX */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +#define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +/* #define FLASH_FREERTOS */ +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +/* #define PIOS_INCLUDE_FLASH_INTERNAL */ + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +/* #define PIOS_GPS_MINIMAL */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/ccf3d/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/ccf3d/firmware/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..3922297fff --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/inc/pios_usb_board_data.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 +#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 4 + +#include /* USB_* macros */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_CCF3D, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/ccf3d/firmware/main.cpp b/flight/targets/boards/ccf3d/firmware/main.cpp new file mode 100644 index 0000000000..937a44efb8 --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/main.cpp @@ -0,0 +1,95 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @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 + */ + +extern "C" { +#include "inc/openpilot.h" +#include +/* Task Priorities */ + +/* Global Variables */ +extern void Stack_Change(void); +} /* extern "C" */ + +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/ccf3d/firmware/pios_board.c b/flight/targets/boards/ccf3d/firmware/pios_board.c new file mode 100644 index 0000000000..cdb980073c --- /dev/null +++ b/flight/targets/boards/ccf3d/firmware/pios_board.c @@ -0,0 +1,343 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the CCF3D board. + *****************************************************************************/ +/* + * 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 "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + + +static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook(); +static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +int32_t init_test; +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the serial flash */ + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { + PIOS_Assert(0); + } +#endif + + uintptr_t flash_id; + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_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)) { + PIOS_DEBUG_Assert(0); + } + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + /* Configure the main IO port */ + static const PIOS_BOARD_IO_UART_Function usart_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, + }; + + uint8_t hwsettings_cc_mainport; + HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); + + if (hwsettings_cc_mainport < NELEMENTS(usart_main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, usart_main_function_map[hwsettings_cc_mainport]); + } + + + /* Configure the flexi port */ + static const PIOS_BOARD_IO_UART_Function usart_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, + }; + + uint8_t hwsettings_cc_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); + + if (hwsettings_cc_flexiport < NELEMENTS(usart_flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, usart_flexi_function_map[hwsettings_cc_flexiport]); + } + + if (hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_PPM) { +#if defined(PIOS_INCLUDE_PPM_FLEXI) + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_flexi_cfg); +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + } else if (hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_I2C) { + // F303CC has no I2C on CC3D flexi. + } + + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); + + switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: +#if defined(PIOS_INCLUDE_HCSR04) + { + uint32_t pios_hcsr04_id; + PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); + } +#endif + break; + case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: +#if defined(PIOS_INCLUDE_PWM) + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_cfg); +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: +#if defined(PIOS_INCLUDE_PPM) + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_pin8_cfg); +#endif /* PIOS_INCLUDE_PPM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: +#if defined(PIOS_INCLUDE_PPM) + PIOS_BOARD_IO_Configure_PPM_RCVR(&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_RCVR(&pios_ppm_cfg); +#endif /* PIOS_INCLUDE_PPM */ +#if defined(PIOS_INCLUDE_PWM) + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_with_ppm_cfg); +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: + break; + } + +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + +#ifndef PIOS_ENABLE_DEBUG_PINS + switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: + case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: + PIOS_Servo_Init(&pios_servo_cfg); + break; + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: + PIOS_Servo_Init(&pios_servo_rcvr_cfg); + break; + } +#else + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); +#endif /* PIOS_ENABLE_DEBUG_PINS */ + + 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); + + // Attach the board config check hook + SANITYCHECK_AttachHook(&CopterControlConfigHook); + // trigger a config check if actuatorsettings are updated + ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); +} + +SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook() +{ + // inhibit usage of oneshot for non supported RECEIVER port modes + uint8_t recmode; + + HwSettingsCC_RcvrPortGet(&recmode); + uint8_t flexiMode; + uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM]; + ActuatorSettingsBankModeGet(modes); + HwSettingsCC_FlexiPortGet(&flexiMode); + + switch ((HwSettingsCC_RcvrPortOptions)recmode) { + // Those modes allows oneshot usage + case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: + case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: + if ((recmode == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT || + flexiMode == HWSETTINGS_CC_FLEXIPORT_PPM) && + (modes[3] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || + modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT125 || + modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT42 || + modes[3] == ACTUATORSETTINGS_BANKMODE_MULTISHOT)) { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT; + } else { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; + } + + // inhibit oneshot for the following modes + case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: + for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { + if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || + modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125 || + modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT42 || + modes[i] == ACTUATORSETTINGS_BANKMODE_MULTISHOT) { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; + } + + return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; + } + } + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; +} +// trigger a configuration check if ActuatorSettings are changed. +void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + configuration_check(); +} + +/** + * @} + */ diff --git a/flight/targets/boards/ccf3d/pios_board.h b/flight/targets/boards/ccf3d/pios_board.h new file mode 100644 index 0000000000..6d0f67d468 --- /dev/null +++ b/flight/targets/boards/ccf3d/pios_board.h @@ -0,0 +1,286 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. + * @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_H +#define PIOS_BOARD_H +// ------------------------ +// Timers and Channels Used +// ------------------------ +/* + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ + +// ------------------------ +// DMA Channels Used +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#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_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 +#define PIOS_I2C_EXTERNAL_ADAPTER (0) +#define PIOS_I2C_FLEXI_ADAPTER (0) + +// ------------------------ +// PIOS_BMP085 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 +extern uint32_t pios_spi_gyro_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_gyro_id) +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Gyro Z +// PIOS_ADC_PinGet(1) = Gyro Y +// PIOS_ADC_PinGet(2) = Gyro X +// ------------------------- +// #define PIOS_ADC_OVERSAMPLING_RATE 1 +#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 +#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 + +#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) +#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 +#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 +#define PIOS_ADC_PIN1_ADC ADC2 +#define PIOS_ADC_PIN1_ADC_NUMBER 1 + +#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) +#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 +#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 +#define PIOS_ADC_PIN2_ADC ADC1 +#define PIOS_ADC_PIN2_ADC_NUMBER 2 + +#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) +#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 +#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 +#define PIOS_ADC_PIN3_ADC ADC2 +#define PIOS_ADC_PIN3_ADC_NUMBER 2 + +#define PIOS_ADC_NUM_PINS 3 + +#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } +#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } +#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } +#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } +#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } +#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) +#define PIOS_ADC_NUM_ADC_CHANNELS 2 +#define PIOS_ADC_USE_ADC2 1 +#define PIOS_ADC_CLOCK_FUNCTION RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12, ENABLE) +#define PIOS_ADC_ADCCLK RCC_ADC12PLLCLK_Div32 +/* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ +/* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ +/* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ +/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ +#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_181Cycles5 +/* Sample time: */ +/* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ +/* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ +/* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW + +// Currently analog acquistion hard coded at 480 Hz +// PCKL2 = HCLK / 16 +// ADCCLK = PCLK2 / 2 +#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 48 + +#define PIOS_ADC_TEMPERATURE_PIN 0 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// USB +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 + +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/ccf3d/pios_usb_board_data.c b/flight/targets/boards/ccf3d/pios_usb_board_data.c new file mode 100644 index 0000000000..bd29a792ca --- /dev/null +++ b/flight/targets/boards/ccf3d/pios_usb_board_data.c @@ -0,0 +1,94 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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 + */ + +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ + +static const uint8_t usb_product_id[12] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'C', 0, + 'C', 0, + 'F', 0, + '3', 0, + 'D', 0, +}; + +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; + + PIOS_SYS_SerialNumberGet((char *)sn); + + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 4222540ef3..949234a6d9 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,797 +485,121 @@ void PIOS_ADC_handler() { PIOS_ADC_DMA_Handler(); } + +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(uint32_t board_revision) +{ + return (board_revision == BOARD_REVISION_CC) ? &pios_adc_cfg : 0; +} #endif /* if defined(PIOS_INCLUDE_ADC) */ #include "pios_tim_priv.h" -static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, -}; - -static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -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, - }, - }, - }, -}; - -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, - }, - }, - }, -}; - - -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, - }, - }, - }, - - // 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, - }, - }, - }, -}; - -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, -}; - -#if defined(PIOS_INCLUDE_USART) - -#include "pios_usart_priv.h" - -static const struct pios_usart_cfg pios_usart_generic_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_generic_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, - }, - }, -}; - -#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 TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, }; -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 = { +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannel = TIM1_CC_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_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 = { +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannel = TIM2_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_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 = { +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannel = TIM3_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_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 = { +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannel = TIM4_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_IN_FLOATING, - }, - }, }; -#endif /* PIOS_INCLUDE_SRXL */ +#include "pios_servo_config.h" -#if defined(PIOS_INCLUDE_IBUS) -/* - * IBUS USART - */ -#include +static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { + 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_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, - }, - }, +static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { + 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), }; -#endif /* PIOS_INCLUDE_IBUS */ -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include +static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { + 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_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, - }, - }, + // Receiver port pins + // S3-S6 inputs are used as outputs in this case + 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_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, -}; +static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11, GPIO_PartialRemap2_TIM2); -#endif /* PIOS_INCLUDE_SBUS */ +#if defined(PIOS_INCLUDE_USART) -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { +#include "pios_usart_priv.h" + +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 = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -1283,7 +607,7 @@ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -1293,25 +617,9 @@ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_hkosd_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 = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -1319,7 +627,7 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -1329,7 +637,6 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { }, }; - #endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -1653,48 +960,88 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { .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" - -#endif /* PIOS_INCLUDE_USB */ - -#if defined(PIOS_INCLUDE_COM_MSG) +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; -#include + break; + case BOARD_REVISION_CC3D: + return &pios_usb_main_cfg_cc3d; -#endif /* PIOS_INCLUDE_COM_MSG */ + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; +} -#if defined(PIOS_INCLUDE_USB_HID) -#include +#endif /* PIOS_INCLUDE_USB */ -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" +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_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, +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return (board_revision == BOARD_REVISION_CC3D) ? &pios_mpu6000_cfg : 0; +} +#endif /* PIOS_INCLUDE_MPU6000 */ - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ +#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/inc/common.h b/flight/targets/boards/coptercontrol/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/coptercontrol/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c index 20549d1fa0..3495976df3 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 4fd27d5ee3..39c4e9e8a7 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; /** @@ -44,6 +48,7 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; + void PIOS_Board_Init(void) { if (board_init_complete) { @@ -84,7 +89,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/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 86539ebfdb..a7d081ad37 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -94,7 +94,6 @@ ifndef TESTAPP SRC += $(FLIGHT_UAVOBJ_DIR)/objectpersistence.c SRC += $(FLIGHT_UAVOBJ_DIR)/gcstelemetrystats.c SRC += $(FLIGHT_UAVOBJ_DIR)/flighttelemetrystats.c - SRC += $(FLIGHT_UAVOBJ_DIR)/faultsettings.c SRC += $(FLIGHT_UAVOBJ_DIR)/flightstatus.c SRC += $(FLIGHT_UAVOBJ_DIR)/systemstats.c SRC += $(FLIGHT_UAVOBJ_DIR)/systemalarms.c diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h deleted file mode 100644 index 16364d7fa0..0000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h +++ /dev/null @@ -1,81 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -// #define PIOS_LED_LED1_GPIO_PORT GPIOC -// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -// #define PIOS_LED_LED2_GPIO_PORT GPIOC -// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } - - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX -#endif - -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index a6f521c81a..44f94665a0 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -88,7 +88,7 @@ /* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ -/* #define PIOS_INCLUDE_MS5611 */ +/* #define PIOS_INCLUDE_MS56XX */ /* #define PIOS_INCLUDE_MPXV */ /* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_HCSR04 */ @@ -161,7 +161,6 @@ #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 #define HEAP_LIMIT_CRITICAL 40 #define IRQSTACK_LIMIT_WARNING 100 #define IRQSTACK_LIMIT_CRITICAL 60 @@ -175,10 +174,13 @@ #define PIOS_STABILIZATION_STACK_SIZE 400 #ifdef DIAG_TASKS -#define PIOS_SYSTEM_STACK_SIZE 760 +#define PIOS_SYSTEM_STACK_SIZE 880 +#define HEAP_LIMIT_WARNING 150 #else -#define PIOS_SYSTEM_STACK_SIZE 660 +#define PIOS_SYSTEM_STACK_SIZE 700 +#define HEAP_LIMIT_WARNING 220 #endif + #define PIOS_TELEM_RX_STACK_SIZE 410 #define PIOS_TELEM_TX_STACK_SIZE 560 #define PIOS_EVENTDISPATCHER_STACK_SIZE 95 diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h deleted file mode 100644 index 4e8aa47c66..0000000000 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ /dev/null @@ -1,48 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. - * Central compile time config for the project. - * @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_CONFIG_POSIX_H -#define PIOS_CONFIG_POSIX_H - - -/* Enable/Disable PiOS Modules */ -#define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_LED -#define PIOS_INCLUDE_FREERTOS -#define PIOS_INCLUDE_CALLBACKSCHEDULER -#define PIOS_INCLUDE_TASK_MONITOR -#define PIOS_INCLUDE_COM -#define PIOS_INCLUDE_UDP -#define PIOS_INCLUDE_SERVO - - -/* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 - -#endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 7556dff228..86098d8781 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -30,18 +30,17 @@ #include #include #include -#include -#include #include #include #include + #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif -#if defined(PIOS_INCLUDE_ADXL345) -#include -#endif + +#include +#include /* * Pull in the board-specific static HW definitions. @@ -53,185 +52,39 @@ */ #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 */ /** * PIOS_Board_Init() @@ -258,12 +111,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; @@ -276,7 +129,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)) { @@ -284,7 +137,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)) { @@ -306,6 +159,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ @@ -322,8 +176,6 @@ void PIOS_Board_Init(void) PIOS_IAP_WriteBootCmd(2, 0); } - HwSettingsInitialize(); - #ifndef ERASE_FLASH #ifdef PIOS_INCLUDE_WDG /* Initialize watchdog as early as possible to catch faults during init */ @@ -352,368 +204,38 @@ 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); - } - -#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); + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_flexiport); - 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; + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } -#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; + 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_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 */ +#endif /* PIOS_INCLUDE_I2C */ 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; - } + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_flexi_cfg); #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; + /* Configure main USART port */ + uint8_t hwsettings_mainport; + HwSettingsCC_MainPortGet(&hwsettings_mainport); - 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: -#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; + 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 +253,36 @@ 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_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) - { - 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_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) - { - 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_RCVR(&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_RCVR(&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_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); @@ -828,40 +307,29 @@ 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); // Attach the board config check hook SANITYCHECK_AttachHook(&CopterControlConfigHook); // trigger a config check if actuatorsettings are updated - ActuatorSettingsInitialize(); ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); } diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c deleted file mode 100644 index 7fa958a38e..0000000000 --- a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c +++ /dev/null @@ -1,145 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board specific static initializers for hardware for the OpenPilot board. - * @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 "inc/openpilot.h" -#include -#include -#include - -/** - * PIOS_Board_Init() - * initializes all the core systems on this specific hardware - * called from System/openpilot.c - */ -void PIOS_Board_Init(void) -{ - /* Delay system */ - PIOS_DELAY_Init(); - - /* Initialize the delayed callback library */ - PIOS_CALLBACKSCHEDULER_Initialize(); - - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - /* Initialize the alarms library */ - AlarmsInitialize(); - - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the PiOS library */ - PIOS_COM_Init(); -} - - -const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, -}; -const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, -}; -const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, -}; - -#ifdef PIOS_COM_AUX -/* - * AUX USART - */ -const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, -}; -#endif - -/* - * Board specific number of devices. - */ -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, -#endif -}; - -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); - -/* - * COM devices - */ - -/* - * Board specific number of devices. - */ -extern const struct pios_com_driver pios_serial_com_driver; -extern const struct pios_com_driver pios_udp_com_driver; - -struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, -#ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, -#endif -}; - -const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs); - -/** - * @} - */ diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 1750b4d9af..370955e2af 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -88,7 +88,6 @@ // System Settings // ------------------------- #define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) // ------------------------- // Interrupt Priorities @@ -117,50 +116,50 @@ 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 + +// 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 // // See also pios_board.c // ------------------------- -#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_MAX_DEVS 3 -#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_TELEM_RF_RX_BUF_LEN 32 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 -extern uint32_t pios_com_bridge_id; -#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_GPS_RX_BUF_LEN 32 -extern uint32_t pios_com_vcp_id; -#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#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_HKOSD_TX_BUF_LEN 22 -extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 -extern uint32_t pios_com_msp_id; -#define PIOS_COM_MSP (pios_com_msp_id) +#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 -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/board_hw_defs.c b/flight/targets/boards/discoveryf4bare/board_hw_defs.c index 0c833776b6..b733c6f6fc 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); } @@ -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,78 +600,17 @@ static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = { }; #endif /* PIOS_INCLUDE_FLASH */ +#ifdef PIOS_INCLUDE_USART #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN USART */ -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 = { +static const struct pios_usart_cfg pios_usart_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 = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -681,7 +620,7 @@ static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -693,119 +632,13 @@ 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 */ - -#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, @@ -815,7 +648,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, @@ -827,173 +660,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 */ - -/* - * 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 = { + .dtr = { // FlexIO pin 9 .gpio = GPIOC, .init = { @@ -1004,7 +675,7 @@ static const struct pios_usart_cfg pios_usart_rcvrport_cfg = { }, }, - .tx = { + .tx = { // * 7: PC6 = TIM8 CH1, USART6 TX .gpio = GPIOC, .init = { @@ -1014,10 +685,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 = { @@ -1027,9 +698,10 @@ 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, } }; +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -1385,212 +1057,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 +1132,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 +1234,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 +1300,201 @@ 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_MS56XX) +#include "pios_ms56xx.h" +static const struct pios_ms56xx_cfg pios_ms56xx_cfg = { + .address = 0x77, + .version = MS56XX_VERSION_5611, + .oversampling = MS56XX_OSR_512, +}; +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms56xx_cfg; +} +#endif /* PIOS_INCLUDE_MS56XX */ + + +/** + * 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_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 +}; + +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/inc/common.h b/flight/targets/boards/discoveryf4bare/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/discoveryf4bare/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/discoveryf4bare/bootloader/main.c b/flight/targets/boards/discoveryf4bare/bootloader/main.c index 09a54a0947..c85ef675e6 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(); @@ -81,7 +82,7 @@ int main() // is 2.7 volts check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CableConnected(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); diff --git a/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c b/flight/targets/boards/discoveryf4bare/bootloader/pios_board.c index 5352c7f73b..fd64e58fa0 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,7 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +69,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/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index d9e9c67f4c..f720fb7c5a 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -25,7 +25,7 @@ endif include ../board-info.mk include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk -# REVO C++ support +# REVO C++ support USE_CXX = YES # ARM DSP library @@ -59,6 +59,7 @@ OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge SRC += $(FLIGHTLIB)/notification.c @@ -87,11 +88,6 @@ ifndef TESTAPP SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c - #ifeq ($(DEBUG), YES) - SRC += $(OPSYSTEM)/dcc_stdio.c - SRC += $(OPSYSTEM)/cm3_fault_handlers.c - #endif - ## Misc library functions SRC += $(FLIGHTLIB)/instrumentation.c SRC += $(FLIGHTLIB)/paths.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index b704f4ab0d..53a928f57b 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -77,8 +77,6 @@ UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += oplinkreceiver -UAVOBJSRCFILENAMES += overosyncstats -UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan @@ -108,6 +106,11 @@ UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwdiscoveryf4baresettings +UAVOBJSRCFILENAMES += hwpikoblxsettings +UAVOBJSRCFILENAMES += hwspracingf3evosettings +UAVOBJSRCFILENAMES += hwspracingf3settings +UAVOBJSRCFILENAMES += hwtinyfishsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired diff --git a/flight/targets/boards/discoveryf4bare/firmware/cm3_fault_handlers.c b/flight/targets/boards/discoveryf4bare/firmware/cm3_fault_handlers.c deleted file mode 100644 index e74ffa76ad..0000000000 --- a/flight/targets/boards/discoveryf4bare/firmware/cm3_fault_handlers.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * cm3_fault_handlers.c - * - * Created on: Apr 24, 2011 - * Author: msmith - */ - -#include -#include "inc/dcc_stdio.h" -#ifdef STM32F4XX -# include -#endif -#ifdef STM32F2XX -# include -#endif - -#define FAULT_TRAMPOLINE(_vec) \ - __attribute__((naked, no_instrument_function)) \ - void \ - _vec##_Handler(void) \ - { \ - __asm(".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ - } \ - struct hack - -struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -FAULT_TRAMPOLINE(HardFault); -FAULT_TRAMPOLINE(BusFault); -FAULT_TRAMPOLINE(UsageFault); - -/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) - -void HardFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void BusFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void UsageFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_board_sim.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_board_sim.h deleted file mode 100644 index 68a32b116c..0000000000 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_board_sim.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -#define PIOS_LED_ALARM 0 -#define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_aux_id; -extern uint32_t pios_com_spectrum_id; - -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) -#endif - -#define PIOS_GCSRCVR_TIMEOUT_MS 200 -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index 47388c9887..577c10ec61 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -86,9 +86,10 @@ // #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 +/* #define PIOS_INCLUDE_MS56XX */ // #define PIOS_INCLUDE_MPXV // #define PIOS_INCLUDE_ETASV3 /* #define PIOS_INCLUDE_HCSR04 */ @@ -104,7 +105,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_GCSRCVR -// #define PIOS_INCLUDE_OPLINKRCVR +#define PIOS_INCLUDE_OPLINKRCVR /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR @@ -152,6 +153,7 @@ #define PIOS_INCLUDE_GPS_UBX_PARSER #define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION +#define PIOS_INCLUDE_FRSKY_SENSORHUB /* Stabilization options */ /* #define PIOS_QUATERNION_STABILIZATION */ diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index a58fd1d312..32dad3573e 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -30,19 +30,19 @@ #include #include #include -#include -#include -#include -#include -#include #include -#include -#include +#include + +#include "hwdiscoveryf4baresettings.h" +#include "firmwareiapobj.h" #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif +#include +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -53,327 +53,80 @@ */ #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) -{ - 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); - } +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif - 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); - } +static HwDiscoveryF4BareSettingsData boardHwSettings; - 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) +static void hwDiscoveryF4BareSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - /* Set up the receiver port. Later this should be optional */ - uint32_t pios_pwm_id; + HwDiscoveryF4BareSettingsData currentBoardHwSettings; - PIOS_PWM_Init(&pios_pwm_id, pwm_cfg); + HwDiscoveryF4BareSettingsGet(¤tBoardHwSettings); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwDiscoveryF4BareSettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 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; +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, + [HWSETTINGS_RM_RCVRPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, +}; - PIOS_PPM_Init(&pios_ppm_id, ppm_cfg); +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, + [HWSETTINGS_RM_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, +}; - 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 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, + [HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, +}; /** * PIOS_Board_Init() * 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) @@ -387,17 +140,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; @@ -446,12 +202,22 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; - HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ + HwDiscoveryF4BareSettingsConnectCallback(hwDiscoveryF4BareSettingsUpdatedCb); + HwDiscoveryF4BareSettingsGet(&boardHwSettings); + + if (boardHwSettings.BoardType != 0) { + FirmwareIAPObjInitialize(); + + FirmwareIAPObjData iap; + FirmwareIAPObjGet(&iap); + + iap.BoardType = boardHwSettings.BoardType; + iap.BoardRevision = boardHwSettings.BoardRevision; + + FirmwareIAPObjSet(&iap); + } /* Initialize the alarms library */ AlarmsInitialize(); @@ -477,402 +243,52 @@ void PIOS_Board_Init(void) AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } + /* Configure IO ports */ - // 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); - } + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - 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); + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } - 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; +#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); - 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); - } + 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_COM */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - - if (usb_hid_present || usb_cdc_present) { - PIOS_USBHOOK_Activate(); + PIOS_DELAY_WaitmS(50); } -#endif /* PIOS_INCLUDE_USB */ +#endif - /* Configure IO ports */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif - /* 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); + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - /* 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; - } /* hwsettings_rm_flexiport */ - - - /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); - /* Fetch the OPinkSettings 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); - bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_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)) { - 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_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, is_oneway, ppm_mode, ppm_only); - PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - /* 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); - } - - /* 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; - } - - OPLinkStatusSet(&oplinkStatus); + PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #endif /* PIOS_INCLUDE_RFM22B */ + #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) const struct pios_servo_cfg *pios_servo_cfg; @@ -883,14 +299,17 @@ 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: #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_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -903,16 +322,16 @@ 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_RCVR(&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; } - 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); + PIOS_BOARD_IO_Configure_PWM_RCVR(&pios_pwm_ppm_cfg); } break; @@ -922,60 +341,13 @@ void PIOS_Board_Init(void) pios_servo_cfg = &pios_servo_cfg_out_in; break; } +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif - // 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); - 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_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif #ifndef PIOS_ENABLE_DEBUG_PINS // pios_servo_cfg points to the correct configuration based on input port settings @@ -984,6 +356,13 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif + PIOS_BOARD_Sensors_Configure(); + +#ifdef PIOS_INCLUDE_WS2811 + PIOS_WS2811_Init(&pios_ws2811_id, &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, @@ -993,61 +372,6 @@ 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); - } - } - } #endif // PIOS_INCLUDE_ADC } diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index a6373ccd39..048e2fe531 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -85,71 +85,59 @@ // 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 // // 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 // // 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 */ +#define PIOS_COM_MAX_DEVS 4 + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif // ------------------------- // Packet Handler @@ -215,8 +203,6 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 -#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 -#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 // ------------------------- // Receiver PPM input diff --git a/flight/targets/boards/gpsplatinum/board-info.mk b/flight/targets/boards/gpsplatinum/board-info.mk index c86ef35b8c..60485b0470 100644 --- a/flight/targets/boards/gpsplatinum/board-info.mk +++ b/flight/targets/boards/gpsplatinum/board-info.mk @@ -1,6 +1,6 @@ BOARD_TYPE := 0x0A BOARD_REVISION := 0x01 -BOOTLOADER_VERSION := 0x01 +BOOTLOADER_VERSION := 0x02 HW_TYPE := 0x01 MCU := cortex-m0 diff --git a/flight/targets/boards/gpsplatinum/board_hw_defs.c b/flight/targets/boards/gpsplatinum/board_hw_defs.c index 83b7f4528f..95f43d2f0d 100644 --- a/flight/targets/boards/gpsplatinum/board_hw_defs.c +++ b/flight/targets/boards/gpsplatinum/board_hw_defs.c @@ -62,7 +62,7 @@ static const struct pios_gpio pios_leds_gpsp[] = { .GPIO_Speed = GPIO_Speed_Level_1, }, }, - .active_low = false + .active_low = true }, }; @@ -268,22 +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, - .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 = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -292,7 +277,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/inc/common.h b/flight/targets/boards/gpsplatinum/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/gpsplatinum/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/gpsplatinum/bootloader/main.c b/flight/targets/boards/gpsplatinum/bootloader/main.c index a11fa2f3d6..8f49d55c3f 100644 --- a/flight/targets/boards/gpsplatinum/bootloader/main.c +++ b/flight/targets/boards/gpsplatinum/bootloader/main.c @@ -161,10 +161,10 @@ int main() } led_pwm_step(period1, sweep_steps1, stopwatch, false); led_pwm_step(period2, sweep_steps2, stopwatch, true); - JumpToApp |= (stopwatch > BL_WAIT_TIME) && ((DeviceState == BLidle) || (DeviceState == DFUidle)); + JumpToApp |= !ssp_dfu && (stopwatch > BL_WAIT_TIME) && ((DeviceState == BLidle) || (DeviceState == DFUidle)); DataDownload(start); - if (JumpToApp == true && !ssp_dfu) { + if (JumpToApp == true) { jump_to_app(); } } diff --git a/flight/targets/boards/gpsplatinum/bootloader/pios_board.c b/flight/targets/boards/gpsplatinum/bootloader/pios_board.c index 14bddba5e4..8c3d730df5 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/gpsp.c b/flight/targets/boards/gpsplatinum/firmware/gpsp.c index 0768c2a55d..8bed9b50cd 100644 --- a/flight/targets/boards/gpsplatinum/firmware/gpsp.c +++ b/flight/targets/boards/gpsplatinum/firmware/gpsp.c @@ -42,6 +42,7 @@ /* Global Variables */ extern void Stack_Change(void); +extern void GPSPSystemModStart(void); /** * OpenPilot Main function: @@ -54,32 +55,19 @@ extern void Stack_Change(void); */ int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); + /* + * Initialize the system module, which configures the board and + * starts the other modules. + */ + GPSPSystemModStart(); - /* Initialize modules */ - MODULE_INITIALISE_ALL /* swap the stack to use the IRQ stack */ Stack_Change(); - /* Start the FreeRTOS scheduler, which should never return. - * - * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls - * (schedules) function files (modules). These functions never return from their - * while loops, which explains why each module has a while(1){} segment. Thus, - * the OpenPilot software actually starts at the vTaskStartScheduler() function, - * even though this is somewhat obscure. - * - */ + /* Start the FreeRTOS scheduler */ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ diff --git a/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h b/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h index 5a36646cf1..1f1ac785aa 100644 --- a/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h +++ b/flight/targets/boards/gpsplatinum/firmware/inc/pios_config.h @@ -83,9 +83,10 @@ // #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 */ +/* #define PIOS_INCLUDE_MS56XX */ /* #define PIOS_INCLUDE_MPXV */ /* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_HCSR04 */ diff --git a/flight/targets/boards/gpsplatinum/firmware/pios_board.c b/flight/targets/boards/gpsplatinum/firmware/pios_board.c index 11b99d8253..4bc62fda36 100644 --- a/flight/targets/boards/gpsplatinum/firmware/pios_board.c +++ b/flight/targets/boards/gpsplatinum/firmware/pios_board.c @@ -31,7 +31,6 @@ #include #include #include -#include /* * Pull in the board-specific static HW definitions. @@ -115,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 diff --git a/flight/targets/boards/nucleof303re/board-info.mk b/flight/targets/boards/nucleof303re/board-info.mk new file mode 100644 index 0000000000..b623f2763c --- /dev/null +++ b/flight/targets/boards/nucleof303re/board-info.mk @@ -0,0 +1,23 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x03 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +CHIP := STM32F303xE + +OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg +OPENOCD_CONFIG := stm32f3x.cfg + +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region + +# Some KB for settings storage (32kb) +EE_BANK_BASE := 0x08004000 # EEPROM storage area +EE_BANK_SIZE := 0x00008000 # EEPROM storage size + +FW_BANK_BASE := 0x0800C000 # Start of firmware flash +FW_BANK_SIZE := 0x00034000 # Should include FW_DESC_SIZE (208kB) + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/nucleof303re/board_hw_defs.c b/flight/targets/boards/nucleof303re/board_hw_defs.c new file mode 100644 index 0000000000..ddabcbf5eb --- /dev/null +++ b/flight/targets/boards/nucleof303re/board_hw_defs.c @@ -0,0 +1,851 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the NucleoF303RE board. + *****************************************************************************/ +/* + * 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 + */ + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_gpio pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = true + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg = { + .gpios = pios_leds, + .num_gpios = NELEMENTS(pios_leds), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Gyro interface */ + +void PIOS_SPI_MPU9250_irq_handler(void); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_MPU9250_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_MPU9250_irq_handler"))); + +static const struct pios_spi_cfg pios_spi_mpu9250_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +static uint32_t pios_spi_mpu9250_id; +void PIOS_SPI_MPU9250_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_mpu9250_id); +} + + +/* SDCARD Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_SDCARD_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_SDCARD_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_SDCARD_irq_handler"))); +static const struct pios_spi_cfg pios_spi_sdcard_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, /* NOTE: was GPIO_Mode_IN_FLOATING */ + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + } + }, + }, +}; + +static uint32_t pios_spi_sdcard_id; +void PIOS_SPI_SDCARD_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" +#include "pios_flash_internal_priv.h" + +static const struct pios_flash_internal_cfg flash_internal_system_cfg = {}; + +static const struct flashfs_logfs_cfg flashfs_internal_cfg = { + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00004000, /* 16K bytes */ + .page_size = 0x00004000, /* 16K bytes */ +}; + +// static const struct flashfs_logfs_cfg flashfs_internal_user_cfg = { +// .fs_magic = 0x99abcfef, +// .total_fs_size = USER_EE_BANK_SIZE, /* 128K bytes (2x16KB sectors) */ +// .arena_size = 0x00020000, /* 64 * slot size = 16K bytes = 1 sector */ +// .slot_size = 0x00000100, /* 256 bytes */ +// +// .start_offset = USER_EE_BANK_BASE, /* start after the bootloader */ +// .sector_size = 0x00020000, /* 128K bytes */ +// .page_size = 0x00020000, /* 128K bytes */ +// }; +#endif /* PIOS_INCLUDE_FLASH */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_15_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM15_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_16_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM16_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_17_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 +#define GPIO_AF_PA15_TIM8 GPIO_AF_2 +#define GPIO_AF_PA3_TIM15 GPIO_AF_9 +#define GPIO_AF_PA6_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM3 GPIO_AF_2 + +static const struct pios_tim_channel pios_tim_servoport_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 2, A, 3), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, A, 6), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, A, 7), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), // bank 3 +}; + +static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11); + + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART2 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_cfg[] = { + USART_CONFIG(USART1, A, 10, A, 9), + USART_CONFIG(USART2, A, 15, A, 14), + USART_CONFIG(USART3, B, 11, B, 10), +}; + + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +static const struct pios_tim_channel pios_tim_ppm_channel = TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, A, 15); + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_ppm_channel, + .num_channels = 1, +}; +#endif /* PIOS_INCLUDE_PPM */ + +/* + * SONAR Inputs + */ +#if defined(PIOS_INCLUDE_HCSR04) +#include + +static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { + { + .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, + }, +}; + +const struct pios_hcsr04_cfg pios_hcsr04_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_ev_irq_handler(void); +void PIOS_I2C_er_irq_handler(void); +void I2C1_EV_EXTI23_IRQHandler() __attribute__((alias("PIOS_I2C_ev_irq_handler"))); +void I2C1_ER_IRQHandler() __attribute__((alias("PIOS_I2C_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_cfg = { + .regs = I2C1, + .remapSCL = GPIO_AF_4, + .remapSDA = GPIO_AF_4, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DigitalFilter = 0x00, + .I2C_AnalogFilter = I2C_AnalogFilter_Enable, +// .I2C_Timing = 0x70310309, // 50kHz I2C @ 8MHz input -> PRESC=0x7, SCLDEL=0x3, SDADEL=0x1, SCLH=0x03, SCLL=0x09 + .I2C_Timing = 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + .pin_source = GPIO_PinSource6, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + .pin_source = GPIO_PinSource7, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_id; +void PIOS_I2C_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_id); +} + +void PIOS_I2C_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_id); +} + + +#endif /* PIOS_INCLUDE_I2C */ + + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +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_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Channel1_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC2, + .dma = { + .irq = { + .flags = (DMA2_FLAG_TC1 | DMA2_FLAG_TE1 | DMA2_FLAG_HT1), + .init = { + .NVIC_IRQChannel = DMA2_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Channel1, + } + }, + .half_flag = DMA2_FLAG_HT1, + .full_flag = DMA2_FLAG_TC1, +}; + +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" +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 +}; + +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 */ + +#ifdef PIOS_INCLUDE_BMP280 +#include "pios_bmp280.h" +static const struct pios_bmp280_cfg bmp280_config = { + .oversampling = BMP280_ULTRA_HIGH_RESOLUTION +}; +#endif + +/** + * 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_Line13, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .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_Line13, // 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_270DEG, + .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/nucleof303re/bootloader/Makefile b/flight/targets/boards/nucleof303re/bootloader/Makefile new file mode 100644 index 0000000000..6a2fec6385 --- /dev/null +++ b/flight/targets/boards/nucleof303re/bootloader/Makefile @@ -0,0 +1,29 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for Nucleo F303RE, board revision $(BOARD_REVISION)) diff --git a/flight/targets/boards/nucleof303re/bootloader/inc/pios_config.h b/flight/targets/boards/nucleof303re/bootloader/inc/pios_config.h new file mode 100644 index 0000000000..c0d4bb4ff0 --- /dev/null +++ b/flight/targets/boards/nucleof303re/bootloader/inc/pios_config.h @@ -0,0 +1,51 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + */ diff --git a/flight/targets/boards/nucleof303re/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/nucleof303re/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..915cb8bfa1 --- /dev/null +++ b/flight/targets/boards/nucleof303re/bootloader/inc/pios_usb_board_data.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 2 + +#include /* struct usb_* */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_NUCLEOF303RE, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" + +/* + * The bootloader uses a simplified report structure + * BL: ... + * FW: ... + * This define changes the behaviour in pios_usb_hid.c + */ +#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/nucleof303re/bootloader/main.c b/flight/targets/boards/nucleof303re/bootloader/main.c new file mode 100644 index 0000000000..044608da54 --- /dev/null +++ b/flight/targets/boards/nucleof303re/bootloader/main.c @@ -0,0 +1,230 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This is the file with the main function of the LibrePilot BootLoader + * @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 +#include +#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) + +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + + +//////////////////////////////////////// +uint8_t tempcount = 0; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; +uint8_t User_DFU_request = FALSE; +static uint8_t mReceive_Buffer[63]; +/* Private function prototypes -----------------------------------------------*/ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); +uint8_t processRX(); +void jump_to_app(); + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + USB_connected = PIOS_USB_CableConnected(0); + + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } + + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } + + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; + + if (JumpToApp == TRUE) { + jump_to_app(); + } + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } + + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) /*|| (DeviceState == DFUidle && !USB_connected)*/)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } +} + +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + JumpToApp = FALSE; + return; + } +} +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} + +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); +} diff --git a/flight/targets/boards/nucleof303re/bootloader/pios_board.c b/flight/targets/boards/nucleof303re/bootloader/pios_board.c new file mode 100644 index 0000000000..81127b1962 --- /dev/null +++ b/flight/targets/boards/nucleof303re/bootloader/pios_board.c @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the NucleoF303RE board. + * @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 "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 + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" +#include + +uint32_t pios_com_telem_usb_id; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +static bool board_init_complete = false; +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(ENABLE); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + +#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_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)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + +#endif /* PIOS_INCLUDE_USB */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar + + board_init_complete = true; +} + +void PIOS_ADC_DMA_Handler() +{} diff --git a/flight/targets/boards/nucleof303re/firmware/Makefile b/flight/targets/boards/nucleof303re/firmware/Makefile new file mode 100644 index 0000000000..f6d69f6073 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/Makefile @@ -0,0 +1,113 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +#MODULES += Notify + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += Battery +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/nucleof303re/firmware/UAVObjects.inc b/flight/targets/boards/nucleof303re/firmware/UAVObjects.inc new file mode 100644 index 0000000000..a38f6b5eb6 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/UAVObjects.inc @@ -0,0 +1,134 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwspracingf3evosettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/nucleof303re/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/nucleof303re/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 0000000000..5fea013d7a --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/nucleof303re/firmware/inc/openpilot.h b/flight/targets/boards/nucleof303re/firmware/inc/openpilot.h new file mode 100644 index 0000000000..5576baca66 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @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 OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h b/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h new file mode 100644 index 0000000000..abe4f0eaf6 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h @@ -0,0 +1,193 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2015 + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_ADC +#define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +/* #define PIOS_INCLUDE_MPU6000 */ +/* #define PIOS_MPU6000_ACCEL */ +/* #define PIOS_INCLUDE_HMC5843 */ +#define PIOS_INCLUDE_HMC5X83 +/* #define PIOS_HMC5883_HAS_GPIOS */ +/* #define PIOS_INCLUDE_MPU9250 */ +/* #define PIOS_MPU9250_ACCEL */ +/* #define PIOS_MPU9250_MAG */ +/* #define PIOS_INCLUDE_BMP280 */ +/* #define PIOS_INCLUDE_BMP085 */ +/* #define PIOS_INCLUDE_MS56XX */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +#define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +#define FLASH_FREERTOS +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +#define PIOS_INCLUDE_FLASH_INTERNAL + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +/* #define PIOS_GPS_MINIMAL */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/nucleof303re/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/nucleof303re/firmware/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..99a6bcf194 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/inc/pios_usb_board_data.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 +#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 4 + +#include /* USB_* macros */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_NUCLEOF303RE, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/nucleof303re/firmware/main.cpp b/flight/targets/boards/nucleof303re/firmware/main.cpp new file mode 100644 index 0000000000..d1ebd36253 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/main.cpp @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @brief These files are the core system files for LibrePilot. + * They are the ground layer just above PiOS. In practice, LibrePilot actually starts + * in the main() function of main.cpp + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @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 + */ + +extern "C" { +#include "inc/openpilot.h" +#include +/* Task Priorities */ + +/* Global Variables */ +extern void Stack_Change(void); +} /* extern "C" */ + +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/nucleof303re/firmware/pios_board.c b/flight/targets/boards/nucleof303re/firmware/pios_board.c new file mode 100644 index 0000000000..64eae9c537 --- /dev/null +++ b/flight/targets/boards/nucleof303re/firmware/pios_board.c @@ -0,0 +1,256 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the NucleoF303RE board. + *****************************************************************************/ +/* + * 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 "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif + + +static HwSPRacingF3EVOSettingsData boardHwSettings; + +static void hwSPRacingF3EVOSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HwSPRacingF3EVOSettingsData currentBoardHwSettings; + + HwSPRacingF3EVOSettingsGet(¤tBoardHwSettings); + + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwSPRacingF3EVOSettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + } +} + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ + +void PIOS_Board_Init(void) +{ +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the mpu9250 */ + + if (PIOS_SPI_Init(&pios_spi_mpu9250_id, &pios_spi_mpu9250_cfg)) { + PIOS_DEBUG_Assert(0); + } + + /* Set up the SPI interface to the sdcard */ + if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { + PIOS_DEBUG_Assert(0); + } +#endif + +#if defined(PIOS_INCLUDE_FLASH) + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + + // initialize the internal settings storage flash + if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_system_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + +// if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_internal_user_cfg, &pios_internal_flash_driver, flash_id)) { +// PIOS_DEBUG_Assert(0); +// } + +#endif /* if defined(PIOS_INCLUDE_FLASH) */ + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + HwSPRacingF3EVOSettingsSetDefaults(HwSPRacingF3EVOSettingsHandle(), 0); + + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + /* Set up pulse timers: 2, 3, 8, 15 */ + // PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + // PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_15_cfg); + // PIOS_TIM_InitClock(&tim_16_cfg); + // PIOS_TIM_InitClock(&tim_17_cfg); + + +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + + HwSPRacingF3EVOSettingsConnectCallback(hwSPRacingF3EVOSettingsUpdatedCb); + + HwSPRacingF3EVOSettingsGet(&boardHwSettings); + + static const PIOS_BOARD_IO_UART_Function uart_function_map[] = { + [HWSPRACINGF3EVOSETTINGS_UARTPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok. + [HWSPRACINGF3EVOSETTINGS_UARTPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + }; + + for (unsigned int i = 0; i < HWSPRACINGF3EVOSETTINGS_UARTPORT_NUMELEM; ++i) { + if (boardHwSettings.UARTPort[i] < NELEMENTS(uart_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[i], uart_function_map[boardHwSettings.UARTPort[i]]); + } + } + +#ifndef PIOS_ENABLE_DEBUG_PINS + PIOS_Servo_Init(&pios_servo_cfg); +#else + PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels); +#endif + + switch (boardHwSettings.LEDPort) { + case HWSPRACINGF3EVOSETTINGS_LEDPORT_WS281X: +#if defined(PIOS_INCLUDE_WS2811) + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg); +#endif + break; + default: + break; + } + +#if defined(PIOS_INCLUDE_I2C) + // init I2C1 for use with the internal baro + if (PIOS_I2C_Init(&pios_i2c_id, &pios_i2c_cfg)) { + PIOS_DEBUG_Assert(0); + } + PIOS_DELAY_WaitmS(50); +#endif + + PIOS_BOARD_Sensors_Configure(); + + PIOS_LED_On(PIOS_LED_HEARTBEAT); + + /* 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/nucleof303re/pios_board.h b/flight/targets/boards/nucleof303re/pios_board.h new file mode 100644 index 0000000000..65d4272bbf --- /dev/null +++ b/flight/targets/boards/nucleof303re/pios_board.h @@ -0,0 +1,245 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the NucleoF303RE board. + * @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_H +#define PIOS_BOARD_H +// ------------------------ +// Timers and Channels Used +// ------------------------ +/* + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ + +// ------------------------ +// DMA Channels Used +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#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_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_RCC_HSE_BYPASS + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 +extern uint32_t pios_i2c_id; +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_id) +#define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_id) + +// ------------------------ +// PIOS_BMP085 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Current sensor +// PIOS_ADC_PinGet(1) = RSSI sensor +// PIOS_ADC_PinGet(2) = Battery voltage +// ------------------------- +#define PIOS_DMA_PIN_CONFIG \ + { \ + { GPIOA, GPIO_Pin_5, ADC_Channel_2, false }, /* ADC_1 / Current */ \ + { GPIOB, GPIO_Pin_2, ADC_Channel_12, false }, /* ADC_2 / RSSI */ \ + { GPIOA, GPIO_Pin_4, ADC_Channel_1, false }, /* Battery voltage */ \ + } + +/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ +/* which is annoying because this then determines the rate at which we generate buffer turnover events */ +/* the objective here is to get enough buffer space to support 100Hz averaging rate */ +#define PIOS_ADC_NUM_CHANNELS 3 +#define PIOS_ADC_MAX_OVERSAMPLING 32 +#define PIOS_ADC_USE_ADC2 0 + +// #define PIOS_ADC_USE_TEMP_SENSOR +// #define PIOS_ADC_TEMPERATURE_PIN 4 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// USB +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 + +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/nucleof303re/pios_usb_board_data.c b/flight/targets/boards/nucleof303re/pios_usb_board_data.c new file mode 100644 index 0000000000..21f0be4d35 --- /dev/null +++ b/flight/targets/boards/nucleof303re/pios_usb_board_data.c @@ -0,0 +1,102 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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 + */ + +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ + +static const uint8_t usb_product_id[28] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'N', 0, + 'u', 0, + 'c', 0, + 'l', 0, + 'e', 0, + 'o', 0, + ' ', 0, + 'F', 0, + '3', 0, + '0', 0, + '3', 0, + 'R', 0, + 'E', 0, +}; + +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; + + PIOS_SYS_SerialNumberGet((char *)sn); + + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index 548f965ddf..85130655db 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; } @@ -691,25 +691,9 @@ const struct pios_ppm_out_cfg pios_flexi_ppm_out_cfg = { /* * SERIAL USART */ -static const struct pios_usart_cfg pios_usart_serial_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 = { + .rx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -717,7 +701,7 @@ static const struct pios_usart_cfg pios_usart_serial_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -727,25 +711,9 @@ static const struct pios_usart_cfg pios_usart_serial_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_telem_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 = { + .rx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_11, @@ -753,7 +721,7 @@ static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = { .GPIO_Mode = GPIO_Mode_IPU, }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -827,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" @@ -839,29 +812,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/oplinkmini/bootloader/inc/common.h b/flight/targets/boards/oplinkmini/bootloader/inc/common.h deleted file mode 100644 index 1212e06fda..0000000000 --- a/flight/targets/boards/oplinkmini/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotBL OpenPilot BootLoader - * @brief These files contain the code to the OpenPilot MB Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/oplinkmini/bootloader/main.c b/flight/targets/boards/oplinkmini/bootloader/main.c index 28cfcc30b4..566caea735 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) @@ -56,6 +57,8 @@ uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS //////////////////////////////////////// uint8_t tempcount = 0; +extern uint32_t pios_com_telem_usb_id; + /* Extern variables ----------------------------------------------------------*/ DFUStates DeviceState; int16_t status = 0; @@ -211,7 +214,7 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) uint8_t processRX() { - if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + if (PIOS_COM_MSG_Receive(pios_com_telem_usb_id, mReceive_Buffer, sizeof(mReceive_Buffer))) { processComand(mReceive_Buffer); } return TRUE; @@ -219,5 +222,5 @@ uint8_t processRX() int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) { - return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); + return PIOS_COM_MSG_Send(pios_com_telem_usb_id, msg, msg_len); } diff --git a/flight/targets/boards/oplinkmini/bootloader/pios_board.c b/flight/targets/boards/oplinkmini/bootloader/pios_board.c index 4dad876dc1..5eea6e97a5 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/firmware/Makefile b/flight/targets/boards/oplinkmini/firmware/Makefile index bf7c50a121..534bbf5e25 100644 --- a/flight/targets/boards/oplinkmini/firmware/Makefile +++ b/flight/targets/boards/oplinkmini/firmware/Makefile @@ -29,7 +29,6 @@ override USE_DSP_LIB := NO # List of mandatory modules to include MODULES += RadioComBridge -MODULES += ComUsbBridge # List of optional modules to include OPTMODULES = diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h deleted file mode 100644 index 16364d7fa0..0000000000 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_board_posix.h +++ /dev/null @@ -1,81 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -// #define PIOS_LED_LED1_GPIO_PORT GPIOC -// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -// #define PIOS_LED_LED2_GPIO_PORT GPIOC -// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } - - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX -#endif - -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 074d876062..0cb1bfce00 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -84,7 +84,7 @@ /* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ -/* #define PIOS_INCLUDE_MS5611 */ +/* #define PIOS_INCLUDE_MS56XX */ /* #define PIOS_INCLUDE_MPXV */ /* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_HCSR04 */ @@ -146,26 +146,20 @@ /* #define PIOS_QUATERNION_STABILIZATION */ /* Performance counters */ -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 /* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 /* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 724 -#define PIOS_SYSTEM_STACK_SIZE 460 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 800 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 #endif /* PIOS_CONFIG_H */ /** diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h deleted file mode 100644 index 82712de0de..0000000000 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h +++ /dev/null @@ -1,55 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. - * Central compile time config for the project. - * @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_CONFIG_POSIX_H -#define PIOS_CONFIG_POSIX_H - - -/* Enable/Disable PiOS Modules */ -#define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_LED -#define PIOS_INCLUDE_FREERTOS -#define PIOS_INCLUDE_CALLBACKSCHEDULER -#define PIOS_INCLUDE_TASK_MONITOR -#define PIOS_INCLUDE_COM -#define PIOS_INCLUDE_UDP -#define PIOS_INCLUDE_SERVO - - -/* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 - -/* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 - - -#endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index a6f7574ca1..fe49aae235 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -32,11 +32,13 @@ #include #include #include +#include #include #include #ifdef PIOS_INCLUDE_SERVO #include #endif +#include /* * Pull in the board-specific static HW definitions. @@ -48,79 +50,29 @@ */ #include "../board_hw_defs.c" -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256 - -#define PIOS_COM_TELEM_VCP_RX_BUF_LEN 256 -#define PIOS_COM_TELEM_VCP_TX_BUF_LEN 256 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 - -#define PIOS_COM_TELEM_RX_BUF_LEN 256 -#define PIOS_COM_TELEM_TX_BUF_LEN 256 - -uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telem_vcp_id = 0; -uint32_t pios_com_telem_uart_main_id = 0; -uint32_t pios_com_telem_uart_flexi_id = 0; -uint32_t pios_com_telemetry_id = 0; -uint32_t pios_com_bridge_id = 0; -uint32_t pios_com_vcp_id = 0; -#if defined(PIOS_INCLUDE_PPM) -uint32_t pios_ppm_rcvr_id = 0; -#endif +uint32_t pios_com_hid_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_gcs_id = 0; +uint32_t pios_com_gcs_out_id = 0; #if defined(PIOS_INCLUDE_PPM_OUT) -uint32_t pios_ppm_out_id = 0; +uint32_t pios_ppm_out_id = 0; +bool ppm_rssi = false; #endif #if defined(PIOS_INCLUDE_RFM22B) -uint32_t pios_rfm22b_id = 0; -uint32_t pios_com_rfm22b_id = 0; -uint32_t pios_com_radio_id = 0; +#include +uint32_t pios_com_pri_radio_out_id = 0; +uint32_t pios_com_aux_radio_out_id = 0; #endif uintptr_t pios_uavo_settings_fs_id; -uintptr_t pios_user_fs_id = 0; - -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); - } -} +uintptr_t pios_user_fs_id = 0; +static uint8_t servo_count = 0; // Forward definitions -static void PIOS_Board_PPM_callback(const int16_t *channels); +static void PIOS_Board_PPM_callback(uint32_t context, const int16_t *channels); /** * PIOS_Board_Init() @@ -149,6 +101,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; /* Set up the SPI interface to the rfm22b */ if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) { @@ -181,11 +134,20 @@ void PIOS_Board_Init(void) PIOS_IAP_WriteBootCmd(2, 0); } -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ + OPLinkReceiverInitialize(); + /* Retrieve the settings object. */ + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + /* Determine the modem protocols */ + bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR); + bool ppm_only = (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL); + bool ppm_mode = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) || + (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL)); + ppm_rssi = (oplinkSettings.PPMOutRSSI == OPLINKSETTINGS_PPMOUTRSSI_TRUE); + bool servo_main = false; + bool servo_flexi = false; #if defined(PIOS_INCLUDE_TIM) /* Set up pulse timers */ @@ -227,7 +189,7 @@ void PIOS_Board_Init(void) 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, + if (PIOS_COM_Init(&pios_com_hid_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); @@ -245,7 +207,7 @@ void PIOS_Board_Init(void) uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { PIOS_Assert(0); @@ -254,51 +216,45 @@ void PIOS_Board_Init(void) #endif // Configure the main port - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - 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; - bool servo_flexi = false; + uint32_t mainComSpeed = 0; + switch (oplinkSettings.MainComSpeed) { + case OPLINKSETTINGS_MAINCOMSPEED_4800: + mainComSpeed = 4800; + break; + case OPLINKSETTINGS_MAINCOMSPEED_9600: + mainComSpeed = 9600; + break; + case OPLINKSETTINGS_MAINCOMSPEED_19200: + mainComSpeed = 19200; + break; + case OPLINKSETTINGS_MAINCOMSPEED_38400: + mainComSpeed = 38400; + break; + case OPLINKSETTINGS_MAINCOMSPEED_57600: + mainComSpeed = 57600; + break; + case OPLINKSETTINGS_MAINCOMSPEED_115200: + mainComSpeed = 115200; + break; + case OPLINKSETTINGS_MAINCOMSPEED_DISABLED: + break; + } switch (oplinkSettings.MainPort) { case OPLINKSETTINGS_MAINPORT_TELEMETRY: case OPLINKSETTINGS_MAINPORT_SERIAL: - { - /* Configure the main port for uart serial */ #ifndef PIOS_RFM22B_DEBUG_ON_TELEM - PIOS_Board_configure_com(&pios_usart_serial_cfg, - PIOS_COM_TELEM_RX_BUF_LEN, PIOS_COM_TELEM_TX_BUF_LEN, - &pios_usart_com_driver, &pios_com_telem_uart_main_id); - - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; -#endif - break; - } - case OPLINKSETTINGS_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_serial_cfg, - PIOS_COM_TELEM_RX_BUF_LEN, PIOS_COM_TELEM_TX_BUF_LEN, - &pios_usart_com_driver, &pios_com_bridge_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; case OPLINKSETTINGS_MAINPORT_PPM: - { #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) else { PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_main_ppm_out_cfg); @@ -306,69 +262,66 @@ void PIOS_Board_Init(void) #endif /* PIOS_INCLUDE_PPM_OUT */ #endif /* PIOS_INCLUDE_PPM */ break; - } case OPLINKSETTINGS_MAINPORT_PWM: servo_main = true; - break; - case OPLINKSETTINGS_MAINPORT_DISABLED: + default: break; } // Configure the flexi port + uint32_t flexiComSpeed = 0; + switch (oplinkSettings.FlexiComSpeed) { + case OPLINKSETTINGS_FLEXICOMSPEED_4800: + flexiComSpeed = 4800; + break; + case OPLINKSETTINGS_FLEXICOMSPEED_9600: + flexiComSpeed = 9600; + break; + case OPLINKSETTINGS_FLEXICOMSPEED_19200: + flexiComSpeed = 19200; + break; + case OPLINKSETTINGS_FLEXICOMSPEED_38400: + flexiComSpeed = 38400; + break; + case OPLINKSETTINGS_FLEXICOMSPEED_57600: + flexiComSpeed = 57600; + break; + case OPLINKSETTINGS_FLEXICOMSPEED_115200: + flexiComSpeed = 115200; + break; + case OPLINKSETTINGS_FLEXICOMSPEED_DISABLED: + break; + } switch (oplinkSettings.FlexiPort) { case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: case OPLINKSETTINGS_FLEXIPORT_SERIAL: - { - /* Configure the flexi port as uart serial */ - PIOS_Board_configure_com(&pios_usart_telem_flexi_cfg, - PIOS_COM_TELEM_RX_BUF_LEN, PIOS_COM_TELEM_TX_BUF_LEN, - &pios_usart_com_driver, - &pios_com_telem_uart_flexi_id); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; - break; - } - case OPLINKSETTINGS_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_telem_flexi_cfg, - PIOS_COM_TELEM_RX_BUF_LEN, PIOS_COM_TELEM_TX_BUF_LEN, - &pios_usart_com_driver, - &pios_com_bridge_id); +#ifndef PIOS_RFM22B_DEBUG_ON_TELEM + 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; case OPLINKSETTINGS_FLEXIPORT_PPM: - { #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); - } - } else { + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_flexi_cfg); + } +#if defined(PIOS_INCLUDE_PPM_OUT) + else { PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_flexi_ppm_out_cfg); } +#endif /* PIOS_INCLUDE_PPM_OUT */ #endif /* PIOS_INCLUDE_PPM */ break; - } case OPLINKSETTINGS_FLEXIPORT_PWM: servo_flexi = true; - break; - case OPLINKSETTINGS_FLEXIPORT_DISABLED: - break; - } - - // Configure the USB VCP port - switch (oplinkSettings.VCPPort) { - case OPLINKSETTINGS_VCPPORT_SERIAL: - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; - break; - case OPLINKSETTINGS_VCPPORT_COMBRIDGE: - PIOS_COM_VCP = PIOS_COM_TELEM_USB_VCP; - break; - case OPLINKSETTINGS_VCPPORT_DISABLED: + default: break; } + /* Configure the PWM servo outputs. */ #if defined(PIOS_INCLUDE_SERVO) if (servo_main) { if (servo_flexi) { @@ -382,150 +335,96 @@ void PIOS_Board_Init(void) servo_count = 2; PIOS_Servo_Init(&pios_servo_flexi_cfg); } -#endif - - // Initialize out status object. - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); - // 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; + // Set bank modes and activate output. We need to do this here because oplm does not run Actuator module. + PIOS_Servo_SetBankMode(0, PIOS_SERVO_BANK_MODE_PWM); + PIOS_Servo_SetBankMode(1, PIOS_SERVO_BANK_MODE_PWM); + PIOS_Servo_SetActive(0b11); +#endif - /* Initalize 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; + PIOS_BOARD_IO_Configure_RFM22B(); - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; + /* 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); + } - PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg); - PIOS_OpenLRS_RegisterPPMCallback(openlrs_id, PIOS_Board_PPM_callback); -#endif /* PIOS_INCLUDE_OPENLRS */ + // Configure the primary radio stream destination. + switch (oplinkSettings.RadioPriStream) { + case OPLINKSETTINGS_RADIOPRISTREAM_DISABLED: + break; + case OPLINKSETTINGS_RADIOPRISTREAM_HID: + // HID is always connected to GCS + pios_com_gcs_id = pios_com_hid_id; + pios_com_gcs_out_id = pios_com_pri_radio_id; + break; + case OPLINKSETTINGS_RADIOPRISTREAM_MAIN: + // Is the main port configured for telemetry (GCS)? + if (oplinkSettings.MainPort == OPLINKSETTINGS_MAINPORT_TELEMETRY) { + pios_com_gcs_id = pios_com_main_id; + pios_com_gcs_out_id = pios_com_pri_radio_id; } 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)) { - 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_rfm22b_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); - } - - // 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 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_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); - } - - // Reinitialize the modem to affect the changes. - PIOS_RFM22B_Reinit(pios_rfm22b_id); - } // openlrs - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + PIOS_COM_LinkComPair(pios_com_pri_radio_id, pios_com_main_id, false, false); + } + break; + case OPLINKSETTINGS_RADIOPRISTREAM_FLEXI: + // Is the flexi port configured for telemetry (GCS)? + if (oplinkSettings.FlexiPort == OPLINKSETTINGS_FLEXIPORT_TELEMETRY) { + pios_com_gcs_id = pios_com_flexi_id; + pios_com_gcs_out_id = pios_com_pri_radio_id; + } else { + PIOS_COM_LinkComPair(pios_com_pri_radio_id, pios_com_flexi_id, false, false); + } + break; + case OPLINKSETTINGS_RADIOPRISTREAM_VCP: + // VCP is never connected to GCS + PIOS_COM_LinkComPair(pios_com_pri_radio_id, pios_com_vcp_id, false, false); + break; } - // Update the object - OPLinkStatusSet(&oplinkStatus); - - // Update the com baud rate. - uint32_t comBaud = 9600; - switch (oplinkSettings.ComSpeed) { - case OPLINKSETTINGS_COMSPEED_4800: - comBaud = 4800; - break; - case OPLINKSETTINGS_COMSPEED_9600: - comBaud = 9600; + // Configure the Auxiliary radio stream destination. + switch (oplinkSettings.RadioAuxStream) { + case OPLINKSETTINGS_RADIOAUXSTREAM_DISABLED: break; - case OPLINKSETTINGS_COMSPEED_19200: - comBaud = 19200; + case OPLINKSETTINGS_RADIOAUXSTREAM_HID: + // HID is always connected to GCS + pios_com_gcs_id = pios_com_hid_id; + pios_com_gcs_out_id = pios_com_aux_radio_id; break; - case OPLINKSETTINGS_COMSPEED_38400: - comBaud = 38400; + case OPLINKSETTINGS_RADIOAUXSTREAM_MAIN: + // Is the main port configured for telemetry (GCS)? + if (oplinkSettings.MainPort == OPLINKSETTINGS_MAINPORT_TELEMETRY) { + pios_com_gcs_id = pios_com_main_id; + pios_com_gcs_out_id = pios_com_aux_radio_id; + } else { + PIOS_COM_LinkComPair(pios_com_aux_radio_id, pios_com_main_id, false, false); + } break; - case OPLINKSETTINGS_COMSPEED_57600: - comBaud = 57600; + case OPLINKSETTINGS_RADIOAUXSTREAM_FLEXI: + // Is the flexi port configured for telemetry (GCS)? + if (oplinkSettings.FlexiPort == OPLINKSETTINGS_FLEXIPORT_TELEMETRY) { + pios_com_gcs_id = pios_com_flexi_id; + pios_com_gcs_out_id = pios_com_aux_radio_id; + } else { + PIOS_COM_LinkComPair(pios_com_aux_radio_id, pios_com_flexi_id, false, false); + } break; - case OPLINKSETTINGS_COMSPEED_115200: - comBaud = 115200; + case OPLINKSETTINGS_RADIOAUXSTREAM_VCP: + // VCP is never connected to GCS + PIOS_COM_LinkComPair(pios_com_aux_radio_id, pios_com_vcp_id, false, false); break; } - if (PIOS_COM_TELEMETRY) { - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, comBaud); + + // Configure the VCP COM bridge + switch (oplinkSettings.VCPBridge) { + case OPLINKSETTINGS_VCPBRIDGE_MAIN: + PIOS_COM_LinkComPair(pios_com_vcp_id, pios_com_main_id, true, true); + break; + case OPLINKSETTINGS_VCPBRIDGE_FLEXI: + PIOS_COM_LinkComPair(pios_com_vcp_id, pios_com_flexi_id, true, true); + break; + case OPLINKSETTINGS_VCPBRIDGE_DISABLED: + break; } /* Remap AFIO pin */ @@ -536,19 +435,47 @@ void PIOS_Board_Init(void) #endif } -static void PIOS_Board_PPM_callback(const int16_t *channels) +static void PIOS_Board_PPM_callback(__attribute__((unused)) uint32_t context, const int16_t *channels) { #if defined(PIOS_INCLUDE_PPM) && defined(PIOS_INCLUDE_PPM_OUT) if (pios_ppm_out_id) { for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { - if (channels[i] != PIOS_RCVR_INVALID) { + if ((channels[i] != PIOS_RCVR_INVALID) && (channels[i] != PIOS_RCVR_TIMEOUT)) { PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, channels[i]); } } + // Rssi channel output is added after RC channels + // Output Rssi from 1000µs to 2000µs (-127dBm to -16dBm range) + if (ppm_rssi) { + int8_t rssi; + int16_t ppm_value; + OPLinkStatusRSSIGet(&rssi); + ppm_value = 1000 + ((rssi + 127) * 9); + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, RFM22B_PPM_NUM_CHANNELS, ppm_value); + } + } else { + // If there is no PPM output defined, try sending the control outputs as a UAVObject. + OPLinkReceiverData recv_data; + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { + if (i < RFM22B_PPM_NUM_CHANNELS) { + recv_data.Channel[i] = channels[i]; + } else { + recv_data.Channel[i] = PIOS_RCVR_TIMEOUT; + } + } + // Update the RSSI and quality fields. + int8_t rssi; + OPLinkStatusRSSIGet(&rssi); + recv_data.RSSI = rssi; + uint16_t quality; + OPLinkStatusLinkQualityGet(&quality); + // Link quality is 0-128, so scale it down to 0-100 + recv_data.LinkQuality = quality * 100 / 128; + OPLinkReceiverSet(&recv_data); } #if defined(PIOS_INCLUDE_SERVO) for (uint8_t i = 0; i < servo_count; ++i) { - uint16_t val = (channels[i] == PIOS_RCVR_INVALID) ? 0 : channels[i]; + uint16_t val = ((channels[i] == PIOS_RCVR_INVALID) || (channels[i] == PIOS_RCVR_TIMEOUT)) ? 0 : channels[i]; PIOS_Servo_Set(i, val); } #endif /* PIOS_INCLUDE_SERVO */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c deleted file mode 100644 index 7fa958a38e..0000000000 --- a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c +++ /dev/null @@ -1,145 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board specific static initializers for hardware for the OpenPilot board. - * @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 "inc/openpilot.h" -#include -#include -#include - -/** - * PIOS_Board_Init() - * initializes all the core systems on this specific hardware - * called from System/openpilot.c - */ -void PIOS_Board_Init(void) -{ - /* Delay system */ - PIOS_DELAY_Init(); - - /* Initialize the delayed callback library */ - PIOS_CALLBACKSCHEDULER_Initialize(); - - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - - /* Initialize the alarms library */ - AlarmsInitialize(); - - /* Initialize the task monitor */ - if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { - PIOS_Assert(0); - } - - /* Initialize the PiOS library */ - PIOS_COM_Init(); -} - - -const struct pios_udp_cfg pios_udp0_cfg = { - .ip = "0.0.0.0", - .port = 9000, -}; -const struct pios_udp_cfg pios_udp1_cfg = { - .ip = "0.0.0.0", - .port = 9001, -}; -const struct pios_udp_cfg pios_udp2_cfg = { - .ip = "0.0.0.0", - .port = 9002, -}; - -#ifdef PIOS_COM_AUX -/* - * AUX USART - */ -const struct pios_udp_cfg pios_udp3_cfg = { - .ip = "0.0.0.0", - .port = 9003, -}; -#endif - -/* - * Board specific number of devices. - */ -struct pios_udp_dev pios_udp_devs[] = { -#define PIOS_UDP_TELEM 0 - { - .cfg = &pios_udp0_cfg, - }, -#define PIOS_UDP_GPS 1 - { - .cfg = &pios_udp1_cfg, - }, -#define PIOS_UDP_LOCAL 2 - { - .cfg = &pios_udp2_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_UDP_AUX 3 - { - .cfg = &pios_udp3_cfg, - }, -#endif -}; - -uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); - -/* - * COM devices - */ - -/* - * Board specific number of devices. - */ -extern const struct pios_com_driver pios_serial_com_driver; -extern const struct pios_com_driver pios_udp_com_driver; - -struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, -#ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, -#endif -}; - -const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs); - -/** - * @} - */ diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index 749657ea79..752ec3054d 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Defines PiOS board hardware for the OpenPilot OPLink Mini board. * @see The GNU Public License (GPL) Version 3 * @@ -72,11 +73,10 @@ #define PIOS_WDG_REGISTER BKP_DR4 #define PIOS_WDG_TELEMETRYTX 0x0001 #define PIOS_WDG_TELEMETRYRX 0x0002 -#define PIOS_WDG_RADIOTX 0x0004 -#define PIOS_WDG_RADIORX 0x0008 +#define PIOS_WDG_TELEMRADIOTX 0x0004 +#define PIOS_WDG_TELEMRADIORX 0x0008 #define PIOS_WDG_RFM22B 0x000f #define PIOS_WDG_PPMINPUT 0x0010 -#define PIOS_WDG_SERIALRX 0x0020 // ------------------------ // TELEMETRY @@ -138,7 +138,6 @@ // System Settings // ------------------------- #define PIOS_MASTER_CLOCK 72000000 -#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) // ------------------------- // Interrupt Priorities @@ -160,44 +159,61 @@ 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 // // See also pios_board.c // ------------------------- -#define PIOS_COM_MAX_DEVS 5 - -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_telem_vcp_id; -extern uint32_t pios_com_telem_uart_main_id; -extern uint32_t pios_com_telem_uart_flexi_id; -extern uint32_t pios_com_telemetry_id; -extern uint32_t pios_com_rfm22b_id; -extern uint32_t pios_com_radio_id; -extern uint32_t pios_com_bridge_id; +#define PIOS_COM_MAX_DEVS 5 + +/* USB telemetry */ +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256 + +/* VCP Bridge */ +#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; extern uint32_t pios_com_vcp_id; -extern uint32_t pios_ppm_rcvr_id; +extern uint32_t pios_com_main_id; +extern uint32_t pios_com_flexi_id; +extern uint32_t pios_com_pri_radio_id; +extern uint32_t pios_com_aux_radio_id; +// The port that the GCS is connected to +extern uint32_t pios_com_gcs_id; +// The destination port from the GCS port +extern uint32_t pios_com_gcs_out_id; +// The destination port from the VCP com bridge +extern uint32_t pios_com_bridge_id; +// The destination port for the primary radio port +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_out_id; -#define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_USB PIOS_COM_TELEM_USB_HID -#define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_id) -#define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) -#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) -#define PIOS_COM_RFM22B (pios_com_rfm22b_id) -#define PIOS_COM_RADIO (pios_com_radio_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 +#define PIOS_COM_HID (pios_com_hid_id) +#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_MAIN (pios_com_main_id) +#define PIOS_COM_FLEXI (pios_com_flexi_id) +#define PIOS_COM_PRI_RADIO (pios_com_pri_radio_id) +#define PIOS_COM_AUX_RADIO (pios_com_aux_radio_id) +#define PIOS_COM_PRI_RADIO_OUT (pios_com_pri_radio_out_id) +#define PIOS_COM_AUX_RADIO_OUT (pios_com_aux_radio_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_OUTPUT (pios_ppm_out_id) +#define RFM22_DEBUG 1 // ------------------------- // ADC diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c index ed86e0915c..d38d509ce6 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/inc/common.h b/flight/targets/boards/osd/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/osd/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/osd/bootloader/main.c b/flight/targets/boards/osd/bootloader/main.c index c333ab9af3..36252cc839 100644 --- a/flight/targets/boards/osd/bootloader/main.c +++ b/flight/targets/boards/osd/bootloader/main.c @@ -76,7 +76,7 @@ int main() PIOS_Board_Init(); PIOS_IAP_Init(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CableConnected(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); diff --git a/flight/targets/boards/osd/bootloader/pios_board.c b/flight/targets/boards/osd/bootloader/pios_board.c index 7e9858f18a..bbd7232e9d 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/Makefile b/flight/targets/boards/osd/firmware/Makefile index 74043edcc5..3842d1c233 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -70,11 +70,6 @@ ifndef TESTAPP SRC += $(OPSYSTEM)/font_outlined8x14.c SRC += $(OPSYSTEM)/font_outlined8x8.c - #ifeq ($(DEBUG), YES) - SRC += $(OPSYSTEM)/dcc_stdio.c - SRC += $(OPSYSTEM)/cm3_fault_handlers.c - #endif - ## Misc library functions SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/auxmagsupport.c diff --git a/flight/targets/boards/osd/firmware/cm3_fault_handlers.c b/flight/targets/boards/osd/firmware/cm3_fault_handlers.c deleted file mode 100644 index e74ffa76ad..0000000000 --- a/flight/targets/boards/osd/firmware/cm3_fault_handlers.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * cm3_fault_handlers.c - * - * Created on: Apr 24, 2011 - * Author: msmith - */ - -#include -#include "inc/dcc_stdio.h" -#ifdef STM32F4XX -# include -#endif -#ifdef STM32F2XX -# include -#endif - -#define FAULT_TRAMPOLINE(_vec) \ - __attribute__((naked, no_instrument_function)) \ - void \ - _vec##_Handler(void) \ - { \ - __asm(".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ - } \ - struct hack - -struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -FAULT_TRAMPOLINE(HardFault); -FAULT_TRAMPOLINE(BusFault); -FAULT_TRAMPOLINE(UsageFault); - -/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) - -void HardFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void BusFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void UsageFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} diff --git a/flight/targets/boards/osd/firmware/dcc_stdio.c b/flight/targets/boards/osd/firmware/dcc_stdio.c deleted file mode 100644 index 1a522e9a0d..0000000000 --- a/flight/targets/boards/osd/firmware/dcc_stdio.c +++ /dev/null @@ -1,149 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* Copyright (C) 2008 by Frederik Kriewtz * -* frederik@kriewitz.eu * -* * -* 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 2 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 "inc/dcc_stdio.h" - -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 - -/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel - * DCRDR[7:0] is used by target for status - * DCRDR[15:8] is used by target for write buffer - * DCRDR[23:16] is used for by host for status - * DCRDR[31:24] is used for by host for write buffer */ - -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) - -#define BUSY 1 - -void dbg_write(unsigned long dcc_data) -{ - int len = 4; - - while (len--) { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY) { - ; - } - - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } -} - -void dbg_trace_point(unsigned long number) -{ - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); -} - -void dbg_write_u32(const unsigned long *val, long len) -{ - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - - while (len > 0) { - dbg_write(*val); - - val++; - len--; - } -} - -void dbg_write_u16(const unsigned short *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16 : 0x0000); - - dbg_write(dcc_data); - - val += 2; - len -= 2; - } -} - -void dbg_write_u8(const unsigned char *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); - - dbg_write(dcc_data); - - val += 4; - len -= 4; - } -} - -void dbg_write_str(const char *msg) -{ - long len; - unsigned long dcc_data; - - for (len = 0; msg[len] && (len < 65536); len++) { - ; - } - - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); - - msg += 4; - len -= 4; - } -} - -void dbg_write_char(char msg) -{ - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); -} - -void dbg_write_hex32(const unsigned long val) -{ - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; - - for (int shift = 28; shift >= 0; shift -= 4) { - dbg_write_char(hextab[(val >> shift) & 0xf]); - } -} diff --git a/flight/targets/boards/osd/firmware/inc/dcc_stdio.h b/flight/targets/boards/osd/firmware/inc/dcc_stdio.h deleted file mode 100644 index 3d73549182..0000000000 --- a/flight/targets/boards/osd/firmware/inc/dcc_stdio.h +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* * -* 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 2 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 DCC_STDIO_H -#define DCC_STDIO_H - -void dbg_trace_point(unsigned long number); - -void dbg_write_u32(const unsigned long *val, long len); -void dbg_write_u16(const unsigned short *val, long len); -void dbg_write_u8(const unsigned char *val, long len); - -void dbg_write_str(const char *msg); -void dbg_write_char(char msg); -void dbg_write_hex32(const unsigned long val); - -#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index 75ef3286ad..316b78a840 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -84,7 +84,7 @@ /* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5X83_HAS_GPIOS */ #define PIOS_INCLUDE_BMP085 -/* #define PIOS_INCLUDE_MS5611 */ +/* #define PIOS_INCLUDE_MS56XX */ /* #define PIOS_INCLUDE_MPXV */ /* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_HCSR04 */ diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index 680c93c2e4..aebfe97a26 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; @@ -183,8 +170,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - - HwSettingsInitialize(); + SETTINGS_INITIALISE_ALL; #ifdef PIOS_INCLUDE_WDG /* Initialize watchdog as early as possible to catch faults during init */ @@ -216,125 +202,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/pikoblx/board-info.mk b/flight/targets/boards/pikoblx/board-info.mk new file mode 100644 index 0000000000..2a17235605 --- /dev/null +++ b/flight/targets/boards/pikoblx/board-info.mk @@ -0,0 +1,23 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x05 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +CHIP := STM32F303xC + +OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg +OPENOCD_CONFIG := stm32f3x.cfg + +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region + +# Some KB for settings storage (32kb) +EE_BANK_BASE := 0x08004000 # EEPROM storage area +EE_BANK_SIZE := 0x00008000 # EEPROM storage size + +FW_BANK_BASE := 0x0800C000 # Start of firmware flash +FW_BANK_SIZE := 0x00034000 # Should include FW_DESC_SIZE (208kB) + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/pikoblx/board_hw_defs.c b/flight/targets/boards/pikoblx/board_hw_defs.c new file mode 100644 index 0000000000..84b330d11f --- /dev/null +++ b/flight/targets/boards/pikoblx/board_hw_defs.c @@ -0,0 +1,561 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @brief Defines board specific static initializers for hardware for the PikoBLX board. + *****************************************************************************/ +/* + * 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 + */ + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_gpio pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = true + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = true + }, + [PIOS_BUZZER_ALARM] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg = { + .gpios = pios_leds, + .num_gpios = NELEMENTS(pios_leds), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Gyro interface */ + +static const struct pios_spi_cfg pios_spi_mpu6000_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi_mpu6000_id; + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" +#include "pios_flash_internal_priv.h" + +static const struct pios_flash_internal_cfg flash_internal_system_cfg = {}; + +static const struct flashfs_logfs_cfg flashfs_internal_cfg = { + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (16x2KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 8 sectors */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00000800, /* 2K bytes */ + .page_size = 0x00000800, /* 2K bytes */ +}; + +#endif /* PIOS_INCLUDE_FLASH */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_15_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM15_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_16_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM16_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_17_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 +#define GPIO_AF_PA15_TIM8 GPIO_AF_2 +#define GPIO_AF_PA3_TIM15 GPIO_AF_9 +#define GPIO_AF_PA6_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM3 GPIO_AF_2 +#define GPIO_AF_PA10_TIM2 GPIO_AF_10 + +#define GPIO_AF_PA4_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM17 GPIO_AF_1 +#define GPIO_AF_PB8_TIM16 GPIO_AF_1 + +#define TIM1_CH1_DMA_INSTANCE 1 +#define TIM1_CH1_DMA_CHANNEL 2 + +#define TIM16_CH1_DMA_INSTANCE 2 +#define TIM16_CH1_DMA_CHANNEL 3 + +static const struct pios_tim_channel pios_tim_servoport_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, A, 4), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, A, 6), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 2, A, 3), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM1, 1, A, 8), // transponder (bank 4) +}; + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART2 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_cfg[] = { + USART_CONFIG(USART1, B, 7, B, 6), + USART_CONFIG(USART2, B, 4, B, 3), + USART_CONFIG(USART3, B, 11, B, 10), +}; + + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +static const struct pios_tim_channel pios_tim_ppm = TIM_SERVO_CHANNEL_CONFIG(TIM17, 1, A, 7); + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm, + .num_channels = 1, +}; +#endif /* PIOS_INCLUDE_PPM */ + + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +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_WS2811) +#include "pios_ws2811_cfg.h" + +static const struct pios_ws2811_cfg pios_ws2811_cfg = PIOS_WS2811_CONFIG(TIM16, 1, B, 8); + +void DMA1_Channel2_IRQHandler() +{ + PIOS_WS2811_DMA_irq_handler(); +} + +#endif /* PIOS_INCLUDE_WS2811 */ + +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Channel1_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC2, + .dma = { + .irq = { + .flags = (DMA2_FLAG_TC1 | DMA2_FLAG_TE1 | DMA2_FLAG_HT1), + .init = { + .NVIC_IRQChannel = DMA2_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Channel1, + } + }, + .half_flag = DMA2_FLAG_HT1, + .full_flag = DMA2_FLAG_TC1, +}; + +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) */ + +/** + * Configuration for 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_Line15, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .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_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_270DEG, +#ifdef PIOS_INCLUDE_SPI + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, +#endif /* PIOS_INCLUDE_SPI */ + .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/pikoblx/bootloader/Makefile b/flight/targets/boards/pikoblx/bootloader/Makefile new file mode 100644 index 0000000000..333af7ea60 --- /dev/null +++ b/flight/targets/boards/pikoblx/bootloader/Makefile @@ -0,0 +1,29 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for PikoBLX, board revision $(BOARD_REVISION)) diff --git a/flight/targets/boards/pikoblx/bootloader/inc/pios_config.h b/flight/targets/boards/pikoblx/bootloader/inc/pios_config.h new file mode 100644 index 0000000000..bd8022938d --- /dev/null +++ b/flight/targets/boards/pikoblx/bootloader/inc/pios_config.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/pikoblx/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/pikoblx/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..f2c08c8c7f --- /dev/null +++ b/flight/targets/boards/pikoblx/bootloader/inc/pios_usb_board_data.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 2 + +#include /* struct usb_* */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_PIKOBLX, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" + +/* + * The bootloader uses a simplified report structure + * BL: ... + * FW: ... + * This define changes the behaviour in pios_usb_hid.c + */ +#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/pikoblx/bootloader/main.c b/flight/targets/boards/pikoblx/bootloader/main.c new file mode 100644 index 0000000000..044608da54 --- /dev/null +++ b/flight/targets/boards/pikoblx/bootloader/main.c @@ -0,0 +1,230 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This is the file with the main function of the LibrePilot BootLoader + * @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 +#include +#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) + +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + + +//////////////////////////////////////// +uint8_t tempcount = 0; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; +uint8_t User_DFU_request = FALSE; +static uint8_t mReceive_Buffer[63]; +/* Private function prototypes -----------------------------------------------*/ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); +uint8_t processRX(); +void jump_to_app(); + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + USB_connected = PIOS_USB_CableConnected(0); + + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } + + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } + + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; + + if (JumpToApp == TRUE) { + jump_to_app(); + } + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } + + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) /*|| (DeviceState == DFUidle && !USB_connected)*/)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } +} + +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + JumpToApp = FALSE; + return; + } +} +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} + +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); +} diff --git a/flight/targets/boards/pikoblx/bootloader/pios_board.c b/flight/targets/boards/pikoblx/bootloader/pios_board.c new file mode 100644 index 0000000000..182287ab47 --- /dev/null +++ b/flight/targets/boards/pikoblx/bootloader/pios_board.c @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the PikoBLX board. + * @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 "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 + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uint32_t pios_com_telem_usb_id; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +static bool board_init_complete = false; +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(ENABLE); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + +#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_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)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + +#endif /* PIOS_INCLUDE_USB */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar + + board_init_complete = true; +} + +void PIOS_ADC_DMA_Handler() +{} diff --git a/flight/targets/boards/pikoblx/firmware/Makefile b/flight/targets/boards/pikoblx/firmware/Makefile new file mode 100644 index 0000000000..1dc1dd323e --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/Makefile @@ -0,0 +1,113 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +MODULES += Notify + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += Battery +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/pikoblx/firmware/UAVObjects.inc b/flight/targets/boards/pikoblx/firmware/UAVObjects.inc new file mode 100644 index 0000000000..2ec9de7cdc --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/UAVObjects.inc @@ -0,0 +1,134 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwpikoblxsettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/pikoblx/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/pikoblx/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 0000000000..5fea013d7a --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/pikoblx/firmware/inc/openpilot.h b/flight/targets/boards/pikoblx/firmware/inc/openpilot.h new file mode 100644 index 0000000000..5576baca66 --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @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 OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/pikoblx/firmware/inc/pios_config.h b/flight/targets/boards/pikoblx/firmware/inc/pios_config.h new file mode 100644 index 0000000000..71b840b8e2 --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/inc/pios_config.h @@ -0,0 +1,195 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2015 + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_ADC +// #define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +#define PIOS_INCLUDE_MPU6000 +#define PIOS_MPU6000_ACCEL +/* #define PIOS_INCLUDE_HMC5843 */ +/* #define PIOS_INCLUDE_HMC5X83 */ +/* #define PIOS_HMC5883_HAS_GPIOS */ +/* #define PIOS_INCLUDE_MPU9250 */ +/* #define PIOS_MPU9250_ACCEL */ +/* #define PIOS_MPU9250_MAG */ +/* #define PIOS_INCLUDE_BMP280 */ +/* #define PIOS_INCLUDE_BMP085 */ +/* #define PIOS_INCLUDE_MS56XX */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +#define PIOS_INCLUDE_WS2811 + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +#define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +#define FLASH_FREERTOS +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +#define PIOS_INCLUDE_FLASH_INTERNAL + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +/* #define PIOS_GPS_MINIMAL */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/pikoblx/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/pikoblx/firmware/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..9292033157 --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/inc/pios_usb_board_data.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 +#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 4 + +#include /* USB_* macros */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_PIKOBLX, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/pikoblx/firmware/main.cpp b/flight/targets/boards/pikoblx/firmware/main.cpp new file mode 100644 index 0000000000..d1ebd36253 --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/main.cpp @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @brief These files are the core system files for LibrePilot. + * They are the ground layer just above PiOS. In practice, LibrePilot actually starts + * in the main() function of main.cpp + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @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 + */ + +extern "C" { +#include "inc/openpilot.h" +#include +/* Task Priorities */ + +/* Global Variables */ +extern void Stack_Change(void); +} /* extern "C" */ + +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/pikoblx/firmware/pios_board.c b/flight/targets/boards/pikoblx/firmware/pios_board.c new file mode 100644 index 0000000000..830d9f81a1 --- /dev/null +++ b/flight/targets/boards/pikoblx/firmware/pios_board.c @@ -0,0 +1,265 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the PikoBLX board. + *****************************************************************************/ +/* + * 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 "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif + +void RevoSettingsDataOverrideDefaults(RevoSettingsData *data) +{ + /* This board has no barometer, so adjust default fusion algorithm to one that does not depend on working baro */ + data->FusionAlgorithm = REVOSETTINGS_FUSIONALGORITHM_ACRONOSENSORS; +} + + +static HwPikoBLXSettingsData boardHwSettings; + +static void hwPikoBLXSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HwPikoBLXSettingsData currentBoardHwSettings; + + HwPikoBLXSettingsGet(¤tBoardHwSettings); + + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwPikoBLXSettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + } +} + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ + +void PIOS_Board_Init(void) +{ +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the mpu9250 */ + + if (PIOS_SPI_Init(&pios_spi_mpu6000_id, &pios_spi_mpu6000_cfg)) { + PIOS_DEBUG_Assert(0); + } +#endif + +#if defined(PIOS_INCLUDE_FLASH) + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + + // initialize the internal settings storage flash + if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_system_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + +// if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_internal_user_cfg, &pios_internal_flash_driver, flash_id)) { +// PIOS_DEBUG_Assert(0); +// } + +#endif /* if defined(PIOS_INCLUDE_FLASH) */ + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + HwPikoBLXSettingsSetDefaults(HwPikoBLXSettingsHandle(), 0); + + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + // PIOS_TIM_InitClock(&tim_4_cfg); + // PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_15_cfg); + PIOS_TIM_InitClock(&tim_16_cfg); + PIOS_TIM_InitClock(&tim_17_cfg); + + +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + + HwPikoBLXSettingsConnectCallback(hwPikoBLXSettingsUpdatedCb); + + HwPikoBLXSettingsGet(&boardHwSettings); + + static const PIOS_BOARD_IO_UART_Function uart_function_map[] = { + [HWPIKOBLXSETTINGS_UARTPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWPIKOBLXSETTINGS_UARTPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWPIKOBLXSETTINGS_UARTPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWPIKOBLXSETTINGS_UARTPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok. + [HWPIKOBLXSETTINGS_UARTPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWPIKOBLXSETTINGS_UARTPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWPIKOBLXSETTINGS_UARTPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWPIKOBLXSETTINGS_UARTPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWPIKOBLXSETTINGS_UARTPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWPIKOBLXSETTINGS_UARTPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWPIKOBLXSETTINGS_UARTPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWPIKOBLXSETTINGS_UARTPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWPIKOBLXSETTINGS_UARTPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWPIKOBLXSETTINGS_UARTPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWPIKOBLXSETTINGS_UARTPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + }; + + for (unsigned int i = 0; i < HWPIKOBLXSETTINGS_UARTPORT_NUMELEM; ++i) { + if (boardHwSettings.UARTPort[i] < NELEMENTS(uart_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[i], uart_function_map[boardHwSettings.UARTPort[i]]); + } + } + +#ifdef PIOS_INCLUDE_PPM + if (boardHwSettings.PPMPort == HWPIKOBLXSETTINGS_PPMPORT_ENABLED) { + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); + } +#endif + +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + +#ifndef PIOS_ENABLE_DEBUG_PINS + PIOS_Servo_Init(&pios_servo_cfg); +#else + PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels); +#endif + + switch (boardHwSettings.LEDPort) { + case HWPIKOBLXSETTINGS_LEDPORT_WS281X: +#if defined(PIOS_INCLUDE_WS2811) + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg); +#endif + break; + default: + break; + } + + + PIOS_BOARD_Sensors_Configure(); + + PIOS_LED_On(PIOS_LED_HEARTBEAT); + + /* 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/pikoblx/pios_board.h b/flight/targets/boards/pikoblx/pios_board.h new file mode 100644 index 0000000000..f3654b0b08 --- /dev/null +++ b/flight/targets/boards/pikoblx/pios_board.h @@ -0,0 +1,241 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the PikoBLX board + * @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_H +#define PIOS_BOARD_H +// ------------------------ +// Timers and Channels Used +// ------------------------ +/* + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ + +// ------------------------ +// DMA Channels Used +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#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_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 +#define PIOS_BUZZER_ALARM 2 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 0 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 +extern uint32_t pios_spi_mpu6000_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi_mpu6000_id) + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 3 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Current sensor +// PIOS_ADC_PinGet(1) = Battery voltage +// ------------------------- +#define PIOS_DMA_PIN_CONFIG \ + { \ + { GPIOB, GPIO_Pin_2, ADC_Channel_12, false }, /* ADC_2 / Current */ \ + { GPIOA, GPIO_Pin_5, ADC_Channel_2, false }, /* ADC_1 / Voltage */ \ + } + +/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ +/* which is annoying because this then determines the rate at which we generate buffer turnover events */ +/* the objective here is to get enough buffer space to support 100Hz averaging rate */ +#define PIOS_ADC_NUM_CHANNELS 2 +#define PIOS_ADC_MAX_OVERSAMPLING 32 +#define PIOS_ADC_USE_ADC2 0 + +// #define PIOS_ADC_USE_TEMP_SENSOR +// #define PIOS_ADC_TEMPERATURE_PIN 4 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// USB +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 + +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_MAX_DEVS 1 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/pikoblx/pios_usb_board_data.c b/flight/targets/boards/pikoblx/pios_usb_board_data.c new file mode 100644 index 0000000000..13b1af87ca --- /dev/null +++ b/flight/targets/boards/pikoblx/pios_usb_board_data.c @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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 + */ + +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ + +static const uint8_t usb_product_id[18] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'P', 0, + 'i', 0, + 'k', 0, + 'o', 0, + ' ', 0, + 'B', 0, + 'L', 0, + 'X', 0, +}; + +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; + + PIOS_SYS_SerialNumberGet((char *)sn); + + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 74770d9804..3780965668 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 @@ -463,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); } @@ -598,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); } @@ -656,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: @@ -786,33 +787,17 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { #endif /* PIOS_INCLUDE_FLASH */ +#ifdef PIOS_INCLUDE_USART #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN USART */ + 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,7 +807,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, @@ -833,44 +818,27 @@ static const struct pios_usart_cfg pios_usart_main_cfg = { }, }, }; -#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, +/* + * FLEXI PORT + */ +static const struct pios_usart_cfg pios_usart_flexi_cfg = { + .regs = USART3, + .remap = GPIO_AF_USART3, + .rx = { + .gpio = GPIOB, .init = { - .GPIO_Pin = GPIO_Pin_10, + .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 = GPIOA, + .tx = { + .gpio = GPIOB, .init = { - .GPIO_Pin = GPIO_Pin_9, + .GPIO_Pin = GPIO_Pin_10, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_AF, .GPIO_OType = GPIO_OType_PP, @@ -879,678 +847,188 @@ 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, +/* FLEXI-IO (Receiver) port */ +static const struct pios_usart_cfg pios_usart_flexiio_cfg = { + .regs = USART6, + .remap = GPIO_AF_USART6, + .dtr = { + // FlexIO pin 9 + .gpio = GPIOC, .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN, + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_25MHz, + .GPIO_Mode = GPIO_Mode_OUT, .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, + .tx = { + // * 7: PC6 = TIM8 CH1, USART6 TX + .gpio = GPIOC, .init = { - .GPIO_Pin = GPIO_Pin_10, + .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, }, - .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 = { + .rx = { + // * 8: PC7 = TIM8 CH2, USART6 RX .gpio = GPIOC, .init = { - .GPIO_Pin = GPIO_Pin_0, + .GPIO_Pin = GPIO_Pin_7, .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_Mode = GPIO_Mode_AF, .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, + .pin_source = GPIO_PinSource7, + } }; +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_I2C) + +#include -#ifdef PIOS_INCLUDE_COM_FLEXI /* - * FLEXI PORT + * I2C Adapters */ -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, - }, +void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void); +void PIOS_I2C_mag_pressureadapter_er_irq_handler(void); +void I2C1_EV_IRQHandler() +__attribute__((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler"))); +void I2C1_ER_IRQHandler() +__attribute__((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = { + .regs = I2C1, + .remapSCL = GPIO_AF_I2C1, + .remapSDA = GPIO_AF_I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ }, - .rx = { + .transfer_timeout_ms = 50, + .scl = { .gpio = GPIOB, .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Pin = GPIO_Pin_8, .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, }, }, - .tx = { + .sda = { .gpio = GPIOB, .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Pin = GPIO_Pin_9, .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, }, }, -}; - -#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, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, .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 + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, }, - .tx = { +}; + +uint32_t pios_i2c_mag_pressure_adapter_id; +void PIOS_I2C_mag_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); +} + +void PIOS_I2C_mag_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); +} + + +void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); +void PIOS_I2C_flexiport_adapter_er_irq_handler(void); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { + .regs = I2C2, + .remapSCL = GPIO_AF_I2C2, + .remapSDA = GPIO_AF_I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { .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 + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_UP, }, }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { + .sda = { .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 + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_UP, }, }, -}; - -#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, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, }, - .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 - }, - }, -}; - -#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 = { - // FlexIO pin 9 - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_25MHz, - .GPIO_Mode = GPIO_Mode_OUT, - .GPIO_OType = GPIO_OType_PP, - }, - }, - - .tx = { - // * 7: PC6 = TIM8 CH1, USART6 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 - }, - .pin_source = GPIO_PinSource6, - }, - - .rx = { - // * 8: PC7 = TIM8 CH2, USART6 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 - }, - .pin_source = GPIO_PinSource7, - } -}; - -#if defined(PIOS_INCLUDE_COM) - -#include - -#endif /* PIOS_INCLUDE_COM */ - -#if defined(PIOS_INCLUDE_I2C) - -#include - -/* - * I2C Adapters - */ -void PIOS_I2C_mag_pressure_adapter_ev_irq_handler(void); -void PIOS_I2C_mag_pressureadapter_er_irq_handler(void); -void I2C1_EV_IRQHandler() -__attribute__((alias("PIOS_I2C_mag_pressure_adapter_ev_irq_handler"))); -void I2C1_ER_IRQHandler() -__attribute__((alias("PIOS_I2C_mag_pressure_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_mag_pressure_adapter_cfg = { - .regs = I2C1, - .remapSCL = GPIO_AF_I2C1, - .remapSDA = GPIO_AF_I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_NOPULL, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_mag_pressure_adapter_id; -void PIOS_I2C_mag_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); -} - -void PIOS_I2C_mag_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); -} - - -void PIOS_I2C_flexiport_adapter_ev_irq_handler(void); -void PIOS_I2C_flexiport_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexiport_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_flexiport_adapter_cfg = { - .regs = I2C2, - .remapSCL = GPIO_AF_I2C2, - .remapSDA = GPIO_AF_I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_UP, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_OType = GPIO_OType_OD, - .GPIO_PuPd = GPIO_PuPd_UP, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }, @@ -1710,268 +1188,80 @@ static const TIM_TimeBaseInitTypeDef tim_apb2_time_base = { }; static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_apb2_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_apb1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; -static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_apb2_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_12_cfg = { - .timer = TIM12, - .time_base_init = &tim_apb1_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - - -/** - * Pios servo configuration structures - * Using TIM3, TIM9, TIM2, TIM5 - */ -#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, + .timer = TIM1, + .time_base_init = &tim_apb2_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .remap = GPIO_AF_TIM5, }, - // 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, +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_apb1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .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, +}; +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_apb2_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .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, +static const struct pios_tim_clock_cfg tim_12_cfg = { + .timer = TIM12, + .time_base_init = &tim_apb1_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_BRK_TIM12_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, }, - .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, - }, +/** + * Pios servo configuration structures + * Using TIM3, TIM9, TIM2, TIM5 + */ +#include +#include - { - // * 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, - }, +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), + // 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(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 @@ -2029,102 +1319,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 = { @@ -2240,51 +1440,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 */ - -#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 @@ -2419,3 +1576,205 @@ 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_MS56XX) +#include "pios_ms56xx.h" +static const struct pios_ms56xx_cfg pios_ms56xx_cfg = { + .address = 0x77, + .version = MS56XX_VERSION_5611, + .oversampling = MS56XX_OSR_4096 +}; +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms56xx_cfg; +} +#endif /* PIOS_INCLUDE_MS56XX */ + + +/** + * 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_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, +}; + +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/bootloader/inc/common.h b/flight/targets/boards/revolution/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/revolution/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revolution/bootloader/main.c b/flight/targets/boards/revolution/bootloader/main.c index 09a54a0947..c85ef675e6 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(); @@ -81,7 +82,7 @@ int main() // is 2.7 volts check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CableConnected(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); diff --git a/flight/targets/boards/revolution/bootloader/pios_board.c b/flight/targets/boards/revolution/bootloader/pios_board.c index 5352c7f73b..8c0394c04d 100644 --- a/flight/targets/boards/revolution/bootloader/pios_board.c +++ b/flight/targets/boards/revolution/bootloader/pios_board.c @@ -25,6 +25,15 @@ #include #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. @@ -39,6 +48,7 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +78,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/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 616f45953c..5904b23373 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += Actuator MODULES += GPS MODULES += TxPID MODULES += CameraStab +MODULES += CameraControl MODULES += Battery MODULES += FirmwareIAP MODULES += Radio @@ -54,8 +55,10 @@ MODULES += Notify OPTMODULES += AutoTune OPTMODULES += ComUsbBridge +OPTMODULES += UAVOHottBridge OPTMODULES += UAVOMSPBridge OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge SRC += $(FLIGHTLIB)/notification.c @@ -85,11 +88,6 @@ ifndef TESTAPP SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c - #ifeq ($(DEBUG), YES) - SRC += $(OPSYSTEM)/dcc_stdio.c - SRC += $(OPSYSTEM)/cm3_fault_handlers.c - #endif - ## Misc library functions SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 6100c47502..81f4718804 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -77,8 +77,6 @@ UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += oplinkreceiver -UAVOBJSRCFILENAMES += overosyncstats -UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan @@ -112,6 +110,8 @@ UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus @@ -124,6 +124,8 @@ UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += hottbridgesettings +UAVOBJSRCFILENAMES += hottbridgestatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter UAVOBJSRCFILENAMES += systemidentsettings diff --git a/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c b/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c deleted file mode 100644 index e74ffa76ad..0000000000 --- a/flight/targets/boards/revolution/firmware/cm3_fault_handlers.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * cm3_fault_handlers.c - * - * Created on: Apr 24, 2011 - * Author: msmith - */ - -#include -#include "inc/dcc_stdio.h" -#ifdef STM32F4XX -# include -#endif -#ifdef STM32F2XX -# include -#endif - -#define FAULT_TRAMPOLINE(_vec) \ - __attribute__((naked, no_instrument_function)) \ - void \ - _vec##_Handler(void) \ - { \ - __asm(".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ - } \ - struct hack - -struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -FAULT_TRAMPOLINE(HardFault); -FAULT_TRAMPOLINE(BusFault); -FAULT_TRAMPOLINE(UsageFault); - -/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) - -void HardFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void BusFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void UsageFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} diff --git a/flight/targets/boards/revolution/firmware/dcc_stdio.c b/flight/targets/boards/revolution/firmware/dcc_stdio.c deleted file mode 100644 index 1a522e9a0d..0000000000 --- a/flight/targets/boards/revolution/firmware/dcc_stdio.c +++ /dev/null @@ -1,149 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* Copyright (C) 2008 by Frederik Kriewtz * -* frederik@kriewitz.eu * -* * -* 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 2 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 "inc/dcc_stdio.h" - -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 - -/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel - * DCRDR[7:0] is used by target for status - * DCRDR[15:8] is used by target for write buffer - * DCRDR[23:16] is used for by host for status - * DCRDR[31:24] is used for by host for write buffer */ - -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) - -#define BUSY 1 - -void dbg_write(unsigned long dcc_data) -{ - int len = 4; - - while (len--) { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY) { - ; - } - - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } -} - -void dbg_trace_point(unsigned long number) -{ - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); -} - -void dbg_write_u32(const unsigned long *val, long len) -{ - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - - while (len > 0) { - dbg_write(*val); - - val++; - len--; - } -} - -void dbg_write_u16(const unsigned short *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16 : 0x0000); - - dbg_write(dcc_data); - - val += 2; - len -= 2; - } -} - -void dbg_write_u8(const unsigned char *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); - - dbg_write(dcc_data); - - val += 4; - len -= 4; - } -} - -void dbg_write_str(const char *msg) -{ - long len; - unsigned long dcc_data; - - for (len = 0; msg[len] && (len < 65536); len++) { - ; - } - - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); - - msg += 4; - len -= 4; - } -} - -void dbg_write_char(char msg) -{ - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); -} - -void dbg_write_hex32(const unsigned long val) -{ - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; - - for (int shift = 28; shift >= 0; shift -= 4) { - dbg_write_char(hextab[(val >> shift) & 0xf]); - } -} diff --git a/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h b/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h deleted file mode 100644 index 3d73549182..0000000000 --- a/flight/targets/boards/revolution/firmware/inc/dcc_stdio.h +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* * -* 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 2 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 DCC_STDIO_H -#define DCC_STDIO_H - -void dbg_trace_point(unsigned long number); - -void dbg_write_u32(const unsigned long *val, long len); -void dbg_write_u16(const unsigned short *val, long len); -void dbg_write_u8(const unsigned char *val, long len); - -void dbg_write_str(const char *msg); -void dbg_write_char(char msg); -void dbg_write_hex32(const unsigned long val); - -#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h deleted file mode 100644 index 68a32b116c..0000000000 --- a/flight/targets/boards/revolution/firmware/inc/pios_board_sim.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -#define PIOS_LED_ALARM 0 -#define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_aux_id; -extern uint32_t pios_com_spectrum_id; - -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) -#endif - -#define PIOS_GCSRCVR_TIMEOUT_MS 200 -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index efe32a4be4..6694ff33d8 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -86,9 +86,10 @@ #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 +#define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_MS4525DO @@ -119,6 +120,7 @@ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_SERVO +#define PIOS_INCLUDE_HOTT_BRIDGE /* #define PIOS_INCLUDE_I2C_ESC */ /* #define PIOS_INCLUDE_OVERO */ /* #define PIOS_OVERO_SPI */ @@ -136,6 +138,7 @@ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM #define PIOS_INCLUDE_OPENLRS +/* #define OPENLRS_SIMPLE_BEACON_TONE */ /* #define PIOS_INCLUDE_PPM_OUT */ /* #define PIOS_RFM22B_DEBUG_ON_TELEM */ @@ -158,6 +161,7 @@ #define PIOS_INCLUDE_GPS_UBX_PARSER #define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION +#define PIOS_INCLUDE_FRSKY_SENSORHUB /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 7f7ddee074..675d24a0ec 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -30,21 +30,16 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include -#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif +#include +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -55,358 +50,69 @@ */ #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, -}; +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id; -static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { -#ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = NULL, +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; #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, +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, + [HWSETTINGS_RM_RCVRPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + [HWSETTINGS_RM_RCVRPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, }; -#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 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, + [HWSETTINGS_RM_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + [HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, }; -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, +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, + [HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + [HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, }; -#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_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_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; -#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) -{ - 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 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); -} /** * PIOS_Board_Init() * 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; @@ -422,12 +128,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); } @@ -436,7 +142,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); } @@ -476,11 +182,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ + SETTINGS_INITIALISE_ALL; /* Initialize the alarms library */ AlarmsInitialize(); @@ -507,500 +209,57 @@ 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 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); - } - - 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); - } + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); + } - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } +#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); - 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; + 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_EXBUS */ - break; - } /* hwsettings_rm_flexiport */ + PIOS_DELAY_WaitmS(50); + } +#endif - /* 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; +#if defined(PIOS_INCLUDE_USB) + 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; 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); + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); - /* Fetch the OPinkSettings 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 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) - 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)) { - 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; + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - // 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_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); - } - - /* 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; - } - - OPLinkStatusSet(&oplinkStatus); + PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) @@ -1013,12 +272,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_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -1030,8 +293,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMMSP: case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: 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_RCVR(&pios_ppm_cfg); if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { // configure servo outputs and the remaining 5 inputs as outputs @@ -1040,7 +304,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_RCVR(&pios_pwm_ppm_cfg); } break; @@ -1050,60 +314,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; - } - -#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); - 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_GCS_RCVR(); +#endif #ifndef PIOS_ENABLE_DEBUG_PINS // pios_servo_cfg points to the correct configuration based on input port settings @@ -1112,6 +325,18 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #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_id, &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, @@ -1121,67 +346,6 @@ 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 - -#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); - } - } - } #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 a97fdbd670..811b8aad90 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -103,74 +103,61 @@ // 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_MS56XX_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 // // 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 // // 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 */ +#define PIOS_COM_MAX_DEVS 4 + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif // ------------------------- // Packet Handler @@ -236,8 +223,6 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 -#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 -#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 // ------------------------- // Receiver PPM input @@ -318,7 +303,7 @@ extern uint32_t pios_packet_handler; { GPIOA, GPIO_Pin_3, ADC_Channel_3, false }, /* Servo pin 3 */ \ { GPIOA, GPIO_Pin_2, ADC_Channel_2, false }, /* Servo pin 4 */ \ { GPIOA, GPIO_Pin_1, ADC_Channel_1, false }, /* Servo pin 5 */ \ - { GPIOA, GPIO_Pin_0, ADC_Channel_9, false }, /* Servo pin 6 */ \ + { GPIOA, GPIO_Pin_0, ADC_Channel_0, false }, /* Servo pin 6 */ \ { NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \ { NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \ } diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 63f0199138..8aeda6ab83 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_CLOCK_FUNC RCC_AHB1PeriphClockCmd -#define MAIN_USART_INVERTER_CLOCK_PERIPH RCC_AHB1Periph_GPIOC - -#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 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 // 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 +110,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,87 +224,26 @@ 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 */ +#ifdef PIOS_INCLUDE_USART #include -#ifdef PIOS_INCLUDE_COM_TELEM - /* * MAIN USART */ -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 = { +static const struct pios_usart_cfg pios_usart_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 = { + .rx = { .gpio = MAIN_USART_RX_GPIO, .init = { .GPIO_Pin = MAIN_USART_RX_PIN, @@ -319,7 +253,7 @@ static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = MAIN_USART_TX_GPIO, .init = { .GPIO_Pin = MAIN_USART_TX_PIN, @@ -331,175 +265,13 @@ 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 = 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 = { + .rx = { .gpio = FLEXI_USART_RX_GPIO, .init = { .GPIO_Pin = FLEXI_USART_RX_PIN, @@ -509,303 +281,7 @@ static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { .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 = { - .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 = { + .tx = { .gpio = FLEXI_USART_TX_GPIO, .init = { .GPIO_Pin = FLEXI_USART_TX_PIN, @@ -815,7 +291,17 @@ 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, +// }, +// }, }; +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -837,7 +323,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, @@ -890,17 +376,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); } @@ -1347,37 +833,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 +925,148 @@ 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_MS56XX) +#include "pios_ms56xx.h" +static const struct pios_ms56xx_cfg pios_ms56xx_cfg = { + .address = 0x77, + .version = MS56XX_VERSION_5611, + .oversampling = MS56XX_OSR_4096, +}; +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms56xx_cfg; +} +#endif /* PIOS_INCLUDE_MS56XX */ + +/** + * Configuration for the MPU9250 chip + */ +#if defined(PIOS_INCLUDE_MPU9250) +#include "pios_mpu9250.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, +}; + +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/bootloader/inc/common.h b/flight/targets/boards/revonano/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/revonano/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revonano/bootloader/main.c b/flight/targets/boards/revonano/bootloader/main.c index 09a54a0947..c85ef675e6 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(); @@ -81,7 +82,7 @@ int main() // is 2.7 volts check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CableConnected(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); diff --git a/flight/targets/boards/revonano/bootloader/pios_board.c b/flight/targets/boards/revonano/bootloader/pios_board.c index 5352c7f73b..3639353b95 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,7 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +70,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/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 67d01fac7a..68cb18769e 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += Actuator MODULES += GPS MODULES += TxPID MODULES += CameraStab +MODULES += CameraControl MODULES += Battery MODULES += FirmwareIAP MODULES += PathPlanner @@ -55,6 +56,8 @@ OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge +OPTMODULES += UAVOHottBridge SRC += $(FLIGHTLIB)/notification.c @@ -84,11 +87,6 @@ ifndef TESTAPP SRC += $(PIOSCOMMON)/pios_flash_eeprom.c SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c - #ifeq ($(DEBUG), YES) - SRC += $(OPSYSTEM)/dcc_stdio.c - SRC += $(OPSYSTEM)/cm3_fault_handlers.c - #endif - ## Misc library functions SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c diff --git a/flight/targets/boards/revonano/firmware/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index 6100c47502..81f4718804 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -77,8 +77,6 @@ UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += oplinkreceiver -UAVOBJSRCFILENAMES += overosyncstats -UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan @@ -112,6 +110,8 @@ UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus @@ -124,6 +124,8 @@ UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += hottbridgesettings +UAVOBJSRCFILENAMES += hottbridgestatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter UAVOBJSRCFILENAMES += systemidentsettings diff --git a/flight/targets/boards/revonano/firmware/cm3_fault_handlers.c b/flight/targets/boards/revonano/firmware/cm3_fault_handlers.c deleted file mode 100644 index e74ffa76ad..0000000000 --- a/flight/targets/boards/revonano/firmware/cm3_fault_handlers.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * cm3_fault_handlers.c - * - * Created on: Apr 24, 2011 - * Author: msmith - */ - -#include -#include "inc/dcc_stdio.h" -#ifdef STM32F4XX -# include -#endif -#ifdef STM32F2XX -# include -#endif - -#define FAULT_TRAMPOLINE(_vec) \ - __attribute__((naked, no_instrument_function)) \ - void \ - _vec##_Handler(void) \ - { \ - __asm(".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ - } \ - struct hack - -struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -FAULT_TRAMPOLINE(HardFault); -FAULT_TRAMPOLINE(BusFault); -FAULT_TRAMPOLINE(UsageFault); - -/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) - -void HardFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void BusFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void UsageFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} diff --git a/flight/targets/boards/revonano/firmware/dcc_stdio.c b/flight/targets/boards/revonano/firmware/dcc_stdio.c deleted file mode 100644 index 1a522e9a0d..0000000000 --- a/flight/targets/boards/revonano/firmware/dcc_stdio.c +++ /dev/null @@ -1,149 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* Copyright (C) 2008 by Frederik Kriewtz * -* frederik@kriewitz.eu * -* * -* 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 2 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 "inc/dcc_stdio.h" - -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 - -/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel - * DCRDR[7:0] is used by target for status - * DCRDR[15:8] is used by target for write buffer - * DCRDR[23:16] is used for by host for status - * DCRDR[31:24] is used for by host for write buffer */ - -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) - -#define BUSY 1 - -void dbg_write(unsigned long dcc_data) -{ - int len = 4; - - while (len--) { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY) { - ; - } - - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } -} - -void dbg_trace_point(unsigned long number) -{ - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); -} - -void dbg_write_u32(const unsigned long *val, long len) -{ - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - - while (len > 0) { - dbg_write(*val); - - val++; - len--; - } -} - -void dbg_write_u16(const unsigned short *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16 : 0x0000); - - dbg_write(dcc_data); - - val += 2; - len -= 2; - } -} - -void dbg_write_u8(const unsigned char *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); - - dbg_write(dcc_data); - - val += 4; - len -= 4; - } -} - -void dbg_write_str(const char *msg) -{ - long len; - unsigned long dcc_data; - - for (len = 0; msg[len] && (len < 65536); len++) { - ; - } - - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); - - msg += 4; - len -= 4; - } -} - -void dbg_write_char(char msg) -{ - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); -} - -void dbg_write_hex32(const unsigned long val) -{ - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; - - for (int shift = 28; shift >= 0; shift -= 4) { - dbg_write_char(hextab[(val >> shift) & 0xf]); - } -} diff --git a/flight/targets/boards/revonano/firmware/inc/dcc_stdio.h b/flight/targets/boards/revonano/firmware/inc/dcc_stdio.h deleted file mode 100644 index 3d73549182..0000000000 --- a/flight/targets/boards/revonano/firmware/inc/dcc_stdio.h +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* * -* 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 2 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 DCC_STDIO_H -#define DCC_STDIO_H - -void dbg_trace_point(unsigned long number); - -void dbg_write_u32(const unsigned long *val, long len); -void dbg_write_u16(const unsigned short *val, long len); -void dbg_write_u8(const unsigned char *val, long len); - -void dbg_write_str(const char *msg); -void dbg_write_char(char msg); -void dbg_write_hex32(const unsigned long val); - -#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_board_sim.h b/flight/targets/boards/revonano/firmware/inc/pios_board_sim.h deleted file mode 100644 index 68a32b116c..0000000000 --- a/flight/targets/boards/revonano/firmware/inc/pios_board_sim.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -#define PIOS_LED_ALARM 0 -#define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_aux_id; -extern uint32_t pios_com_spectrum_id; - -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) -#endif - -#define PIOS_GCSRCVR_TIMEOUT_MS 200 -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index 49a64f09a9..d784db7833 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -88,7 +88,7 @@ #define PIOS_INCLUDE_HMC5X83 // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ -#define PIOS_INCLUDE_MS5611 +#define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_MS4525DO @@ -113,7 +113,7 @@ #define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_IBUS #define PIOS_INCLUDE_GCSRCVR -// #define PIOS_INCLUDE_OPLINKRCVR +#define PIOS_INCLUDE_OPLINKRCVR /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR @@ -122,6 +122,7 @@ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_SERVO +#define PIOS_INCLUDE_HOTT_BRIDGE /* #define PIOS_INCLUDE_I2C_ESC */ /* #define PIOS_INCLUDE_OVERO */ /* #define PIOS_OVERO_SPI */ @@ -160,6 +161,7 @@ #define PIOS_INCLUDE_GPS_UBX_PARSER #define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION +#define PIOS_INCLUDE_FRSKY_SENSORHUB /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 0de1b0eed1..a6e89c839a 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -30,21 +30,19 @@ #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 +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -55,286 +53,15 @@ */ #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); +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif - 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 +71,39 @@ 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, + [HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + [HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, +}; + +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_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + [HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, +}; + 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) @@ -363,18 +118,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 @@ -384,7 +133,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); } @@ -424,11 +173,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ + SETTINGS_INITIALISE_ALL; /* Initialize the alarms library */ AlarmsInitialize(); @@ -452,365 +197,34 @@ 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; + 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); + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); - 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; + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } - 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); - } +#if defined(PIOS_INCLUDE_I2C) + 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_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); +#endif - /* Configure main USART port */ + /* Configure MainPort */ 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); + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } - /* 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); - } - - 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 */ - - #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) const struct pios_servo_cfg *pios_servo_cfg; @@ -828,7 +242,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_RCVR(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: @@ -840,7 +254,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_RCVR(&pios_ppm_cfg); break; #endif /* PIOS_INCLUDE_PPM */ @@ -851,16 +265,13 @@ 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_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif #ifdef PIOS_INCLUDE_WS2811 #include @@ -870,7 +281,7 @@ void PIOS_Board_Init(void) // No other choices but servo pin 1 on nano if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED) { pios_tim_servoport_all_pins[0] = dummmy_timer; // free timer 1 - PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[0]); + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg, &pios_ws2811_pin_cfg[0]); } #endif // PIOS_INCLUDE_WS2811 @@ -884,36 +295,11 @@ void PIOS_Board_Init(void) PIOS_DELAY_WaitmS(50); -#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); - PIOS_MPU9250_CONFIG_Configure(); - PIOS_MPU9250_MainRegister(); - 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 + PIOS_BOARD_Sensors_Configure(); // Attach the board config check hook SANITYCHECK_AttachHook(&RevoNanoConfigHook); // trigger a config check if actuatorsettings are updated - ActuatorSettingsInitialize(); ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); } diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 43c55959c3..73c3e10bd6 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -103,75 +103,57 @@ // 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_MS56XX_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) + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif // ------------------------- // PIOS_USART // // 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 // // 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 */ - +#define PIOS_COM_MAX_DEVS 4 // ------------------------- // Packet Handler // ------------------------- @@ -236,8 +218,6 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 -#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 -#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 // ------------------------- // Receiver PPM input @@ -262,7 +242,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 // ------------------------- diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c index bda92eb18c..519e9d3474 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,11 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { /* * AUX USART SBUS ( UART/ S Bus label on rev2) */ + 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 +725,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, @@ -814,24 +746,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 +756,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 +770,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 +1625,299 @@ 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; +} +#endif /* PIOS_INCLUDE_USB */ -#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 */ +/** + * Sensor configurations + */ -#if defined(PIOS_INCLUDE_COM_MSG) +#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) */ -#include +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" -#endif /* PIOS_INCLUDE_COM_MSG */ +#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 */ + +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, +}; -#if defined(PIOS_INCLUDE_USB_HID) && !defined(PIOS_INCLUDE_USB_CDC) -#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 = 0, - .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 */ -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_USB_CDC) -#include +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 */ -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS56XX) +#include "pios_ms56xx.h" +static const struct pios_ms56xx_cfg pios_ms56xx_cfg = { + .address = 0x77, + .version = MS56XX_VERSION_5611, + .oversampling = MS56XX_OSR_4096, +}; +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms56xx_cfg; +} +#endif /* PIOS_INCLUDE_MS56XX */ - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, +/** + * 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 */ -#include +/** + * 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, + }, + }, +}; -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_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_USB_HID && PIOS_INCLUDE_USB_CDC */ + +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/inc/common.h b/flight/targets/boards/revoproto/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/revoproto/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/revoproto/bootloader/main.c b/flight/targets/boards/revoproto/bootloader/main.c index 09a54a0947..c85ef675e6 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(); @@ -81,7 +82,7 @@ int main() // is 2.7 volts check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CableConnected(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); diff --git a/flight/targets/boards/revoproto/bootloader/pios_board.c b/flight/targets/boards/revoproto/bootloader/pios_board.c index 7e9858f18a..72fda9e5d7 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/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 809fda8266..16af9aefb2 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += Actuator MODULES += GPS MODULES += TxPID MODULES += CameraStab +MODULES += CameraControl MODULES += Battery MODULES += FirmwareIAP MODULES += PathPlanner @@ -82,11 +83,6 @@ ifndef TESTAPP SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c - #ifeq ($(DEBUG), YES) - SRC += $(OPSYSTEM)/dcc_stdio.c - SRC += $(OPSYSTEM)/cm3_fault_handlers.c - #endif - ## Misc library functions SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index cba723f6b3..5774e7ae43 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -21,9 +21,9 @@ # (all architectures) UAVOBJSRCFILENAMES = UAVOBJSRCFILENAMES += statusgrounddrive -UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += statusvtolautotakeoff UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += vtolselftuningstats UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired @@ -112,6 +112,8 @@ UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus @@ -125,7 +127,7 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation -# UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += perfcounter UAVOBJSRCFILENAMES += systemidentsettings UAVOBJSRCFILENAMES += systemidentstate diff --git a/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c b/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c deleted file mode 100644 index e74ffa76ad..0000000000 --- a/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * cm3_fault_handlers.c - * - * Created on: Apr 24, 2011 - * Author: msmith - */ - -#include -#include "inc/dcc_stdio.h" -#ifdef STM32F4XX -# include -#endif -#ifdef STM32F2XX -# include -#endif - -#define FAULT_TRAMPOLINE(_vec) \ - __attribute__((naked, no_instrument_function)) \ - void \ - _vec##_Handler(void) \ - { \ - __asm(".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ - } \ - struct hack - -struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -FAULT_TRAMPOLINE(HardFault); -FAULT_TRAMPOLINE(BusFault); -FAULT_TRAMPOLINE(UsageFault); - -/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) - -void HardFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void BusFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void UsageFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} diff --git a/flight/targets/boards/revoproto/firmware/dcc_stdio.c b/flight/targets/boards/revoproto/firmware/dcc_stdio.c deleted file mode 100644 index 1a522e9a0d..0000000000 --- a/flight/targets/boards/revoproto/firmware/dcc_stdio.c +++ /dev/null @@ -1,149 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* Copyright (C) 2008 by Frederik Kriewtz * -* frederik@kriewitz.eu * -* * -* 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 2 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 "inc/dcc_stdio.h" - -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 - -/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel - * DCRDR[7:0] is used by target for status - * DCRDR[15:8] is used by target for write buffer - * DCRDR[23:16] is used for by host for status - * DCRDR[31:24] is used for by host for write buffer */ - -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) - -#define BUSY 1 - -void dbg_write(unsigned long dcc_data) -{ - int len = 4; - - while (len--) { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY) { - ; - } - - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } -} - -void dbg_trace_point(unsigned long number) -{ - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); -} - -void dbg_write_u32(const unsigned long *val, long len) -{ - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - - while (len > 0) { - dbg_write(*val); - - val++; - len--; - } -} - -void dbg_write_u16(const unsigned short *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16 : 0x0000); - - dbg_write(dcc_data); - - val += 2; - len -= 2; - } -} - -void dbg_write_u8(const unsigned char *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); - - dbg_write(dcc_data); - - val += 4; - len -= 4; - } -} - -void dbg_write_str(const char *msg) -{ - long len; - unsigned long dcc_data; - - for (len = 0; msg[len] && (len < 65536); len++) { - ; - } - - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); - - msg += 4; - len -= 4; - } -} - -void dbg_write_char(char msg) -{ - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); -} - -void dbg_write_hex32(const unsigned long val) -{ - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; - - for (int shift = 28; shift >= 0; shift -= 4) { - dbg_write_char(hextab[(val >> shift) & 0xf]); - } -} diff --git a/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h b/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h deleted file mode 100644 index 3d73549182..0000000000 --- a/flight/targets/boards/revoproto/firmware/inc/dcc_stdio.h +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* * -* 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 2 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 DCC_STDIO_H -#define DCC_STDIO_H - -void dbg_trace_point(unsigned long number); - -void dbg_write_u32(const unsigned long *val, long len); -void dbg_write_u16(const unsigned short *val, long len); -void dbg_write_u8(const unsigned char *val, long len); - -void dbg_write_str(const char *msg); -void dbg_write_char(char msg); -void dbg_write_hex32(const unsigned long val); - -#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h deleted file mode 100644 index d1bcd16bc4..0000000000 --- a/flight/targets/boards/revoproto/firmware/inc/pios_board_sim.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -#define PIOS_LED_ALARM 0 -#define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_aux_id; -extern uint32_t pios_com_spectrum_id; - -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX -#endif - -#define PIOS_GCSRCVR_TIMEOUT_MS 200 -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 4682648554..7238d58851 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -83,9 +83,10 @@ #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 +#define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 /* #define PIOS_INCLUDE_HCSR04 */ @@ -99,7 +100,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_GCSRCVR -/* #define PIOS_INCLUDE_OPLINKRCVR */ +#define PIOS_INCLUDE_OPLINKRCVR /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 025acce6eb..9f8fb1f58b 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 @@ -412,8 +58,6 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm void PIOS_Board_Init(void) { - const struct pios_board_info *bdinfo = &pios_board_info_blob; - /* Delay system */ PIOS_DELAY_Init(); @@ -487,8 +131,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - - HwSettingsInitialize(); + SETTINGS_INITIALISE_ALL; /* Initialize the alarms library */ AlarmsInitialize(); @@ -516,293 +159,100 @@ 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); - -#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 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 +264,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 +276,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_RCVR(&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_RCVR(&pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ case HWSETTINGS_RV_RCVRPORT_OUTPUTS: @@ -929,17 +321,13 @@ void PIOS_Board_Init(void) } #endif +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#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_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif #ifndef PIOS_ENABLE_DEBUG_PINS switch (hwsettings_rcvrport) { @@ -972,69 +360,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 60572cdc73..960334acf9 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -81,64 +81,66 @@ // 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_MS56XX_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 // // 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 // // 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) +#define PIOS_COM_MAX_DEVS 4 + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif // ------------------------ // TELEMETRY diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 52093b2e9b..31027d2c09 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -79,7 +79,6 @@ UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += objectpersistence -UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan diff --git a/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h b/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h deleted file mode 100644 index 3d73549182..0000000000 --- a/flight/targets/boards/simposix/firmware/inc/dcc_stdio.h +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* * -* 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 2 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 DCC_STDIO_H -#define DCC_STDIO_H - -void dbg_trace_point(unsigned long number); - -void dbg_write_u32(const unsigned long *val, long len); -void dbg_write_u16(const unsigned short *val, long len); -void dbg_write_u8(const unsigned char *val, long len); - -void dbg_write_str(const char *msg); -void dbg_write_char(char msg); -void dbg_write_hex32(const unsigned long val); - -#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index d422ddeff5..401f089471 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 @@ -67,7 +67,7 @@ // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL // #define PIOS_INCLUDE_L3GD20 -// #define PIOS_INCLUDE_MS5611 +// #define PIOS_INCLUDE_MS56XX // #define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ @@ -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 diff --git a/flight/targets/boards/simposix/firmware/pios_board.c b/flight/targets/boards/simposix/firmware/pios_board.c index 68ba9491ac..cfbfce1c37 100644 --- a/flight/targets/boards/simposix/firmware/pios_board.c +++ b/flight/targets/boards/simposix/firmware/pios_board.c @@ -142,8 +142,6 @@ void PIOS_Board_Init(void) EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); - UAVObjectsInitializeAll(); /* Initialize the alarms library */ diff --git a/flight/targets/boards/sparky2/board_hw_defs.c b/flight/targets/boards/sparky2/board_hw_defs.c index 25afb14e23..2344d5f8e6 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); } @@ -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,569 +540,17 @@ static const struct flashfs_logfs_cfg flashfs_internal_cfg = { #endif /* PIOS_INCLUDE_FLASH */ +#ifdef PIOS_INCLUDE_USART #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 = { - .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 - }, - }, -}; - -// 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 + * MAIN PORT */ -#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 = { +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, @@ -1112,7 +560,7 @@ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOA, .init = { .GPIO_Pin = GPIO_Pin_9, @@ -1124,26 +572,13 @@ static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { +/* + * 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, @@ -1153,7 +588,7 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .tx = { + .tx = { .gpio = GPIOB, .init = { .GPIO_Pin = GPIO_Pin_10, @@ -1170,104 +605,10 @@ static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { * RCVR PORT */ -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_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 = { - .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 - }, - }, -}; - - -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 = { +static const struct pios_usart_cfg pios_usart_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 = { + .rx = { .gpio = GPIOC, .init = { .GPIO_Pin = GPIO_Pin_7, @@ -1278,9 +619,7 @@ static const struct pios_usart_cfg pios_usart_dsm_rcvr_cfg = { }, }, }; - -#endif /* PIOS_INCLUDE_DSM */ - +#endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -1295,14 +634,14 @@ static const struct pios_usart_cfg pios_usart_dsm_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, @@ -1355,17 +694,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); } @@ -1441,8 +780,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 */ @@ -1720,7 +1059,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 +1082,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 +1098,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 +1214,146 @@ 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_MS56XX) +#include "pios_ms56xx.h" +static const struct pios_ms56xx_cfg pios_ms56xx_cfg = { + .address = 0x77, + .version = MS56XX_VERSION_5611, + .oversampling = MS56XX_OSR_4096, +}; +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms56xx_cfg; +} +#endif /* PIOS_INCLUDE_MS56xx */ + +/** + * Configuration for the MPU9250 chip + */ +#if defined(PIOS_INCLUDE_MPU9250) +#include "pios_mpu9250.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, +}; + +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/bootloader/inc/common.h b/flight/targets/boards/sparky2/bootloader/inc/common.h deleted file mode 100644 index e64b89014c..0000000000 --- a/flight/targets/boards/sparky2/bootloader/inc/common.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup CopterControlBL CopterControl BootLoader - * @brief These files contain the code to the CopterControl Bootloader. - * - * @{ - * @file common.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This file contains various common defines for the BootLoader - * @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 COMMON_H_ -#define COMMON_H_ - -// #include "board.h" - -typedef enum { - start, keepgoing, -} DownloadAction; - -/**************************************************/ -/* OP_DFU states */ -/**************************************************/ - -typedef enum { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - BLidle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, -// 12 -} DFUStates; -/**************************************************/ -/* OP_DFU commands */ -/**************************************************/ -typedef enum { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep -// 12 -} DFUCommands; - -typedef enum { - High_Density, Medium_Density -} DeviceType; -/**************************************************/ -/* OP_DFU transfer types */ -/**************************************************/ -typedef enum { - FW, // 0 - Descript -// 2 -} DFUTransfer; -/**************************************************/ -/* OP_DFU transfer port */ -/**************************************************/ -typedef enum { - Usb, // 0 - Serial -// 2 -} DFUPort; -/**************************************************/ -/* OP_DFU programable programable HW types */ -/**************************************************/ -typedef enum { - Self_flash, // 0 - Remote_flash_via_spi -// 1 -} DFUProgType; -/**************************************************/ -/* OP_DFU programable sources */ -/**************************************************/ -#define USB 0 -#define SPI 1 - -#define DownloadDelay 100000 - -#define MAX_DEL_RETRYS 3 -#define MAX_WRI_RETRYS 3 - -#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/sparky2/bootloader/main.c b/flight/targets/boards/sparky2/bootloader/main.c index 70b18409e4..6ed56133ff 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(); @@ -82,7 +83,7 @@ int main() // is 2.7 volts check_bor(); - USB_connected = PIOS_USB_CheckAvailable(0); + USB_connected = PIOS_USB_CableConnected(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); diff --git a/flight/targets/boards/sparky2/bootloader/pios_board.c b/flight/targets/boards/sparky2/bootloader/pios_board.c index 5352c7f73b..dfdbcbfcee 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 @@ -39,6 +38,7 @@ uint32_t pios_com_telem_usb_id; static bool board_init_complete = false; + void PIOS_Board_Init() { if (board_init_complete) { @@ -68,7 +68,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/Makefile b/flight/targets/boards/sparky2/firmware/Makefile index 7ee3837a40..c387dba520 100644 --- a/flight/targets/boards/sparky2/firmware/Makefile +++ b/flight/targets/boards/sparky2/firmware/Makefile @@ -42,6 +42,7 @@ MODULES += Actuator MODULES += GPS MODULES += TxPID MODULES += CameraStab +MODULES += CameraControl MODULES += Battery MODULES += FirmwareIAP MODULES += Radio @@ -56,6 +57,7 @@ OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge SRC += $(FLIGHTLIB)/notification.c @@ -85,11 +87,6 @@ ifndef TESTAPP SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c - #ifeq ($(DEBUG), YES) - SRC += $(OPSYSTEM)/dcc_stdio.c - SRC += $(OPSYSTEM)/cm3_fault_handlers.c - #endif - ## Misc library functions SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c diff --git a/flight/targets/boards/sparky2/firmware/UAVObjects.inc b/flight/targets/boards/sparky2/firmware/UAVObjects.inc index 4835fa6490..24a083fc2a 100644 --- a/flight/targets/boards/sparky2/firmware/UAVObjects.inc +++ b/flight/targets/boards/sparky2/firmware/UAVObjects.inc @@ -77,8 +77,6 @@ UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += oplinkreceiver -UAVOBJSRCFILENAMES += overosyncstats -UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan @@ -112,6 +110,8 @@ UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += receiverstatus UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus diff --git a/flight/targets/boards/sparky2/firmware/cm3_fault_handlers.c b/flight/targets/boards/sparky2/firmware/cm3_fault_handlers.c deleted file mode 100644 index e74ffa76ad..0000000000 --- a/flight/targets/boards/sparky2/firmware/cm3_fault_handlers.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * cm3_fault_handlers.c - * - * Created on: Apr 24, 2011 - * Author: msmith - */ - -#include -#include "inc/dcc_stdio.h" -#ifdef STM32F4XX -# include -#endif -#ifdef STM32F2XX -# include -#endif - -#define FAULT_TRAMPOLINE(_vec) \ - __attribute__((naked, no_instrument_function)) \ - void \ - _vec##_Handler(void) \ - { \ - __asm(".syntax unified\n" \ - "MOVS R0, #4 \n" \ - "MOV R1, LR \n" \ - "TST R0, R1 \n" \ - "BEQ 1f \n" \ - "MRS R0, PSP \n" \ - "B " #_vec "_Handler2 \n" \ - "1: \n" \ - "MRS R0, MSP \n" \ - "B " #_vec "_Handler2 \n" \ - ".syntax divided\n"); \ - } \ - struct hack - -struct cm3_frame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -FAULT_TRAMPOLINE(HardFault); -FAULT_TRAMPOLINE(BusFault); -FAULT_TRAMPOLINE(UsageFault); - -/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ -#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) - -void HardFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nHARD FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(HFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void BusFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nBUS FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(BFAR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} - -void UsageFault_Handler2(struct cm3_frame *frame) -{ - dbg_write_str("\nUSAGE FAULT"); - dbg_write_hex32(frame->pc); - dbg_write_char('\n'); - dbg_write_hex32(SCB_REG(CFSR)); - dbg_write_char('\n'); - for (;;) { - ; - } -} diff --git a/flight/targets/boards/sparky2/firmware/dcc_stdio.c b/flight/targets/boards/sparky2/firmware/dcc_stdio.c deleted file mode 100644 index 1a522e9a0d..0000000000 --- a/flight/targets/boards/sparky2/firmware/dcc_stdio.c +++ /dev/null @@ -1,149 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* Copyright (C) 2008 by Frederik Kriewtz * -* frederik@kriewitz.eu * -* * -* 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 2 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 "inc/dcc_stdio.h" - -#define TARGET_REQ_TRACEMSG 0x00 -#define TARGET_REQ_DEBUGMSG_ASCII 0x01 -#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) -#define TARGET_REQ_DEBUGCHAR 0x02 - -/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel - * DCRDR[7:0] is used by target for status - * DCRDR[15:8] is used by target for write buffer - * DCRDR[23:16] is used for by host for status - * DCRDR[31:24] is used for by host for write buffer */ - -#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) - -#define BUSY 1 - -void dbg_write(unsigned long dcc_data) -{ - int len = 4; - - while (len--) { - /* wait for data ready */ - while (NVIC_DBG_DATA_R & BUSY) { - ; - } - - /* write our data and set write flag - tell host there is data*/ - NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); - dcc_data >>= 8; - } -} - -void dbg_trace_point(unsigned long number) -{ - dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); -} - -void dbg_write_u32(const unsigned long *val, long len) -{ - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); - - while (len > 0) { - dbg_write(*val); - - val++; - len--; - } -} - -void dbg_write_u16(const unsigned short *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 16 : 0x0000); - - dbg_write(dcc_data); - - val += 2; - len -= 2; - } -} - -void dbg_write_u8(const unsigned char *val, long len) -{ - unsigned long dcc_data; - - dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = val[0] - | ((len > 1) ? val[1] << 8 : 0x00) - | ((len > 2) ? val[2] << 16 : 0x00) - | ((len > 3) ? val[3] << 24 : 0x00); - - dbg_write(dcc_data); - - val += 4; - len -= 4; - } -} - -void dbg_write_str(const char *msg) -{ - long len; - unsigned long dcc_data; - - for (len = 0; msg[len] && (len < 65536); len++) { - ; - } - - dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); - - while (len > 0) { - dcc_data = msg[0] - | ((len > 1) ? msg[1] << 8 : 0x00) - | ((len > 2) ? msg[2] << 16 : 0x00) - | ((len > 3) ? msg[3] << 24 : 0x00); - dbg_write(dcc_data); - - msg += 4; - len -= 4; - } -} - -void dbg_write_char(char msg) -{ - dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); -} - -void dbg_write_hex32(const unsigned long val) -{ - static const char hextab[] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' - }; - - for (int shift = 28; shift >= 0; shift -= 4) { - dbg_write_char(hextab[(val >> shift) & 0xf]); - } -} diff --git a/flight/targets/boards/sparky2/firmware/inc/dcc_stdio.h b/flight/targets/boards/sparky2/firmware/inc/dcc_stdio.h deleted file mode 100644 index 3d73549182..0000000000 --- a/flight/targets/boards/sparky2/firmware/inc/dcc_stdio.h +++ /dev/null @@ -1,36 +0,0 @@ -/*************************************************************************** -* Copyright (C) 2008 by Dominic Rath * -* Dominic.Rath@gmx.de * -* Copyright (C) 2008 by Spencer Oliver * -* spen@spen-soft.co.uk * -* * -* 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 2 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 DCC_STDIO_H -#define DCC_STDIO_H - -void dbg_trace_point(unsigned long number); - -void dbg_write_u32(const unsigned long *val, long len); -void dbg_write_u16(const unsigned short *val, long len); -void dbg_write_u8(const unsigned char *val, long len); - -void dbg_write_str(const char *msg); -void dbg_write_char(char msg); -void dbg_write_hex32(const unsigned long val); - -#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/sparky2/firmware/inc/pios_board_sim.h b/flight/targets/boards/sparky2/firmware/inc/pios_board_sim.h deleted file mode 100644 index 68a32b116c..0000000000 --- a/flight/targets/boards/sparky2/firmware/inc/pios_board_sim.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - ****************************************************************************** - * - * @file pios_board.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. - * @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_H -#define PIOS_BOARD_H - - -// ------------------------ -// PIOS_LED -// ------------------------ -#define PIOS_LED_ALARM 0 -#define PIOS_LED_HEARTBEAT 1 -#define PIOS_LED_NUM 2 - -// ------------------------- -// COM -// -// See also pios_board_posix.c -// ------------------------- -// #define PIOS_USART_TX_BUFFER_SIZE 256 -#define PIOS_COM_BUFFER_SIZE 1024 -#define PIOS_COM_MAX_DEVS 255 -#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_TCP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE - -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_aux_id; -extern uint32_t pios_com_spectrum_id; - -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_GPS (pios_com_gps_id) - -#ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG (PIOS_COM_AUX) -#endif - -#define PIOS_GCSRCVR_TIMEOUT_MS 200 -/** - * glue macros for file IO - * STM32 uses DOSFS for file IO - */ -#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL - -#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL - -#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length - -#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) - - -#define PIOS_FCLOSE(file) fclose(file) - -#define PIOS_FUNLINK(file) unlink((char *)filename) - -#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/sparky2/firmware/inc/pios_config.h b/flight/targets/boards/sparky2/firmware/inc/pios_config.h index e823a82a12..53be1a0ea9 100644 --- a/flight/targets/boards/sparky2/firmware/inc/pios_config.h +++ b/flight/targets/boards/sparky2/firmware/inc/pios_config.h @@ -88,7 +88,7 @@ #define PIOS_INCLUDE_HMC5X83 /* Sparky2 5X83s are all external and thus don't have GPIOs #define PIOS_HMC5X83_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ -#define PIOS_INCLUDE_MS5611 +#define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_MS4525DO @@ -141,6 +141,7 @@ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM #define PIOS_INCLUDE_OPENLRS +/* #define OPENLRS_SIMPLE_BEACON_TONE */ /* #define PIOS_INCLUDE_PPM_OUT */ /* #define PIOS_RFM22B_DEBUG_ON_TELEM */ @@ -163,6 +164,7 @@ #define PIOS_INCLUDE_GPS_UBX_PARSER #define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION +#define PIOS_INCLUDE_FRSKY_SENSORHUB /* Stabilization options */ #define PIOS_QUATERNION_STABILIZATION diff --git a/flight/targets/boards/sparky2/firmware/pios_board.c b/flight/targets/boards/sparky2/firmware/pios_board.c index e8a82a2f4d..8f39399a49 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 +#include /* * Pull in the board-specific static HW definitions. @@ -57,347 +53,51 @@ */ #include "../board_hw_defs.c" -/** - * Sensor configurations - */ +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id; -#if defined(PIOS_INCLUDE_ADC) +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif -#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, +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, }; -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, +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, + [HWSETTINGS_SPK2_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, }; -#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, +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, + [HWSETTINGS_SPK2_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, }; -#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 - -#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; -#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) -{ - 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_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; -} - -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); -} /** * PIOS_Board_Init() @@ -405,13 +105,8 @@ static void PIOS_Board_PPM_callback(const int16_t *channels) * 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) @@ -426,27 +121,21 @@ 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); } -#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; // 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); } @@ -484,14 +173,7 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); -#if defined(PIOS_INCLUDE_RFM22B) - OPLinkSettingsInitialize(); - OPLinkStatusInitialize(); -#endif /* PIOS_INCLUDE_RFM22B */ -#if defined(PIOS_INCLUDE_HMC5X83) - AuxMagSettingsInitialize(); -#endif /* PIOS_INCLUDE_HMC5X83 */ + SETTINGS_INITIALISE_ALL; /* Initialize the alarms library */ AlarmsInitialize(); @@ -519,110 +201,29 @@ 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 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); - } - } -#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; + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); + } - 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; +#if defined(PIOS_INCLUDE_I2C) + /* Set up internal I2C bus */ + if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { + PIOS_DEBUG_Assert(0); + } + PIOS_DELAY_WaitmS(50); - 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 */ + 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); + } +#endif /* Moved this here to allow binding on flexiport */ #if defined(PIOS_INCLUDE_FLASH) @@ -632,428 +233,65 @@ 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 */ + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); + } - /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + PIOS_BOARD_IO_Configure_RFM22B(); - /* Fetch the OPinkSettings 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 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) - 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)) { - 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; + uint8_t hwsettings_radioaux; + HwSettingsRadioAuxStreamGet(&hwsettings_radioaux); - // 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_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); - } - - /* 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); - } - } else { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; - } - - OPLinkStatusSet(&oplinkStatus); + PIOS_BOARD_IO_Configure_RadioAuxStream(hwsettings_radioaux); #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 - // 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); - 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; - } -#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; - } - - 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 (hwsettings_rcvrport < NELEMENTS(rcvr_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_rcvr_cfg, rcvr_function_map[hwsettings_rcvrport]); } -#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); +#if defined(PIOS_INCLUDE_PPM) + if (hwsettings_rcvrport == HWSETTINGS_SPK2_RCVRPORT_PPM) { + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ +#endif -#if defined(PIOS_INCLUDE_OPLINKRCVR) - { - OPLinkReceiverInitialize(); - 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 */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif -#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 + 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_id, &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, @@ -1063,101 +301,7 @@ void PIOS_Board_Init(void) .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); - PIOS_MPU9250_CONFIG_Configure(); - PIOS_MPU9250_MainRegister(); - 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 - 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); - } - } - } -#endif // PIOS_INCLUDE_ADC +#endif /* PIOS_INCLUDE_ADC */ } /** diff --git a/flight/targets/boards/sparky2/pios_board.h b/flight/targets/boards/sparky2/pios_board.h index 63986b0fb8..825cf18d72 100644 --- a/flight/targets/boards/sparky2/pios_board.h +++ b/flight/targets/boards/sparky2/pios_board.h @@ -105,74 +105,61 @@ // 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_MS56XX_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 // // 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_6 +#define PIOS_USART_INVERTER_ENABLE Bit_SET +#define PIOS_USART_INVERTER_DISABLE Bit_RESET // ------------------------- // PIOS_COM // // 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 */ +#define PIOS_COM_MAX_DEVS 4 + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif // ------------------------- // Packet Handler @@ -238,8 +225,6 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 -#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 -#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 // ------------------------- // Receiver PPM input @@ -321,7 +306,7 @@ extern uint32_t pios_packet_handler; { GPIOA, GPIO_Pin_3, ADC_Channel_3, false }, /* Servo pin 3 */ \ { GPIOA, GPIO_Pin_2, ADC_Channel_2, false }, /* Servo pin 4 */ \ { GPIOA, GPIO_Pin_1, ADC_Channel_1, false }, /* Servo pin 5 */ \ - { GPIOA, GPIO_Pin_0, ADC_Channel_9, false }, /* Servo pin 6 */ \ + { GPIOA, GPIO_Pin_0, ADC_Channel_0, false }, /* Servo pin 6 */ \ { NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \ { NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \ } diff --git a/flight/targets/boards/spracingf3/board-info.mk b/flight/targets/boards/spracingf3/board-info.mk new file mode 100644 index 0000000000..b64fd5e527 --- /dev/null +++ b/flight/targets/boards/spracingf3/board-info.mk @@ -0,0 +1,24 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x01 +BOOTLOADER_VERSION := 0x00 +HW_TYPE := 0x01 + +MCU := cortex-m4 +PIOS_DEVLIB := $(PIOS)/stm32f30x +CHIP := STM32F303xC +BOARD := STM32F303CCT_SRF3_Rev1 +MODEL := HD + +OPENOCD_JTAG_CONFIG := jlink.cfg +OPENOCD_CONFIG := stm32f3x.cfg + +# Note: These must match the values in link_$(BOARD)_memory.ld +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region + +FW_BANK_BASE := 0x08004000 # Start of firmware flash +FW_BANK_SIZE := 0x0003C000 # Should include FW_DESC_SIZE + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/spracingf3/board_hw_defs.c b/flight/targets/boards/spracingf3/board_hw_defs.c new file mode 100644 index 0000000000..3c926587f1 --- /dev/null +++ b/flight/targets/boards/spracingf3/board_hw_defs.c @@ -0,0 +1,859 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the SPRacing F3 board. + *****************************************************************************/ +/* + * 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 + */ + +#if defined(PIOS_INCLUDE_LED) + +#include + + +static const struct pios_gpio pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, + [PIOS_BUZZER_ALARM] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg = { + .gpios = pios_leds, + .num_gpios = NELEMENTS(pios_leds), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" + +#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 = { + .vector = pios_board_internal_mag_handler, + .line = EXTI_Line14, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .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_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line14, // 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_GetExternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return 0; // external HMC5883 will conflict with internal one. +} +const struct pios_hmc5x83_cfg *PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_hmc5x83_internal_cfg; +} +#endif /* PIOS_INCLUDE_HMC5X83 */ + +/** + * Configuration for the MS5611 chip + */ +#if defined(PIOS_INCLUDE_MS56XX) +#include "pios_ms56xx.h" +static const struct pios_ms56xx_cfg pios_ms56xx_cfg = { + .address = 0x77, + .version = MS56XX_VERSION_5611, + .oversampling = MS56XX_OSR_4096, +}; +const struct pios_ms56xx_cfg *PIOS_BOARD_HW_DEFS_GetMS56xxCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_ms56xx_cfg; +} +#endif /* PIOS_INCLUDE_MS56XX */ + + +/** + * Configuration for 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_Line13, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .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_Line13, // 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, +#ifdef PIOS_MPU6000_I2C_ADDR + .i2c_addr = PIOS_MPU6000_I2C_ADDR, +#endif + .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, + // 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_FIFO_EN, + .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, +}; + +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu6000_cfg; +} +#endif /* PIOS_INCLUDE_MPU6000 */ + + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Flash/Accel Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_flash_accel_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +static const struct pios_spi_cfg pios_spi_flash_accel_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, /* NOTE: was GPIO_Mode_IN_FLOATING */ + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + } + }, /*{ + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + }*/ + }, +}; + +static uint32_t pios_spi_flash_accel_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); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" + +static const struct flashfs_logfs_cfg flashfs_w25x_cfg = { + .fs_magic = 0x99abcdef, + .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00001000, /* 4K bytes */ + .page_size = 0x00000100, /* 256 bytes */ +}; + + +static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ +}; + +#include "pios_flash.h" + +#endif /* PIOS_INCLUDE_FLASH */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_15_16_17_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_15_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM15_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_16_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM16_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_17_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_15_16_17_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 +#define GPIO_AF_PB10_TIM2 GPIO_AF_1 +#define GPIO_AF_PA6_TIM16 GPIO_AF_1 +#define GPIO_AF_PA7_TIM17 GPIO_AF_1 +#define GPIO_AF_PA11_TIM4 GPIO_AF_10 +#define GPIO_AF_PA12_TIM4 GPIO_AF_10 +#define GPIO_AF_PA2_TIM15 GPIO_AF_9 +#define GPIO_AF_PA3_TIM15 GPIO_AF_9 + +#define TIM1_CH1_DMA_INSTANCE 1 +#define TIM1_CH1_DMA_CHANNEL 2 + +static const struct pios_tim_channel pios_tim_servoport_io1_io2_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM16, 1, A, 6), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM17, 1, A, 7), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM4, 1, A, 11), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM4, 2, A, 12), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM4, 4, B, 9), // bank 4 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 1, A, 2), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 2, A, 3), // bank 2 + + // Receiver port pins + // S3-S6 inputs are used as outputs in this case + + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, B, 10), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, B, 4), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, B, 5), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), +}; + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART2 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_cfg[] = { + USART_CONFIG(USART1, A, 10, A, 9), + USART_CONFIG(USART2, A, 15, A, 14), + USART_CONFIG(USART3, B, 11, B, 10), +}; + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_io1_io2_pins, + .num_channels = 8, +}; + +// const struct pios_servo_cfg pios_servo_rcvr_cfg = { +// .tim_oc_init = { +// .TIM_OCMode = TIM_OCMode_PWM1, +// .TIM_OutputState = TIM_OutputState_Enable, +// .TIM_OutputNState = TIM_OutputNState_Disable, +// .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, +// .TIM_OCPolarity = TIM_OCPolarity_High, +// .TIM_OCNPolarity = TIM_OCPolarity_High, +// .TIM_OCIdleState = TIM_OCIdleState_Reset, +// .TIM_OCNIdleState = TIM_OCNIdleState_Reset, +// }, +// .channels = pios_tim_servoport_io1_io2_pins, +// .num_channels = NELEMENTS(pios_tim_servoport_io1_io2_pins), +// }; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_servoport_io1_io2_pins[8], + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM */ + +/* + * PWM Inputs + */ +#if defined(PIOS_INCLUDE_PWM) +#include + +const struct pios_pwm_cfg pios_pwm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_servoport_io1_io2_pins[8], + .num_channels = 8, +}; + +#endif /* if defined(PIOS_INCLUDE_PWM) */ + + +/* + * SONAR Inputs + */ +#if defined(PIOS_INCLUDE_HCSR04) +#include + +static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { + { + .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, + }, +}; + +const struct pios_hcsr04_cfg pios_hcsr04_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_ev_irq_handler(void); +void PIOS_I2C_er_irq_handler(void); +void I2C1_EV_EXTI23_IRQHandler() __attribute__((alias("PIOS_I2C_ev_irq_handler"))); +void I2C1_ER_IRQHandler() __attribute__((alias("PIOS_I2C_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_cfg = { + .regs = I2C1, + .remapSCL = GPIO_AF_4, + .remapSDA = GPIO_AF_4, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DigitalFilter = 0x00, + .I2C_AnalogFilter = I2C_AnalogFilter_Enable, +// .I2C_Timing = 0x70310309, // 50kHz I2C @ 8MHz input -> PRESC=0x7, SCLDEL=0x3, SDADEL=0x1, SCLH=0x03, SCLL=0x09 + .I2C_Timing = 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + .pin_source = GPIO_PinSource6, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + .pin_source = GPIO_PinSource7, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_id; +void PIOS_I2C_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_id); +} + +void PIOS_I2C_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_id); +} + + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_INCLUDE_GCSRCVR) +#include "pios_gcsrcvr_priv.h" +#endif /* PIOS_INCLUDE_GCSRCVR */ + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_WS2811) +#include "pios_ws2811_cfg.h" + +static const struct pios_ws2811_cfg pios_ws2811_cfg = PIOS_WS2811_CONFIG(TIM1, 1, A, 8); + +void DMA1_Channel2_IRQHandler() +{ + PIOS_WS2811_DMA_irq_handler(); +} + +#endif /* PIOS_INCLUDE_WS2811 */ + +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Channel1_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC2, + .dma = { + .irq = { + .flags = (DMA2_FLAG_TC1 | DMA2_FLAG_TE1 | DMA2_FLAG_HT1), + .init = { + .NVIC_IRQChannel = DMA2_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Channel1, + } + }, + .half_flag = DMA2_FLAG_HT1, + .full_flag = DMA2_FLAG_TC1, +}; + +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) */ diff --git a/flight/targets/boards/spracingf3/bootloader/Makefile b/flight/targets/boards/spracingf3/bootloader/Makefile new file mode 100644 index 0000000000..3cd53ef127 --- /dev/null +++ b/flight/targets/boards/spracingf3/bootloader/Makefile @@ -0,0 +1,39 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +PIOS_OMITS_USB = YES +PIOS_APPS_MINIMAL = YES + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +## The standard CMSIS startup +SRC += $(FLIGHTLIB)/ssp.c +SRC += $(PIOSCOMMON)/pios_com.c +SRC += $(FLIGHTLIB)/fifo_buffer.c + + + +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + diff --git a/flight/targets/boards/spracingf3/bootloader/inc/pios_config.h b/flight/targets/boards/spracingf3/bootloader/inc/pios_config.h new file mode 100644 index 0000000000..40cb146665 --- /dev/null +++ b/flight/targets/boards/spracingf3/bootloader/inc/pios_config.h @@ -0,0 +1,51 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3/bootloader/main.c b/flight/targets/boards/spracingf3/bootloader/main.c new file mode 100644 index 0000000000..6b378ee8e5 --- /dev/null +++ b/flight/targets/boards/spracingf3/bootloader/main.c @@ -0,0 +1,276 @@ +/** + ****************************************************************************** + * @addtogroup SPRacingF3 serial BootLoader + * @brief These files contain the code to the SPRacingF3 serial Bootloader. + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @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 +#include +#include +#include +#include +#include +#include +#include +#include + +extern void FLASH_Download(); +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len); +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) +#define UART_BUFFER_SIZE 256 +#define BL_WAIT_TIME 6 * 1000 * 1000 +#define DFU_BUFFER_SIZE 63 + +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +static uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + +static uint8_t process_buffer[DFU_BUFFER_SIZE]; +static uint8_t rx_buffer[UART_BUFFER_SIZE]; +static uint8_t txBuf[MAX_PACKET_BUF_SIZE]; +static uint8_t rxBuf[MAX_PACKET_BUF_SIZE]; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState = DFUidle; +int16_t status = 0; +bool JumpToApp = false; +bool ssp_dfu = false; // signal that ssp data has been received +bool User_DFU_request = true; + + +/* Private function prototypes -----------------------------------------------*/ +static void led_pwm_step(uint16_t pwm_period, uint16_t pwm_sweep_steps, uint32_t stopwatch, bool default_state); +static uint32_t LedPWM(uint16_t pwm_period, uint16_t pwm_sweep_steps, uint32_t count); +static void processRX(); +static void jump_to_app(); + + +static void SSP_CallBack(uint8_t *buf, uint16_t len); +static int16_t SSP_SerialRead(void); +static void SSP_SerialWrite(uint8_t); + + +static const PortConfig_t ssp_portConfig = { + .rxBuf = rxBuf, + .rxBufSize = MAX_PACKET_DATA_LEN, + .txBuf = txBuf, + .txBufSize = MAX_PACKET_DATA_LEN, + .max_retry = 1, + .timeoutLen = 5000, + .pfCallBack = SSP_CallBack, + .pfSerialRead = SSP_SerialRead, + .pfSerialWrite = SSP_SerialWrite, + .pfGetTime = PIOS_DELAY_GetuS, +}; + +static Port_t ssp_port; +static t_fifo_buffer ssp_buffer; + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + if (PIOS_IAP_CheckRequest() == false) { + PIOS_DELAY_WaitmS(500); + User_DFU_request = false; + DeviceState = BLidle; + PIOS_IAP_ClearRequest(); + } + + // Initialize the SSP layer between serial port and DFU + fifoBuf_init(&ssp_buffer, rx_buffer, UART_BUFFER_SIZE); + ssp_Init(&ssp_port, &ssp_portConfig); + + uint32_t stopwatch = 0; + const uint32_t start_time = PIOS_DELAY_GetuS(); + while (true) { + /* Update the stopwatch */ + stopwatch = PIOS_DELAY_GetuSSince(start_time); + + processRX(); + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + // PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + // PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + sweep_steps1 = 100; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + led_pwm_step(period1, sweep_steps1, stopwatch, false); + led_pwm_step(period2, sweep_steps2, stopwatch, true); + JumpToApp |= (stopwatch > BL_WAIT_TIME) && ((DeviceState == BLidle) || (DeviceState == DFUidle)); + DataDownload(start); + + if (JumpToApp == true && !ssp_dfu) { + jump_to_app(); + } + } +} + +void led_pwm_step(uint16_t pwm_period, uint16_t pwm_sweep_steps, uint32_t stopwatch, bool default_state) +{ + if (pwm_period != 0) { + if (LedPWM(pwm_period, pwm_sweep_steps, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + if (default_state) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } +} +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + return; + } +} + +uint32_t LedPWM(uint16_t pwm_period, uint16_t pwm_sweep_steps, uint32_t count) +{ + const uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + const uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} +uint32_t process_count = 0; +void processRX() +{ + do { + ssp_ReceiveProcess(&ssp_port); + status = ssp_SendProcess(&ssp_port); + } while ((status != SSP_TX_IDLE) && (status != SSP_TX_ACKED)); + + if (fifoBuf_getUsed(&ssp_buffer) >= DFU_BUFFER_SIZE) { + for (int32_t x = 0; x < DFU_BUFFER_SIZE; ++x) { + process_buffer[x] = fifoBuf_getByte(&ssp_buffer); + } + process_count++; + processComand(process_buffer); + } +} +uint32_t callback_cnt = 0; +uint32_t read_cnt = 0; +uint32_t write_cnt = 0; +uint32_t rx_check_cnt = 0; +void SSP_CallBack(uint8_t *buf, uint16_t len) +{ + ssp_dfu = true; + callback_cnt++; + fifoBuf_putData(&ssp_buffer, buf, len); +} + +int16_t SSP_SerialRead(void) +{ + uint8_t byte; + + rx_check_cnt++; + if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, &byte, 1, 0) == 1) { + read_cnt++; + return byte; + } else { + return -1; + } +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return ssp_SendData(&ssp_port, msg, msg_len); +} + +void SSP_SerialWrite(uint8_t value) +{ + write_cnt++; + PIOS_COM_SendChar(PIOS_COM_TELEM_USB, value); +} diff --git a/flight/targets/boards/spracingf3/bootloader/pios_board.c b/flight/targets/boards/spracingf3/bootloader/pios_board.c new file mode 100644 index 0000000000..d1d8116c17 --- /dev/null +++ b/flight/targets/boards/spracingf3/bootloader/pios_board.c @@ -0,0 +1,85 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Defines board specific static initializers for hardware for the SPRacing F3 board. + * @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 + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" +#include + +uint32_t pios_com_telem_usb_id; + +#define PIOS_COM_MAIN_RX_BUF_LEN 256 +#define PIOS_COM_MAIN_TX_BUF_LEN 256 +uint8_t rx_buffer[PIOS_COM_MAIN_RX_BUF_LEN]; +uint8_t tx_buffer[PIOS_COM_MAIN_TX_BUF_LEN]; + + +static void setupCom(); +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + FLASH_PrefetchBufferCmd(ENABLE); + +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ + setupCom(); +} + +void setupCom() +{ + uint32_t pios_usart_generic_id; + + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_cfg[0])) { + 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_ChangeConfig(PIOS_COM_TELEM_USB, PIOS_COM_Word_length_8b, PIOS_COM_Parity_No, PIOS_COM_StopBits_1, 57600); +} diff --git a/flight/targets/boards/spracingf3/firmware/Makefile b/flight/targets/boards/spracingf3/firmware/Makefile new file mode 100644 index 0000000000..4448553323 --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/Makefile @@ -0,0 +1,114 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +PIOS_OMITS_USB = YES + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +MODULES += Notify + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += Battery +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/spracingf3/firmware/UAVObjects.inc b/flight/targets/boards/spracingf3/firmware/UAVObjects.inc new file mode 100644 index 0000000000..f9c08bb3d6 --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/UAVObjects.inc @@ -0,0 +1,134 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwspracingf3settings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/spracingf3/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/spracingf3/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 0000000000..5fea013d7a --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/spracingf3/firmware/inc/openpilot.h b/flight/targets/boards/spracingf3/firmware/inc/openpilot.h new file mode 100644 index 0000000000..5576baca66 --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @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 OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3/firmware/inc/pios_config.h b/flight/targets/boards/spracingf3/firmware/inc/pios_config.h new file mode 100644 index 0000000000..b4f856a2be --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/inc/pios_config.h @@ -0,0 +1,195 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2015 + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_ADC +#define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +// #define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +/* #define PIOS_INCLUDE_USB */ +/* #define PIOS_INCLUDE_USB_HID */ +/* #define PIOS_INCLUDE_USB_CDC */ +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +#define PIOS_INCLUDE_MPU6000 +#define PIOS_MPU6000_ACCEL +/* #define PIOS_INCLUDE_HMC5843 */ +#define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83_INTERNAL +#define PIOS_HMC5883_HAS_GPIOS +/* #define PIOS_INCLUDE_BMP085 */ +#define PIOS_INCLUDE_MS56XX +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +#define PIOS_INCLUDE_WS2811 + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +#define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +#define FLASH_FREERTOS +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +/* #define PIOS_INCLUDE_FLASH_INTERNAL */ + +/* #define PIOS_INCLUDE_DEBUGLOG */ + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +// #define PIOS_GPS_MINIMAL +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3/firmware/main.cpp b/flight/targets/boards/spracingf3/firmware/main.cpp new file mode 100644 index 0000000000..c86e5b1a40 --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/main.cpp @@ -0,0 +1,100 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @brief These files are the core system files for LibrePilot. + * They are the ground layer just above PiOS. In practice, LibrePilot actually starts + * in the main() function of main.cpp + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @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 + */ + +extern "C" { +#include "inc/openpilot.h" +#include +#include + + +/* Global Variables */ + +/* Local Variables */ +} +extern void Stack_Change(void); +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3/firmware/pios_board.c b/flight/targets/boards/spracingf3/firmware/pios_board.c new file mode 100644 index 0000000000..364ed552fc --- /dev/null +++ b/flight/targets/boards/spracingf3/firmware/pios_board.c @@ -0,0 +1,283 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the SPRacing F3 board. + *****************************************************************************/ +/* + * 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 "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +static HwSPRacingF3SettingsData boardHwSettings; + +static void hwSPRacingF3SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HwSPRacingF3SettingsData currentBoardHwSettings; + + HwSPRacingF3SettingsGet(¤tBoardHwSettings); + + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwSPRacingF3SettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + } +} + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ + +void PIOS_Board_Init(void) +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the serial flash */ + + + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) { + PIOS_Assert(0); + } + +#endif + + uintptr_t flash_id; + + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_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)) { + PIOS_DEBUG_Assert(0); + } + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + HwSPRacingF3SettingsSetDefaults(HwSPRacingF3SettingsHandle(), 0); + + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_15_cfg); + PIOS_TIM_InitClock(&tim_16_cfg); + PIOS_TIM_InitClock(&tim_17_cfg); + + HwSPRacingF3SettingsConnectCallback(hwSPRacingF3SettingsUpdatedCb); + + HwSPRacingF3SettingsGet(&boardHwSettings); + + boardHwSettings.UARTPort[0] = HWSPRACINGF3SETTINGS_UARTPORT_TELEMETRY; /* UART1 should always be telemetry */ + boardHwSettings.UARTPort[2] = HWSPRACINGF3SETTINGS_UARTPORT_DISABLED; /* UART3 has conflict with SWD, so we will let it be disabled for a moment */ + + static const PIOS_BOARD_IO_UART_Function uart_function_map[] = { + [HWSPRACINGF3SETTINGS_UARTPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSPRACINGF3SETTINGS_UARTPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSPRACINGF3SETTINGS_UARTPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSPRACINGF3SETTINGS_UARTPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok. + [HWSPRACINGF3SETTINGS_UARTPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSPRACINGF3SETTINGS_UARTPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSPRACINGF3SETTINGS_UARTPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSPRACINGF3SETTINGS_UARTPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSPRACINGF3SETTINGS_UARTPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSPRACINGF3SETTINGS_UARTPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSPRACINGF3SETTINGS_UARTPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSPRACINGF3SETTINGS_UARTPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSPRACINGF3SETTINGS_UARTPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSPRACINGF3SETTINGS_UARTPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWSPRACINGF3SETTINGS_UARTPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + }; + + for (unsigned int i = 0; i < HWSPRACINGF3SETTINGS_UARTPORT_NUMELEM; ++i) { + if (boardHwSettings.UARTPort[i] < NELEMENTS(uart_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[i], uart_function_map[boardHwSettings.UARTPort[i]]); + } + } + + /* Configure IO ports (IO-1 & IO-2) */ + + bool use_ppm = false; + + /* then do PPM */ + if ((boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_PPMPWM) + || (boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_PPM) + || (boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_PPMOUTPUTS)) { +#if defined(PIOS_INCLUDE_PPM) + use_ppm = true; + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); +#endif /* PIOS_INCLUDE_PPM */ + } + + if ((boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_OUTPUTS) + || (boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_PPMOUTPUTS)) { + /* configure remaining inputs as additional outputs */ + } else if ((boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_PPMPWM) + || (boardHwSettings.IOPorts == HWSPRACINGF3SETTINGS_IOPORTS_PWM)) { +#if defined(PIOS_INCLUDE_PWM) + /* configure remaining inputs as pwm input */ + + struct pios_pwm_cfg pwm_cfg = pios_pwm_cfg; + + if (use_ppm) { + ++(pwm_cfg.channels); + --(pwm_cfg.num_channels); + } + + PIOS_BOARD_IO_Configure_PWM_RCVR(&pwm_cfg); // is this ok? pwm_cfg is on stack +#endif /* if defined(PIOS_INCLUDE_PWM) */ + } + +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + +#ifndef PIOS_ENABLE_DEBUG_PINS + // TODO: make use of HWSPRACINGF3SETTINGS_IOPORTS_[PPM]OUTPUTS + PIOS_Servo_Init(&pios_servo_cfg); +#else + PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels); +#endif + + switch (boardHwSettings.LEDPort) { + case HWSPRACINGF3SETTINGS_LEDPORT_WS281X: +#if defined(PIOS_INCLUDE_WS2811) + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg); +#endif + break; + default: + break; + } + +#if defined(PIOS_INCLUDE_I2C) + // init I2C1 for use with the internal mag and baro + if (PIOS_I2C_Init(&pios_i2c_id, &pios_i2c_cfg)) { + PIOS_DEBUG_Assert(0); + } +#endif + + PIOS_DELAY_WaitmS(50); + + 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); +} + + +/** + * @} + */ diff --git a/flight/targets/boards/spracingf3/pios_board.h b/flight/targets/boards/spracingf3/pios_board.h new file mode 100644 index 0000000000..988cdb6611 --- /dev/null +++ b/flight/targets/boards/spracingf3/pios_board.h @@ -0,0 +1,248 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the SPRacing F3 board. + * @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_H +#define PIOS_BOARD_H +// ------------------------ +// Timers and Channels Used +// ------------------------ +/* + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ + +// ------------------------ +// DMA Channels Used +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#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_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_BUZZER_ALARM 1 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 +extern uint32_t pios_i2c_id; +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_id) +#define PIOS_I2C_MS56XX_INTERNAL_ADAPTER (pios_i2c_id) +#define PIOS_I2C_HMC5X83_INTERNAL_ADAPTER (pios_i2c_id) +#define PIOS_I2C_MPU6000_ADAPTER (pios_i2c_id) +#define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_id) + +// ------------------------ +// PIOS_BMP085 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Current sensor +// PIOS_ADC_PinGet(1) = RSSI sensor +// PIOS_ADC_PinGet(2) = Battery voltage +// ------------------------- +#define PIOS_DMA_PIN_CONFIG \ + { \ + { GPIOA, GPIO_Pin_5, ADC_Channel_2, false }, /* ADC_1 / Current */ \ + { GPIOB, GPIO_Pin_2, ADC_Channel_12, false }, /* ADC_2 / RSSI */ \ + { GPIOA, GPIO_Pin_4, ADC_Channel_1, false }, /* Battery voltage */ \ + } + +/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ +/* which is annoying because this then determines the rate at which we generate buffer turnover events */ +/* the objective here is to get enough buffer space to support 100Hz averaging rate */ +#define PIOS_ADC_NUM_CHANNELS 3 +#define PIOS_ADC_MAX_OVERSAMPLING 32 +#define PIOS_ADC_USE_ADC2 0 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// MPU6000 +// ------------------------- + +#define PIOS_MPU6050_I2C_ADDR_AD0_LOW 0x68 +#define PIOS_MPU6050_I2C_ADDR_AD0_HIGH 0x69 + +#define PIOS_MPU6000_I2C_ADDR PIOS_MPU6050_I2C_ADDR_AD0_LOW + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/spracingf3evo/board-info.mk b/flight/targets/boards/spracingf3evo/board-info.mk new file mode 100644 index 0000000000..acc23de3e5 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/board-info.mk @@ -0,0 +1,23 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x02 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +CHIP := STM32F303xC + +OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg +OPENOCD_CONFIG := stm32f3x.cfg + +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region + +# Some KB for settings storage (32kb) +EE_BANK_BASE := 0x08004000 # EEPROM storage area +EE_BANK_SIZE := 0x00008000 # EEPROM storage size + +FW_BANK_BASE := 0x0800C000 # Start of firmware flash +FW_BANK_SIZE := 0x00034000 # Should include FW_DESC_SIZE (208kB) + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/spracingf3evo/board_hw_defs.c b/flight/targets/boards/spracingf3evo/board_hw_defs.c new file mode 100644 index 0000000000..2e4d75b4a4 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/board_hw_defs.c @@ -0,0 +1,821 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the SPRacing F3 EVO board. + *****************************************************************************/ +/* + * 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 + */ + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_gpio pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = true + }, + [PIOS_BUZZER_ALARM] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg = { + .gpios = pios_leds, + .num_gpios = NELEMENTS(pios_leds), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Gyro interface */ + +static const struct pios_spi_cfg pios_spi_mpu9250_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi_mpu9250_id; + +/* SDCARD Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_SDCARD_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_SDCARD_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_SDCARD_irq_handler"))); +static const struct pios_spi_cfg pios_spi_sdcard_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, /* NOTE: was GPIO_Mode_IN_FLOATING */ + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + } + }, + }, +}; + +static uint32_t pios_spi_sdcard_id; +void PIOS_SPI_SDCARD_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" +#include "pios_flash_internal_priv.h" + +static const struct pios_flash_internal_cfg flash_internal_system_cfg = {}; + +static const struct flashfs_logfs_cfg flashfs_internal_cfg = { + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (16x2KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 8 sectors */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00000800, /* 2K bytes */ + .page_size = 0x00000800, /* 2K bytes */ +}; + +#endif /* PIOS_INCLUDE_FLASH */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_15_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM15_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_16_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM16_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_17_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 +#define GPIO_AF_PA15_TIM8 GPIO_AF_2 +#define GPIO_AF_PA3_TIM15 GPIO_AF_9 +#define GPIO_AF_PA6_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM3 GPIO_AF_2 +#define GPIO_AF_PA10_TIM2 GPIO_AF_10 + + +#define TIM1_CH1_DMA_INSTANCE 1 +#define TIM1_CH1_DMA_CHANNEL 2 + +static const struct pios_tim_channel pios_tim_servoport_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 2, A, 3), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, A, 6), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, A, 7), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1), // bank 3 +}; + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART2 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_cfg[] = { + USART_CONFIG(USART1, A, 10, A, 9), + USART_CONFIG(USART2, A, 15, A, 14), + USART_CONFIG(USART3, B, 11, B, 10), +}; + + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +static const struct pios_tim_channel pios_tim_ppm = TIM_SERVO_CHANNEL_CONFIG(TIM8, 1, A, 15); + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_ppm, + .num_channels = 1, +}; +#endif /* PIOS_INCLUDE_PPM */ + +/* + * SONAR Inputs + */ +#if defined(PIOS_INCLUDE_HCSR04) +#include + +static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { + { + .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, + }, +}; + +const struct pios_hcsr04_cfg pios_hcsr04_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_ev_irq_handler(void); +void PIOS_I2C_er_irq_handler(void); +void I2C1_EV_EXTI23_IRQHandler() __attribute__((alias("PIOS_I2C_ev_irq_handler"))); +void I2C1_ER_IRQHandler() __attribute__((alias("PIOS_I2C_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_cfg = { + .regs = I2C1, + .remapSCL = GPIO_AF_4, + .remapSDA = GPIO_AF_4, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DigitalFilter = 0x00, + .I2C_AnalogFilter = I2C_AnalogFilter_Enable, +// .I2C_Timing = 0x70310309, // 50kHz I2C @ 8MHz input -> PRESC=0x7, SCLDEL=0x3, SDADEL=0x1, SCLH=0x03, SCLL=0x09 + .I2C_Timing = 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + .pin_source = GPIO_PinSource6, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }, + .pin_source = GPIO_PinSource7, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_id; +void PIOS_I2C_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_id); +} + +void PIOS_I2C_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_id); +} + + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +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_WS2811) +#include "pios_ws2811_cfg.h" + +static const struct pios_ws2811_cfg pios_ws2811_cfg = PIOS_WS2811_CONFIG(TIM1, 1, A, 8); + +void DMA1_Channel2_IRQHandler() +{ + PIOS_WS2811_DMA_irq_handler(); +} + +#endif /* PIOS_INCLUDE_WS2811 */ + +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Channel1_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC2, + .dma = { + .irq = { + .flags = (DMA2_FLAG_TC1 | DMA2_FLAG_TE1 | DMA2_FLAG_HT1), + .init = { + .NVIC_IRQChannel = DMA2_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Channel1, + } + }, + .half_flag = DMA2_FLAG_HT1, + .full_flag = DMA2_FLAG_TC1, +}; + +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" +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 +}; +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 */ + +#ifdef PIOS_INCLUDE_BMP280 +#include "pios_bmp280.h" +static const struct pios_bmp280_cfg bmp280_config = { + .oversampling = BMP280_ULTRA_HIGH_RESOLUTION +}; +const struct pios_bmp280_cfg *PIOS_BOARD_HW_DEFS_GetBMP280Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &bmp280_config; +} +#endif + +/** + * Configuration for the MPU9250 chip + */ +#if defined(PIOS_INCLUDE_MPU9250) +#include "pios_mpu9250.h" +static const struct pios_exti_cfg pios_exti_mpu9250_cfg __exti_config = { + .vector = PIOS_MPU9250_IRQHandler, + .line = EXTI_Line13, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .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_Line13, // 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_270DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .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/spracingf3evo/bootloader/Makefile b/flight/targets/boards/spracingf3evo/bootloader/Makefile new file mode 100644 index 0000000000..d9f8aa112e --- /dev/null +++ b/flight/targets/boards/spracingf3evo/bootloader/Makefile @@ -0,0 +1,29 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for SPRacingF3_EVO, board revision $(BOARD_REVISION)) diff --git a/flight/targets/boards/spracingf3evo/bootloader/inc/pios_config.h b/flight/targets/boards/spracingf3evo/bootloader/inc/pios_config.h new file mode 100644 index 0000000000..9a0292159c --- /dev/null +++ b/flight/targets/boards/spracingf3evo/bootloader/inc/pios_config.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3evo/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/spracingf3evo/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..932323aa9c --- /dev/null +++ b/flight/targets/boards/spracingf3evo/bootloader/inc/pios_usb_board_data.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 2 + +#include /* struct usb_* */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_SPRACINGF3EVO, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" + +/* + * The bootloader uses a simplified report structure + * BL: ... + * FW: ... + * This define changes the behaviour in pios_usb_hid.c + */ +#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/spracingf3evo/bootloader/main.c b/flight/targets/boards/spracingf3evo/bootloader/main.c new file mode 100644 index 0000000000..1492d21645 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/bootloader/main.c @@ -0,0 +1,230 @@ +/** + ****************************************************************************** + * @addtogroup SPRacingF3 EVO serial BootLoader + * @brief These files contain the code to the SPRacingF3 EVO serial Bootloader. + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @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 +#include +#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) + +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + + +//////////////////////////////////////// +uint8_t tempcount = 0; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; +uint8_t User_DFU_request = FALSE; +static uint8_t mReceive_Buffer[63]; +/* Private function prototypes -----------------------------------------------*/ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); +uint8_t processRX(); +void jump_to_app(); + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + USB_connected = PIOS_USB_CableConnected(0); + + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } + + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } + + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; + + if (JumpToApp == TRUE) { + jump_to_app(); + } + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } + + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) /*|| (DeviceState == DFUidle && !USB_connected)*/)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } +} + +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + JumpToApp = FALSE; + return; + } +} +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} + +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); +} diff --git a/flight/targets/boards/spracingf3evo/bootloader/pios_board.c b/flight/targets/boards/spracingf3evo/bootloader/pios_board.c new file mode 100644 index 0000000000..ac81f2996b --- /dev/null +++ b/flight/targets/boards/spracingf3evo/bootloader/pios_board.c @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Defines board specific static initializers for hardware for the SPRacing F3 EVO board. + * @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 "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 + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uint32_t pios_com_telem_usb_id; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +static bool board_init_complete = false; +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(ENABLE); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + +#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_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)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + +#endif /* PIOS_INCLUDE_USB */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar + + board_init_complete = true; +} + +void PIOS_ADC_DMA_Handler() +{} diff --git a/flight/targets/boards/spracingf3evo/firmware/Makefile b/flight/targets/boards/spracingf3evo/firmware/Makefile new file mode 100644 index 0000000000..1dc1dd323e --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/Makefile @@ -0,0 +1,113 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +MODULES += Notify + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += Battery +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge +OPTMODULES += UAVOFrSKYSensorHubBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/spracingf3evo/firmware/UAVObjects.inc b/flight/targets/boards/spracingf3evo/firmware/UAVObjects.inc new file mode 100644 index 0000000000..a38f6b5eb6 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/UAVObjects.inc @@ -0,0 +1,134 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwspracingf3evosettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/spracingf3evo/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/spracingf3evo/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 0000000000..5fea013d7a --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/spracingf3evo/firmware/inc/openpilot.h b/flight/targets/boards/spracingf3evo/firmware/inc/openpilot.h new file mode 100644 index 0000000000..5576baca66 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @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 OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h b/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h new file mode 100644 index 0000000000..89bd45e3b0 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h @@ -0,0 +1,195 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_ADC +#define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +/* #define PIOS_INCLUDE_MPU6000 */ +/* #define PIOS_MPU6000_ACCEL */ +/* #define PIOS_INCLUDE_HMC5843 */ +#define PIOS_INCLUDE_HMC5X83 +/* #define PIOS_HMC5883_HAS_GPIOS */ +#define PIOS_INCLUDE_MPU9250 +#define PIOS_MPU9250_ACCEL +#define PIOS_MPU9250_MAG +#define PIOS_INCLUDE_BMP280 +/* #define PIOS_INCLUDE_BMP085 */ +/* #define PIOS_INCLUDE_MS56XX */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +#define PIOS_INCLUDE_WS2811 + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +#define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +#define FLASH_FREERTOS +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +#define PIOS_INCLUDE_FLASH_INTERNAL + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +/* #define PIOS_GPS_MINIMAL */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3evo/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/spracingf3evo/firmware/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..8f95408cfb --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/inc/pios_usb_board_data.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 +#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 4 + +#include /* USB_* macros */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_SPRACINGF3EVO, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/spracingf3evo/firmware/main.cpp b/flight/targets/boards/spracingf3evo/firmware/main.cpp new file mode 100644 index 0000000000..d1ebd36253 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/main.cpp @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @brief These files are the core system files for LibrePilot. + * They are the ground layer just above PiOS. In practice, LibrePilot actually starts + * in the main() function of main.cpp + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @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 + */ + +extern "C" { +#include "inc/openpilot.h" +#include +/* Task Priorities */ + +/* Global Variables */ +extern void Stack_Change(void); +} /* extern "C" */ + +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/spracingf3evo/firmware/pios_board.c b/flight/targets/boards/spracingf3evo/firmware/pios_board.c new file mode 100644 index 0000000000..59fc7432e1 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/firmware/pios_board.c @@ -0,0 +1,273 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the SPRacing F3 EVO board. + *****************************************************************************/ +/* + * 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 "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif + + +static HwSPRacingF3EVOSettingsData boardHwSettings; + +static void hwSPRacingF3EVOSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HwSPRacingF3EVOSettingsData currentBoardHwSettings; + + HwSPRacingF3EVOSettingsGet(¤tBoardHwSettings); + + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwSPRacingF3EVOSettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + } +} + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ + +void PIOS_Board_Init(void) +{ +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the mpu9250 */ + + if (PIOS_SPI_Init(&pios_spi_mpu9250_id, &pios_spi_mpu9250_cfg)) { + PIOS_DEBUG_Assert(0); + } + + /* Set up the SPI interface to the sdcard */ + if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { + PIOS_DEBUG_Assert(0); + } +#endif + +#if defined(PIOS_INCLUDE_FLASH) + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + + // initialize the internal settings storage flash + if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_system_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + +// if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_internal_user_cfg, &pios_internal_flash_driver, flash_id)) { +// PIOS_DEBUG_Assert(0); +// } + +#endif /* if defined(PIOS_INCLUDE_FLASH) */ + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + HwSPRacingF3EVOSettingsSetDefaults(HwSPRacingF3EVOSettingsHandle(), 0); + + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + /* Set up pulse timers: 2, 3, 8, 15 */ + // PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + // PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_15_cfg); + // PIOS_TIM_InitClock(&tim_16_cfg); + // PIOS_TIM_InitClock(&tim_17_cfg); + + +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + + HwSPRacingF3EVOSettingsConnectCallback(hwSPRacingF3EVOSettingsUpdatedCb); + + HwSPRacingF3EVOSettingsGet(&boardHwSettings); + + static const PIOS_BOARD_IO_UART_Function uart_function_map[] = { + [HWSPRACINGF3EVOSETTINGS_UARTPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok. + [HWSPRACINGF3EVOSETTINGS_UARTPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWSPRACINGF3EVOSETTINGS_UARTPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + }; + + for (unsigned int i = 0; i < HWSPRACINGF3EVOSETTINGS_UARTPORT_NUMELEM; ++i) { + if (boardHwSettings.UARTPort[i] < NELEMENTS(uart_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[i], uart_function_map[boardHwSettings.UARTPort[i]]); + } + } + + /* allow PPM on uart2 only, to avoid uart1&3 TIM2_CH4 conflict with output PWMSync, OneShot and MultiShot modes */ + if (boardHwSettings.UARTPort[1] == HWSPRACINGF3EVOSETTINGS_UARTPORT_PPM) { + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); + } + +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + +#ifndef PIOS_ENABLE_DEBUG_PINS + PIOS_Servo_Init(&pios_servo_cfg); +#else + PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels); +#endif + + switch (boardHwSettings.LEDPort) { + case HWSPRACINGF3EVOSETTINGS_LEDPORT_WS281X: +#if defined(PIOS_INCLUDE_WS2811) + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg); +#endif + break; + default: + break; + } + +#if defined(PIOS_INCLUDE_I2C) + + // init I2C1 for use with the internal baro + if (PIOS_I2C_Init(&pios_i2c_id, &pios_i2c_cfg)) { + PIOS_DEBUG_Assert(0); + } + PIOS_DELAY_WaitmS(50); + +#endif + + PIOS_BOARD_Sensors_Configure(); + + PIOS_LED_On(PIOS_LED_HEARTBEAT); + + /* 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/spracingf3evo/pios_board.h b/flight/targets/boards/spracingf3evo/pios_board.h new file mode 100644 index 0000000000..629b33783c --- /dev/null +++ b/flight/targets/boards/spracingf3evo/pios_board.h @@ -0,0 +1,254 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the SPRacing F3 EVO board. + * @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_H +#define PIOS_BOARD_H +// ------------------------ +// Timers and Channels Used +// ------------------------ +/* + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ + +// ------------------------ +// DMA Channels Used +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#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_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_BUZZER_ALARM 1 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 1 +extern uint32_t pios_i2c_id; +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_id) +#define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_id) +#define PIOS_I2C_BMP280_INTERNAL_ADAPTER (pios_i2c_id) + +// ------------------------ +// PIOS_BMP085 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 +extern uint32_t pios_spi_mpu9250_id; +#define PIOS_SPI_MPU9250_ADAPTER (pios_spi_mpu9250_id) + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Current sensor +// PIOS_ADC_PinGet(1) = RSSI sensor +// PIOS_ADC_PinGet(2) = Battery voltage +// ------------------------- +#define PIOS_DMA_PIN_CONFIG \ + { \ + { GPIOA, GPIO_Pin_5, ADC_Channel_2, false }, /* ADC_1 / Current */ \ + { GPIOB, GPIO_Pin_2, ADC_Channel_12, false }, /* ADC_2 / RSSI */ \ + { GPIOA, GPIO_Pin_4, ADC_Channel_1, false }, /* Battery voltage */ \ + } + +/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ +/* which is annoying because this then determines the rate at which we generate buffer turnover events */ +/* the objective here is to get enough buffer space to support 100Hz averaging rate */ +#define PIOS_ADC_NUM_CHANNELS 3 +#define PIOS_ADC_MAX_OVERSAMPLING 32 +#define PIOS_ADC_USE_ADC2 0 + +// #define PIOS_ADC_USE_TEMP_SENSOR +// #define PIOS_ADC_TEMPERATURE_PIN 4 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// USB +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 + +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/spracingf3evo/pios_usb_board_data.c b/flight/targets/boards/spracingf3evo/pios_usb_board_data.c new file mode 100644 index 0000000000..cb02370e75 --- /dev/null +++ b/flight/targets/boards/spracingf3evo/pios_usb_board_data.c @@ -0,0 +1,104 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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 + */ + +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ + +static const uint8_t usb_product_id[30] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'S', 0, + 'P', 0, + 'R', 0, + 'a', 0, + 'c', 0, + 'i', 0, + 'n', 0, + 'g', 0, + 'F', 0, + '3', 0, + '_', 0, + 'E', 0, + 'V', 0, + 'O', 0, +}; + +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; + + PIOS_SYS_SerialNumberGet((char *)sn); + + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/targets/boards/tinyfish/board-info.mk b/flight/targets/boards/tinyfish/board-info.mk new file mode 100644 index 0000000000..34539298d3 --- /dev/null +++ b/flight/targets/boards/tinyfish/board-info.mk @@ -0,0 +1,23 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x06 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +CHIP := STM32F303xC + +OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg +OPENOCD_CONFIG := stm32f3x.cfg + +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region + +# Some KB for settings storage (32kb) +EE_BANK_BASE := 0x08004000 # EEPROM storage area +EE_BANK_SIZE := 0x00008000 # EEPROM storage size + +FW_BANK_BASE := 0x0800C000 # Start of firmware flash +FW_BANK_SIZE := 0x00034000 # Should include FW_DESC_SIZE (208kB) + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/tinyfish/board_hw_defs.c b/flight/targets/boards/tinyfish/board_hw_defs.c new file mode 100644 index 0000000000..6f1ef54677 --- /dev/null +++ b/flight/targets/boards/tinyfish/board_hw_defs.c @@ -0,0 +1,643 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @brief Defines board specific static initializers for hardware for the tinyFISH board. + *****************************************************************************/ +/* + * 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 + */ + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_gpio pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, + [PIOS_LED_ALARM] = { /* or PC15 ?? */ + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, + [PIOS_BUZZER_ALARM] = { /* (or PB2 ??) */ + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg = { + .gpios = pios_leds, + .num_gpios = NELEMENTS(pios_leds), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* SPI Flash interface */ + +static const struct pios_spi_cfg pios_spi2_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi2_id; + +static const struct pios_spi_cfg pios_spi1_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { /* MPU6000 */ + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi1_id; + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" +#include "pios_flash_internal_priv.h" + +static const struct pios_flash_internal_cfg flash_internal_system_cfg = {}; + +static const struct flashfs_logfs_cfg flashfs_internal_cfg = { + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (16x2KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 8 sectors */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00000800, /* 2K bytes */ + .page_size = 0x00000800, /* 2K bytes */ +}; + +#endif /* PIOS_INCLUDE_FLASH */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_15_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM15_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_16_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM16_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_17_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 +#define GPIO_AF_PA15_TIM8 GPIO_AF_2 +#define GPIO_AF_PA3_TIM15 GPIO_AF_9 +#define GPIO_AF_PA6_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM3 GPIO_AF_2 +#define GPIO_AF_PA10_TIM2 GPIO_AF_10 + +#define GPIO_AF_PA4_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM17 GPIO_AF_1 +#define GPIO_AF_PB8_TIM16 GPIO_AF_1 + +#define GPIO_AF_PB9_TIM8 GPIO_AF_10 +#define GPIO_AF_PA2_TIM15 GPIO_AF_9 +#define GPIO_AF_PB10_TIM2 GPIO_AF_1 + +#define TIM1_CH1_DMA_INSTANCE 1 +#define TIM1_CH1_DMA_CHANNEL 2 + +#define TIM16_CH1_DMA_INSTANCE 2 +#define TIM16_CH1_DMA_CHANNEL 3 + +static const struct pios_tim_channel pios_tim_servoport_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, B, 9), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 2, A, 3), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 1, A, 2), // bank 3 + + // following two are here so we can have two additional PWM outputs instead of UART3 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11), // bank 4 (UART3 RX) + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, B, 10), // bank 4 (UART3 TX) +}; + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART2 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_cfg[] = { + USART_CONFIG(USART1, B, 7, B, 6), /* RX_DEBUG, TX_??? */ + USART_CONFIG(USART2, A, 15, A, 14), /* RX_SBUS, TX_FRSKY_TELEMETRY */ + USART_CONFIG(USART3, B, 11, B, 10), +}; + + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins) - 2, +}; + +const struct pios_servo_cfg pios_servo_uart3_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins), +}; + + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +static const struct pios_tim_channel pios_tim_ppm = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11); + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm, + .num_channels = 1, +}; +#endif /* PIOS_INCLUDE_PPM */ + + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +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_WS2811) +#include "pios_ws2811_cfg.h" + +static const struct pios_ws2811_cfg pios_ws2811_cfg = PIOS_WS2811_CONFIG(TIM1, 1, A, 8); + +void DMA1_Channel2_IRQHandler() +{ + PIOS_WS2811_DMA_irq_handler(); +} + +#endif /* PIOS_INCLUDE_WS2811 */ + +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Channel5_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC3, + .dma = { + .irq = { + .flags = (DMA2_FLAG_TC5 | DMA2_FLAG_TE5 | DMA2_FLAG_HT5), + .init = { + .NVIC_IRQChannel = DMA2_Channel5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Channel5, + } + }, + .half_flag = DMA2_FLAG_HT5, + .full_flag = DMA2_FLAG_TC5, +}; + +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) */ + +/** + * Configuration for 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_Line13, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .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_Line13, // 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 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_BOTTOM_90DEG, +#ifdef PIOS_INCLUDE_SPI + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, +#endif /* PIOS_INCLUDE_SPI */ + .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/tinyfish/bootloader/Makefile b/flight/targets/boards/tinyfish/bootloader/Makefile new file mode 100644 index 0000000000..f31cf6e42a --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/Makefile @@ -0,0 +1,29 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for tinyFISH FC, board revision $(BOARD_REVISION)) diff --git a/flight/targets/boards/tinyfish/bootloader/inc/pios_config.h b/flight/targets/boards/tinyfish/bootloader/inc/pios_config.h new file mode 100644 index 0000000000..bd8022938d --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/inc/pios_config.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/tinyfish/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..8efcf63c24 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/inc/pios_usb_board_data.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 2 + +#include /* struct usb_* */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_TINYFISH, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" + +/* + * The bootloader uses a simplified report structure + * BL: ... + * FW: ... + * This define changes the behaviour in pios_usb_hid.c + */ +#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/tinyfish/bootloader/main.c b/flight/targets/boards/tinyfish/bootloader/main.c new file mode 100644 index 0000000000..044608da54 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/main.c @@ -0,0 +1,230 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This is the file with the main function of the LibrePilot BootLoader + * @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 +#include +#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) + +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + + +//////////////////////////////////////// +uint8_t tempcount = 0; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; +uint8_t User_DFU_request = FALSE; +static uint8_t mReceive_Buffer[63]; +/* Private function prototypes -----------------------------------------------*/ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); +uint8_t processRX(); +void jump_to_app(); + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + USB_connected = PIOS_USB_CableConnected(0); + + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } + + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } + + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; + + if (JumpToApp == TRUE) { + jump_to_app(); + } + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } + + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) /*|| (DeviceState == DFUidle && !USB_connected)*/)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } +} + +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + JumpToApp = FALSE; + return; + } +} +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} + +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); +} diff --git a/flight/targets/boards/tinyfish/bootloader/pios_board.c b/flight/targets/boards/tinyfish/bootloader/pios_board.c new file mode 100644 index 0000000000..fbb6b50f71 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/pios_board.c @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the tinyFISH board. + * @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 "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 + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uint32_t pios_com_telem_usb_id; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +static bool board_init_complete = false; +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(ENABLE); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + +#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_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)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + +#endif /* PIOS_INCLUDE_USB */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar + + board_init_complete = true; +} + +void PIOS_ADC_DMA_Handler() +{} diff --git a/flight/targets/boards/tinyfish/firmware/Makefile b/flight/targets/boards/tinyfish/firmware/Makefile new file mode 100644 index 0000000000..0020b7351d --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/Makefile @@ -0,0 +1,113 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# 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 FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +MODULES += Notify +MODULES += Battery +MODULES += UAVOFrSKYSensorHubBridge + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/tinyfish/firmware/UAVObjects.inc b/flight/targets/boards/tinyfish/firmware/UAVObjects.inc new file mode 100644 index 0000000000..e337dbd6c9 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/UAVObjects.inc @@ -0,0 +1,134 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# 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 +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwtinyfishsettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/tinyfish/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/tinyfish/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 0000000000..5fea013d7a --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/openpilot.h b/flight/targets/boards/tinyfish/firmware/inc/openpilot.h new file mode 100644 index 0000000000..5576baca66 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @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 OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/pios_config.h b/flight/targets/boards/tinyfish/firmware/inc/pios_config.h new file mode 100644 index 0000000000..c25d039fc5 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/pios_config.h @@ -0,0 +1,197 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2015 + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @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_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +#define PIOS_INCLUDE_DEBUG_CONSOLE +#define DEBUG_LEVEL 0 +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_ADC +// #define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +#define PIOS_INCLUDE_MPU6000 +#define PIOS_MPU6000_ACCEL +/* #define PIOS_INCLUDE_HMC5843 */ +/* #define PIOS_INCLUDE_HMC5X83 */ +/* #define PIOS_HMC5883_HAS_GPIOS */ +/* #define PIOS_INCLUDE_MPU9250 */ +/* #define PIOS_MPU9250_ACCEL */ +/* #define PIOS_MPU9250_MAG */ +/* #define PIOS_INCLUDE_BMP280 */ +/* #define PIOS_INCLUDE_BMP085 */ +/* #define PIOS_INCLUDE_MS56XX */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +#define PIOS_INCLUDE_WS2811 + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +#define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR + +#define PIOS_INCLUDE_FRSKY_SENSORHUB + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +#define FLASH_FREERTOS +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +#define PIOS_INCLUDE_FLASH_INTERNAL + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +/* #define PIOS_GPS_MINIMAL */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/tinyfish/firmware/inc/pios_usb_board_data.h new file mode 100644 index 0000000000..36342d32d7 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/pios_usb_board_data.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 +#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 4 + +#include /* USB_* macros */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_TINYFISH, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/tinyfish/firmware/main.cpp b/flight/targets/boards/tinyfish/firmware/main.cpp new file mode 100644 index 0000000000..d1ebd36253 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/main.cpp @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @brief These files are the core system files for LibrePilot. + * They are the ground layer just above PiOS. In practice, LibrePilot actually starts + * in the main() function of main.cpp + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @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 + */ + +extern "C" { +#include "inc/openpilot.h" +#include +/* Task Priorities */ + +/* Global Variables */ +extern void Stack_Change(void); +} /* extern "C" */ + +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/firmware/pios_board.c b/flight/targets/boards/tinyfish/firmware/pios_board.c new file mode 100644 index 0000000000..5535c0f05d --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/pios_board.c @@ -0,0 +1,291 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the tinyFISH board. + *****************************************************************************/ +/* + * 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 "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif + +void FlightBatterySettingsDataOverrideDefaults(FlightBatterySettingsData *data) +{ + data->SensorCalibrations.VoltageFactor = 8.8f; + data->SensorCalibrations.CurrentFactor = 0.07f; +} + +void RevoSettingsDataOverrideDefaults(RevoSettingsData *data) +{ + /* This board has no barometer, so adjust default fusion algorithm to one that does not depend on working baro */ + data->FusionAlgorithm = REVOSETTINGS_FUSIONALGORITHM_ACRONOSENSORS; +} + +static HwTinyFISHSettingsData boardHwSettings; + +static void hwTinyFISHSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HwTinyFISHSettingsData currentBoardHwSettings; + + HwTinyFISHSettingsGet(¤tBoardHwSettings); + + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwTinyFISHSettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + } +} + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ + +void PIOS_Board_Init(void) +{ +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(pios_board_info_blob.board_rev); + PIOS_DEBUG_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the mpu6000 */ + + if (PIOS_SPI_Init(&pios_spi1_id, &pios_spi1_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_SPI_Init(&pios_spi2_id, &pios_spi2_cfg)) { + PIOS_DEBUG_Assert(0); + } +#endif + +#if defined(PIOS_INCLUDE_FLASH) + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + + // initialize the internal settings storage flash + if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_system_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + + /* init SPI flash here */ + +#endif /* if defined(PIOS_INCLUDE_FLASH) */ + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + SETTINGS_INITIALISE_ALL; + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + HwTinyFISHSettingsSetDefaults(HwTinyFISHSettingsHandle(), 0); + + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); +// PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_15_cfg); +// PIOS_TIM_InitClock(&tim_16_cfg); +// PIOS_TIM_InitClock(&tim_17_cfg); + +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + HwTinyFISHSettingsConnectCallback(hwTinyFISHSettingsUpdatedCb); + + HwTinyFISHSettingsGet(&boardHwSettings); + + static const PIOS_BOARD_IO_UART_Function uart_function_map[] = { + [HWTINYFISHSETTINGS_UART3PORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWTINYFISHSETTINGS_UART3PORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWTINYFISHSETTINGS_UART3PORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWTINYFISHSETTINGS_UART3PORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok. + [HWTINYFISHSETTINGS_UART3PORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWTINYFISHSETTINGS_UART3PORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWTINYFISHSETTINGS_UART3PORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWTINYFISHSETTINGS_UART3PORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWTINYFISHSETTINGS_UART3PORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWTINYFISHSETTINGS_UART3PORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWTINYFISHSETTINGS_UART3PORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWTINYFISHSETTINGS_UART3PORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWTINYFISHSETTINGS_UART3PORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + [HWTINYFISHSETTINGS_UART3PORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWTINYFISHSETTINGS_UART3PORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, + }; + + if (boardHwSettings.UART3Port < NELEMENTS(uart_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[2], uart_function_map[boardHwSettings.UART3Port]); + } + +#ifdef PIOS_INCLUDE_PPM + if (boardHwSettings.UART3Port == HWTINYFISHSETTINGS_UART3PORT_PPM) { + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); + } +#endif + if (PIOS_COM_DEBUG) { + PIOS_COM_ChangeBaud(PIOS_COM_DEBUG, 57600); + DEBUG_PRINTF(0, "\r\n\r\ntinyFISH FC booting\r\n"); + } + + /* Configure SBus as primary usart client. + * PIOS_BOARD_IO_Configure_UART() will also lock COM port config so that secondary client cannot mess with settings */ + if (boardHwSettings.UART3Port != HWTINYFISHSETTINGS_UART3PORT_SBUS) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[1], PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED); + } + + /* Configure Frsky SensorHub as secondary client. */ + if (boardHwSettings.UART3Port != HWTINYFISHSETTINGS_UART3PORT_FRSKYSENSORHUB) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[1], PIOS_BOARD_IO_UART_FRSKY_SENSORHUB); + } + + /* and what to do with UART1? There is RX_DEBUG only connected. Maybe make it into configurable ComBridge? */ + if (boardHwSettings.UART1Port == HWTINYFISHSETTINGS_UART1PORT_COMBRIDGE) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[0], PIOS_BOARD_IO_UART_COMBRIDGE); + } + +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCS_RCVR(); +#endif + +#ifdef PIOS_INCLUDE_OPLINKRCVR + PIOS_BOARD_IO_Configure_OPLink_RCVR(); +#endif + +#ifdef PIOS_ENABLE_DEBUG_PINS + PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels); +#else + if (boardHwSettings.UART3Port == HWTINYFISHSETTINGS_UART3PORT_OUTPUTS) { + PIOS_Servo_Init(&pios_servo_uart3_cfg); + } else { + PIOS_Servo_Init(&pios_servo_cfg); + } +#endif /* PIOS_ENABLE_DEBUG_PINS */ + + switch (boardHwSettings.LEDPort) { + case HWTINYFISHSETTINGS_LEDPORT_WS281X: +#if defined(PIOS_INCLUDE_WS2811) + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg); +#endif + break; + default: + break; + } + + PIOS_BOARD_Sensors_Configure(); + + PIOS_LED_On(PIOS_LED_HEARTBEAT); +} + + +/** + * @} + */ diff --git a/flight/targets/boards/tinyfish/pios_board.h b/flight/targets/boards/tinyfish/pios_board.h new file mode 100644 index 0000000000..45e4f69e4b --- /dev/null +++ b/flight/targets/boards/tinyfish/pios_board.h @@ -0,0 +1,215 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the tinyFISH board + * @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_H +#define PIOS_BOARD_H + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#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_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 +#define PIOS_BUZZER_ALARM 2 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 0 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 +extern uint32_t pios_spi1_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi1_id) + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 3 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Current sensor +// PIOS_ADC_PinGet(1) = Battery voltage +// ------------------------- +#define PIOS_DMA_PIN_CONFIG \ + { \ + { GPIOB, GPIO_Pin_0, ADC_Channel_12, false }, /* ADC_2 / Current */ \ + { GPIOB, GPIO_Pin_1, ADC_Channel_1, false }, /* ADC_1 / Voltage */ \ + } + +/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ +/* which is annoying because this then determines the rate at which we generate buffer turnover events */ +/* the objective here is to get enough buffer space to support 100Hz averaging rate */ +#define PIOS_ADC_NUM_CHANNELS 2 +#define PIOS_ADC_MAX_OVERSAMPLING 32 + +// #define PIOS_ADC_USE_TEMP_SENSOR +// #define PIOS_ADC_TEMPERATURE_PIN 4 + +#define PIOS_ADC_CURRENT_PIN 0 +#define PIOS_ADC_VOLTAGE_PIN 1 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// USB +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 + +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_MAX_DEVS 1 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/tinyfish/pios_usb_board_data.c b/flight/targets/boards/tinyfish/pios_usb_board_data.c new file mode 100644 index 0000000000..be0bb584e4 --- /dev/null +++ b/flight/targets/boards/tinyfish/pios_usb_board_data.c @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB 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 + */ + +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ + +static const uint8_t usb_product_id[18] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 't', 0, + 'i', 0, + 'n', 0, + 'y', 0, + 'F', 0, + 'I', 0, + 'S', 0, + 'H', 0, +}; + +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; + + PIOS_SYS_SerialNumberGet((char *)sn); + + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/targets/common/bootloader_updater/Makefile b/flight/targets/common/bootloader_updater/Makefile index fae84250fe..6c895e2e03 100644 --- a/flight/targets/common/bootloader_updater/Makefile +++ b/flight/targets/common/bootloader_updater/Makefile @@ -41,7 +41,7 @@ SRC += $(OPSYSTEM)/main.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(PIOSCOMMON)/pios_led.c ## PIOS Hardware -ifeq ($(MCU),cortex-m3) +ifneq (,$(findstring STM32F10,$(CHIP))) include $(PIOS)/stm32f10x/library.mk ## The standard CMSIS startup @@ -50,16 +50,20 @@ ifeq ($(MCU),cortex-m3) # Set linker-script name depending on selected submodel name LDFLAGS += -T$(LINKER_SCRIPTS_PATH)/link_$(BOARD)_memory.ld LDFLAGS += -T$(LINKER_SCRIPTS_PATH)/link_$(BOARD)_sections.ld -else ifeq ($(MCU),cortex-m4) +else ifneq (,$(findstring STM32F4,$(CHIP))) include $(PIOS)/stm32f4xx/library.mk # Set linker-script name depending on selected submodel name # Using the _compat ld script ensure that the bootloader updated is compiled with irqstack in sram # This allow to be used with bootloader earlier than rel5 LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_APP)) - SRC += $(HWDEFSINC)/firmware/cm3_fault_handlers.c - SRC += $(HWDEFSINC)/firmware/dcc_stdio.c +else ifneq (,$(findstring STM32F30,$(CHIP))) + include $(PIOS)/stm32f30x/library.mk + # Set linker-script name depending on selected submodel name + # Using the _compat ld script ensure that the bootloader updated is compiled with irqstack in sram + # This allow to be used with bootloader earlier than rel5 +# LDFLAGS += $(addprefix -T,$(LINKER_SCRIPTS_APP)) else - $(error Unsupported MCU for BootloaderUpdater: $(MCU)) + $(error Unsupported MCU for BootloaderUpdater: $(CHIP)) endif # List any extra directories to look for include files here. diff --git a/flight/tests/math/Makefile b/flight/tests/math/Makefile index e3ae691da6..b6340d0598 100644 --- a/flight/tests/math/Makefile +++ b/flight/tests/math/Makefile @@ -32,7 +32,10 @@ endif include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk EXTRAINCDIRS += $(TOPDIR) -EXTRAINCDIRS += $(FLIGHT_ROOT_DIR)/flight/libraries/math +EXTRAINCDIRS += $(FLIGHTLIB)/math +EXTRAINCDIRS += $(FLIGHTLIB) EXTRAINCDIRS += $(PIOS)/inc +SRC += $(FLIGHTLIB)/CoordinateConversions.c + include $(FLIGHT_ROOT_DIR)/make/unittest.mk diff --git a/flight/tests/math/coordinateconversiontest.cpp b/flight/tests/math/coordinateconversiontest.cpp new file mode 100644 index 0000000000..168de169a2 --- /dev/null +++ b/flight/tests/math/coordinateconversiontest.cpp @@ -0,0 +1,82 @@ +#include "gtest/gtest.h" +#define __STDC_WANT_DEC_FP__ /* Tell implementation that we want Decimal FP */ + +#include /* printf */ +#include /* abort */ +#include /* memset */ + +extern "C" { +#include +} + +#define epsilon_deg 0.00001f +#define epsilon_int_deg ((int32_t)(epsilon_deg * 1e7)) +#define epsilon_metric 0.2f +#define epsilon_int_metric ((int32_t)(epsilon_metric * 1e4)) + +// To use a test fixture, derive a class from testing::Test. +class CoordinateConversionsTestRaw : public testing::Test {}; + +// ****** convert Lat,Lon,Alt to ECEF ************ +// void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]); + +// ****** convert ECEF to Lat,Lon,Alt ********* +// void ECEF2LLA(const float ECEF[3], int32_t LLA[3]); + +// void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]); + +// ****** Express LLA in a local NED Base Frame and back ******** +// void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]); +// void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]); + +// ****** Express ECEF in a local NED Base Frame and back ******** +// void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]); +// void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3] + +TEST_F(CoordinateConversionsTestRaw, LLA2ECEF) { + int32_t LLAi[3] = { + 419291818, + 125571688, + 50 * 1e4 + }; + int32_t LLAfromECEF[3]; + + float ecef[3]; + + LLA2ECEF(LLAi, ecef); + ECEF2LLA(ecef, LLAfromECEF); + + EXPECT_NEAR(LLAi[0], LLAfromECEF[0], epsilon_int_deg); + EXPECT_NEAR(LLAi[1], LLAfromECEF[1], epsilon_int_deg); + EXPECT_NEAR(LLAi[2], LLAfromECEF[2], epsilon_int_metric); +} + + +TEST_F(CoordinateConversionsTestRaw, LLA2NED) { + int32_t LLAi[3] = { + 419291818, + 125571688, + 50 * 1e4 + }; + int32_t LLAfromNED[3]; + + int32_t HomeLLAi[3] = { + 419291600, + 125571300, + 24 * 1e4 + }; + + float Rne[3][3]; + float baseECEF[3]; + float NED[3]; + + RneFromLLA(HomeLLAi, Rne); + LLA2ECEF(HomeLLAi, baseECEF); + + LLA2Base(LLAi, baseECEF, Rne, NED); + Base2LLA(NED, baseECEF, Rne, LLAfromNED); + + EXPECT_NEAR(LLAi[0], LLAfromNED[0], epsilon_int_deg); + EXPECT_NEAR(LLAi[1], LLAfromNED[1], epsilon_int_deg); + EXPECT_NEAR(LLAi[2], LLAfromNED[2], epsilon_int_metric); +} diff --git a/flight/tests/math/unittest.cpp b/flight/tests/math/mathmishtest.cpp similarity index 100% rename from flight/tests/math/unittest.cpp rename to flight/tests/math/mathmishtest.cpp diff --git a/flight/uavobjects/inc/uavobject.h.template b/flight/uavobjects/inc/uavobject.h.template index 349a01c1d4..d16f14d8c2 100644 --- a/flight/uavobjects/inc/uavobject.h.template +++ b/flight/uavobjects/inc/uavobject.h.template @@ -40,6 +40,7 @@ #ifndef $(NAMEUC)_H #define $(NAMEUC)_H #include +$(INCLUDE) /* Object constants */ #define $(NAMEUC)_OBJID $(OBJIDHEX) #define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST) @@ -71,6 +72,8 @@ typedef struct { */ typedef $(NAME)DataPacked __attribute__((aligned(4))) $(NAME)Data; +void $(NAME)DataOverrideDefaults($(NAME)Data * data); + /* Typesafe Object access functions */ static inline int32_t $(NAME)Get($(NAME)Data * dataOut) { return UAVObjGetData($(NAME)Handle(), dataOut); @@ -94,7 +97,7 @@ static inline int32_t $(NAME)ConnectFastCallback(UAVObjEventCallback cb) { return UAVObjConnectCallback($(NAME)Handle(), cb, EV_MASK_ALL_UPDATES, true); } static inline uint16_t $(NAME)CreateInstance() { - return UAVObjCreateInstance($(NAME)Handle(), &$(NAME)SetDefaults); + return UAVObjCreateInstance($(NAME)Handle()); } static inline void $(NAME)RequestUpdate() { UAVObjRequestUpdate($(NAME)Handle()); diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index 7317e99df1..4831e79dbe 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/flight/uavobjects/inc/uavobjectmanager.h @@ -182,21 +182,27 @@ typedef struct { uint32_t lastQueueErrorID; } UAVObjStats; +typedef struct { + uint32_t id; + UAVObjInitializeCallback init_callback; + uint16_t instance_size; +} __attribute__((packed, aligned(4))) UAVObjType; + int32_t UAVObjInitialize(); void UAVObjGetStats(UAVObjStats *statsOut); void UAVObjClearStats(); -UAVObjHandle UAVObjRegister(uint32_t id, bool isSingleInstance, bool isSettings, bool isPriority, uint32_t num_bytes, UAVObjInitializeCallback initCb); +UAVObjHandle UAVObjRegister(const UAVObjType *type, bool isSingleInstance, bool isSettings, bool isPriority); UAVObjHandle UAVObjGetByID(uint32_t id); uint32_t UAVObjGetID(UAVObjHandle obj); uint32_t UAVObjGetNumBytes(UAVObjHandle obj); uint16_t UAVObjGetNumInstances(UAVObjHandle obj); UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj); -uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback initCb); +uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle); bool UAVObjIsSingleInstance(UAVObjHandle obj); bool UAVObjIsMetaobject(UAVObjHandle obj); bool UAVObjIsSettings(UAVObjHandle obj); bool UAVObjIsPriority(UAVObjHandle obj); -int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn); +int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn, bool create); int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut); uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc); int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId); @@ -208,10 +214,12 @@ int32_t UAVObjDeleteSettings(); int32_t UAVObjSaveMetaobjects(); int32_t UAVObjLoadMetaobjects(); int32_t UAVObjDeleteMetaobjects(); +int32_t UAVObjSetDefaults(UAVObjHandle obj_handle); int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn); int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void *dataIn, uint32_t offset, uint32_t size); int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut); int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offset, uint32_t size); +int32_t UAVObjSetInstanceDefaults(UAVObjHandle obj_handle, uint16_t instId); int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn); int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn, uint32_t offset, uint32_t size); int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void *dataOut); diff --git a/flight/uavobjects/inc/uavobjectprivate.h b/flight/uavobjects/inc/uavobjectprivate.h index 88229f44ec..2177539fdd 100644 --- a/flight/uavobjects/inc/uavobjectprivate.h +++ b/flight/uavobjects/inc/uavobjectprivate.h @@ -112,14 +112,13 @@ struct UAVOMeta { /* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */ struct UAVOData { - struct UAVOBase base; - uint32_t id; + struct UAVOBase base; + const UAVObjType *type; /* this contains id, instance_size and data initializer */ /* * Embed the Meta object as another complete UAVO * inside the payload for this UAVO. */ struct UAVOMeta metaObj; - uint16_t instance_size; } __attribute__((packed, aligned(4))); /* Augmented type for Single Instance Data UAVO */ diff --git a/flight/uavobjects/uavobject.c.template b/flight/uavobjects/uavobject.c.template index c25cb4abf2..70a46cfeb5 100644 --- a/flight/uavobjects/uavobject.c.template +++ b/flight/uavobjects/uavobject.c.template @@ -46,6 +46,10 @@ static UAVObjHandle handle __attribute__((section("__DATA,_uavo_handles"))); static UAVObjHandle handle __attribute__((section("_uavo_handles"))); #endif +#if $(NAMEUC)_ISSETTINGS +SETTINGS_INITCALL($(NAME)Initialize); +#endif + /** * Initialize object. * \return 0 Success @@ -63,14 +67,24 @@ int32_t $(NAME)Initialize(void) return -2; } + static const UAVObjType objType = { + .id = $(NAMEUC)_OBJID, + .instance_size = $(NAMEUC)_NUMBYTES, + .init_callback = &$(NAME)SetDefaults, + }; + // Register object with the object manager - handle = UAVObjRegister($(NAMEUC)_OBJID, - $(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_ISPRIORITY, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults); + handle = UAVObjRegister(&objType, + $(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_ISPRIORITY); // Done return handle ? 0 : -1; } +static inline void DataOverrideDefaults(__attribute__((unused)) $(NAME)Data * data) {} + +void $(NAME)DataOverrideDefaults($(NAME)Data * data) __attribute__((weak, alias("DataOverrideDefaults"))); + /** * Initialize object fields and metadata with the default values. * If a default value is not specified the object fields @@ -84,6 +98,7 @@ void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId) UAVObjGetInstanceData(obj, instId, &data); memset(&data, 0, sizeof($(NAME)Data)); $(INITFIELDS) + $(NAME)DataOverrideDefaults(&data); UAVObjSetInstanceData(obj, instId, &data); // Initialize object metadata to their default values diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 9916af9c0a..2be68fe6d7 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -278,33 +278,30 @@ static struct UAVOData *UAVObjAllocMulti(uint32_t num_bytes) /** * Register and new object in the object manager. - * \param[in] id Unique object ID + * \param[in] pointer to UAVObjType structure that holds Unique object ID, instance size, initialization function * \param[in] isSingleInstance Is this a single instance or multi-instance object * \param[in] isSettings Is this a settings object - * \param[in] numBytes Number of bytes of object data (for one instance) - * \param[in] initCb Default field and metadata initialization function + * \param[in] isPriority * \return Object handle, or NULL if failure. * \return */ -UAVObjHandle UAVObjRegister(uint32_t id, - bool isSingleInstance, bool isSettings, bool isPriority, - uint32_t num_bytes, - UAVObjInitializeCallback initCb) +UAVObjHandle UAVObjRegister(const UAVObjType *type, + bool isSingleInstance, bool isSettings, bool isPriority) { struct UAVOData *uavo_data = NULL; xSemaphoreTakeRecursive(mutex, portMAX_DELAY); /* Don't allow duplicate registrations */ - if (UAVObjGetByID(id)) { + if (UAVObjGetByID(type->id)) { goto unlock_exit; } /* Map the various flags to one of the UAVO types we understand */ if (isSingleInstance) { - uavo_data = UAVObjAllocSingle(num_bytes); + uavo_data = UAVObjAllocSingle(type->instance_size); } else { - uavo_data = UAVObjAllocMulti(num_bytes); + uavo_data = UAVObjAllocMulti(type->instance_size); } if (!uavo_data) { @@ -312,8 +309,7 @@ UAVObjHandle UAVObjRegister(uint32_t id, } /* Fill in the details about this UAVO */ - uavo_data->id = id; - uavo_data->instance_size = num_bytes; + uavo_data->type = type; if (isSettings) { uavo_data->base.flags.isSettings = true; // settings defaults to being sent with priority @@ -325,8 +321,8 @@ UAVObjHandle UAVObjRegister(uint32_t id, UAVObjInitMetaData(&uavo_data->metaObj); /* Initialize object fields and metadata to default values */ - if (initCb) { - initCb((UAVObjHandle)uavo_data, 0); + if (type->init_callback) { + type->init_callback((UAVObjHandle)uavo_data, 0); } /* Always try to load the meta object from flash */ @@ -360,11 +356,11 @@ UAVObjHandle UAVObjGetByID(uint32_t id) // Look for object UAVO_LIST_ITERATE(tmp_obj) - if (tmp_obj->id == id) { + if (tmp_obj->type->id == id) { found_obj = (UAVObjHandle *)tmp_obj; goto unlock_exit; } - if (MetaObjectId(tmp_obj->id) == id) { + if (MetaObjectId(tmp_obj->type->id) == id) { found_obj = (UAVObjHandle *)&(tmp_obj->metaObj); goto unlock_exit; } @@ -391,12 +387,12 @@ uint32_t UAVObjGetID(UAVObjHandle obj_handle) /* We have a meta object, find our containing UAVO */ struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj); - return MetaObjectId(uavo_data->id); + return MetaObjectId(uavo_data->type->id); } else { /* We have a data object, augment our pointer */ struct UAVOData *uavo_data = (struct UAVOData *)uavo_base; - return uavo_data->id; + return uavo_data->type->id; } } @@ -420,7 +416,7 @@ uint32_t UAVObjGetNumBytes(UAVObjHandle obj) /* We have a data object, augment our pointer */ struct UAVOData *uavo = (struct UAVOData *)uavo_base; - instance_size = uavo->instance_size; + instance_size = uavo->type->instance_size; } return instance_size; @@ -478,8 +474,7 @@ uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle) * \param[in] obj The object handle * \return The instance ID or 0 if an error */ -uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, - UAVObjInitializeCallback initCb) +uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle) { PIOS_Assert(obj_handle); @@ -501,8 +496,8 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, } // Initialize instance data - if (initCb) { - initCb(obj_handle, instId); + if (((struct UAVOData *)obj_handle)->type->init_callback) { + ((struct UAVOData *)obj_handle)->type->init_callback(obj_handle, instId); } unlock_exit: @@ -516,9 +511,10 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, * \param[in] obj The object handle * \param[in] instId The instance ID * \param[in] dataIn The byte array + * \param[in] create Create the object if it does not already exist. * \return 0 if success or -1 if failure */ -int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn) +int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn, bool create) { PIOS_Assert(obj_handle); @@ -543,14 +539,15 @@ int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *da instEntry = getInstance(obj, instId); // If the instance does not exist create it and any other instances before it - if (instEntry == NULL) { + if ((instEntry == NULL) && create) { instEntry = createInstance(obj, instId); - if (instEntry == NULL) { - goto unlock_exit; - } } + if (instEntry == NULL) { + goto unlock_exit; + } + // Set the data - memcpy(InstanceData(instEntry), dataIn, obj->instance_size); + memcpy(InstanceData(instEntry), dataIn, obj->type->instance_size); } // Fire event @@ -596,7 +593,7 @@ int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut) goto unlock_exit; } // Pack data - memcpy(dataOut, InstanceData(instEntry), obj->instance_size); + memcpy(dataOut, InstanceData(instEntry), obj->type->instance_size); } rc = 0; @@ -638,7 +635,7 @@ uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc) goto unlock_exit; } // Update crc - crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->instance_size); + crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->type->instance_size); } unlock_exit: @@ -677,7 +674,7 @@ void UAVObjInstanceWriteToLog(UAVObjHandle obj_handle, uint16_t instId) goto unlock_exit; } // Pack data - PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, obj->instance_size, (uint8_t *)InstanceData(instEntry)); + PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, obj->type->instance_size, (uint8_t *)InstanceData(instEntry)); } unlock_exit: @@ -857,6 +854,11 @@ xSemaphoreGiveRecursive(mutex); return rc; } +int32_t UAVObjSetDefaults(UAVObjHandle obj_handle) +{ + return UAVObjSetInstanceDefaults(obj_handle, 0); +} + /** * Set the object data * \param[in] obj The object handle @@ -901,6 +903,42 @@ int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offs return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size); } +int32_t UAVObjSetInstanceDefaults(UAVObjHandle obj_handle, uint16_t instId) +{ + PIOS_Assert(obj_handle); + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + int32_t rc = -1; + + if (IsMetaobject(obj_handle)) { + if (instId != 0) { + goto unlock_exit; + } +// How do we set meta data defaults? +// MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes; + } else { + // Check access level + if (UAVObjReadOnly(obj_handle)) { + goto unlock_exit; + } + + // Set default data + if (((struct UAVOData *)obj_handle)->type->init_callback) { + ((struct UAVOData *)obj_handle)->type->init_callback(obj_handle, instId); + } + } + + // Fire event + sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED); + rc = 0; + +unlock_exit: + xSemaphoreGiveRecursive(mutex); + return rc; +} + /** * Set the data of a specific object instance * \param[in] obj The object handle @@ -940,7 +978,7 @@ int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, goto unlock_exit; } // Set data - memcpy(InstanceData(instEntry), dataIn, obj->instance_size); + memcpy(InstanceData(instEntry), dataIn, obj->type->instance_size); } // Fire event @@ -1000,7 +1038,7 @@ int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, con } // Check for overrun - if ((size + offset) > obj->instance_size) { + if ((size + offset) > obj->type->instance_size) { goto unlock_exit; } @@ -1055,7 +1093,7 @@ int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, goto unlock_exit; } // Set data - memcpy(dataOut, InstanceData(instEntry), obj->instance_size); + memcpy(dataOut, InstanceData(instEntry), obj->type->instance_size); } rc = 0; @@ -1108,7 +1146,7 @@ int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, voi } // Check for overrun - if ((size + offset) > obj->instance_size) { + if ((size + offset) > obj->type->instance_size) { goto unlock_exit; } @@ -1597,7 +1635,7 @@ static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId) } /* Create the actual instance */ - uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size; + uint32_t size = sizeof(struct UAVOMultiInst) + obj->type->instance_size; instEntry = (struct UAVOMultiInst *)pios_malloc(size); if (!instEntry) { return NULL; @@ -1687,7 +1725,7 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, } // Add queue to list - event = (struct ObjectEventEntry *)pios_malloc(sizeof(struct ObjectEventEntry)); + event = (struct ObjectEventEntry *)pios_fastheapmalloc(sizeof(struct ObjectEventEntry)); if (event == NULL) { return -1; } diff --git a/flight/uavobjects/uavobjectpersistence.c b/flight/uavobjects/uavobjectpersistence.c index 4acbd41e5d..0f0395ce7b 100644 --- a/flight/uavobjects/uavobjectpersistence.c +++ b/flight/uavobjects/uavobjectpersistence.c @@ -121,6 +121,8 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId) int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId) { PIOS_Assert(obj_handle); - PIOS_FLASHFS_ObjDelete(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId); + if (PIOS_FLASHFS_ObjDelete(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId) == 0) { + UAVObjSetInstanceDefaults(obj_handle, instId); + } return 0; } diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index e485882efb..04a2e64005 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -61,6 +61,7 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position); int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle); int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle); +int32_t UAVTalkReceiveObjectNoCreate(UAVTalkConnection connectionHandle); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset); void UAVTalkAddStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset); void UAVTalkResetStats(UAVTalkConnection connection); diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index c537fba676..4cdac6b671 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -51,7 +51,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout); static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data); +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, bool create); static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId); // UavTalk Process FSM functions static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); @@ -557,7 +557,28 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) return -1; } - return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer); + return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, true); +} + +/** + * Complete receiving a UAVTalk packet. This will cause the packet to be unpacked, acked, etc. + * This version will not create/unpack an object if it does not already exist. + * \param[in] connectionHandle UAVTalkConnection to be used + * \return 0 Success + * \return -1 Failure + */ +int32_t UAVTalkReceiveObjectNoCreate(UAVTalkConnection connectionHandle) +{ + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + + UAVTalkInputProcessor *iproc = &connection->iproc; + if (iproc->state != UAVTALK_STATE_COMPLETE) { + return -1; + } + + return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, false); } /** @@ -592,7 +613,7 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) * \return 0 Success * \return -1 Failure */ -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data) +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, bool create) { UAVObjHandle obj; int32_t ret = 0; @@ -613,8 +634,8 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, ui case UAVTALK_TYPE_OBJ_TS: // All instances not allowed for OBJ messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { - // Unpack object, if the instance does not exist it will be created! - if (UAVObjUnpack(obj, instId, data) == 0) { + // Unpack object, if create is true and the instance does not exist it will be created! + if (UAVObjUnpack(obj, instId, data, create) == 0) { // Check if this object acks a pending OBJ_REQ message // any OBJ message can ack a pending OBJ_REQ message // even one that was not sent in response to the OBJ_REQ message @@ -632,8 +653,8 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, ui UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId); // All instances not allowed for OBJ_ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { - // Unpack object, if the instance does not exist it will be created! - if (UAVObjUnpack(obj, instId, data) == 0) { + // Unpack object, if create is true and the instance does not exist it will be created! + if (UAVObjUnpack(obj, instId, data, create) == 0) { UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId); // Object updated or created, transmit ACK sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL); diff --git a/ground/gcs/copydata.pro b/ground/gcs/copydata.pro index e2fc81a67d..ca7c8ae478 100644 --- a/ground/gcs/copydata.pro +++ b/ground/gcs/copydata.pro @@ -87,35 +87,11 @@ win32 { Qt5MultimediaWidgets$${DS}.dll \ Qt5Quick$${DS}.dll \ Qt5QuickWidgets$${DS}.dll \ - Qt5Qml$${DS}.dll \ - libicuin57.dll \ - libicudt57.dll \ - libicuuc57.dll \ - libstdc++-6.dll \ - libwinpthread-1.dll \ - libpcre-1.dll \ - libpcre16-0.dll \ - zlib1.dll \ - libharfbuzz-0.dll \ - libgraphite2.dll \ - libfreetype-6.dll \ - libbz2-1.dll \ - libpng16-16.dll \ - libjpeg-8.dll \ - libglib-2.0-0.dll \ - libintl-8.dll \ - libiconv-2.dll - - contains(QT_ARCH, i386) { - QT_DLLS += \ - libgcc_s_dw2-1.dll - } else { - QT_DLLS += \ - libgcc_s_seh-1.dll - } + Qt5Qml$${DS}.dll for(dll, QT_DLLS) { addCopyFileTarget($${dll},$$[QT_INSTALL_BINS],$${GCS_APP_PATH}) + addCopyDependenciesTarget($${dll},$$[QT_INSTALL_BINS],$${GCS_APP_PATH}) } # copy OpenSSL DLLs @@ -141,6 +117,7 @@ win32 { for(plugin, QT_PLUGINS) { addCopyFileTarget($${plugin},$$[QT_INSTALL_PLUGINS],$${GCS_QT_PLUGINS_PATH}) + win32:addCopyDependenciesTarget($${plugin},$$[QT_INSTALL_PLUGINS],$${GCS_APP_PATH}) } # Copy QtQuick2 complete directories diff --git a/ground/gcs/gcs.pri b/ground/gcs/gcs.pri index cb25cd54f8..f36f6290ea 100644 --- a/ground/gcs/gcs.pri +++ b/ground/gcs/gcs.pri @@ -33,19 +33,46 @@ defineTest(addCopyFileTarget) { src = $$2/$$1 dest = $$3/$$1 - $${file}.target = $$dest - $${file}.depends = $$src + target = $${file} + $${target}.target = $$dest + $${target}.depends = $$src # create directory. Better would be an order only dependency - $${file}.commands = -@$(MKDIR) \"$$dirname(dest)\" $$addNewline() - $${file}.commands += $(COPY_FILE) \"$$src\" \"$$dest\" + $${target}.commands = -@$(MKDIR) \"$$dirname(dest)\" $$addNewline() + $${target}.commands += $(COPY_FILE) \"$$src\" \"$$dest\" - QMAKE_EXTRA_TARGETS += $$file - POST_TARGETDEPS += $$eval($${file}.target) + QMAKE_EXTRA_TARGETS += $$target + POST_TARGETDEPS += $$eval($${target}.target) - export($${file}.target) - export($${file}.depends) - export($${file}.commands) + export($${target}.target) + export($${target}.depends) + export($${target}.commands) + export(QMAKE_EXTRA_TARGETS) + export(POST_TARGETDEPS) + + return(true) +} + +defineTest(addCopyDependenciesTarget) { + file = $$1 + src = $$2/$$1 + dest = $$3 + + target_file = $${OUT_PWD}/deps/$${file}.deps + + target = $${file}_deps + $${target}.target = $$target_file + $${target}.depends = $$src + + $${target}.commands = -@$(MKDIR) \"$$dirname(target_file)\" $$addNewline() + $${target}.commands += $$(PYTHON) $$(ROOT_DIR)/make/copy_dependencies.py --dest \"$$dest\" --files \"$$src\" --excludes OPENGL32.DLL > \"$$target_file\" + + QMAKE_EXTRA_TARGETS += $$target + POST_TARGETDEPS += $$eval($${target}.target) + + export($${target}.target) + export($${target}.depends) + export($${target}.commands) export(QMAKE_EXTRA_TARGETS) export(POST_TARGETDEPS) @@ -57,22 +84,23 @@ defineTest(addCopyDirTarget) { src = $$2/$$1 dest = $$3/$$1 - $${dir}.target = $$dest - $${dir}.depends = $$src + target = $${dir} + $${target}.target = $$dest + $${target}.depends = $$src # Windows does not update directory timestamp if files are modified - win32: $${dir}.depends += FORCE + win32:$${target}depends += FORCE - $${dir}.commands = @rm -rf \"$$dest\" $$addNewline() + $${target}.commands = @rm -rf \"$$dest\" $$addNewline() # create directory. Better would be an order only dependency - $${dir}.commands += -@$(MKDIR) \"$$dirname(dest)\" $$addNewline() - $${dir}.commands += $(COPY_DIR) \"$$src\" \"$$dest\" + $${target}.commands += -@$(MKDIR) \"$$dirname(dest)\" $$addNewline() + $${target}.commands += $(COPY_DIR) \"$$src\" \"$$dest\" - QMAKE_EXTRA_TARGETS += $$dir - POST_TARGETDEPS += $$eval($${dir}.target) + QMAKE_EXTRA_TARGETS += $$target + POST_TARGETDEPS += $$eval($${target}.target) - export($${dir}.target) - export($${dir}.depends) - export($${dir}.commands) + export($${target}.target) + export($${target}.depends) + export($${target}.commands) export(QMAKE_EXTRA_TARGETS) export(POST_TARGETDEPS) diff --git a/ground/gcs/src/app/main.cpp b/ground/gcs/src/app/main.cpp index 9644bea5cb..b5a42b1ca0 100644 --- a/ground/gcs/src/app/main.cpp +++ b/ground/gcs/src/app/main.cpp @@ -2,7 +2,7 @@ ****************************************************************************** * * @file main.cpp - * @author The LibrePilot Team http://www.librepilot.org Copyright (C) 2015. + * @author The LibrePilot Team http://www.librepilot.org Copyright (C) 2017. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. * @brief @@ -82,17 +82,16 @@ */ #include "qtsingleapplication.h" -#include "utils/xmlconfig.h" +#include "gcssplashscreen.h" #include "utils/pathutils.h" +#include "utils/settingsutils.h" #include "utils/filelogger.h" -#include "gcssplashscreen.h" #include #include #include #include -#include #include #include #include @@ -101,12 +100,9 @@ #include #include #include -#include #include -#include -#include -#include +#include #include typedef QList PluginSpecSet; @@ -284,9 +280,13 @@ void systemInit() QSurfaceFormat format = QSurfaceFormat::defaultFormat(); format.setSwapInterval(0); QSurfaceFormat::setDefaultFormat(format); + + // see https://bugreports.qt.io/browse/QTBUG-40332 + int timeout = std::numeric_limits::max(); + qputenv("QT_BEARER_POLL_TIMEOUT", QString::number(timeout).toLatin1()); } -static FileLogger *logger; +static FileLogger *logger = NULL; void mainMessageOutput(QtMsgType type, const QMessageLogContext &context, const QString &msg) { @@ -295,14 +295,27 @@ void mainMessageOutput(QtMsgType type, const QMessageLogContext &context, const Q_DECLARE_METATYPE(QtMsgType) -void logInit(QString fileName) +void logInit(QString &fileName) { qRegisterMetaType(); - qInstallMessageHandler(mainMessageOutput); - logger = new FileLogger(); - if (!logger->start(fileName)) { + logger = new FileLogger(fileName); + if (!logger->start()) { + delete logger; + logger = NULL; displayError(msgLogfileOpenFailed(fileName)); + return; + } + qInstallMessageHandler(mainMessageOutput); +} + +void logDeinit() +{ + if (!logger) { + return; } + qInstallMessageHandler(0); + delete logger; + logger = NULL; } AppOptions options() @@ -337,78 +350,6 @@ AppOptionValues parseCommandLine(SharedTools::QtSingleApplication &app, return appOptionValues; } -void loadFactoryDefaults(QSettings &settings, AppOptionValues &appOptionValues) -{ - QDir directory(Utils::GetDataPath() + QString("configurations")); - - qDebug() << "Looking for factory defaults configuration files in:" << directory.absolutePath(); - - QString fileName; - - // check if command line option -config-file contains a file name - QString commandLine = appOptionValues.value(CONFIG_FILE_OPTION); - if (!commandLine.isEmpty()) { - QFileInfo fi(commandLine); - if (fi.isRelative()) { - // file name specified on command line has a relative path - commandLine = directory.absolutePath() + QDir::separator() + commandLine; - } - if (QFile::exists(commandLine)) { - fileName = commandLine; - qDebug() << "Configuration file" << fileName << "specified on command line will be loaded."; - } else { - qWarning() << "Configuration file" << commandLine << "specified on command line does not exist."; - } - } - - if (fileName.isEmpty()) { - // check default file - if (QFile::exists(directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME)) { - // use default file name - fileName = directory.absolutePath() + QDir::separator() + DEFAULT_CONFIG_FILENAME; - qDebug() << "Default configuration file" << fileName << "will be loaded."; - } else { - qWarning() << "No default configuration file found in" << directory.absolutePath(); - } - } - - if (fileName.isEmpty()) { - // TODO should we exit violently? - qCritical() << "No default configuration file found!"; - return; - } - - // create settings from file - QSettings qs(fileName, XmlConfig::XmlSettingsFormat); - - // transfer loaded settings to application settings - QStringList keys = qs.allKeys(); - foreach(QString key, keys) { - settings.setValue(key, qs.value(key)); - } - - qDebug() << "Configuration file" << fileName << "was loaded."; -} - -void overrideSettings(QSettings &settings, int argc, char * *argv) -{ - // Options like -D My/setting=test - QRegExp rx("([^=]+)=(.*)"); - - for (int i = 0; i < argc; ++i) { - if (CONFIG_OPTION == QString(argv[i])) { - if (rx.indexIn(argv[++i]) > -1) { - QString key = rx.cap(1); - QString value = rx.cap(2); - qDebug() << "User setting" << key << "set to value" << value; - settings.setValue(key, value); - } - } - } - - settings.sync(); -} - void loadTranslators(QString language, QTranslator &translator, QTranslator &qtTranslator) { const QString &creatorTrPath = Utils::GetDataPath() + QLatin1String("translations"); @@ -430,21 +371,17 @@ void loadTranslators(QString language, QTranslator &translator, QTranslator &qtT } } -int main(int argc, char * *argv) +int runApplication(int argc, char * *argv) { QElapsedTimer timer; timer.start(); - // low level init - systemInit(); - // create application SharedTools::QtSingleApplication app(APP_NAME, argc, argv); QCoreApplication::setApplicationName(APP_NAME); QCoreApplication::setOrganizationName(ORG_NAME); - QSettings::setDefaultFormat(XmlConfig::XmlSettingsFormat); // initialize the plugin manager ExtensionSystem::PluginManager pluginManager; @@ -452,7 +389,7 @@ int main(int argc, char * *argv) pluginManager.setPluginPaths(Utils::GetPluginPaths()); // parse command line - qDebug() << "Command line" << app.arguments(); + qDebug() << "main - command line" << app.arguments(); QString errorMessage; AppOptionValues appOptionValues = parseCommandLine(app, pluginManager, errorMessage); if (!errorMessage.isEmpty()) { @@ -463,39 +400,32 @@ int main(int argc, char * *argv) return -1; } + // start logging to file if requested if (appOptionValues.contains(LOG_FILE_OPTION)) { QString logFileName = appOptionValues.value(LOG_FILE_OPTION); logInit(logFileName); // relog command line arguments for the benefit of the file logger... - qDebug() << "Command line" << app.arguments(); + qDebug() << "main - command line" << app.arguments(); } + // init settings + Utils::initSettings(appOptionValues.value(CONFIG_FILE_OPTION)); + // load user settings - // Must be done before any QSettings class is created - // keep this in sync with the MainWindow ctor in coreplugin/mainwindow.cpp - QString settingsPath = Utils::GetDataPath(); - qDebug() << "Loading system settings from" << settingsPath; - QSettings::setPath(XmlConfig::XmlSettingsFormat, QSettings::SystemScope, settingsPath); QSettings settings; - qDebug() << "Loading user settings from" << settings.fileName(); + qDebug() << "main - loading user settings from" << settings.fileName(); - // need to reset all user settings? - if (appOptionValues.contains(RESET_OPTION)) { - qDebug() << "Resetting user settings!"; - settings.clear(); - } - - // check if we have user settings - if (!settings.allKeys().count()) { - // no user settings, load the factory defaults - qDebug() << "No user settings found, loading factory defaults..."; - loadFactoryDefaults(settings, appOptionValues); + // need to reset user settings? + if (settings.allKeys().isEmpty() || appOptionValues.contains(RESET_OPTION)) { + qDebug() << "main - resetting user settings"; + Utils::resetToFactoryDefaults(settings); } + Utils::mergeFactoryDefaults(settings); // override settings with command line provided values // take notice that the overridden values will be saved in the user settings and will continue to be effective // in subsequent GCS runs - overrideSettings(settings, argc, argv); + Utils::overrideSettings(settings, argc, argv); // initialize GCS locale // use the value defined by the General/Locale setting or default to system Locale. @@ -549,8 +479,8 @@ int main(int argc, char * *argv) } if (!coreplugin) { QString nativePaths = QDir::toNativeSeparators(Utils::GetPluginPaths().join(QLatin1String(","))); - const QString reason = QCoreApplication::translate("Application", "Could not find 'Core.pluginspec' in %1").arg( - nativePaths); + const QString reason = QCoreApplication::translate("Application", "Could not find '%1.pluginspec' in %2") + .arg(CORE_PLUGIN_NAME).arg(nativePaths); displayError(msgCoreLoadFailure(reason)); return 1; } @@ -581,19 +511,17 @@ int main(int argc, char * *argv) return 1; } - { - QStringList errors; - foreach(ExtensionSystem::PluginSpec * p, pluginManager.plugins()) { - if (p->hasError()) { - errors.append(p->errorString()); - } - } - if (!errors.isEmpty()) { - QMessageBox::warning(0, - QCoreApplication::translate("Application", "%1 - Plugin loader messages").arg(GCS_BIG_NAME), - errors.join(QString::fromLatin1("\n\n"))); + QStringList errors; + foreach(ExtensionSystem::PluginSpec * p, pluginManager.plugins()) { + if (p->hasError()) { + errors.append(p->errorString()); } } + if (!errors.isEmpty()) { + QMessageBox::warning(0, + QCoreApplication::translate("Application", "%1 - Plugin loader messages").arg(GCS_BIG_NAME), + errors.join(QString::fromLatin1("\n\n"))); + } if (isFirstInstance) { // Set up lock and remote arguments for the first instance only. @@ -612,15 +540,22 @@ int main(int argc, char * *argv) delete splash; } - qDebug() << "main - main took" << timer.elapsed() << "ms"; - + qDebug() << "main - starting GCS took" << timer.elapsed() << "ms"; int ret = app.exec(); - qDebug() << "main - GCS ran for" << timer.elapsed() << "ms"; - if (logger) { - delete logger; - } + return ret; +} + +int main(int argc, char * *argv) +{ + // low level init + systemInit(); + + int ret = runApplication(argc, argv); + + // close log file if needed + logDeinit(); return ret; } diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/common.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/common.h index 28cc825b19..f0a7dfacaa 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/common.h +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/common.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file common.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -23,10 +24,11 @@ * 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 COMMON_H + */ +#ifndef COMMON_H #define COMMON_H -enum decodeState_ { +enum DecodeState { decode_len1_e = 0, decode_seqNo_e, decode_data_e, @@ -34,10 +36,10 @@ enum decodeState_ { decode_crc2_e, decode_idle_e }; + enum ReceiveState { state_escaped_e = 0, state_unescaped_e }; - #endif // COMMON_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp index 41891dbf9b..1b142d7e36 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/main.cpp @@ -1,20 +1,51 @@ -#include -#include "../../../libs/qextserialport/src/qextserialport.h" -#include -#include +/** + ****************************************************************************** + * + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Uploader Serial and USB Uploader Plugin + * @{ + * @brief The USB and Serial protocol uploader plugin + *****************************************************************************/ +/* + * 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 "qssp.h" #include "port.h" #include "qsspt.h" -int main(int argc, char *argv[]) -{ + +#include "../../../libs/qextserialport/src/qextserialport.h" + +#include +#include +#include + #define MAX_PACKET_DATA_LEN 255 #define MAX_PACKET_BUF_SIZE (1 + 1 + 255 + 2) +int main(int argc, char *argv[]) +{ uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; port *info; PortSettings settings; + settings.BaudRate = BAUD57600; settings.DataBits = DATA_8; settings.FlowControl = FLOW_OFF; diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp index 961baeaa84..23700fb46a 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file port.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -25,15 +26,15 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "port.h" -#include "delay.h" + +#include #include -port::port(QString name, bool debug) : debug(debug), mstatus(port::closed) + +port::port(QString name, bool debug) : mstatus(port::closed), debug(debug) { timer.start(); - sport = new QSerialPort(); - sport->setPortName(name); + sport = new QSerialPort(name, this); if (sport->open(QIODevice::ReadWrite)) { - sport->setReadBufferSize(0); if (sport->setBaudRate(QSerialPort::Baud57600) && sport->setDataBits(QSerialPort::Data8) && sport->setParity(QSerialPort::NoParity) @@ -41,7 +42,6 @@ port::port(QString name, bool debug) : debug(debug), mstatus(port::closed) && sport->setFlowControl(QSerialPort::NoFlowControl)) { mstatus = port::open; } - // sport->setDtr(); } else { qDebug() << sport->error(); mstatus = port::error; @@ -62,6 +62,7 @@ int16_t port::pfSerialRead(void) { char c[1]; + // TODO why the wait ? (gcs uploader dfu does not have it) sport->waitForBytesWritten(1); if (sport->bytesAvailable() || sport->waitForReadyRead(0)) { sport->read(c, 1); @@ -72,7 +73,9 @@ int16_t port::pfSerialRead(void) } rxDebugBuff.append(c[0]); } - } else { return -1; } + } else { + return -1; + } return (uint8_t)c[0]; } @@ -89,6 +92,7 @@ void port::pfSerialWrite(uint8_t c) } txDebugBuff.append(cc[0]); } + // TODO why the wait ? (gcs uploader dfu does not have it) sport->waitForBytesWritten(1); } diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h index 16cf65579e..01203f748d 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/port.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file port.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -26,19 +27,30 @@ */ #ifndef PORT_H #define PORT_H -#include -#include -#include -#include -#include + #include "common.h" -class port { +#include +#include + +#include + +class QSerialPort; + +class port : public QObject { + Q_OBJECT; + public: enum portstatus { open, closed, error }; + + port(QString name, bool debug); + virtual ~port(); + portstatus status(); + virtual int16_t pfSerialRead(void); // function to read a character from the serial input stream virtual void pfSerialWrite(uint8_t); // function to write a byte to be sent out the serial port virtual uint32_t pfGetTime(void); + uint8_t retryCount; // how many times have we tried to transmit the 'send' packet uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet uint16_t max_retry; // Maximum number of retrys for a single transmit. @@ -54,25 +66,25 @@ class port { uint8_t *rxBuf; // receive buffer. Used to store data as a packet is received. uint16_t sendSynch; // flag to indicate that we should send a synchronize packet to the host // this is required when switching from the application to the bootloader - // and vice-versa. This fixes the firwmare download timeout. - // when this flag is set to true, the next time we send a packet we will first // send a synchronize packet. - ReceiveState InputState; - decodeState_ DecodeState; + // and vice-versa. This fixes the firmware download timeout. + // when this flag is set to true, the next time we send a packet we will first + // send a synchronize packet. + ReceiveState inputState; + DecodeState decodeState; uint16_t SendState; uint16_t crc; uint32_t RxError; uint32_t TxError; uint16_t flags; - port(QString name, bool debug); - virtual ~port(); - portstatus status(); + private: - bool debug; - QByteArray rxDebugBuff; - QByteArray txDebugBuff; portstatus mstatus; QTime timer; QSerialPort *sport; + + bool debug; + QByteArray rxDebugBuff; + QByteArray txDebugBuff; }; #endif // PORT_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp index 66ce3588ea..7381791c61 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file qssp.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -26,12 +27,12 @@ */ #include "qssp.h" +#include #include #include #include - /** PRIVATE DEFINITIONS **/ #define SYNC 225 // Sync character used in Serial Protocol #define ESC 224 // ESC character used in Serial Protocol @@ -143,8 +144,8 @@ void qssp::ssp_Init(const PortConfig_t *const info) thisport->rxSeqNo = 255; thisport->txSeqNo = 255; thisport->SendState = SSP_IDLE; - thisport->InputState = (ReceiveState)0; - thisport->DecodeState = (decodeState_)0; + thisport->inputState = (ReceiveState)0; + thisport->decodeState = (DecodeState)0; thisport->TxError = 0; thisport->RxError = 0; thisport->txSeqNo = 0; @@ -174,7 +175,7 @@ int16_t qssp::ssp_SendProcess() sf_SetSendTimeout(); value = SSP_TX_WAITING; } else { - // Give up, # of trys has exceded the limit + // Give up, # of tries has exceeded the limit value = SSP_TX_TIMEOUT; CLEARBIT(thisport->flags, ACK_RECEIVED); thisport->SendState = SSP_IDLE; @@ -258,6 +259,7 @@ uint16_t qssp::ssp_SendDataBlock(uint8_t *data, uint16_t length) uint16_t retval = FALSE; packet_status = ssp_SendData(data, length); // send the data + // TODO the loop is non blocking and will spin as fast as the CPU allows while (packet_status == SSP_TX_WAITING) { // check the status (void)ssp_ReceiveProcess(); // process any bytes received. packet_status = ssp_SendProcess(); // check the send status @@ -343,7 +345,7 @@ int16_t qssp::ssp_SendData(const uint8_t *data, const uint16_t length) } /*! - * \brief Attempts to synchronize the sequence numbers with the other end of the connectin. + * \brief Attempts to synchronize the sequence numbers with the other end of the connection. * \param thisport = which port to use * \return true = success * \return false = failed to receive an ACK to our synch request @@ -374,7 +376,7 @@ uint16_t qssp::ssp_Synchronise() packet_status = ssp_SendData(NULL, 0); #endif while (packet_status == SSP_TX_WAITING) { // we loop until we time out. - (void)ssp_ReceiveProcess(); // do the receive process + ssp_ReceiveProcess(); // do the receive process packet_status = ssp_SendProcess(); // do the send process } thisport->sendSynch = FALSE; @@ -391,7 +393,6 @@ uint16_t qssp::ssp_Synchronise() retval = FALSE; break; } - ; return retval; } @@ -417,7 +418,6 @@ void qssp::sf_SendPacket() thisport->retryCount++; } - /*! * \brief converts data to transport layer protocol packet format. * \param txbuf = buffer to use when forming the packet @@ -603,20 +603,20 @@ int16_t qssp::sf_ReceiveState(uint8_t c) { int16_t retval = SSP_RX_RECEIVING; - switch (thisport->InputState) { + switch (thisport->inputState) { case state_unescaped_e: if (c == SYNC) { - thisport->DecodeState = decode_len1_e; + thisport->decodeState = decode_len1_e; } else if (c == ESC) { - thisport->InputState = state_escaped_e; + thisport->inputState = state_escaped_e; } else { retval = sf_DecodeState(c); } break; // end of unescaped state. case state_escaped_e: - thisport->InputState = state_unescaped_e; + thisport->inputState = state_unescaped_e; if (c == SYNC) { - thisport->DecodeState = decode_len1_e; + thisport->decodeState = decode_len1_e; } else if (c == ESC_SYNC) { retval = sf_DecodeState(SYNC); } else { @@ -653,20 +653,26 @@ int16_t qssp::sf_DecodeState(uint8_t c) { int16_t retval; - switch (thisport->DecodeState) { + switch (thisport->decodeState) { case decode_idle_e: // 'c' is ignored in this state as the only way to leave the idle state is // recognition of the SYNC byte in the sf_ReceiveState function. retval = SSP_RX_IDLE; break; case decode_len1_e: + if (c == 0) { + thisport->decodeState = decode_idle_e; + thisport->RxError++; + retval = SSP_RX_IDLE; + break; + } thisport->rxBuf[LENGTH] = c; thisport->rxBufLen = c; if (thisport->rxBufLen <= thisport->rxBufSize) { - thisport->DecodeState = decode_seqNo_e; + thisport->decodeState = decode_seqNo_e; retval = SSP_RX_RECEIVING; } else { - thisport->DecodeState = decode_idle_e; + thisport->decodeState = decode_idle_e; retval = SSP_RX_IDLE; } break; @@ -678,9 +684,9 @@ int16_t qssp::sf_DecodeState(uint8_t c) thisport->crc = sf_crc16(thisport->crc, c); if (thisport->rxBufLen > 0) { - thisport->DecodeState = decode_data_e; + thisport->decodeState = decode_data_e; } else { - thisport->DecodeState = decode_crc1_e; + thisport->decodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; @@ -688,17 +694,17 @@ int16_t qssp::sf_DecodeState(uint8_t c) thisport->rxBuf[(thisport->rxBufPos)++] = c; thisport->crc = sf_crc16(thisport->crc, c); if (thisport->rxBufPos == (thisport->rxBufLen + 2)) { - thisport->DecodeState = decode_crc1_e; + thisport->decodeState = decode_crc1_e; } retval = SSP_RX_RECEIVING; break; case decode_crc1_e: thisport->crc = sf_crc16(thisport->crc, c); - thisport->DecodeState = decode_crc2_e; + thisport->decodeState = decode_crc2_e; retval = SSP_RX_RECEIVING; break; case decode_crc2_e: - thisport->DecodeState = decode_idle_e; + thisport->decodeState = decode_idle_e; // verify the CRC value for the packet if (sf_crc16(thisport->crc, c) == 0) { // TODO shouldn't the return value of sf_ReceivePacket() be checked? @@ -710,7 +716,7 @@ int16_t qssp::sf_DecodeState(uint8_t c) } break; default: - thisport->DecodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. + thisport->decodeState = decode_idle_e; // unknown state so reset to idle state and wait for the next start of a packet. retval = SSP_RX_IDLE; break; } @@ -794,6 +800,7 @@ int16_t qssp::sf_ReceivePacket() } return value; } + qssp::qssp(port *info, bool debug) : debug(debug) { thisport = info; @@ -808,13 +815,14 @@ qssp::qssp(port *info, bool debug) : debug(debug) thisport->rxSeqNo = 255; thisport->txSeqNo = 255; thisport->SendState = SSP_IDLE; - thisport->InputState = (ReceiveState)0; - thisport->DecodeState = (decodeState_)0; + thisport->inputState = (ReceiveState)0; + thisport->decodeState = (DecodeState)0; thisport->TxError = 0; thisport->RxError = 0; thisport->txSeqNo = 0; thisport->rxSeqNo = 0; } + void qssp::pfCallBack(uint8_t *buf, uint16_t size) { Q_UNUSED(size); diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.h index 0ffd9af47c..1a7f7a8f91 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.h +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qssp.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file qssp.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -26,9 +27,12 @@ */ #ifndef QSSP_H #define QSSP_H -#include -#include "port.h" + #include "common.h" +#include "port.h" + +#include + /** LOCAL DEFINITIONS **/ #ifndef TRUE #define TRUE 1 @@ -39,6 +43,7 @@ #endif #define SPP_USES_CRC + #define SSP_TX_IDLE 0 // not expecting a ACK packet (no current transmissions in progress) #define SSP_TX_WAITING 1 // waiting for a valid ACK to arrive #define SSP_TX_TIMEOUT 2 // failed to receive a valid ACK in the timeout period, after retrying. @@ -56,7 +61,6 @@ #define SSP_RX_ACK 6 #define SSP_RX_SYNCH 7 - typedef struct { uint8_t *pbuff; uint16_t length; @@ -74,42 +78,41 @@ typedef struct { // function returns time in number of seconds that has elapsed from a given reference point } PortConfig_t; - /** Public Data **/ - /** EXTERNAL FUNCTIONS **/ class qssp { private: port *thisport; - decodeState_ DecodeState_t; - /** PRIVATE FUNCTIONS **/ + bool debug; + // static void sf_SendSynchPacket( Port_t *thisport ); - uint16_t sf_crc16(uint16_t crc, uint8_t data); + uint16_t sf_crc16(uint16_t crc, uint8_t data); void sf_write_byte(uint8_t c); void sf_SetSendTimeout(); - uint16_t sf_CheckTimeout(); + uint16_t sf_CheckTimeout(); int16_t sf_DecodeState(uint8_t c); int16_t sf_ReceiveState(uint8_t c); void sf_SendPacket(); void sf_SendAckPacket(uint8_t seqNumber); - void sf_MakePacket(uint8_t *buf, const uint8_t *pdata, uint16_t length, uint8_t seqNo); + void sf_MakePacket(uint8_t *buf, const uint8_t *pdata, uint16_t length, uint8_t seqNo); int16_t sf_ReceivePacket(); - uint16_t ssp_SendDataBlock(uint8_t *data, uint16_t length); - bool debug; + uint16_t ssp_SendDataBlock(uint8_t *data, uint16_t length); + public: - /** PUBLIC FUNCTIONS **/ - virtual void pfCallBack(uint8_t *, uint16_t); // call back function that is called when a full packet has been received + qssp(port *info, bool debug); + int16_t ssp_ReceiveProcess(); int16_t ssp_SendProcess(); uint16_t ssp_SendString(char *str); int16_t ssp_SendData(const uint8_t *data, const uint16_t length); void ssp_Init(const PortConfig_t *const info); - int16_t ssp_ReceiveByte(); + int16_t ssp_ReceiveByte(); uint16_t ssp_Synchronise(); - qssp(port *info, bool debug); + + virtual void pfCallBack(uint8_t *, uint16_t); // call back function that is called when a full packet has been received }; #endif // QSSP_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.cpp index 564e758055..d9fe482616 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.cpp +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file qsspt.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -26,17 +27,27 @@ */ #include "qsspt.h" +#include + qsspt::qsspt(port *info, bool debug) : qssp(info, debug), endthread(false), datapending(false), debug(debug) {} +qsspt::~qsspt() +{ + endthread = true; + // TODO bad... + wait(1000); +} + void qsspt::run() { + // TODO the loop is non blocking and will spin as fast as the CPU allows while (!endthread) { - receivestatus = this->ssp_ReceiveProcess(); - sendstatus = this->ssp_SendProcess(); + receivestatus = ssp_ReceiveProcess(); + sendstatus = ssp_SendProcess(); sendbufmutex.lock(); if (datapending && receivestatus == SSP_TX_IDLE) { - this->ssp_SendData(mbuf, msize); + ssp_SendData(mbuf, msize); datapending = false; } sendbufmutex.unlock(); @@ -45,13 +56,11 @@ void qsspt::run() } } } + bool qsspt::sendData(uint8_t *buf, uint16_t size) { if (debug) { - QByteArray data; - for (int i = 0; i < size; i++) { - data.append((uint8_t)buf[i]); - } + QByteArray data((const char *)buf, size); qDebug() << "SSP TX " << data.toHex(); } if (datapending) { @@ -62,6 +71,8 @@ bool qsspt::sendData(uint8_t *buf, uint16_t size) mbuf = buf; msize = size; sendbufmutex.unlock(); + // TODO why do we wait 10 seconds ? why do we then ignore the timeout ? + // There is a ssp_SendDataBlock method... msendwait.lock(); sendwait.wait(&msendwait, 10000); msendwait.unlock(); @@ -71,21 +82,19 @@ bool qsspt::sendData(uint8_t *buf, uint16_t size) void qsspt::pfCallBack(uint8_t *buf, uint16_t size) { if (debug) { - qDebug() << "receive callback" << buf[0] << buf[1] << buf[2] << buf[3] << buf[4] << "array size=" << queue.count(); - } - QByteArray array; - for (int x = 0; x < size; x++) { - array.append(buf[x]); + qDebug() << "receive callback" << buf[0] << buf[1] << buf[2] << buf[3] << buf[4] << "queue size=" << queue.count(); } + QByteArray data((const char *)buf, size); mutex.lock(); - queue.enqueue(array); - // queue.enqueue((const char *)&buf); + queue.enqueue(data); mutex.unlock(); } + int qsspt::packets_Available() { return queue.count(); } + int qsspt::read_Packet(void *data) { mutex.lock(); @@ -101,8 +110,3 @@ int qsspt::read_Packet(void *data) mutex.unlock(); return arr.length(); } -qsspt::~qsspt() -{ - endthread = true; - wait(1000); -} diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.h index 0ec228ce67..d640f7ac04 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.h +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/qsspt.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file qsspt.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup Uploader Serial and USB Uploader Plugin @@ -28,20 +29,24 @@ #define QSSPT_H #include "qssp.h" + #include #include #include #include + class qsspt : public qssp, public QThread { public: qsspt(port *info, bool debug); + ~qsspt(); + void run(); + int packets_Available(); int read_Packet(void *); - ~qsspt(); bool sendData(uint8_t *buf, uint16_t size); + private: - virtual void pfCallBack(uint8_t *, uint16_t); uint8_t *mbuf; uint16_t msize; QQueue queue; @@ -54,6 +59,8 @@ class qsspt : public qssp, public QThread { QWaitCondition sendwait; QMutex msendwait; bool debug; + + virtual void pfCallBack(uint8_t *, uint16_t); }; #endif // QSSPT_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp_test.pro b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp_test.pro index e0fdc09e06..65c944daad 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp_test.pro +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/SSP/ssp_test.pro @@ -4,39 +4,47 @@ # #------------------------------------------------- -QT += core - -QT -= gui -DEFINES += QEXTSERIALPORT_LIBRARY +TEMPLATE = app TARGET = ssp_test -CONFIG += console -CONFIG -= app_bundle -TEMPLATE = app +QT += core +QT -= gui + +CONFIG += console +CONFIG -= app_bundle -SOURCES += main.cpp \ +DEFINES += QEXTSERIALPORT_LIBRARY + +SOURCES += \ + main.cpp \ qssp.cpp \ port.cpp \ qsspt.cpp -HEADERS += ../../../libs/qextserialport/src/qextserialport.h \ - ../../../libs/qextserialport/src/qextserialenumerator.h \ - ../../../libs/qextserialport/src/qextserialport_global.h \ + +HEADERS += \ + ../../../libs/qextserialport/src/qextserialport.h \ + ../../../libs/qextserialport/src/qextserialenumerator.h \ + ../../../libs/qextserialport/src/qextserialport_global.h \ qssp.h \ port.h \ common.h \ qsspt.h -SOURCES += ../../../libs/qextserialport/src/qextserialport.cpp -unix:SOURCES += ../../../libs/qextserialport/src/posix_qextserialport.cpp -unix:!macx:SOURCES += ../../../libs/qextserialport/src/qextserialenumerator_unix.cpp +SOURCES += ../../../libs/qextserialport/src/qextserialport.cpp + +unix:SOURCES += ../../../libs/qextserialport/src/posix_qextserialport.cpp +unix:!macx:SOURCES += ../../../libs/qextserialport/src/qextserialenumerator_unix.cpp macx { - SOURCES += ../../../libs/qextserialport/src/qextserialenumerator_osx.cpp - LIBS += -framework IOKit -framework CoreFoundation + SOURCES += ../../../libs/qextserialport/src/qextserialenumerator_osx.cpp + LIBS += -framework IOKit -framework CoreFoundation } win32 { - SOURCES += ../../../libs/qextserialport/src/win_qextserialport.cpp \ -../../../libs/qextserialport/src/qextserialenumerator_win.cpp - DEFINES += WINVER=0x0501 # needed for mingw to pull in appropriate dbt business...probably a better way to do this - LIBS += -lsetupapi + SOURCES += \ + ../../../libs/qextserialport/src/win_qextserialport.cpp \ + ../../../libs/qextserialport/src/qextserialenumerator_win.cpp + + # needed for mingw to pull in appropriate dbt business...probably a better way to do this + DEFINES += WINVER=0x0501 + LIBS += -lsetupapi } diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp deleted file mode 100644 index dd238f0149..0000000000 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/delay.cpp +++ /dev/null @@ -1,16 +0,0 @@ -/* - * 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 "delay.h" diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/delay.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/delay.h deleted file mode 100644 index cc0bfa630e..0000000000 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/delay.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * 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 DELAY_H -#define DELAY_H -#include - -class delay : public QThread { -public: - static void msleep(unsigned long msecs) - { - QThread::msleep(msecs); - } -}; -#endif // DELAY_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/dfu.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/dfu.cpp new file mode 100644 index 0000000000..369d136533 --- /dev/null +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/dfu.cpp @@ -0,0 +1,1139 @@ +/** + ****************************************************************************** + * + * @file dfu.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Uploader Uploader Plugin + * @{ + * @brief The uploader plugin + *****************************************************************************/ +/* + * 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 "dfu.h" + +#include "SSP/port.h" +#include "SSP/qsspt.h" + +// #include +// #include +// #include + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace DFU; + +DFUObject::DFUObject(bool _debug, bool _use_serial, QString portname) : + debug(_debug), use_serial(_use_serial), mready(true) +{ + numberOfDevices = 0; + serialhandle = NULL; + port *info = NULL; + + qRegisterMetaType("Status"); + + if (use_serial) { + info = new port(portname, false); + info->rxBuf = sspRxBuf; + info->rxBufSize = MAX_PACKET_DATA_LEN; + info->txBuf = sspTxBuf; + info->txBufSize = MAX_PACKET_DATA_LEN; + info->max_retry = 3; + info->timeoutLen = 100; + if (info->status() != port::open) { + cout << "Could not open serial port\n"; + mready = false; + return; + } + serialhandle = new qsspt(info, false /*debug*/); + + int count = 0; + while (!serialhandle->ssp_Synchronise() && (count < 10)) { + if (debug) { + qDebug() << "SYNC failed, resending..."; + } + count++; + } + if (count == 10) { + mready = false; + qDebug() << "SYNC failed"; + return; + } + + // transfer ownership of port to serialhandle thread + info->moveToThread(serialhandle); + connect(serialhandle, SIGNAL(finished()), info, SLOT(deleteLater())); + + // start the serialhandle thread + serialhandle->start(); + } else { +/* + hidHandle = new opHID_hidapi(); + + mready = false; + QEventLoop m_eventloop; + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); + QList devices; + devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); + if (devices.length() == 1) { + if (hidHandle->open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { + mready = true; + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); + } else { + hidHandle->close(0); + } + } else { + // Wait for the board to appear on the USB bus: + USBSignalFilter filter(0x20a0, -1, -1, USBMonitor::Bootloader); + connect(&filter, SIGNAL(deviceDiscovered()), &m_eventloop, SLOT(quit())); + for (int x = 0; x < 4; ++x) { + qDebug() << "DFU trying to detect bootloader:" << x; + + if (x == 0) { + QTimer::singleShot(10000, &m_eventloop, SLOT(quit())); + } else { + QTimer::singleShot(2000, &m_eventloop, SLOT(quit())); + } + m_eventloop.exec(); + devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); + qDebug() << "Devices length: " << devices.length(); + if (devices.length() == 1) { + qDebug() << "Opening device"; + if (hidHandle->open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); + qDebug() << "DFU detected after delay"; + mready = true; + qDebug() << "Detected"; + break; + } else { + hidHandle->close(0); + } + } else { + qDebug() << devices.length() << " device(s) detected, don't know what to do!"; + mready = false; + } + } + } + */ + } +} + +DFUObject::~DFUObject() +{ + if (use_serial) { + delete serialhandle; + } else { +/* + if (hidHandle) { + hidHandle->close(0); + delete hidHandle; + } + */ + } +} + +bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) +{ + QFile file(sfile); + + if (!file.open(QIODevice::WriteOnly)) { + if (debug) { + qDebug() << "Can't open file" << sfile; + } + return false; + } + file.write(array); + file.close(); + return true; +} + +/** + Tells the mainboard to enter DFU Mode. + */ +bool DFUObject::enterDFU(int const &devNumber) +{ + char buf[BUF_LEN]; + + buf[0] = 0x02; // reportID + buf[1] = DFU::EnterDFU; // DFU Command + buf[2] = 0; // DFU Count + buf[3] = 0; // DFU Count + buf[4] = 0; // DFU Count + buf[5] = 0; // DFU Count + buf[6] = devNumber; // DFU Data0 + buf[7] = 1; // DFU Data1 + buf[8] = 1; // DFU Data2 + buf[9] = 1; // DFU Data3 + + int result = sendData(buf, BUF_LEN); + if (result < 1) { + return false; + } + if (debug) { + qDebug() << "EnterDFU: " << result << " bytes sent"; + } + return true; +} + +/** + Tells the board to get ready for an upload. It will in particular + erase the memory to make room for the data. You will have to query + its status to wait until erase is done before doing the actual upload. + */ +bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc) +{ + int lastPacketCount; + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { + ++numberOfPackets; + lastPacketCount = pad; + } + char buf[BUF_LEN]; + buf[0] = 0x02; // reportID + buf[1] = setStartBit(DFU::Upload); // DFU Command + buf[2] = numberOfPackets >> 24; // DFU Count + buf[3] = numberOfPackets >> 16; // DFU Count + buf[4] = numberOfPackets >> 8; // DFU Count + buf[5] = numberOfPackets; // DFU Count + buf[6] = (int)type; // DFU Data0 + buf[7] = lastPacketCount; // DFU Data1 + buf[8] = crc >> 24; + buf[9] = crc >> 16; + buf[10] = crc >> 8; + buf[11] = crc; + if (debug) { + qDebug() << "Number of packets:" << numberOfPackets << " Size of last packet:" << lastPacketCount; + } + + int result = sendData(buf, BUF_LEN); + QThread::msleep(1000); + + if (debug) { + qDebug() << result << " bytes sent"; + } + if (result > 0) { + return true; + } + return false; +} + +/** + Does the actual data upload to the board. Needs to be called once the + board is ready to accept data following a StartUpload command, and it is erased. + */ +bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data) +{ + int lastPacketCount; + qint32 numberOfPackets = numberOfBytes / 4 / 14; + int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; + + if (pad == 0) { + lastPacketCount = 14; + } else { + ++numberOfPackets; + lastPacketCount = pad; + } + if (debug) { + qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes"; + } + char buf[BUF_LEN]; + buf[0] = 0x02; // reportID + buf[1] = DFU::Upload; // DFU Command + int packetsize; + float percentage; + int laspercentage = 0; + for (qint32 packetcount = 0; packetcount < numberOfPackets; ++packetcount) { + percentage = (float)(packetcount + 1) / numberOfPackets * 100; + if (laspercentage != (int)percentage) { + printProgBar((int)percentage, "UPLOADING"); + } + laspercentage = (int)percentage; + if (packetcount == numberOfPackets) { + packetsize = lastPacketCount; + } else { + packetsize = 14; + } + // qDebug()<> 24; // DFU Count + buf[3] = packetcount >> 16; // DFU Count + buf[4] = packetcount >> 8; // DFU Count + buf[5] = packetcount; // DFU Count + char *pointer = data.data(); + pointer = pointer + 4 * 14 * packetcount; + // if (debug) { + // qDebug() << "Packet Number=" << packetcount << "Data0=" << (int)data[0] << " Data1=" << (int)data[1] << " Data0=" << (int)data[2] << " Data0=" << (int)data[3] << " buf6=" << (int)buf[6] << " buf7=" << (int)buf[7] << " buf8=" << (int)buf[8] << " buf9=" << (int)buf[9]; + // } + CopyWords(pointer, buf + 6, packetsize * 4); + // for (int y=0;yappend(buf + 6, size); + } + + StatusRequest(); + return true; +} + +/** + Resets the device + */ +int DFUObject::ResetDevice(void) +{ + char buf[BUF_LEN]; + + buf[0] = 0x02; // reportID + buf[1] = DFU::Reset; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + buf[8] = 0; + buf[9] = 0; + + return sendData(buf, BUF_LEN); + // return hidHandle->send(0,buf, BUF_LEN, 500); +} + +int DFUObject::AbortOperation(void) +{ + char buf[BUF_LEN]; + + buf[0] = 0x02; // reportID + buf[1] = DFU::Abort_Operation; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + buf[8] = 0; + buf[9] = 0; + + return sendData(buf, BUF_LEN); +} + +/** + Starts the firmware (leaves bootloader and boots the main software) + */ +int DFUObject::JumpToApp(bool safeboot, bool erase) +{ + char buf[BUF_LEN]; + + buf[0] = 0x02; // reportID + buf[1] = DFU::JumpFW; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + if (safeboot) { + /* force system to safe boot mode (hwsettings == defaults) */ + buf[8] = 0x5A; + buf[9] = 0xFE; + } else { + buf[8] = 0; + buf[9] = 0; + } + if (erase) { + // force data flash clear + buf[10] = 0x00; + buf[11] = 0x00; + buf[12] = 0xFA; + buf[13] = 0x5F; + + buf[14] = 0x00; + buf[15] = 0x00; + buf[16] = 0x00; + buf[17] = 0x01; + + buf[18] = 0x00; + buf[19] = 0x00; + buf[20] = 0x00; + buf[21] = 0x00; + } else { + buf[10] = 0x00; + buf[11] = 0x00; + buf[12] = 0x00; + buf[13] = 0x00; + + buf[14] = 0x00; + buf[15] = 0x00; + buf[16] = 0x00; + buf[17] = 0x00; + + buf[18] = 0x00; + buf[19] = 0x00; + buf[20] = 0x00; + buf[21] = 0x00; + } + + return sendData(buf, BUF_LEN); +} + +DFU::Status DFUObject::StatusRequest() +{ + char buf[BUF_LEN]; + + buf[0] = 0x02; // reportID + buf[1] = DFU::Status_Request; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + buf[8] = 0; + buf[9] = 0; + + int result = sendData(buf, BUF_LEN); + if (debug) { + qDebug() << "StatusRequest: " << result << " bytes sent"; + } + result = receiveData(buf, BUF_LEN); + if (debug) { + qDebug() << "StatusRequest: " << result << " bytes received"; + } + if (buf[1] == DFU::Status_Rep) { + return (DFU::Status)buf[6]; + } else { + return DFU::abort; + } +} + +/** + Ask the bootloader for the list of devices available + */ +bool DFUObject::findDevices() +{ + devices.clear(); + char buf[BUF_LEN]; + buf[0] = 0x02; // reportID + buf[1] = DFU::Req_Capabilities; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + buf[8] = 0; + buf[9] = 0; + + int result = sendData(buf, BUF_LEN); + if (result < 1) { + return false; + } + + result = receiveData(buf, BUF_LEN); + if (result < 1) { + return false; + } + + numberOfDevices = buf[7]; + RWFlags = buf[8]; + RWFlags = RWFlags << 8 | buf[9]; + + if (buf[1] == DFU::Rep_Capabilities) { + for (int x = 0; x < numberOfDevices; ++x) { + device dev; + dev.Readable = (bool)(RWFlags >> (x * 2) & 1); + dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1); + devices.append(dev); + buf[0] = 0x02; // reportID + buf[1] = DFU::Req_Capabilities; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = x + 1; + buf[7] = 0; + buf[8] = 0; + buf[9] = 0; + sendData(buf, BUF_LEN); + receiveData(buf, BUF_LEN); + devices[x].ID = buf[14]; + devices[x].ID = devices[x].ID << 8 | (quint8)buf[15]; + devices[x].BL_Version = buf[7]; + devices[x].SizeOfDesc = buf[8]; + + quint32 aux; + aux = (quint8)buf[10]; + aux = aux << 8 | (quint8)buf[11]; + aux = aux << 8 | (quint8)buf[12]; + aux = aux << 8 | (quint8)buf[13]; + + devices[x].FW_CRC = aux; + + + aux = (quint8)buf[2]; + aux = aux << 8 | (quint8)buf[3]; + aux = aux << 8 | (quint8)buf[4]; + aux = aux << 8 | (quint8)buf[5]; + devices[x].SizeOfCode = aux; + } + if (debug) { + qDebug() << "Found " << numberOfDevices << " devices"; + for (int x = 0; x < numberOfDevices; ++x) { + qDebug() << "Device #" << x + 1; + qDebug() << "Device ID=" << devices[x].ID; + qDebug() << "Device Readable=" << devices[x].Readable; + qDebug() << "Device Writable=" << devices[x].Writable; + qDebug() << "Device SizeOfCode=" << devices[x].SizeOfCode; + qDebug() << "Device SizeOfDesc=" << devices[x].SizeOfDesc; + qDebug() << "BL Version=" << devices[x].BL_Version; + qDebug() << "FW CRC=" << devices[x].FW_CRC; + } + } + } + return true; +} + +bool DFUObject::EndOperation() +{ + char buf[BUF_LEN]; + + buf[0] = 0x02; // reportID + buf[1] = DFU::END; // DFU Command + buf[2] = 0; + buf[3] = 0; + buf[4] = 0; + buf[5] = 0; + buf[6] = 0; + buf[7] = 0; + buf[8] = 0; + buf[9] = 0; + + int result = sendData(buf, BUF_LEN); + if (debug) { + qDebug() << result << " bytes sent"; + } + if (result > 0) { + return true; + } + return false; +} + +/** + Starts a firmware upload (asynchronous) + */ +bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify, int device) +{ + if (isRunning()) { + return false; + } + requestedOperation = DFU::Upload; + requestFilename = sfile; + requestDevice = device; + requestVerify = verify; + start(); + return true; +} + +DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify, int device) +{ + DFU::Status ret; + + if (debug) { + qDebug() << "Starting Firmware Uploading..."; + } + + QFile file(sfile); + + if (!file.open(QIODevice::ReadOnly)) { + if (debug) { + qDebug() << "Failed to open file" << sfile; + } + return DFU::abort; + } + + QByteArray arr = file.readAll(); + + if (debug) { + qDebug() << "Bytes Loaded=" << arr.length(); + } + if (arr.length() % 4 != 0) { + int pad = arr.length() / 4; + ++pad; + pad = pad * 4; + pad = pad - arr.length(); + arr.append(QByteArray(pad, 255)); + } + if (devices[device].SizeOfCode < (quint32)arr.length()) { + if (debug) { + qDebug() << "ERROR file to big for device"; + } + return DFU::abort;; + } + + quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode); + if (debug) { + qDebug() << "NEW FIRMWARE CRC=" << crc; + } + + if (!StartUpload(arr.length(), DFU::FW, crc)) { + ret = StatusRequest(); + if (debug) { + qDebug() << "StartUpload failed"; + qDebug() << "StartUpload returned:" << StatusToString(ret); + } + return ret; + } + + emit operationProgress("Erasing, please wait..."); + + if (debug) { + qDebug() << "Erasing memory"; + } + if (StatusRequest() == DFU::abort) { + return DFU::abort; + } + + // TODO: why is there a loop there? The "if" statement + // will cause a break or return anyway!! + for (int x = 0; x < 3; ++x) { + ret = StatusRequest(); + if (debug) { + qDebug() << "Erase returned: " << StatusToString(ret); + } + if (ret == DFU::uploading) { + break; + } else { + return ret; + } + } + + emit operationProgress("Uploading firmware"); + if (!UploadData(arr.length(), arr)) { + ret = StatusRequest(); + if (debug) { + qDebug() << "Upload failed (upload data)"; + qDebug() << "UploadData returned:" << StatusToString(ret); + } + return ret; + } + if (!EndOperation()) { + ret = StatusRequest(); + if (debug) { + qDebug() << "Upload failed (end operation)"; + qDebug() << "EndOperation returned:" << StatusToString(ret); + } + return ret; + } + ret = StatusRequest(); + if (ret != DFU::Last_operation_Success) { + return ret; + } + + if (verify) { + emit operationProgress("Verifying firmware"); + cout << "Starting code verification\n"; + QByteArray arr2; + StartDownloadT(&arr2, arr.length(), DFU::FW); + if (arr != arr2) { + cout << "Verify:FAILED\n"; + return DFU::abort; + } + } + + if (debug) { + qDebug() << "Status=" << ret; + } + cout << "Firmware Uploading succeeded\n"; + return ret; +} + +DFU::Status DFUObject::CompareFirmware(const QString &sfile, const CompareType &type, int device) +{ + cout << "Starting Firmware Compare...\n"; + QFile file(sfile); + if (!file.open(QIODevice::ReadOnly)) { + if (debug) { + qDebug() << "Cant open file"; + } + return DFU::abort; + } + QByteArray arr = file.readAll(); + + if (debug) { + qDebug() << "Bytes Loaded=" << arr.length(); + } + if (arr.length() % 4 != 0) { + int pad = arr.length() / 4; + ++pad; + pad = pad * 4; + pad = pad - arr.length(); + arr.append(QByteArray(pad, 255)); + } + if (type == DFU::crccompare) { + quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode); + if (crc == devices[device].FW_CRC) { + cout << "Compare Successfull CRC MATCH!\n"; + } else { + cout << "Compare failed CRC DONT MATCH!\n"; + } + return StatusRequest(); + } else { + QByteArray arr2; + StartDownloadT(&arr2, arr.length(), DFU::FW); + if (arr == arr2) { + cout << "Compare Successfull ALL Bytes MATCH!\n"; + } else { + cout << "Compare failed Bytes DONT MATCH!\n"; + } + return StatusRequest(); + } +} + +void DFUObject::CopyWords(char *source, char *destination, int count) +{ + for (int x = 0; x < count; x = x + 4) { + *(destination + x) = source[x + 3]; + *(destination + x + 1) = source[x + 2]; + *(destination + x + 2) = source[x + 1]; + *(destination + x + 3) = source[x + 0]; + } +} + +QString DFUObject::StatusToString(DFU::Status const & status) +{ + switch (status) { + case DFUidle: + return "DFUidle"; + + case uploading: + return ""; + + case wrong_packet_received: + return "wrong_packet_received"; + + case too_many_packets: + return "too_many_packets"; + + case too_few_packets: + return "too_few_packets"; + + case Last_operation_Success: + return "Last_operation_Success"; + + case downloading: + return "downloading"; + + case idle: + return "idle"; + + case Last_operation_failed: + return "Last_operation_failed"; + + case outsideDevCapabilities: + return "outsideDevCapabilities"; + + case CRC_Fail: + return "CRC check FAILED"; + + case failed_jump: + return "Jmp to user FW failed"; + + case abort: + return "abort"; + + case uploadingStarting: + return "Uploading Starting"; + + default: + return "unknown"; + } +} + +/** + Prints a progress bar with percentage & label during an operation. + + Also outputs to stdout if we are in debug mode. + */ +void DFUObject::printProgBar(int const & percent, QString const & label) +{ + emit(progressUpdated(percent)); + + if (debug) { + std::string bar; + for (int i = 0; i < 50; i++) { + if (i < (percent / 2)) { + bar.replace(i, 1, "="); + } else if (i == (percent / 2)) { + bar.replace(i, 1, ">"); + } else { + bar.replace(i, 1, " "); + } + } + + std::cout << "\r" << label.toLatin1().data() << "[" << bar << "] "; + std::cout.width(3); + std::cout << percent << "% " << std::flush; + } +} + +/** + Utility function + */ +quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) +{ + // Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 + + while (Size--) { + // Nibble lookup table for 0x04C11DB7 polynomial + static const quint32 CrcTable[16] = { + 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005, + 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD + }; + + Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits + + Buffer += 1; + + // Process 32-bits, 4 at a time, or 8 rounds + + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; + Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; + } + + return Crc; +} + +/** + Utility function + */ +quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size) +{ + quint32 pad = Size - array.length(); + + array.append(QByteArray(pad, 255)); + quint32 t[Size / 4]; + for (int x = 0; x < array.length() / 4; x++) { + quint32 aux = 0; + aux = (char)array[x * 4 + 3] & 0xFF; + aux = aux << 8; + aux += (char)array[x * 4 + 2] & 0xFF; + aux = aux << 8; + aux += (char)array[x * 4 + 1] & 0xFF; + aux = aux << 8; + aux += (char)array[x * 4 + 0] & 0xFF; + t[x] = aux; + } + return DFUObject::CRC32WideFast(0xFFFFFFFF, Size / 4, (quint32 *)t); +} + +/** + Send data to the bootloader, either through the serial port + of through the HID handle, depending on the mode we're using + */ +int DFUObject::sendData(void *data, int size) +{ +/* + if (!use_serial) { + return hidHandle->send(0, data, size, 5000); + } + */ + + // Serial Mode: + if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) { + if (debug) { + qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0]; + } + return size; + } + if (debug) { + qDebug() << "Serial send OVERRUN"; + } + return -1; +} + +/** + Receive data from the bootloader, either through the serial port + of through the HID handle, depending on the mode we're using + */ +int DFUObject::receiveData(void *data, int size) +{ +/* + if (!use_serial) { + return hidHandle->receive(0, data, size, 10000); + } + */ + + // Serial Mode: + int x; + QTime time; + + time.start(); + while (true) { + if ((x = serialhandle->read_Packet(((char *)data) + 1) != -1) || time.elapsed() > 10000) { + QThread::msleep(10); + if (time.elapsed() > 10000) { + qDebug() << "____timeout"; + } + if (x > size - 1) { + qDebug() << "Error buffer overrun"; + Q_ASSERT(false); + } + return x; + } + } +} + +#define BOARD_ID_MB 1 +#define BOARD_ID_INS 2 +#define BOARD_ID_PIP 3 +#define BOARD_ID_CC 4 +#define BOARD_ID_REVO 9 +#define BOARD_ID_SPARKY2 0x92 + +/** + Gets the type of board connected + */ +DFU::eBoardType DFUObject::GetBoardType(int boardNum) +{ + DFU::eBoardType brdType = eBoardUnkwn; + + // First of all, check what Board type we are talking to + int board = devices[boardNum].ID; + + qDebug() << "Board model: " << board; + switch (board >> 8) { + case BOARD_ID_MB: // Mainboard family + brdType = eBoardMainbrd; + break; + case BOARD_ID_INS: // Inertial Nav + brdType = eBoardINS; + break; + case BOARD_ID_PIP: // PIP RF Modem + brdType = eBoardPip; + break; + case BOARD_ID_CC: // CopterControl family + brdType = eBoardCC; + break; + case BOARD_ID_REVO: // Revo board + brdType = eBoardRevo; + break; + case BOARD_ID_SPARKY2: // Sparky2 board + brdType = eBoardSparky2; + break; + } + return brdType; +} diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/dfu.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/dfu.h new file mode 100644 index 0000000000..8c8c8e5018 --- /dev/null +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/dfu.h @@ -0,0 +1,236 @@ +/** + ****************************************************************************** + * + * @file dfu.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Uploader Uploader Plugin + * @{ + * @brief The uploader plugin + *****************************************************************************/ +/* + * 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 DFU_H +#define DFU_H + +#include +#include +#include +#include +#include + +#define MAX_PACKET_DATA_LEN 255 +#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) + +#define BUF_LEN 64 + +// serial +class qsspt; + +// usb +class opHID_hidapi; + +namespace DFU { +#if QT_VERSION >= QT_VERSION_CHECK(5, 8, 0) +Q_NAMESPACE +#endif +enum TransferTypes { + FW, + Descript +}; + +enum CompareType { + crccompare, + bytetobytecompare +}; + +Q_ENUMS(Status) +enum Status { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + idle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, // 12 + abort // 13 +}; + +enum Actions { + actionNone, + actionProgram, + actionProgramAndVerify, + actionDownload, + actionCompareAll, + actionCompareCrc, + actionListDevs, + actionStatusReq, + actionReset, + actionJump +}; + +enum Commands { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep, // 12 +}; + +enum eBoardType { + eBoardUnkwn = 0, + eBoardMainbrd = 1, + eBoardINS, + eBoardPip = 3, + eBoardCC = 4, + eBoardRevo = 9, + eBoardSparky2 = 0x92, +}; + +struct device { + quint16 ID; + quint32 FW_CRC; + quint8 BL_Version; + int SizeOfDesc; + quint32 SizeOfCode; + bool Readable; + bool Writable; +}; + +class DFUObject : public QThread { + Q_OBJECT; + +public: + static quint32 CRCFromQBArray(QByteArray array, quint32 Size); + + DFUObject(bool debug, bool use_serial, QString port); + + virtual ~DFUObject(); + + // Service commands: + bool enterDFU(int const &devNumber); + bool findDevices(); + int JumpToApp(bool safeboot, bool erase); + int ResetDevice(void); + DFU::Status StatusRequest(); + bool EndOperation(); + int AbortOperation(void); + bool ready() + { + return mready; + } + + // Upload (send to device) commands + DFU::Status UploadDescription(QVariant description); + bool UploadFirmware(const QString &sfile, const bool &verify, int device); + + // Download (get from device) commands: + // DownloadDescription is synchronous + QString DownloadDescription(int const & numberOfChars); + QByteArray DownloadDescriptionAsBA(int const & numberOfChars); + // Asynchronous firmware download: initiates fw download, + // and a downloadFinished signal is emitted when download + // if finished: + bool DownloadFirmware(QByteArray *byteArray, int device); + + // Comparison functions (is this needed?) + DFU::Status CompareFirmware(const QString &sfile, const CompareType &type, int device); + + bool SaveByteArrayToFile(QString const & file, QByteArray const &array); + + // Variables: + QList devices; + int numberOfDevices; + int send_delay; + bool use_delay; + + // Helper functions: + QString StatusToString(DFU::Status const & status); + static quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); + DFU::eBoardType GetBoardType(int boardNum); + +signals: + void progressUpdated(int); + void downloadFinished(); + void uploadFinished(DFU::Status); + void operationProgress(QString status); + +private: + // Generic variables: + bool debug; + bool use_serial; + bool mready; + int RWFlags; + + // Serial + qsspt *serialhandle; + + // USB + // opHID_hidapi *hidHandle; + + int sendData(void *, int); + int receiveData(void *data, int size); + uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; + uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; + + int setStartBit(int command) + { + return command | 0x20; + } + + void CopyWords(char *source, char *destination, int count); + void printProgBar(int const & percent, QString const & label); + bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type, quint32 crc); + bool UploadData(qint32 const & numberOfPackets, QByteArray & data); + + // Thread management: + // Same as startDownload except that we store in an external array: + bool StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type); + DFU::Status UploadFirmwareT(const QString &sfile, const bool &verify, int device); + QMutex mutex; + DFU::Commands requestedOperation; + qint32 requestSize; + DFU::TransferTypes requestTransferType; + QByteArray *requestStorage; + QString requestFilename; + bool requestVerify; + int requestDevice; + +protected: + void run(); // Executes the upload or download operations +}; +} + +Q_DECLARE_METATYPE(DFU::Status) + + +#endif // DFU_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/main.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/main.cpp index 793c6787d7..980883c29f 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/main.cpp +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/main.cpp @@ -1,12 +1,66 @@ -#include +/** + ****************************************************************************** + * + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Uploader Serial and USB Uploader tool + * @{ + * @brief The USB and Serial protocol uploader tool + *****************************************************************************/ +/* + * 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 "dfu.h" + +#include +#include +#include #include -#include "op_dfu.h" #include +#include + +#include void showProgress(QString status); void progressUpdated(int percent); void usage(QTextStream *standardOutput); + QString label; + +// Command Line Options +#define DOWNLOAD "-dn" +#define DEVICE "-d" +#define DOWNDESCRIPTION "-dd" +#define PROGRAMFW "-p" +#define PROGRAMDESC "-w" +#define VERIFY "-v" +#define COMPARECRC "-cc" +#define COMPAREALL "-ca" +#define STATUSREQUEST "-s" +#define LISTDEVICES "-ls" +#define RESET "-r" +#define JUMP "-j" +#define USE_SERIAL "-t" +#define NO_COUNTDOWN "-i" +#define HELP "-h" +#define DEBUG "-debug" +#define USERMODERESET "-ur" + int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); @@ -16,23 +70,20 @@ int main(int argc, char *argv[]) bool verify; bool debug = false; // bool umodereset = false; - OP_DFU::Actions action = OP_DFU::actionNone; + DFU::Actions action = DFU::actionNone; QString file; QString serialport; QString description; int device = -1; QStringList args = QCoreApplication::arguments(); - if (args.contains("-debug")) { + if (args.contains(DEBUG)) { debug = true; } - if (args.contains("-ur")) { + if (args.contains(USERMODERESET)) { // umodereset = true; } - standardOutput << "OpenPilot serial firmware uploader tool." << endl; - if (args.indexOf(PROGRAMFW) + 1 < args.length()) { - file = args[args.indexOf(PROGRAMFW) + 1]; - } + standardOutput << "Serial firmware uploader tool." << endl; if (args.contains(DEVICE)) { if (args.indexOf(DEVICE) + 1 < args.length()) { device = (args[args.indexOf(DEVICE) + 1]).toInt(); @@ -41,7 +92,7 @@ int main(int argc, char *argv[]) device = 0; } - if (argumentCount == 0 || args.contains("-?") || !args.contains(USE_SERIAL)) { + if (argumentCount == 0 || args.contains(HELP) || !args.contains(USE_SERIAL)) { usage(&standardOutput); return -1; } else if (args.contains(PROGRAMFW)) { @@ -55,30 +106,45 @@ int main(int argc, char *argv[]) description = (args[args.indexOf(PROGRAMDESC) + 1]); } } - action = OP_DFU::actionProgram; + if (args.indexOf(PROGRAMFW) + 1 < args.length()) { + file = args[args.indexOf(PROGRAMFW) + 1]; + } else { + // error + } + action = DFU::actionProgram; } else if (args.contains(COMPARECRC) || args.contains(COMPAREALL)) { - // int index; if (args.contains(COMPARECRC)) { - // index = args.indexOf(COMPARECRC); - action = OP_DFU::actionCompareCrc; + if (args.indexOf(COMPARECRC) + 1 < args.length()) { + file = args[args.indexOf(COMPARECRC) + 1]; + } else { + // error + } + action = DFU::actionCompareCrc; } else { - // index = args.indexOf(COMPAREALL); - action = OP_DFU::actionCompareAll; + if (args.indexOf(COMPAREALL) + 1 < args.length()) { + file = args[args.indexOf(COMPAREALL) + 1]; + } else { + // error + } + action = DFU::actionCompareAll; } } else if (args.contains(DOWNLOAD)) { - // int index; - // index = args.indexOf(DOWNLOAD); - action = OP_DFU::actionDownload; + if (args.indexOf(DOWNLOAD) + 1 < args.length()) { + file = args[args.indexOf(DOWNLOAD) + 1]; + } else { + // error + } + action = DFU::actionDownload; } else if (args.contains(STATUSREQUEST)) { - action = OP_DFU::actionStatusReq; + action = DFU::actionStatusReq; } else if (args.contains(RESET)) { - action = OP_DFU::actionReset; + action = DFU::actionReset; } else if (args.contains(JUMP)) { - action = OP_DFU::actionJump; + action = DFU::actionJump; } else if (args.contains(LISTDEVICES)) { - action = OP_DFU::actionListDevs; + action = DFU::actionListDevs; } - if ((file.isEmpty() || device == -1) && action != OP_DFU::actionReset && action != OP_DFU::actionStatusReq && action != OP_DFU::actionListDevs && action != OP_DFU::actionJump) { + if ((file.isEmpty() || device == -1) && action != DFU::actionReset && action != DFU::actionStatusReq && action != DFU::actionListDevs && action != DFU::actionJump) { usage(&standardOutput); return -1; } @@ -89,20 +155,19 @@ int main(int argc, char *argv[]) } } if (debug) { - qDebug() << "Action=" << (int)action << endl; - qDebug() << "File=" << file << endl; - qDebug() << "Device=" << device << endl; - qDebug() << "Action=" << action << endl; - qDebug() << "Desctription" << description << endl; - qDebug() << "Use Serial port" << use_serial << endl; + qDebug() << "Action=" << (int)action; + qDebug() << "File=" << file; + qDebug() << "Device=" << device; + qDebug() << "Action=" << action; + qDebug() << "Description" << description; + qDebug() << "Use Serial port" << use_serial; if (use_serial) { - qDebug() << "Port Name" << serialport << endl; + qDebug() << "Port Name" << serialport; } } if (use_serial) { if (args.contains(NO_COUNTDOWN)) {} else { - standardOutput << "Connect the board" << endl; - label = ""; + showProgress("Connect the board"); for (int i = 0; i < 6; i++) { progressUpdated(i * 100 / 5); QThread::msleep(500); @@ -111,11 +176,13 @@ int main(int argc, char *argv[]) standardOutput << endl << "Connect the board NOW" << endl; QThread::msleep(1000); } + ///////////////////////////////////ACTIONS START/////////////////////////////////////////////////// - OP_DFU::DFUObject dfu(debug, use_serial, serialport); - QObject::connect(&dfu, &OP_DFU::DFUObject::operationProgress, showProgress); - QObject::connect(&dfu, &OP_DFU::DFUObject::progressUpdated, progressUpdated); + DFU::DFUObject dfu(debug, use_serial, serialport); + + QObject::connect(&dfu, &DFU::DFUObject::operationProgress, showProgress); + QObject::connect(&dfu, &DFU::DFUObject::progressUpdated, progressUpdated); if (!dfu.ready()) { return -1; @@ -126,26 +193,26 @@ int main(int argc, char *argv[]) return -1; } if (debug) { - OP_DFU::Status ret = dfu.StatusRequest(); + DFU::Status ret = dfu.StatusRequest(); qDebug() << dfu.StatusToString(ret); } - if (!(action == OP_DFU::actionStatusReq || action == OP_DFU::actionReset || action == OP_DFU::actionJump)) { + if (!(action == DFU::actionStatusReq || action == DFU::actionReset || action == DFU::actionJump)) { dfu.findDevices(); - if (action == OP_DFU::actionListDevs) { - standardOutput << "Found " << dfu.numberOfDevices << "\n" << endl; + if (action == DFU::actionListDevs) { + standardOutput << "Found " << dfu.numberOfDevices << "\n"; for (int x = 0; x < dfu.numberOfDevices; ++x) { - standardOutput << "Device #" << x << "\n" << endl; - standardOutput << "Device ID=" << dfu.devices[x].ID << "\n" << endl; - standardOutput << "Device Readable=" << dfu.devices[x].Readable << "\n" << endl; - standardOutput << "Device Writable=" << dfu.devices[x].Writable << "\n" << endl; - standardOutput << "Device SizeOfCode=" << dfu.devices[x].SizeOfCode << "\n" << endl; - standardOutput << "BL Version=" << dfu.devices[x].BL_Version << "\n" << endl; - standardOutput << "Device SizeOfDesc=" << dfu.devices[x].SizeOfDesc << "\n" << endl; + standardOutput << "Device #" << x << "\n"; + standardOutput << "Device ID=" << dfu.devices[x].ID << "\n"; + standardOutput << "Device Readable=" << dfu.devices[x].Readable << "\n"; + standardOutput << "Device Writable=" << dfu.devices[x].Writable << "\n"; + standardOutput << "Device SizeOfCode=" << dfu.devices[x].SizeOfCode << "\n"; + standardOutput << "BL Version=" << dfu.devices[x].BL_Version << "\n"; + standardOutput << "Device SizeOfDesc=" << dfu.devices[x].SizeOfDesc << "\n"; standardOutput << "FW CRC=" << dfu.devices[x].FW_CRC << "\n"; - int size = ((OP_DFU::device)dfu.devices[x]).SizeOfDesc; + int size = ((DFU::device)dfu.devices[x]).SizeOfDesc; dfu.enterDFU(x); - standardOutput << "Description:" << dfu.DownloadDescription(size).toLatin1().data() << "\n" << endl; + standardOutput << "Description:" << dfu.DownloadDescription(size).toLatin1().data() << "\n"; standardOutput << "\n"; } return 0; @@ -161,12 +228,15 @@ int main(int argc, char *argv[]) standardOutput << "Error:Could not enter DFU mode\n" << endl; return -1; } - if (action == OP_DFU::actionProgram) { - if (((OP_DFU::device)dfu.devices[device]).Writable == false) { + if (action == DFU::actionProgram) { + if (((DFU::device)dfu.devices[device]).Writable == false) { standardOutput << "ERROR device not Writable\n" << endl; return false; } standardOutput << "Uploading..." << endl; + + // this call is asynchronous so the only false status it will report + // is when it is already running... bool retstatus = dfu.UploadFirmware(file.toLatin1(), verify, device); if (!retstatus) { @@ -176,23 +246,23 @@ int main(int argc, char *argv[]) while (!dfu.isFinished()) { QThread::msleep(500); } + // TODO check if upload went well... if (file.endsWith("opfw")) { - QByteArray firmware; QFile fwfile(file); if (!fwfile.open(QIODevice::ReadOnly)) { standardOutput << "Cannot open file " << file << endl; return -1; } - firmware = fwfile.readAll(); - QByteArray desc = firmware.right(100); - OP_DFU::Status status = dfu.UploadDescription(desc); - if (status != OP_DFU::Last_operation_Success) { + QByteArray firmware = fwfile.readAll(); + QByteArray desc = firmware.right(100); + DFU::Status status = dfu.UploadDescription(desc); + if (status != DFU::Last_operation_Success) { standardOutput << "Upload failed with code:" << retstatus << endl; return -1; } } else if (!description.isEmpty()) { - OP_DFU::Status status = dfu.UploadDescription(description); - if (status != OP_DFU::Last_operation_Success) { + DFU::Status status = dfu.UploadDescription(description); + if (status != DFU::Last_operation_Success) { standardOutput << "Upload failed with code:" << status << endl; return -1; } @@ -200,32 +270,35 @@ int main(int argc, char *argv[]) while (!dfu.isFinished()) { QThread::msleep(500); } - standardOutput << "Uploading Succeded!\n" << endl; - } else if (action == OP_DFU::actionDownload) { - if (((OP_DFU::device)dfu.devices[device]).Readable == false) { + standardOutput << "Uploading Succeeded!\n" << endl; + } else if (action == DFU::actionDownload) { + if (((DFU::device)dfu.devices[device]).Readable == false) { standardOutput << "ERROR device not readable\n" << endl; return false; } QByteArray fw; dfu.DownloadFirmware(&fw, 0); + while (!dfu.isFinished()) { + QThread::msleep(500); + } bool ret = dfu.SaveByteArrayToFile(file.toLatin1(), fw); return ret; - } else if (action == OP_DFU::actionCompareCrc) { - dfu.CompareFirmware(file.toLatin1(), OP_DFU::crccompare, device); + } else if (action == DFU::actionCompareCrc) { + dfu.CompareFirmware(file.toLatin1(), DFU::crccompare, device); return 1; - } else if (action == OP_DFU::actionCompareAll) { - if (((OP_DFU::device)dfu.devices[device]).Readable == false) { + } else if (action == DFU::actionCompareAll) { + if (((DFU::device)dfu.devices[device]).Readable == false) { standardOutput << "ERROR device not readable\n" << endl; return false; } - dfu.CompareFirmware(file.toLatin1(), OP_DFU::bytetobytecompare, device); + dfu.CompareFirmware(file.toLatin1(), DFU::bytetobytecompare, device); return 1; } - } else if (action == OP_DFU::actionStatusReq) { + } else if (action == DFU::actionStatusReq) { standardOutput << "Current device status=" << dfu.StatusToString(dfu.StatusRequest()).toLatin1().data() << "\n" << endl; - } else if (action == OP_DFU::actionReset) { + } else if (action == DFU::actionReset) { dfu.ResetDevice(); - } else if (action == OP_DFU::actionJump) { + } else if (action == DFU::actionJump) { dfu.JumpToApp(false, false); } return 0; @@ -254,49 +327,41 @@ void progressUpdated(int percent) bar.replace(i, 1, " "); } } - std::cout << "\r" << label.toLatin1().data() << "[" << bar << "] "; + std::cout << "\r" << label.toLatin1().data() << " [" << bar << "] "; std::cout.width(3); std::cout << percent << "% " << std::flush; } + void usage(QTextStream *standardOutput) { - *standardOutput << "_________________________________________________________________________\n"; - *standardOutput << "| Commands |\n"; - *standardOutput << "| |\n"; - *standardOutput << "| -ls : lists available devices |\n"; - *standardOutput << "| -p : program hw (requires:-d - optionals:-v,-w) |\n"; - *standardOutput << "| -v : verify (requires:-d) |\n"; - *standardOutput << "| -dn : download firmware to file |\n"; - // *standardOutput << "| -dd : download discription (requires:-d) |\n"; - *standardOutput << "| -d : target device number (default 0, first device) |\n"; - // *standardOutput << "| -w : (requires: -p) |\n"; - *standardOutput << "| -ca : compares byte by byte current firmware with file|\n"; - *standardOutput << "| -cc : compares CRC of current firmware with file |\n"; - *standardOutput << "| -s : requests status of device |\n"; - *standardOutput << "| -r : resets the device |\n"; - *standardOutput << "| -j : exits bootloader and jumps to user FW |\n"; - *standardOutput << "| -debug : prints debug information |\n"; - *standardOutput << "| -t : uses serial port |\n"; - *standardOutput << "| -i : immediate, doesn't show the connection countdown|\n"; - // *standardOutput << "| -ur : user mode reset* |\n"; - *standardOutput << "| |\n"; - *standardOutput << "| examples: |\n"; - *standardOutput << "| |\n"; - *standardOutput << "| program and verify the fist device device connected to COM1 |\n"; - *standardOutput << "| OPUploadTool -p c:/gpsp.opfw -v -t COM1 |\n"; - *standardOutput << "| |\n"; - *standardOutput << "| program and verify the fist device device connected to COM1 |\n"; - *standardOutput << "| OPUploadTool -p c:/gpsp.opfw -v -t COM1 |\n"; - *standardOutput << "| |\n"; - *standardOutput << "| Perform a quick compare of FW in file with FW in device #1 |\n"; - *standardOutput << "| OPUploadTool -ch /home/user1/gpsp.opfw -t ttyUSB0 |\n"; - *standardOutput << "| |\n"; - // *standardOutput << "| *requires valid user space firmwares already running |\n"; - *standardOutput << "|________________________________________________________________________|\n"; - *standardOutput << endl; + *standardOutput << "Options:\n"; + *standardOutput << "-ls : lists available devices\n"; + *standardOutput << "-p : program hw (requires:-d - optionals:-v,-w)\n"; + *standardOutput << "-v : verify (requires:-d)\n"; + *standardOutput << "-dn : download firmware to file\n"; + // *standardOutput << "-dd : download discription (requires:-d)\n"; + *standardOutput << "-d : target device number (default 0, first device)\n"; + // *standardOutput << "-w : (requires: -p)\n"; + *standardOutput << "-ca : compares byte by byte current firmware with file\n"; + *standardOutput << "-cc : compares CRC of current firmware with file\n"; + *standardOutput << "-s : requests status of device\n"; + *standardOutput << "-r : resets the device\n"; + *standardOutput << "-j : exits bootloader and jumps to user FW\n"; + *standardOutput << "-debug : prints debug information\n"; + *standardOutput << "-t : uses serial port\n"; + *standardOutput << "-i : immediate, doesn't show the connection countdown\n"; + *standardOutput << "-h : print usage\n"; + // *standardOutput << "-ur : user mode reset*\n"; + // *standardOutput << " *requires valid user space firmwares already running\n"; + *standardOutput << "\n"; + *standardOutput << "Program and verify the first device connected to COM1\n"; + *standardOutput << " UploadTool -p c:/gpsp.opfw -v -t COM1\n"; + *standardOutput << "\n"; + *standardOutput << "Perform a quick compare of FW in file with FW on device #1\n"; + *standardOutput << " UploadTool -ca /home/user1/gpsp.opfw -t ttyUSB0\n"; } void howToUsage(QTextStream *standardOutput) { - *standardOutput << "run the tool with -? for more informations" << endl; + *standardOutput << "run the tool with -h for more informations" << endl; } diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp deleted file mode 100644 index f2b6071da1..0000000000 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp +++ /dev/null @@ -1,1161 +0,0 @@ -/** - ****************************************************************************** - * - * @file op_dfu.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup Uploader Uploader Plugin - * @{ - * @brief The uploader plugin - *****************************************************************************/ -/* - * 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 "op_dfu.h" -#include -#include -#include -#include - -using namespace OP_DFU; - -DFUObject::DFUObject(bool _debug, bool _use_serial, QString portname) : - debug(_debug), use_serial(_use_serial), mready(true) -{ - info = NULL; - numberOfDevices = 0; - - qRegisterMetaType("Status"); - - if (use_serial) { - info = new port(portname, false); - info->rxBuf = sspRxBuf; - info->rxBufSize = MAX_PACKET_DATA_LEN; - info->txBuf = sspTxBuf; - info->txBufSize = MAX_PACKET_DATA_LEN; - info->max_retry = 3; - info->timeoutLen = 100; - if (info->status() != port::open) { - cout << "Could not open serial port\n"; - mready = false; - return; - } - serialhandle = new qsspt(info, false); - - int count = 0; - while ((serialhandle->ssp_Synchronise() == false) && (count < 10)) { - if (debug) { - qDebug() << "SYNC failed, resending"; - } - count++; - } - if (count == 10) { - mready = false; - return; - } - qDebug() << "SYNC Succeded"; - serialhandle->start(); -/* } else { - mready = false; - QEventLoop m_eventloop; - QTimer::singleShot(200, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); - QList devices; - devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); - if (devices.length() == 1) { - if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { - mready = true; - QTimer::singleShot(200, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); - } else { - hidHandle.close(0); - } - } else { - // Wait for the board to appear on the USB bus: - USBSignalFilter filter(0x20a0, -1, -1, USBMonitor::Bootloader); - connect(&filter, SIGNAL(deviceDiscovered()), &m_eventloop, SLOT(quit())); - for (int x = 0; x < 4; ++x) { - qDebug() << "OP_DFU trying to detect bootloader:" << x; - - if (x == 0) { - QTimer::singleShot(10000, &m_eventloop, SLOT(quit())); - } else { - QTimer::singleShot(2000, &m_eventloop, SLOT(quit())); - } - m_eventloop.exec(); - devices = USBMonitor::instance()->availableDevices(0x20a0, -1, -1, USBMonitor::Bootloader); - qDebug() << "Devices length: " << devices.length(); - if (devices.length() == 1) { - qDebug() << "Opening device"; - if (hidHandle.open(1, devices.first().vendorID, devices.first().productID, 0, 0) == 1) { - QTimer::singleShot(200, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); - qDebug() << "OP_DFU detected after delay"; - mready = true; - qDebug() << "Detected"; - break; - } else { - hidHandle.close(0); - } - } else { - qDebug() << devices.length() << " device(s) detected, don't know what to do!"; - mready = false; - } - } - }*/ - } -} - -DFUObject::~DFUObject() -{ - if (use_serial) { - if (mready) { - delete serialhandle; - delete info; - } - } /*else { - hidHandle.close(0); - }*/ -} - -void DFUObject::sendReset(void) -{ - qDebug() << "Requesting user mode reset"; - uint8_t aa[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x62, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // 63 - uint8_t ba[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0xB9, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; - uint8_t ca[255] = { 0x02, 0x3E, 0x3C, 0x20, 0x75, 0x00, 0x20, 0x4F, 0x67, 0x34, 0x10, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; - uint8_t bb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x8D, 0x19, 0x00, 0x00, 0x13 }; - uint8_t ab[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x1B, 0x19, 0x00, 0x00, 0x76 }; - uint8_t cb[255] = { 0x02, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x78, 0x1C, 0x00, 0x00, 0x25 }; - // 125 - /*if (!use_serial) { - hidHandle.send(0, aa, 64, 5000); - hidHandle.send(0, ab, 64, 5000); - delay::msleep(600); - hidHandle.send(0, ba, 64, 5000); - hidHandle.send(0, bb, 64, 5000); - delay::msleep(600); - hidHandle.send(0, ca, 64, 5000); - hidHandle.send(0, cb, 64, 5000); - delay::msleep(100); - hidHandle.close(1); - } else {*/ - char a[255]; - char b[255]; - char c[255]; - memcpy(a, aa + 2, 62); - memcpy(a + 62, ab + 2, 60); - memcpy(b, ba + 2, 62); - memcpy(b + 62, bb + 2, 60); - memcpy(c, ca + 2, 62); - memcpy(c + 62, cb + 2, 60); - for (int x = 0; x < 123; ++x) { - info->pfSerialWrite(a[x]); - } - delay::msleep(600); - for (int x = 0; x < 123; ++x) { - info->pfSerialWrite(b[x]); - } - delay::msleep(600); - for (int x = 0; x < 123; ++x) { - info->pfSerialWrite(c[x]); - } - // } -} - -bool DFUObject::SaveByteArrayToFile(QString const & sfile, const QByteArray &array) -{ - QFile file(sfile); - - if (!file.open(QIODevice::WriteOnly)) { - if (debug) { - qDebug() << "Can't open file"; - } - return false; - } - file.write(array); - file.close(); - return true; -} - -/** - Tells the mainboard to enter DFU Mode. - */ -bool DFUObject::enterDFU(int const &devNumber) -{ - char buf[BUF_LEN]; - - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::EnterDFU; // DFU Command - buf[2] = 0; // DFU Count - buf[3] = 0; // DFU Count - buf[4] = 0; // DFU Count - buf[5] = 0; // DFU Count - buf[6] = devNumber; // DFU Data0 - buf[7] = 1; // DFU Data1 - buf[8] = 1; // DFU Data2 - buf[9] = 1; // DFU Data3 - - int result = sendData(buf, BUF_LEN); - - if (result < 1) { - return false; - } - if (debug) { - qDebug() << "EnterDFU: " << result << " bytes sent"; - } - return true; -} - -/** - Tells the board to get ready for an upload. It will in particular - erase the memory to make room for the data. You will have to query - its status to wait until erase is done before doing the actual upload. - */ -bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & type, quint32 crc) -{ - int lastPacketCount; - qint32 numberOfPackets = numberOfBytes / 4 / 14; - int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; - - if (pad == 0) { - lastPacketCount = 14; - } else { - ++numberOfPackets; - lastPacketCount = pad; - } - char buf[BUF_LEN]; - buf[0] = 0x02; // reportID - buf[1] = setStartBit(OP_DFU::Upload); // DFU Command - buf[2] = numberOfPackets >> 24; // DFU Count - buf[3] = numberOfPackets >> 16; // DFU Count - buf[4] = numberOfPackets >> 8; // DFU Count - buf[5] = numberOfPackets; // DFU Count - buf[6] = (int)type; // DFU Data0 - buf[7] = lastPacketCount; // DFU Data1 - buf[8] = crc >> 24; - buf[9] = crc >> 16; - buf[10] = crc >> 8; - buf[11] = crc; - if (debug) { - qDebug() << "Number of packets:" << numberOfPackets << " Size of last packet:" << lastPacketCount; - } - - int result = sendData(buf, BUF_LEN); - delay::msleep(1000); - - if (debug) { - qDebug() << result << " bytes sent"; - } - if (result > 0) { - return true; - } - return false; -} - - -/** - Does the actual data upload to the board. Needs to be called once the - board is ready to accept data following a StartUpload command, and it is erased. - */ -bool DFUObject::UploadData(qint32 const & numberOfBytes, QByteArray & data) -{ - int lastPacketCount; - qint32 numberOfPackets = numberOfBytes / 4 / 14; - int pad = (numberOfBytes - numberOfPackets * 4 * 14) / 4; - - if (pad == 0) { - lastPacketCount = 14; - } else { - ++numberOfPackets; - lastPacketCount = pad; - } - if (debug) { - qDebug() << "Start Uploading:" << numberOfPackets << "4Bytes"; - } - char buf[BUF_LEN]; - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Upload; // DFU Command - int packetsize; - float percentage; - int laspercentage = 0; - for (qint32 packetcount = 0; packetcount < numberOfPackets; ++packetcount) { - percentage = (float)(packetcount + 1) / numberOfPackets * 100; - if (laspercentage != (int)percentage) { - printProgBar((int)percentage, "UPLOADING"); - } - laspercentage = (int)percentage; - if (packetcount == numberOfPackets) { - packetsize = lastPacketCount; - } else { - packetsize = 14; - } - // qDebug()<> 24; // DFU Count - buf[3] = packetcount >> 16; // DFU Count - buf[4] = packetcount >> 8; // DFU Count - buf[5] = packetcount; // DFU Count - char *pointer = data.data(); - pointer = pointer + 4 * 14 * packetcount; - // if (debug) { - // qDebug() << "Packet Number=" << packetcount << "Data0=" << (int)data[0] << " Data1=" << (int)data[1] << " Data0=" << (int)data[2] << " Data0=" << (int)data[3] << " buf6=" << (int)buf[6] << " buf7=" << (int)buf[7] << " buf8=" << (int)buf[8] << " buf9=" << (int)buf[9]; - // } - CopyWords(pointer, buf + 6, packetsize * 4); - // for (int y=0;yappend(buf + 6, size); - } - - StatusRequest(); - return true; -} - - -/** - Resets the device - */ -int DFUObject::ResetDevice(void) -{ - char buf[BUF_LEN]; - - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Reset; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = 0; - buf[7] = 0; - buf[8] = 0; - buf[9] = 0; - - return sendData(buf, BUF_LEN); - // return hidHandle.send(0,buf, BUF_LEN, 500); -} - -int DFUObject::AbortOperation(void) -{ - char buf[BUF_LEN]; - - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Abort_Operation; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = 0; - buf[7] = 0; - buf[8] = 0; - buf[9] = 0; - - return sendData(buf, BUF_LEN); -} -/** - Starts the firmware (leaves bootloader and boots the main software) - */ -int DFUObject::JumpToApp(bool safeboot, bool erase) -{ - char buf[BUF_LEN]; - - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::JumpFW; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = 0; - buf[7] = 0; - if (safeboot) { - /* force system to safe boot mode (hwsettings == defaults) */ - buf[8] = 0x5A; - buf[9] = 0xFE; - } else { - buf[8] = 0; - buf[9] = 0; - } - if (erase) { - // force data flash clear - buf[10] = 0x00; - buf[11] = 0x00; - buf[12] = 0xFA; - buf[13] = 0x5F; - - buf[14] = 0x00; - buf[15] = 0x00; - buf[16] = 0x00; - buf[17] = 0x01; - - buf[18] = 0x00; - buf[19] = 0x00; - buf[20] = 0x00; - buf[21] = 0x00; - } else { - buf[10] = 0x00; - buf[11] = 0x00; - buf[12] = 0x00; - buf[13] = 0x00; - - buf[14] = 0x00; - buf[15] = 0x00; - buf[16] = 0x00; - buf[17] = 0x00; - - buf[18] = 0x00; - buf[19] = 0x00; - buf[20] = 0x00; - buf[21] = 0x00; - } - - return sendData(buf, BUF_LEN); -} - -OP_DFU::Status DFUObject::StatusRequest() -{ - char buf[BUF_LEN]; - - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Status_Request; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = 0; - buf[7] = 0; - buf[8] = 0; - buf[9] = 0; - - int result = sendData(buf, BUF_LEN); - if (debug) { - qDebug() << "StatusRequest: " << result << " bytes sent"; - } - result = receiveData(buf, BUF_LEN); - if (debug) { - qDebug() << "StatusRequest: " << result << " bytes received"; - } - if (buf[1] == OP_DFU::Status_Rep) { - return (OP_DFU::Status)buf[6]; - } else { - return OP_DFU::abort; - } -} - -/** - Ask the bootloader for the list of devices available - */ -bool DFUObject::findDevices() -{ - devices.clear(); - char buf[BUF_LEN]; - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Req_Capabilities; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = 0; - buf[7] = 0; - buf[8] = 0; - buf[9] = 0; - - int result = sendData(buf, BUF_LEN); - if (result < 1) { - return false; - } - result = receiveData(buf, BUF_LEN); - if (result < 1) { - return false; - } - - numberOfDevices = buf[7]; - RWFlags = buf[8]; - RWFlags = RWFlags << 8 | buf[9]; - - if (buf[1] == OP_DFU::Rep_Capabilities) { - for (int x = 0; x < numberOfDevices; ++x) { - device dev; - dev.Readable = (bool)(RWFlags >> (x * 2) & 1); - dev.Writable = (bool)(RWFlags >> (x * 2 + 1) & 1); - devices.append(dev); - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Req_Capabilities; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = x + 1; - buf[7] = 0; - buf[8] = 0; - buf[9] = 0; - sendData(buf, BUF_LEN); - receiveData(buf, BUF_LEN); - devices[x].ID = buf[14]; - devices[x].ID = devices[x].ID << 8 | (quint8)buf[15]; - devices[x].BL_Version = buf[7]; - devices[x].SizeOfDesc = buf[8]; - - quint32 aux; - aux = (quint8)buf[10]; - aux = aux << 8 | (quint8)buf[11]; - aux = aux << 8 | (quint8)buf[12]; - aux = aux << 8 | (quint8)buf[13]; - - devices[x].FW_CRC = aux; - - - aux = (quint8)buf[2]; - aux = aux << 8 | (quint8)buf[3]; - aux = aux << 8 | (quint8)buf[4]; - aux = aux << 8 | (quint8)buf[5]; - devices[x].SizeOfCode = aux; - } - if (debug) { - qDebug() << "Found " << numberOfDevices << " devices"; - for (int x = 0; x < numberOfDevices; ++x) { - qDebug() << "Device #" << x + 1; - qDebug() << "Device ID=" << devices[x].ID; - qDebug() << "Device Readable=" << devices[x].Readable; - qDebug() << "Device Writable=" << devices[x].Writable; - qDebug() << "Device SizeOfCode=" << devices[x].SizeOfCode; - qDebug() << "Device SizeOfDesc=" << devices[x].SizeOfDesc; - qDebug() << "BL Version=" << devices[x].BL_Version; - qDebug() << "FW CRC=" << devices[x].FW_CRC; - } - } - } - return true; -} - - -bool DFUObject::EndOperation() -{ - char buf[BUF_LEN]; - - buf[0] = 0x02; // reportID - buf[1] = OP_DFU::Op_END; // DFU Command - buf[2] = 0; - buf[3] = 0; - buf[4] = 0; - buf[5] = 0; - buf[6] = 0; - buf[7] = 0; - buf[8] = 0; - buf[9] = 0; - - int result = sendData(buf, BUF_LEN); - if (debug) { - qDebug() << result << " bytes sent"; - } - if (result > 0) { - return true; - } - return false; -} - - -// -/** - Starts a firmware upload (asynchronous) - */ -bool DFUObject::UploadFirmware(const QString &sfile, const bool &verify, int device) -{ - if (isRunning()) { - return false; - } - requestedOperation = OP_DFU::Upload; - requestFilename = sfile; - requestDevice = device; - requestVerify = verify; - start(); - return true; -} - -OP_DFU::Status DFUObject::UploadFirmwareT(const QString &sfile, const bool &verify, int device) -{ - OP_DFU::Status ret; - - if (debug) { - qDebug() << "Starting Firmware Uploading..."; - } - - QFile file(sfile); - - if (!file.open(QIODevice::ReadOnly)) { - if (debug) { - qDebug() << "Cant open file"; - } - return OP_DFU::abort; - } - - QByteArray arr = file.readAll(); - - if (debug) { - qDebug() << "Bytes Loaded=" << arr.length(); - } - if (arr.length() % 4 != 0) { - int pad = arr.length() / 4; - ++pad; - pad = pad * 4; - pad = pad - arr.length(); - arr.append(QByteArray(pad, 255)); - } - if (devices[device].SizeOfCode < (quint32)arr.length()) { - if (debug) { - qDebug() << "ERROR file to big for device"; - } - return OP_DFU::abort;; - } - - quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode); - if (debug) { - qDebug() << "NEW FIRMWARE CRC=" << crc; - } - - if (!StartUpload(arr.length(), OP_DFU::FW, crc)) { - ret = StatusRequest(); - if (debug) { - qDebug() << "StartUpload failed"; - qDebug() << "StartUpload returned:" << StatusToString(ret); - } - return ret; - } - - emit operationProgress(QString("Erasing, please wait...")); - - if (debug) { - qDebug() << "Erasing memory"; - } - if (StatusRequest() == OP_DFU::abort) { - return OP_DFU::abort; - } - - // TODO: why is there a loop there? The "if" statement - // will cause a break or return anyway!! - for (int x = 0; x < 3; ++x) { - ret = StatusRequest(); - if (debug) { - qDebug() << "Erase returned: " << StatusToString(ret); - } - if (ret == OP_DFU::uploading) { - break; - } else { - return ret; - } - } - - emit operationProgress(QString("Uploading firmware")); - if (!UploadData(arr.length(), arr)) { - ret = StatusRequest(); - if (debug) { - qDebug() << "Upload failed (upload data)"; - qDebug() << "UploadData returned:" << StatusToString(ret); - } - return ret; - } - if (!EndOperation()) { - ret = StatusRequest(); - if (debug) { - qDebug() << "Upload failed (end operation)"; - qDebug() << "EndOperation returned:" << StatusToString(ret); - } - return ret; - } - ret = StatusRequest(); - if (ret != OP_DFU::Last_operation_Success) { - return ret; - } - - if (verify) { - emit operationProgress(QString("Verifying firmware")); - cout << "Starting code verification\n"; - QByteArray arr2; - StartDownloadT(&arr2, arr.length(), OP_DFU::FW); - if (arr != arr2) { - cout << "Verify:FAILED\n"; - return OP_DFU::abort; - } - } - - if (debug) { - qDebug() << "Status=" << ret; - } - cout << "Firmware Uploading succeeded\n"; - return ret; -} - - -OP_DFU::Status DFUObject::CompareFirmware(const QString &sfile, const CompareType &type, int device) -{ - cout << "Starting Firmware Compare...\n"; - QFile file(sfile); - if (!file.open(QIODevice::ReadOnly)) { - if (debug) { - qDebug() << "Cant open file"; - } - return OP_DFU::abort; - } - QByteArray arr = file.readAll(); - - if (debug) { - qDebug() << "Bytes Loaded=" << arr.length(); - } - if (arr.length() % 4 != 0) { - int pad = arr.length() / 4; - ++pad; - pad = pad * 4; - pad = pad - arr.length(); - arr.append(QByteArray(pad, 255)); - } - if (type == OP_DFU::crccompare) { - quint32 crc = DFUObject::CRCFromQBArray(arr, devices[device].SizeOfCode); - if (crc == devices[device].FW_CRC) { - cout << "Compare Successfull CRC MATCH!\n"; - } else { - cout << "Compare failed CRC DONT MATCH!\n"; - } - return StatusRequest(); - } else { - QByteArray arr2; - StartDownloadT(&arr2, arr.length(), OP_DFU::FW); - if (arr == arr2) { - cout << "Compare Successfull ALL Bytes MATCH!\n"; - } else { - cout << "Compare failed Bytes DONT MATCH!\n"; - } - return StatusRequest(); - } -} - -void DFUObject::CopyWords(char *source, char *destination, int count) -{ - for (int x = 0; x < count; x = x + 4) { - *(destination + x) = source[x + 3]; - *(destination + x + 1) = source[x + 2]; - *(destination + x + 2) = source[x + 1]; - *(destination + x + 3) = source[x + 0]; - } -} -QString DFUObject::StatusToString(OP_DFU::Status const & status) -{ - switch (status) { - case DFUidle: - return "DFUidle"; - - case uploading: - return ""; - - case wrong_packet_received: - return "wrong_packet_received"; - - case too_many_packets: - return "too_many_packets"; - - case too_few_packets: - return "too_few_packets"; - - case Last_operation_Success: - return "Last_operation_Success"; - - case downloading: - return "downloading"; - - case idle: - return "idle"; - - case Last_operation_failed: - return "Last_operation_failed"; - - case outsideDevCapabilities: - return "outsideDevCapabilities"; - - case CRC_Fail: - return "CRC check FAILED"; - - case failed_jump: - return "Jmp to user FW failed"; - - case abort: - return "abort"; - - case uploadingStarting: - return "Uploading Starting"; - - default: - return "unknown"; - } -} - -/** - Prints a progress bar with percentage & label during an operation. - - Also outputs to stdout if we are in debug mode. - */ -void DFUObject::printProgBar(int const & percent, QString const & label) -{ - std::string bar; - - emit(progressUpdated(percent)); - - if (debug) { - for (int i = 0; i < 50; i++) { - if (i < (percent / 2)) { - bar.replace(i, 1, "="); - } else if (i == (percent / 2)) { - bar.replace(i, 1, ">"); - } else { - bar.replace(i, 1, " "); - } - } - - std::cout << "\r" << label.toLatin1().data() << "[" << bar << "] "; - std::cout.width(3); - std::cout << percent << "% " << std::flush; - } -} - -/** - Utility function - */ -quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) -{ - // Size = Size >> 2; // /4 Size passed in as a byte count, assumed to be a multiple of 4 - - while (Size--) { - static const quint32 CrcTable[16] = { // Nibble lookup table for 0x04C11DB7 polynomial - 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005, - 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD - }; - - Crc = Crc ^ *((quint32 *)Buffer); // Apply all 32-bits - - Buffer += 1; - - // Process 32-bits, 4 at a time, or 8 rounds - - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // Assumes 32-bit reg, masking index to 4-bits - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; // 0x04C11DB7 Polynomial used in STM32 - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; - Crc = (Crc << 4) ^ CrcTable[Crc >> 28]; - } - - return Crc; -} - -/** - Utility function - */ -quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size) -{ - quint32 pad = Size - array.length(); - - array.append(QByteArray(pad, 255)); - quint32 t[Size / 4]; - for (int x = 0; x < array.length() / 4; x++) { - quint32 aux = 0; - aux = (char)array[x * 4 + 3] & 0xFF; - aux = aux << 8; - aux += (char)array[x * 4 + 2] & 0xFF; - aux = aux << 8; - aux += (char)array[x * 4 + 1] & 0xFF; - aux = aux << 8; - aux += (char)array[x * 4 + 0] & 0xFF; - t[x] = aux; - } - return DFUObject::CRC32WideFast(0xFFFFFFFF, Size / 4, (quint32 *)t); -} - - -/** - Send data to the bootloader, either through the serial port - of through the HID handle, depending on the mode we're using - */ -int DFUObject::sendData(void *data, int size) -{ - /*if (!use_serial) { - return hidHandle.send(0, data, size, 5000); - }*/ - - // Serial Mode: - if (serialhandle->sendData((uint8_t *)data + 1, size - 1)) { - if (debug) { - qDebug() << "packet sent" << "data0" << ((uint8_t *)data + 1)[0]; - } - return size; - } - if (debug) { - qDebug() << "Serial send OVERRUN"; - } - return -1; -} - - -/** - Receive data from the bootloader, either through the serial port - of through the HID handle, depending on the mode we're using - */ -int DFUObject::receiveData(void *data, int size) -{ - /*if (!use_serial) { - return hidHandle.receive(0, data, size, 10000); - }*/ - - // Serial Mode: - int x; - QTime time; - - time.start(); - while (true) { - if ((x = serialhandle->read_Packet(((char *)data) + 1) != -1) || time.elapsed() > 10000) { - msleep(10); - if (time.elapsed() > 10000) { - qDebug() << "____timeout"; - } - if (x > size - 1) { - qDebug() << "Error buffer overrun"; - Q_ASSERT(false); - } - return x; - } - } -} - -#define BOARD_ID_MB 1 -#define BOARD_ID_INS 2 -#define BOARD_ID_PIP 3 -#define BOARD_ID_CC 4 -#define BOARD_ID_REVO 9 -#define BOARD_ID_SPARKY2 0x92 - -/** - Gets the type of board connected - */ -OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum) -{ - OP_DFU::eBoardType brdType = eBoardUnkwn; - - // First of all, check what Board type we are talking to - int board = devices[boardNum].ID; - - qDebug() << "Board model: " << board; - switch (board >> 8) { - case BOARD_ID_MB: // Mainboard family - brdType = eBoardMainbrd; - break; - case BOARD_ID_INS: // Inertial Nav - brdType = eBoardINS; - break; - case BOARD_ID_PIP: // PIP RF Modem - brdType = eBoardPip; - break; - case BOARD_ID_CC: // CopterControl family - brdType = eBoardCC; - break; - case BOARD_ID_REVO: // Revo board - brdType = eBoardRevo; - break; - case BOARD_ID_SPARKY2: // Sparky2 board - brdType = eBoardSparky2; - break; - } - return brdType; -} diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h deleted file mode 100644 index ed84865ec7..0000000000 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/op_dfu.h +++ /dev/null @@ -1,235 +0,0 @@ -#ifndef OP_DFU_H -#define OP_DFU_H - -#include -// #include -// #include -// #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "delay.h" -#include -#include -#include -#include -#include "SSP/qssp.h" -#include "SSP/port.h" -#include "SSP/qsspt.h" - -using namespace std; -#define BUF_LEN 64 - -#define MAX_PACKET_DATA_LEN 255 -#define MAX_PACKET_BUF_SIZE (1 + 1 + MAX_PACKET_DATA_LEN + 2) - -namespace OP_DFU { -enum TransferTypes { - FW, - Descript -}; - -enum CompareType { - crccompare, - bytetobytecompare -}; -// Command Line Options -#define DOWNLOAD "-dn" // done -#define DEVICE "-d" // done -#define DOWNDESCRIPTION "-dd" // done -#define PROGRAMFW "-p" // done -#define PROGRAMDESC "-w" // done -#define VERIFY "-v" // done -#define COMPARECRC "-cc" -#define COMPAREALL "-ca" -#define STATUSREQUEST "-s" // done -#define LISTDEVICES "-ls" // done -#define RESET "-r" -#define JUMP "-j" -#define USE_SERIAL "-t" -#define NO_COUNTDOWN "-i" - -Q_ENUMS(Status) -enum Status { - DFUidle, // 0 - uploading, // 1 - wrong_packet_received, // 2 - too_many_packets, // 3 - too_few_packets, // 4 - Last_operation_Success, // 5 - downloading, // 6 - idle, // 7 - Last_operation_failed, // 8 - uploadingStarting, // 9 - outsideDevCapabilities, // 10 - CRC_Fail, // 11 - failed_jump, // 12 - abort // 13 -}; - -enum Actions { - actionNone, - actionProgram, - actionProgramAndVerify, - actionDownload, - actionCompareAll, - actionCompareCrc, - actionListDevs, - actionStatusReq, - actionReset, - actionJump -}; - -enum Commands { - Reserved, // 0 - Req_Capabilities, // 1 - Rep_Capabilities, // 2 - EnterDFU, // 3 - JumpFW, // 4 - Reset, // 5 - Abort_Operation, // 6 - Upload, // 7 - Op_END, // 8 - Download_Req, // 9 - Download, // 10 - Status_Request, // 11 - Status_Rep, // 12 -}; - -enum eBoardType { - eBoardUnkwn = 0, - eBoardMainbrd = 1, - eBoardINS, - eBoardPip = 3, - eBoardCC = 4, - eBoardRevo = 9, - eBoardSparky2 = 0x92, -}; - -struct device { - int ID; - quint32 FW_CRC; - int BL_Version; - int SizeOfDesc; - quint32 SizeOfCode; - bool Readable; - bool Writable; -}; - - -class DFUObject : public QThread { - Q_OBJECT; - -public: - static quint32 CRCFromQBArray(QByteArray array, quint32 Size); - // DFUObject(bool debug); - DFUObject(bool debug, bool use_serial, QString port); - - virtual ~DFUObject(); - - // Service commands: - bool enterDFU(int const &devNumber); - bool findDevices(); - int JumpToApp(bool safeboot, bool erase); - int ResetDevice(void); - void sendReset(void); - OP_DFU::Status StatusRequest(); - bool EndOperation(); - int AbortOperation(void); - bool ready() - { - return mready; - } - - // Upload (send to device) commands - OP_DFU::Status UploadDescription(QVariant description); - bool UploadFirmware(const QString &sfile, const bool &verify, int device); - - // Download (get from device) commands: - // DownloadDescription is synchronous - QString DownloadDescription(int const & numberOfChars); - QByteArray DownloadDescriptionAsBA(int const & numberOfChars); - // Asynchronous firmware download: initiates fw download, - // and a downloadFinished signal is emitted when download - // if finished: - bool DownloadFirmware(QByteArray *byteArray, int device); - - // Comparison functions (is this needed?) - OP_DFU::Status CompareFirmware(const QString &sfile, const CompareType &type, int device); - - bool SaveByteArrayToFile(QString const & file, QByteArray const &array); - - - // Variables: - QList devices; - int numberOfDevices; - int send_delay; - bool use_delay; - - // Helper functions: - QString StatusToString(OP_DFU::Status const & status); - static quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer); - OP_DFU::eBoardType GetBoardType(int boardNum); - - -signals: - void progressUpdated(int); - void downloadFinished(); - void uploadFinished(OP_DFU::Status); - void operationProgress(QString status); - -private: - // Generic variables: - bool debug; - bool use_serial; - bool mready; - int RWFlags; - qsspt *serialhandle; - int sendData(void *, int); - int receiveData(void *data, int size); - uint8_t sspTxBuf[MAX_PACKET_BUF_SIZE]; - uint8_t sspRxBuf[MAX_PACKET_BUF_SIZE]; - port *info; - - - // USB Bootloader: - // opHID_hidapi hidHandle; - int setStartBit(int command) - { - return command | 0x20; - } - - void CopyWords(char *source, char *destination, int count); - void printProgBar(int const & percent, QString const & label); - bool StartUpload(qint32 const &numberOfBytes, TransferTypes const & type, quint32 crc); - bool UploadData(qint32 const & numberOfPackets, QByteArray & data); - - // Thread management: - // Same as startDownload except that we store in an external array: - bool StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, TransferTypes const & type); - OP_DFU::Status UploadFirmwareT(const QString &sfile, const bool &verify, int device); - QMutex mutex; - OP_DFU::Commands requestedOperation; - qint32 requestSize; - OP_DFU::TransferTypes requestTransferType; - QByteArray *requestStorage; - QString requestFilename; - bool requestVerify; - int requestDevice; - -protected: - void run(); // Executes the upload or download operations -}; -} - -Q_DECLARE_METATYPE(OP_DFU::Status) - - -#endif // OP_DFU_H diff --git a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/upload.pro b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/upload.pro index 00f69b7673..2316e11af4 100644 --- a/ground/gcs/src/experimental/USB_UPLOAD_TOOL/upload.pro +++ b/ground/gcs/src/experimental/USB_UPLOAD_TOOL/upload.pro @@ -4,72 +4,27 @@ # #------------------------------------------------- -QT += core +TEMPLATE = app +TARGET = UploadTool -QT -= gui -QT += serialport +QT += core serialport +QT -= gui -TARGET = OPUploadTool CONFIG += console CONFIG -= app_bundle +# don't build both debug and release +CONFIG -= debug_and_release -TEMPLATE = app - -SOURCES += main.cpp \ - op_dfu.cpp \ - delay.cpp \ +SOURCES += \ + main.cpp \ + dfu.cpp \ ./SSP/qssp.cpp \ ./SSP/port.cpp \ ./SSP/qsspt.cpp \ -HEADERS += op_dfu.h \ - delay.h \ +HEADERS += \ + dfu.h \ ./SSP/qssp.h \ ./SSP/port.h \ ./SSP/common.h \ ./SSP/qsspt.h - -# win32 { -# SOURCES += ../../plugins/rawhid/pjrc_rawhid_win.cpp -# -# LIBS += -lhid \ -# -lsetupapi -#} - -#macx { -# SOURCES += ../../plugins/rawhid/pjrc_rawhid_mac.cpp -# SDK = /Developer/SDKs/MacOSX10.5.sdk -# ARCH = -mmacosx-version-min=10.5 \ -# -arch \ -# ppc \ -# -arch \ -# i386 -# LIBS += $(ARCH) \ -# -Wl,-syslibroot,$(SDK) \ -# -framework \ -# IOKit \ -# -framework \ -# CoreFoundation -#} -#linux-g++ { -# SOURCES += ../../plugins/rawhid/pjrc_rawhid_unix.cpp -# LIBS += -lusb -#} -#linux-g++-64 { -# SOURCES += ../../plugins/rawhid/pjrc_rawhid_unix.cpp -# LIBS += -lusb -#} - -#unix:SOURCES += ../../libs/qextserialport/src/posix_qextserialport.cpp -#unix:!macx:SOURCES += ../../libs/qextserialport/src/qextserialenumerator_unix.cpp -#macx { -# SOURCES += ../../libs/qextserialport/src/qextserialenumerator_osx.cpp -# LIBS += -framework IOKit -framework CoreFoundation -#} - -#win32 { -# SOURCES += ../../libs/qextserialport/src/win_qextserialport.cpp \ -#../../libs/qextserialport/src/qextserialenumerator_win.cpp -# DEFINES += WINVER=0x0501 # needed for mingw to pull in appropriate dbt business...probably a better way to do this -# LIBS += -lsetupapi -#} diff --git a/ground/gcs/src/libs/eigen/.hg_archival.txt b/ground/gcs/src/libs/eigen/.hg_archival.txt index c4115d7f6b..0e10abfa21 100644 --- a/ground/gcs/src/libs/eigen/.hg_archival.txt +++ b/ground/gcs/src/libs/eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15 -branch: 3.2 -tag: 3.2.8 +node: 67e894c6cd8f5f1f604b27d37ed47fdf012674ff +branch: 3.3 +tag: 3.3.3 diff --git a/ground/gcs/src/libs/eigen/.hgeol b/ground/gcs/src/libs/eigen/.hgeol index 423676d31b..5327df1615 100644 --- a/ground/gcs/src/libs/eigen/.hgeol +++ b/ground/gcs/src/libs/eigen/.hgeol @@ -1,6 +1,9 @@ [patterns] +*.sh = LF +*.MINPACK = CRLF scripts/*.in = LF debug/msvc/*.dat = CRLF +debug/msvc/*.natvis = CRLF unsupported/test/mpreal/*.* = CRLF ** = native diff --git a/ground/gcs/src/libs/eigen/.hgignore b/ground/gcs/src/libs/eigen/.hgignore index e33ba2e9dc..769a47f1f4 100644 --- a/ground/gcs/src/libs/eigen/.hgignore +++ b/ground/gcs/src/libs/eigen/.hgignore @@ -30,3 +30,5 @@ log patch a a.* +lapack/testing +lapack/reference diff --git a/ground/gcs/src/libs/eigen/.hgtags b/ground/gcs/src/libs/eigen/.hgtags index 8f0097f20f..961b719a9d 100644 --- a/ground/gcs/src/libs/eigen/.hgtags +++ b/ground/gcs/src/libs/eigen/.hgtags @@ -21,13 +21,12 @@ a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1 8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1 bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2 da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1 -4b687cad1d23066f66863f4f87298447298443df 3.2-rc1 -1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2 -ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 -6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1 -1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2 -36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3 -10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4 -bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5 -c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6 -b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7 +a8e0d153fc5e239ef8b06e3665f1f9e8cb8d49c8 before-evaluators +09a8e21866106b49c5dec1d6d543e5794e82efa0 3.3-alpha1 +ce5a455b34c0a0ac3545a1497cb4a16c38ed90e8 3.3-beta1 +69d418c0699907bcd0bf9e0b3ba0a112ed091d85 3.3-beta2 +bef509908b9da05d0d07ffc0da105e2c8c6d3996 3.3-rc1 +04ab5fa4b241754afcf631117572276444c67239 3.3-rc2 +26667be4f70baf4f0d39e96f330714c87b399090 3.3.0 +f562a193118d4f40514e2f4a0ace6e974926ef06 3.3.1 +da9b4e14c2550e0d11078a3c39e6d56eba9905df 3.3.2 diff --git a/ground/gcs/src/libs/eigen/CMakeLists.txt b/ground/gcs/src/libs/eigen/CMakeLists.txt index 77e9f2d354..f5840025b8 100644 --- a/ground/gcs/src/libs/eigen/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/CMakeLists.txt @@ -1,4 +1,5 @@ -project(Eigen) +project(Eigen3) + cmake_minimum_required(VERSION 2.8.5) # guard against in-source builds @@ -7,6 +8,11 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() +# Alias Eigen_*_DIR to Eigen3_*_DIR: + +set(Eigen_SOURCE_DIR ${Eigen3_SOURCE_DIR}) +set(Eigen_BINARY_DIR ${Eigen3_BINARY_DIR}) + # guard against bad build-type strings if (NOT CMAKE_BUILD_TYPE) @@ -92,9 +98,11 @@ else() endif() option(EIGEN_BUILD_BTL "Build benchmark suite" OFF) -if(NOT WIN32) + +# Disable pkgconfig only for native Windows builds +if(NOT WIN32 OR NOT CMAKE_HOST_SYSTEM_NAME MATCHES Windows) option(EIGEN_BUILD_PKGCONFIG "Build pkg-config .pc file for Eigen" ON) -endif(NOT WIN32) +endif() set(CMAKE_INCLUDE_CURRENT_DIR ON) @@ -108,7 +116,8 @@ endif() set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320") macro(ei_add_cxx_compiler_flag FLAG) - string(REGEX REPLACE "-" "" SFLAG ${FLAG}) + string(REGEX REPLACE "-" "" SFLAG1 ${FLAG}) + string(REGEX REPLACE "\\+" "p" SFLAG ${SFLAG1}) check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG}) if(COMPILER_SUPPORT_${SFLAG}) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}") @@ -117,18 +126,13 @@ endmacro(ei_add_cxx_compiler_flag) if(NOT MSVC) # We assume that other compilers are partly compatible with GNUCC - - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexceptions") - set(CMAKE_CXX_FLAGS_DEBUG "-g3") - set(CMAKE_CXX_FLAGS_RELEASE "-g0 -O2") - - # clang outputs some warnings for unknwon flags that are not caught by check_cxx_compiler_flag + + # clang outputs some warnings for unknown flags that are not caught by check_cxx_compiler_flag # adding -Werror turns such warnings into errors check_cxx_compiler_flag("-Werror" COMPILER_SUPPORT_WERROR) if(COMPILER_SUPPORT_WERROR) set(CMAKE_REQUIRED_FLAGS "-Werror") endif() - ei_add_cxx_compiler_flag("-pedantic") ei_add_cxx_compiler_flag("-Wall") ei_add_cxx_compiler_flag("-Wextra") @@ -142,6 +146,18 @@ if(NOT MSVC) ei_add_cxx_compiler_flag("-Wpointer-arith") ei_add_cxx_compiler_flag("-Wwrite-strings") ei_add_cxx_compiler_flag("-Wformat-security") + ei_add_cxx_compiler_flag("-Wshorten-64-to-32") + ei_add_cxx_compiler_flag("-Wlogical-op") + ei_add_cxx_compiler_flag("-Wenum-conversion") + ei_add_cxx_compiler_flag("-Wc++11-extensions") + ei_add_cxx_compiler_flag("-Wdouble-promotion") +# ei_add_cxx_compiler_flag("-Wconversion") + + # -Wshadow is insanely too strict with gcc, hopefully it will become usable with gcc 6 + # if(NOT CMAKE_COMPILER_IS_GNUCXX OR (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER "5.0.0")) + if(NOT CMAKE_COMPILER_IS_GNUCXX) + ei_add_cxx_compiler_flag("-Wshadow") + endif() ei_add_cxx_compiler_flag("-Wno-psabi") ei_add_cxx_compiler_flag("-Wno-variadic-macros") @@ -151,7 +167,8 @@ if(NOT MSVC) ei_add_cxx_compiler_flag("-fno-common") ei_add_cxx_compiler_flag("-fstrict-aliasing") ei_add_cxx_compiler_flag("-wd981") # disable ICC's "operands are evaluated in unspecified order" remark - ei_add_cxx_compiler_flag("-wd2304") # disbale ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor + ei_add_cxx_compiler_flag("-wd2304") # disable ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor + # The -ansi flag must be added last, otherwise it is also used as a linker flag by check_cxx_compiler_flag making it fails # Moreover we should not set both -strict-ansi and -ansi @@ -163,6 +180,11 @@ if(NOT MSVC) else() ei_add_cxx_compiler_flag("-ansi") endif() + + if(ANDROID_NDK) + ei_add_cxx_compiler_flag("-pie") + ei_add_cxx_compiler_flag("-fPIE") + endif() set(CMAKE_REQUIRED_FLAGS "") @@ -196,18 +218,65 @@ if(NOT MSVC) message(STATUS "Enabling SSE4.2 in tests/examples") endif() + option(EIGEN_TEST_AVX "Enable/Disable AVX in tests/examples" OFF) + if(EIGEN_TEST_AVX) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx") + message(STATUS "Enabling AVX in tests/examples") + endif() + + option(EIGEN_TEST_FMA "Enable/Disable FMA in tests/examples" OFF) + if(EIGEN_TEST_FMA AND NOT EIGEN_TEST_NEON) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfma") + message(STATUS "Enabling FMA in tests/examples") + endif() + + option(EIGEN_TEST_AVX512 "Enable/Disable AVX512 in tests/examples" OFF) + if(EIGEN_TEST_AVX512) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx512f -fabi-version=6 -DEIGEN_ENABLE_AVX512") + message(STATUS "Enabling AVX512 in tests/examples") + endif() + + option(EIGEN_TEST_F16C "Enable/Disable F16C in tests/examples" OFF) + if(EIGEN_TEST_F16C) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mf16c") + message(STATUS "Enabling F16C in tests/examples") + endif() + option(EIGEN_TEST_ALTIVEC "Enable/Disable AltiVec in tests/examples" OFF) if(EIGEN_TEST_ALTIVEC) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -maltivec -mabi=altivec") message(STATUS "Enabling AltiVec in tests/examples") endif() + option(EIGEN_TEST_VSX "Enable/Disable VSX in tests/examples" OFF) + if(EIGEN_TEST_VSX) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m64 -mvsx") + message(STATUS "Enabling VSX in tests/examples") + endif() + option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF) if(EIGEN_TEST_NEON) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8") + if(EIGEN_TEST_FMA) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon-vfpv4") + else() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon") + endif() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfloat-abi=hard") + message(STATUS "Enabling NEON in tests/examples") + endif() + + option(EIGEN_TEST_NEON64 "Enable/Disable Neon in tests/examples" OFF) + if(EIGEN_TEST_NEON64) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") message(STATUS "Enabling NEON in tests/examples") endif() + option(EIGEN_TEST_ZVECTOR "Enable/Disable S390X(zEC13) ZVECTOR in tests/examples" OFF) + if(EIGEN_TEST_ZVECTOR) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=z13 -mzvector") + message(STATUS "Enabling S390X(zEC13) ZVECTOR in tests/examples") + endif() + check_cxx_compiler_flag("-fopenmp" COMPILER_SUPPORT_OPENMP) if(COMPILER_SUPPORT_OPENMP) option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF) @@ -284,11 +353,23 @@ if(EIGEN_TEST_NO_EXPLICIT_ALIGNMENT) message(STATUS "Disabling alignment in tests/examples") endif() -option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF) +option(EIGEN_TEST_NO_EXCEPTIONS "Disables C++ exceptions" OFF) +if(EIGEN_TEST_NO_EXCEPTIONS) + ei_add_cxx_compiler_flag("-fno-exceptions") + message(STATUS "Disabling exceptions in tests/examples") +endif() + +option(EIGEN_TEST_CXX11 "Enable testing with C++11 and C++11 features (e.g. Tensor module)." OFF) + +set(EIGEN_CUDA_COMPUTE_ARCH 30 CACHE STRING "The CUDA compute architecture level to target when compiling CUDA code") include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}) # Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR +if(EIGEN_INCLUDE_INSTALL_DIR) + message(WARNING "EIGEN_INCLUDE_INSTALL_DIR is deprecated. Use INCLUDE_INSTALL_DIR instead.") +endif() + if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR) set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed") @@ -298,9 +379,8 @@ else() CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed" ) endif() - set(CMAKEPACKAGE_INSTALL_DIR - "${CMAKE_INSTALL_LIBDIR}/cmake/eigen3" + "${CMAKE_INSTALL_DATADIR}/eigen3/cmake" CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed" ) set(PKGCONFIG_INSTALL_DIR @@ -308,6 +388,7 @@ set(PKGCONFIG_INSTALL_DIR CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed" ) + # similar to set_target_properties but append the property instead of overwriting it macro(ei_add_target_property target prop value) @@ -329,7 +410,7 @@ if(EIGEN_BUILD_PKGCONFIG) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc DESTINATION ${PKGCONFIG_INSTALL_DIR} ) -endif(EIGEN_BUILD_PKGCONFIG) +endif() add_subdirectory(Eigen) @@ -355,6 +436,13 @@ else() add_subdirectory(lapack EXCLUDE_FROM_ALL) endif() +# add SYCL +option(EIGEN_TEST_SYCL "Add Sycl support." OFF) +if(EIGEN_TEST_SYCL) + set (CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" "cmake/Modules/" "${CMAKE_MODULE_PATH}") + include(FindComputeCpp) +endif() + add_subdirectory(unsupported) add_subdirectory(demos EXCLUDE_FROM_ALL) @@ -403,6 +491,7 @@ if(cmake_generator_tolower MATCHES "makefile") message(STATUS "make check | Build and run the unit-tests. Read this page:") message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests") message(STATUS "make blas | Build BLAS library (not the same thing as Eigen)") + message(STATUS "make uninstall| Removes files installed by make install") message(STATUS "--------------+--------------------------------------------------------------") else() message(STATUS "To build/run the unit tests, read this page:") @@ -410,3 +499,98 @@ else() endif() message(STATUS "") + + +set ( EIGEN_VERSION_STRING ${EIGEN_VERSION_NUMBER} ) +set ( EIGEN_VERSION_MAJOR ${EIGEN_WORLD_VERSION} ) +set ( EIGEN_VERSION_MINOR ${EIGEN_MAJOR_VERSION} ) +set ( EIGEN_VERSION_PATCH ${EIGEN_MINOR_VERSION} ) +set ( EIGEN_DEFINITIONS "") +set ( EIGEN_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}" ) +set ( EIGEN_ROOT_DIR ${CMAKE_INSTALL_PREFIX} ) + +# Interface libraries require at least CMake 3.0 +if (NOT CMAKE_VERSION VERSION_LESS 3.0) + include (CMakePackageConfigHelpers) + + # Imported target support + add_library (eigen INTERFACE) + + target_compile_definitions (eigen INTERFACE ${EIGEN_DEFINITIONS}) + target_include_directories (eigen INTERFACE + $ + $ + ) + + # Export as title case Eigen + set_target_properties (eigen PROPERTIES EXPORT_NAME Eigen) + + install (TARGETS eigen EXPORT Eigen3Targets) + + configure_package_config_file ( + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3Config.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake + PATH_VARS EIGEN_INCLUDE_DIR EIGEN_ROOT_DIR + INSTALL_DESTINATION ${CMAKEPACKAGE_INSTALL_DIR} + NO_CHECK_REQUIRED_COMPONENTS_MACRO # Eigen does not provide components + ) + # Remove CMAKE_SIZEOF_VOID_P from Eigen3ConfigVersion.cmake since Eigen does + # not depend on architecture specific settings or libraries. More + # specifically, an Eigen3Config.cmake generated from a 64 bit target can be + # used for 32 bit targets as well (and vice versa). + set (_Eigen3_CMAKE_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P}) + unset (CMAKE_SIZEOF_VOID_P) + write_basic_package_version_file (Eigen3ConfigVersion.cmake + VERSION ${EIGEN_VERSION_NUMBER} + COMPATIBILITY SameMajorVersion) + set (CMAKE_SIZEOF_VOID_P ${_Eigen3_CMAKE_SIZEOF_VOID_P}) + + # The Eigen target will be located in the Eigen3 namespace. Other CMake + # targets can refer to it using Eigen3::Eigen. + export (TARGETS eigen NAMESPACE Eigen3:: FILE Eigen3Targets.cmake) + # Export Eigen3 package to CMake registry such that it can be easily found by + # CMake even if it has not been installed to a standard directory. + export (PACKAGE Eigen3) + + install (EXPORT Eigen3Targets NAMESPACE Eigen3:: DESTINATION ${CMAKEPACKAGE_INSTALL_DIR}) + +else (NOT CMAKE_VERSION VERSION_LESS 3.0) + # Fallback to legacy Eigen3Config.cmake without the imported target + + # If CMakePackageConfigHelpers module is available (CMake >= 2.8.8) + # create a relocatable Config file, otherwise leave the hardcoded paths + include(CMakePackageConfigHelpers OPTIONAL RESULT_VARIABLE CPCH_PATH) + + if(CPCH_PATH) + configure_package_config_file ( + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3ConfigLegacy.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake + PATH_VARS EIGEN_INCLUDE_DIR EIGEN_ROOT_DIR + INSTALL_DESTINATION ${CMAKEPACKAGE_INSTALL_DIR} + NO_CHECK_REQUIRED_COMPONENTS_MACRO # Eigen does not provide components + ) + else() + # The PACKAGE_* variables are defined by the configure_package_config_file + # but without it we define them manually to the hardcoded paths + set(PACKAGE_INIT "") + set(PACKAGE_EIGEN_INCLUDE_DIR ${EIGEN_INCLUDE_DIR}) + set(PACKAGE_EIGEN_ROOT_DIR ${EIGEN_ROOT_DIR}) + configure_file ( ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3ConfigLegacy.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake + @ONLY ESCAPE_QUOTES ) + endif() + + write_basic_package_version_file( Eigen3ConfigVersion.cmake + VERSION ${EIGEN_VERSION_NUMBER} + COMPATIBILITY SameMajorVersion ) + +endif (NOT CMAKE_VERSION VERSION_LESS 3.0) + +install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/cmake/UseEigen3.cmake + ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake + ${CMAKE_CURRENT_BINARY_DIR}/Eigen3ConfigVersion.cmake + DESTINATION ${CMAKEPACKAGE_INSTALL_DIR} ) + +# Add uninstall target +add_custom_target ( uninstall + COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/EigenUninstall.cmake) diff --git a/ground/gcs/src/libs/eigen/CTestConfig.cmake b/ground/gcs/src/libs/eigen/CTestConfig.cmake index 0557c491a3..755b473237 100644 --- a/ground/gcs/src/libs/eigen/CTestConfig.cmake +++ b/ground/gcs/src/libs/eigen/CTestConfig.cmake @@ -4,10 +4,10 @@ ## # The following are required to uses Dart and the Cdash dashboard ## ENABLE_TESTING() ## INCLUDE(CTest) -set(CTEST_PROJECT_NAME "Eigen3.2") +set(CTEST_PROJECT_NAME "Eigen3.3") set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC") set(CTEST_DROP_METHOD "http") set(CTEST_DROP_SITE "manao.inria.fr") -set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2") +set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.3") set(CTEST_DROP_SITE_CDASH TRUE) diff --git a/ground/gcs/src/libs/eigen/Eigen/Array b/ground/gcs/src/libs/eigen/Eigen/Array deleted file mode 100644 index 3d004fb69e..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/Array +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef EIGEN_ARRAY_MODULE_H -#define EIGEN_ARRAY_MODULE_H - -// include Core first to handle Eigen2 support macros -#include "Core" - -#ifndef EIGEN2_SUPPORT - #error The Eigen/Array header does no longer exist in Eigen3. All that functionality has moved to Eigen/Core. -#endif - -#endif // EIGEN_ARRAY_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/CMakeLists.txt b/ground/gcs/src/libs/eigen/Eigen/CMakeLists.txt index a92dd6f6ca..9eb502b792 100644 --- a/ground/gcs/src/libs/eigen/Eigen/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/Eigen/CMakeLists.txt @@ -16,4 +16,4 @@ install(FILES DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel ) -add_subdirectory(src) +install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") diff --git a/ground/gcs/src/libs/eigen/Eigen/Cholesky b/ground/gcs/src/libs/eigen/Eigen/Cholesky index f727f5d89c..369d1f5ec9 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Cholesky +++ b/ground/gcs/src/libs/eigen/Eigen/Cholesky @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_CHOLESKY_MODULE_H #define EIGEN_CHOLESKY_MODULE_H @@ -10,20 +17,22 @@ * * * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. - * Those decompositions are accessible via the following MatrixBase methods: - * - MatrixBase::llt(), + * Those decompositions are also accessible via the following methods: + * - MatrixBase::llt() * - MatrixBase::ldlt() + * - SelfAdjointView::llt() + * - SelfAdjointView::ldlt() * * \code * #include * \endcode */ -#include "src/misc/Solve.h" #include "src/Cholesky/LLT.h" #include "src/Cholesky/LDLT.h" #ifdef EIGEN_USE_LAPACKE -#include "src/Cholesky/LLT_MKL.h" +#include "src/misc/lapacke.h" +#include "src/Cholesky/LLT_LAPACKE.h" #endif #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/CholmodSupport b/ground/gcs/src/libs/eigen/Eigen/CholmodSupport index 88c29a6462..bed8924d31 100644 --- a/ground/gcs/src/libs/eigen/Eigen/CholmodSupport +++ b/ground/gcs/src/libs/eigen/Eigen/CholmodSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H #define EIGEN_CHOLMODSUPPORT_MODULE_H @@ -33,12 +40,8 @@ extern "C" { * */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - #include "src/CholmodSupport/CholmodSupport.h" - #include "src/Core/util/ReenableStupidWarnings.h" #endif // EIGEN_CHOLMODSUPPORT_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/Core b/ground/gcs/src/libs/eigen/Eigen/Core index 509c529e13..0f7fa630dd 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Core +++ b/ground/gcs/src/libs/eigen/Eigen/Core @@ -14,6 +14,58 @@ // first thing Eigen does: stop the compiler from committing suicide #include "src/Core/util/DisableStupidWarnings.h" +// Handle NVCC/CUDA/SYCL +#if defined(__CUDACC__) || defined(__SYCL_DEVICE_ONLY__) + // Do not try asserts on CUDA and SYCL! + #ifndef EIGEN_NO_DEBUG + #define EIGEN_NO_DEBUG + #endif + + #ifdef EIGEN_INTERNAL_DEBUGGING + #undef EIGEN_INTERNAL_DEBUGGING + #endif + + #ifdef EIGEN_EXCEPTIONS + #undef EIGEN_EXCEPTIONS + #endif + + // All functions callable from CUDA code must be qualified with __device__ + #ifdef __CUDACC__ + // Do not try to vectorize on CUDA and SYCL! + #ifndef EIGEN_DONT_VECTORIZE + #define EIGEN_DONT_VECTORIZE + #endif + + #define EIGEN_DEVICE_FUNC __host__ __device__ + // We need math_functions.hpp to ensure that that EIGEN_USING_STD_MATH macro + // works properly on the device side + #include + #else + #define EIGEN_DEVICE_FUNC + #endif + +#else + #define EIGEN_DEVICE_FUNC + +#endif + +// When compiling CUDA device code with NVCC, pull in math functions from the +// global namespace. In host mode, and when device doee with clang, use the +// std versions. +#if defined(__CUDA_ARCH__) && defined(__NVCC__) + #define EIGEN_USING_STD_MATH(FUNC) using ::FUNC; +#else + #define EIGEN_USING_STD_MATH(FUNC) using std::FUNC; +#endif + +#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(__CUDA_ARCH__) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL) + #define EIGEN_EXCEPTIONS +#endif + +#ifdef EIGEN_EXCEPTIONS + #include +#endif + // then include this file where all our macros are defined. It's really important to do it first because // it's where we do all the alignment settings (platform detection and honoring the user's will if he // defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization. @@ -21,7 +73,7 @@ // Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3) // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details. -#if defined(__MINGW32__) && EIGEN_GNUC_AT_LEAST(4,6) +#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6) #pragma GCC optimize ("-fno-ipa-cp-clone") #endif @@ -31,26 +83,26 @@ // and inclusion of their respective header files #include "src/Core/util/MKL_support.h" -// if alignment is disabled, then disable vectorization. Note: EIGEN_ALIGN is the proper check, it takes into -// account both the user's will (EIGEN_DONT_ALIGN) and our own platform checks -#if !EIGEN_ALIGN +// if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into +// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks +#if EIGEN_MAX_ALIGN_BYTES==0 #ifndef EIGEN_DONT_VECTORIZE #define EIGEN_DONT_VECTORIZE #endif #endif -#ifdef _MSC_VER +#if EIGEN_COMP_MSVC #include // for _aligned_malloc -- need it regardless of whether vectorization is enabled - #if (_MSC_VER >= 1500) // 2008 or later + #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later // Remember that usage of defined() in a #define is undefined by the standard. // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP. - #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(_M_X64) + #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64 #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER #endif #endif #else // Remember that usage of defined() in a #define is undefined by the standard - #if (defined __SSE2__) && ( (!defined __GNUC__) || (defined __INTEL_COMPILER) || EIGEN_GNUC_AT_LEAST(4,2) ) + #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) ) #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC #endif #endif @@ -82,6 +134,28 @@ #ifdef __SSE4_2__ #define EIGEN_VECTORIZE_SSE4_2 #endif + #ifdef __AVX__ + #define EIGEN_VECTORIZE_AVX + #define EIGEN_VECTORIZE_SSE3 + #define EIGEN_VECTORIZE_SSSE3 + #define EIGEN_VECTORIZE_SSE4_1 + #define EIGEN_VECTORIZE_SSE4_2 + #endif + #ifdef __AVX2__ + #define EIGEN_VECTORIZE_AVX2 + #endif + #ifdef __FMA__ + #define EIGEN_VECTORIZE_FMA + #endif + #if defined(__AVX512F__) && defined(EIGEN_ENABLE_AVX512) + #define EIGEN_VECTORIZE_AVX512 + #define EIGEN_VECTORIZE_AVX2 + #define EIGEN_VECTORIZE_AVX + #define EIGEN_VECTORIZE_FMA + #ifdef __AVX512DQ__ + #define EIGEN_VECTORIZE_AVX512DQ + #endif + #endif // include files @@ -95,9 +169,10 @@ extern "C" { // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly. // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus: - #if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110 + #if EIGEN_COMP_ICC >= 1110 #include #else + #include #include #include #ifdef EIGEN_VECTORIZE_SSE3 @@ -112,8 +187,20 @@ #ifdef EIGEN_VECTORIZE_SSE4_2 #include #endif + #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512) + #include + #endif #endif } // end extern "C" + #elif defined __VSX__ + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_VSX + #include + // We need to #undef all these ugly tokens defined in + // => use __vector instead of vector + #undef bool + #undef vector + #undef pixel #elif defined __ALTIVEC__ #define EIGEN_VECTORIZE #define EIGEN_VECTORIZE_ALTIVEC @@ -123,13 +210,35 @@ #undef bool #undef vector #undef pixel - #elif defined __ARM_NEON + #elif (defined __ARM_NEON) || (defined __ARM_NEON__) #define EIGEN_VECTORIZE #define EIGEN_VECTORIZE_NEON #include + #elif (defined __s390x__ && defined __VEC__) + #define EIGEN_VECTORIZE + #define EIGEN_VECTORIZE_ZVECTOR + #include #endif #endif +#if defined(__F16C__) && !defined(EIGEN_COMP_CLANG) + // We can use the optimized fp16 to float and float to fp16 conversion routines + #define EIGEN_HAS_FP16_C +#endif + +#if defined __CUDACC__ + #define EIGEN_VECTORIZE_CUDA + #include + #if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500 + #define EIGEN_HAS_CUDA_FP16 + #endif +#endif + +#if defined EIGEN_HAS_CUDA_FP16 + #include + #include +#endif + #if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE) #define EIGEN_HAS_OPENMP #endif @@ -139,7 +248,7 @@ #endif // MSVC for windows mobile does not have the errno.h file -#if !(defined(_MSC_VER) && defined(_WIN32_WCE)) && !defined(__ARMCC_VERSION) +#if !(EIGEN_COMP_MSVC && EIGEN_OS_WINCE) && !EIGEN_COMP_ARM #define EIGEN_HAS_ERRNO #endif @@ -159,29 +268,30 @@ // for min/max: #include +// for std::is_nothrow_move_assignable +#ifdef EIGEN_INCLUDE_TYPE_TRAITS +#include +#endif + // for outputting debug info #ifdef EIGEN_DEBUG_ASSIGN #include #endif // required for __cpuid, needs to be included after cmath -#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE)) +#if EIGEN_COMP_MSVC && EIGEN_ARCH_i386_OR_x86_64 && !EIGEN_OS_WINCE #include #endif -#if defined(_CPPUNWIND) || defined(__EXCEPTIONS) - #define EIGEN_EXCEPTIONS -#endif - -#ifdef EIGEN_EXCEPTIONS - #include -#endif - /** \brief Namespace containing all symbols from the %Eigen library. */ namespace Eigen { inline static const char *SimdInstructionSetsInUse(void) { -#if defined(EIGEN_VECTORIZE_SSE4_2) +#if defined(EIGEN_VECTORIZE_AVX512) + return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2"; +#elif defined(EIGEN_VECTORIZE_AVX) + return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2"; +#elif defined(EIGEN_VECTORIZE_SSE4_2) return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2"; #elif defined(EIGEN_VECTORIZE_SSE4_1) return "SSE, SSE2, SSE3, SSSE3, SSE4.1"; @@ -193,8 +303,12 @@ inline static const char *SimdInstructionSetsInUse(void) { return "SSE, SSE2"; #elif defined(EIGEN_VECTORIZE_ALTIVEC) return "AltiVec"; +#elif defined(EIGEN_VECTORIZE_VSX) + return "VSX"; #elif defined(EIGEN_VECTORIZE_NEON) return "ARM NEON"; +#elif defined(EIGEN_VECTORIZE_ZVECTOR) + return "S390X ZVECTOR"; #else return "None"; #endif @@ -202,42 +316,21 @@ inline static const char *SimdInstructionSetsInUse(void) { } // end namespace Eigen -#define STAGE10_FULL_EIGEN2_API 10 -#define STAGE20_RESOLVE_API_CONFLICTS 20 -#define STAGE30_FULL_EIGEN3_API 30 -#define STAGE40_FULL_EIGEN3_STRICTNESS 40 -#define STAGE99_NO_EIGEN2_SUPPORT 99 - -#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS - #define EIGEN2_SUPPORT - #define EIGEN2_SUPPORT_STAGE STAGE40_FULL_EIGEN3_STRICTNESS -#elif defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API - #define EIGEN2_SUPPORT - #define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API -#elif defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS - #define EIGEN2_SUPPORT - #define EIGEN2_SUPPORT_STAGE STAGE20_RESOLVE_API_CONFLICTS -#elif defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API - #define EIGEN2_SUPPORT - #define EIGEN2_SUPPORT_STAGE STAGE10_FULL_EIGEN2_API -#elif defined EIGEN2_SUPPORT - // default to stage 3, that's what it's always meant - #define EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API - #define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API -#else - #define EIGEN2_SUPPORT_STAGE STAGE99_NO_EIGEN2_SUPPORT +#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || defined EIGEN2_SUPPORT +// This will generate an error message: +#error Eigen2-support is only available up to version 3.2. Please go to "http://eigen.tuxfamily.org/index.php?title=Eigen2" for further information #endif -#ifdef EIGEN2_SUPPORT -#undef minor -#endif +namespace Eigen { // we use size_t frequently and we'll never remember to prepend it with std:: everytime just to // ensure QNX/QCC support using std::size_t; -// gcc 4.6.0 wants std:: for ptrdiff_t +// gcc 4.6.0 wants std:: for ptrdiff_t using std::ptrdiff_t; +} + /** \defgroup Core_Module Core module * This is the main module of Eigen providing dense matrix and vector support * (both fixed and dynamic size) with all the features corresponding to a BLAS library @@ -249,8 +342,8 @@ using std::ptrdiff_t; */ #include "src/Core/util/Constants.h" -#include "src/Core/util/ForwardDeclarations.h" #include "src/Core/util/Meta.h" +#include "src/Core/util/ForwardDeclarations.h" #include "src/Core/util/StaticAssert.h" #include "src/Core/util/XprHelper.h" #include "src/Core/util/Memory.h" @@ -258,41 +351,92 @@ using std::ptrdiff_t; #include "src/Core/NumTraits.h" #include "src/Core/MathFunctions.h" #include "src/Core/GenericPacketMath.h" +#include "src/Core/MathFunctionsImpl.h" -#if defined EIGEN_VECTORIZE_SSE +#if defined EIGEN_VECTORIZE_AVX512 + #include "src/Core/arch/SSE/PacketMath.h" + #include "src/Core/arch/AVX/PacketMath.h" + #include "src/Core/arch/AVX512/PacketMath.h" + #include "src/Core/arch/AVX512/MathFunctions.h" +#elif defined EIGEN_VECTORIZE_AVX + // Use AVX for floats and doubles, SSE for integers + #include "src/Core/arch/SSE/PacketMath.h" + #include "src/Core/arch/SSE/Complex.h" + #include "src/Core/arch/SSE/MathFunctions.h" + #include "src/Core/arch/AVX/PacketMath.h" + #include "src/Core/arch/AVX/MathFunctions.h" + #include "src/Core/arch/AVX/Complex.h" + #include "src/Core/arch/AVX/TypeCasting.h" +#elif defined EIGEN_VECTORIZE_SSE #include "src/Core/arch/SSE/PacketMath.h" #include "src/Core/arch/SSE/MathFunctions.h" #include "src/Core/arch/SSE/Complex.h" -#elif defined EIGEN_VECTORIZE_ALTIVEC + #include "src/Core/arch/SSE/TypeCasting.h" +#elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX) #include "src/Core/arch/AltiVec/PacketMath.h" + #include "src/Core/arch/AltiVec/MathFunctions.h" #include "src/Core/arch/AltiVec/Complex.h" #elif defined EIGEN_VECTORIZE_NEON #include "src/Core/arch/NEON/PacketMath.h" + #include "src/Core/arch/NEON/MathFunctions.h" #include "src/Core/arch/NEON/Complex.h" +#elif defined EIGEN_VECTORIZE_ZVECTOR + #include "src/Core/arch/ZVector/PacketMath.h" + #include "src/Core/arch/ZVector/MathFunctions.h" + #include "src/Core/arch/ZVector/Complex.h" +#endif + +// Half float support +#include "src/Core/arch/CUDA/Half.h" +#include "src/Core/arch/CUDA/PacketMathHalf.h" +#include "src/Core/arch/CUDA/TypeCasting.h" + +#if defined EIGEN_VECTORIZE_CUDA + #include "src/Core/arch/CUDA/PacketMath.h" + #include "src/Core/arch/CUDA/MathFunctions.h" #endif #include "src/Core/arch/Default/Settings.h" -#include "src/Core/Functors.h" +#include "src/Core/functors/TernaryFunctors.h" +#include "src/Core/functors/BinaryFunctors.h" +#include "src/Core/functors/UnaryFunctors.h" +#include "src/Core/functors/NullaryFunctors.h" +#include "src/Core/functors/StlFunctors.h" +#include "src/Core/functors/AssignmentFunctors.h" + +// Specialized functors to enable the processing of complex numbers +// on CUDA devices +#include "src/Core/arch/CUDA/Complex.h" + +#include "src/Core/IO.h" #include "src/Core/DenseCoeffsBase.h" #include "src/Core/DenseBase.h" #include "src/Core/MatrixBase.h" #include "src/Core/EigenBase.h" +#include "src/Core/Product.h" +#include "src/Core/CoreEvaluators.h" +#include "src/Core/AssignEvaluator.h" + #ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874 // at least confirmed with Doxygen 1.5.5 and 1.5.6 #include "src/Core/Assign.h" #endif +#include "src/Core/ArrayBase.h" #include "src/Core/util/BlasUtil.h" #include "src/Core/DenseStorage.h" #include "src/Core/NestByValue.h" -#include "src/Core/ForceAlignedAccess.h" + +// #include "src/Core/ForceAlignedAccess.h" + #include "src/Core/ReturnByValue.h" #include "src/Core/NoAlias.h" #include "src/Core/PlainObjectBase.h" #include "src/Core/Matrix.h" #include "src/Core/Array.h" +#include "src/Core/CwiseTernaryOp.h" #include "src/Core/CwiseBinaryOp.h" #include "src/Core/CwiseUnaryOp.h" #include "src/Core/CwiseNullaryOp.h" @@ -300,32 +444,32 @@ using std::ptrdiff_t; #include "src/Core/SelfCwiseBinaryOp.h" #include "src/Core/Dot.h" #include "src/Core/StableNorm.h" -#include "src/Core/MapBase.h" #include "src/Core/Stride.h" +#include "src/Core/MapBase.h" #include "src/Core/Map.h" +#include "src/Core/Ref.h" #include "src/Core/Block.h" #include "src/Core/VectorBlock.h" -#include "src/Core/Ref.h" #include "src/Core/Transpose.h" #include "src/Core/DiagonalMatrix.h" #include "src/Core/Diagonal.h" #include "src/Core/DiagonalProduct.h" -#include "src/Core/PermutationMatrix.h" -#include "src/Core/Transpositions.h" #include "src/Core/Redux.h" #include "src/Core/Visitor.h" #include "src/Core/Fuzzy.h" -#include "src/Core/IO.h" #include "src/Core/Swap.h" #include "src/Core/CommaInitializer.h" -#include "src/Core/Flagged.h" -#include "src/Core/ProductBase.h" #include "src/Core/GeneralProduct.h" +#include "src/Core/Solve.h" +#include "src/Core/Inverse.h" +#include "src/Core/SolverBase.h" +#include "src/Core/PermutationMatrix.h" +#include "src/Core/Transpositions.h" #include "src/Core/TriangularMatrix.h" #include "src/Core/SelfAdjointView.h" #include "src/Core/products/GeneralBlockPanelKernel.h" #include "src/Core/products/Parallelizer.h" -#include "src/Core/products/CoeffBasedProduct.h" +#include "src/Core/ProductEvaluators.h" #include "src/Core/products/GeneralMatrixVector.h" #include "src/Core/products/GeneralMatrixMatrix.h" #include "src/Core/SolveTriangular.h" @@ -340,6 +484,7 @@ using std::ptrdiff_t; #include "src/Core/products/TriangularSolverVector.h" #include "src/Core/BandMatrix.h" #include "src/Core/CoreIterators.h" +#include "src/Core/ConditionEstimator.h" #include "src/Core/BooleanRedux.h" #include "src/Core/Select.h" @@ -347,18 +492,17 @@ using std::ptrdiff_t; #include "src/Core/Random.h" #include "src/Core/Replicate.h" #include "src/Core/Reverse.h" -#include "src/Core/ArrayBase.h" #include "src/Core/ArrayWrapper.h" #ifdef EIGEN_USE_BLAS -#include "src/Core/products/GeneralMatrixMatrix_MKL.h" -#include "src/Core/products/GeneralMatrixVector_MKL.h" -#include "src/Core/products/GeneralMatrixMatrixTriangular_MKL.h" -#include "src/Core/products/SelfadjointMatrixMatrix_MKL.h" -#include "src/Core/products/SelfadjointMatrixVector_MKL.h" -#include "src/Core/products/TriangularMatrixMatrix_MKL.h" -#include "src/Core/products/TriangularMatrixVector_MKL.h" -#include "src/Core/products/TriangularSolverMatrix_MKL.h" +#include "src/Core/products/GeneralMatrixMatrix_BLAS.h" +#include "src/Core/products/GeneralMatrixVector_BLAS.h" +#include "src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h" +#include "src/Core/products/SelfadjointMatrixMatrix_BLAS.h" +#include "src/Core/products/SelfadjointMatrixVector_BLAS.h" +#include "src/Core/products/TriangularMatrixMatrix_BLAS.h" +#include "src/Core/products/TriangularMatrixVector_BLAS.h" +#include "src/Core/products/TriangularSolverMatrix_BLAS.h" #endif // EIGEN_USE_BLAS #ifdef EIGEN_USE_MKL_VML @@ -369,8 +513,4 @@ using std::ptrdiff_t; #include "src/Core/util/ReenableStupidWarnings.h" -#ifdef EIGEN2_SUPPORT -#include "Eigen2Support" -#endif - #endif // EIGEN_CORE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/Eigen b/ground/gcs/src/libs/eigen/Eigen/Eigen index 19b40ea4e7..654c8dc638 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Eigen +++ b/ground/gcs/src/libs/eigen/Eigen/Eigen @@ -1,2 +1,2 @@ #include "Dense" -//#include "Sparse" +#include "Sparse" diff --git a/ground/gcs/src/libs/eigen/Eigen/Eigen2Support b/ground/gcs/src/libs/eigen/Eigen/Eigen2Support deleted file mode 100644 index 6aa009d209..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/Eigen2Support +++ /dev/null @@ -1,95 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN2SUPPORT_H -#define EIGEN2SUPPORT_H - -#if (!defined(EIGEN2_SUPPORT)) || (!defined(EIGEN_CORE_H)) -#error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header -#endif - -#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING - -#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__) -#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)" -#else -#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)") -#endif - -#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING - -#include "src/Core/util/DisableStupidWarnings.h" - -/** \ingroup Support_modules - * \defgroup Eigen2Support_Module Eigen2 support module - * - * \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. - * - * This module provides a couple of deprecated functions improving the compatibility with Eigen2. - * - * To use it, define EIGEN2_SUPPORT before including any Eigen header - * \code - * #define EIGEN2_SUPPORT - * \endcode - * - */ - -#include "src/Eigen2Support/Macros.h" -#include "src/Eigen2Support/Memory.h" -#include "src/Eigen2Support/Meta.h" -#include "src/Eigen2Support/Lazy.h" -#include "src/Eigen2Support/Cwise.h" -#include "src/Eigen2Support/CwiseOperators.h" -#include "src/Eigen2Support/TriangularSolver.h" -#include "src/Eigen2Support/Block.h" -#include "src/Eigen2Support/VectorBlock.h" -#include "src/Eigen2Support/Minor.h" -#include "src/Eigen2Support/MathFunctions.h" - - -#include "src/Core/util/ReenableStupidWarnings.h" - -// Eigen2 used to include iostream -#include - -#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \ -using Eigen::Matrix##SizeSuffix##TypeSuffix; \ -using Eigen::Vector##SizeSuffix##TypeSuffix; \ -using Eigen::RowVector##SizeSuffix##TypeSuffix; - -#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(TypeSuffix) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \ - -#define EIGEN_USING_MATRIX_TYPEDEFS \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(i) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(f) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(d) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cf) \ -EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cd) - -#define USING_PART_OF_NAMESPACE_EIGEN \ -EIGEN_USING_MATRIX_TYPEDEFS \ -using Eigen::Matrix; \ -using Eigen::MatrixBase; \ -using Eigen::ei_random; \ -using Eigen::ei_real; \ -using Eigen::ei_imag; \ -using Eigen::ei_conj; \ -using Eigen::ei_abs; \ -using Eigen::ei_abs2; \ -using Eigen::ei_sqrt; \ -using Eigen::ei_exp; \ -using Eigen::ei_log; \ -using Eigen::ei_sin; \ -using Eigen::ei_cos; - -#endif // EIGEN2SUPPORT_H diff --git a/ground/gcs/src/libs/eigen/Eigen/Eigenvalues b/ground/gcs/src/libs/eigen/Eigen/Eigenvalues index 53c5a73a27..009e529e19 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Eigenvalues +++ b/ground/gcs/src/libs/eigen/Eigen/Eigenvalues @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_EIGENVALUES_MODULE_H #define EIGEN_EIGENVALUES_MODULE_H @@ -25,6 +32,7 @@ * \endcode */ +#include "src/misc/RealSvd2x2.h" #include "src/Eigenvalues/Tridiagonalization.h" #include "src/Eigenvalues/RealSchur.h" #include "src/Eigenvalues/EigenSolver.h" @@ -37,9 +45,10 @@ #include "src/Eigenvalues/GeneralizedEigenSolver.h" #include "src/Eigenvalues/MatrixBaseEigenvalues.h" #ifdef EIGEN_USE_LAPACKE -#include "src/Eigenvalues/RealSchur_MKL.h" -#include "src/Eigenvalues/ComplexSchur_MKL.h" -#include "src/Eigenvalues/SelfAdjointEigenSolver_MKL.h" +#include "src/misc/lapacke.h" +#include "src/Eigenvalues/RealSchur_LAPACKE.h" +#include "src/Eigenvalues/ComplexSchur_LAPACKE.h" +#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" #endif #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/Geometry b/ground/gcs/src/libs/eigen/Eigen/Geometry index efd9d4504c..716d529529 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Geometry +++ b/ground/gcs/src/libs/eigen/Eigen/Geometry @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_GEOMETRY_MODULE_H #define EIGEN_GEOMETRY_MODULE_H @@ -9,21 +16,17 @@ #include "LU" #include -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - /** \defgroup Geometry_Module Geometry module - * - * * * This module provides support for: * - fixed-size homogeneous transformations * - translation, scaling, 2D and 3D rotations - * - quaternions - * - \ref MatrixBase::cross() "cross product" - * - \ref MatrixBase::unitOrthogonal() "orthognal vector generation" - * - some linear components: parametrized-lines and hyperplanes + * - \link Quaternion quaternions \endlink + * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) + * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) + * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink + * - \link AlignedBox axis aligned bounding boxes \endlink + * - \link umeyama least-square transformation fitting \endlink * * \code * #include @@ -33,27 +36,23 @@ #include "src/Geometry/OrthoMethods.h" #include "src/Geometry/EulerAngles.h" -#if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS - #include "src/Geometry/Homogeneous.h" - #include "src/Geometry/RotationBase.h" - #include "src/Geometry/Rotation2D.h" - #include "src/Geometry/Quaternion.h" - #include "src/Geometry/AngleAxis.h" - #include "src/Geometry/Transform.h" - #include "src/Geometry/Translation.h" - #include "src/Geometry/Scaling.h" - #include "src/Geometry/Hyperplane.h" - #include "src/Geometry/ParametrizedLine.h" - #include "src/Geometry/AlignedBox.h" - #include "src/Geometry/Umeyama.h" - - #if defined EIGEN_VECTORIZE_SSE - #include "src/Geometry/arch/Geometry_SSE.h" - #endif -#endif - -#ifdef EIGEN2_SUPPORT -#include "src/Eigen2Support/Geometry/All.h" +#include "src/Geometry/Homogeneous.h" +#include "src/Geometry/RotationBase.h" +#include "src/Geometry/Rotation2D.h" +#include "src/Geometry/Quaternion.h" +#include "src/Geometry/AngleAxis.h" +#include "src/Geometry/Transform.h" +#include "src/Geometry/Translation.h" +#include "src/Geometry/Scaling.h" +#include "src/Geometry/Hyperplane.h" +#include "src/Geometry/ParametrizedLine.h" +#include "src/Geometry/AlignedBox.h" +#include "src/Geometry/Umeyama.h" + +// Use the SSE optimized version whenever possible. At the moment the +// SSE version doesn't compile when AVX is enabled +#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX +#include "src/Geometry/arch/Geometry_SSE.h" #endif #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/Householder b/ground/gcs/src/libs/eigen/Eigen/Householder index 6e348db5c4..89cd81b1af 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Householder +++ b/ground/gcs/src/libs/eigen/Eigen/Householder @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_HOUSEHOLDER_MODULE_H #define EIGEN_HOUSEHOLDER_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/IterativeLinearSolvers b/ground/gcs/src/libs/eigen/Eigen/IterativeLinearSolvers index 0f4159dc19..957d5750b2 100644 --- a/ground/gcs/src/libs/eigen/Eigen/IterativeLinearSolvers +++ b/ground/gcs/src/libs/eigen/Eigen/IterativeLinearSolvers @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H @@ -12,28 +19,29 @@ * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. * Those solvers are accessible via the following classes: * - ConjugateGradient for selfadjoint (hermitian) matrices, + * - LeastSquaresConjugateGradient for rectangular least-square problems, * - BiCGSTAB for general square matrices. * * These iterative solvers are associated with some preconditioners: * - IdentityPreconditioner - not really useful - * - DiagonalPreconditioner - also called JAcobi preconditioner, work very well on diagonal dominant matrices. - * - IncompleteILUT - incomplete LU factorization with dual thresholding + * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. + * - IncompleteLUT - incomplete LU factorization with dual thresholding * * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. * - * \code - * #include - * \endcode + \code + #include + \endcode */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - +#include "src/IterativeLinearSolvers/SolveWithGuess.h" #include "src/IterativeLinearSolvers/IterativeSolverBase.h" #include "src/IterativeLinearSolvers/BasicPreconditioners.h" #include "src/IterativeLinearSolvers/ConjugateGradient.h" +#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" #include "src/IterativeLinearSolvers/BiCGSTAB.h" #include "src/IterativeLinearSolvers/IncompleteLUT.h" +#include "src/IterativeLinearSolvers/IncompleteCholesky.h" #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/Jacobi b/ground/gcs/src/libs/eigen/Eigen/Jacobi index ba8a4dc36a..17c1d785a1 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Jacobi +++ b/ground/gcs/src/libs/eigen/Eigen/Jacobi @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_JACOBI_MODULE_H #define EIGEN_JACOBI_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/LU b/ground/gcs/src/libs/eigen/Eigen/LU index db57955044..6f6c55629c 100644 --- a/ground/gcs/src/libs/eigen/Eigen/LU +++ b/ground/gcs/src/libs/eigen/Eigen/LU @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_LU_MODULE_H #define EIGEN_LU_MODULE_H @@ -16,25 +23,23 @@ * \endcode */ -#include "src/misc/Solve.h" #include "src/misc/Kernel.h" #include "src/misc/Image.h" #include "src/LU/FullPivLU.h" #include "src/LU/PartialPivLU.h" #ifdef EIGEN_USE_LAPACKE -#include "src/LU/PartialPivLU_MKL.h" +#include "src/misc/lapacke.h" +#include "src/LU/PartialPivLU_LAPACKE.h" #endif #include "src/LU/Determinant.h" -#include "src/LU/Inverse.h" +#include "src/LU/InverseImpl.h" -#if defined EIGEN_VECTORIZE_SSE +// Use the SSE optimized version whenever possible. At the moment the +// SSE version doesn't compile when AVX is enabled +#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX #include "src/LU/arch/Inverse_SSE.h" #endif -#ifdef EIGEN2_SUPPORT - #include "src/Eigen2Support/LU.h" -#endif - #include "src/Core/util/ReenableStupidWarnings.h" #endif // EIGEN_LU_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/LeastSquares b/ground/gcs/src/libs/eigen/Eigen/LeastSquares deleted file mode 100644 index 35137c25db..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/LeastSquares +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef EIGEN_REGRESSION_MODULE_H -#define EIGEN_REGRESSION_MODULE_H - -#ifndef EIGEN2_SUPPORT -#error LeastSquares is only available in Eigen2 support mode (define EIGEN2_SUPPORT) -#endif - -// exclude from normal eigen3-only documentation -#ifdef EIGEN2_SUPPORT - -#include "Core" - -#include "src/Core/util/DisableStupidWarnings.h" - -#include "Eigenvalues" -#include "Geometry" - -/** \defgroup LeastSquares_Module LeastSquares module - * This module provides linear regression and related features. - * - * \code - * #include - * \endcode - */ - -#include "src/Eigen2Support/LeastSquares.h" - -#include "src/Core/util/ReenableStupidWarnings.h" - -#endif // EIGEN2_SUPPORT - -#endif // EIGEN_REGRESSION_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/MetisSupport b/ground/gcs/src/libs/eigen/Eigen/MetisSupport index 6a113f7a87..85c41bf340 100644 --- a/ground/gcs/src/libs/eigen/Eigen/MetisSupport +++ b/ground/gcs/src/libs/eigen/Eigen/MetisSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_METISSUPPORT_MODULE_H #define EIGEN_METISSUPPORT_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/OrderingMethods b/ground/gcs/src/libs/eigen/Eigen/OrderingMethods index 7c0f1fffff..d8ea361936 100644 --- a/ground/gcs/src/libs/eigen/Eigen/OrderingMethods +++ b/ground/gcs/src/libs/eigen/Eigen/OrderingMethods @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_ORDERINGMETHODS_MODULE_H #define EIGEN_ORDERINGMETHODS_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/PaStiXSupport b/ground/gcs/src/libs/eigen/Eigen/PaStiXSupport index 7c616ee5ea..de3a63b4d1 100644 --- a/ground/gcs/src/libs/eigen/Eigen/PaStiXSupport +++ b/ground/gcs/src/libs/eigen/Eigen/PaStiXSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_PASTIXSUPPORT_MODULE_H #define EIGEN_PASTIXSUPPORT_MODULE_H @@ -5,7 +12,6 @@ #include "src/Core/util/DisableStupidWarnings.h" -#include extern "C" { #include #include @@ -35,12 +41,8 @@ extern "C" { * */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - #include "src/PaStiXSupport/PaStiXSupport.h" - #include "src/Core/util/ReenableStupidWarnings.h" #endif // EIGEN_PASTIXSUPPORT_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/PardisoSupport b/ground/gcs/src/libs/eigen/Eigen/PardisoSupport index 99330ce7a7..340edf51fe 100644 --- a/ground/gcs/src/libs/eigen/Eigen/PardisoSupport +++ b/ground/gcs/src/libs/eigen/Eigen/PardisoSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_PARDISOSUPPORT_MODULE_H #define EIGEN_PARDISOSUPPORT_MODULE_H @@ -7,8 +14,6 @@ #include -#include - /** \ingroup Support_modules * \defgroup PardisoSupport_Module PardisoSupport module * diff --git a/ground/gcs/src/libs/eigen/Eigen/QR b/ground/gcs/src/libs/eigen/Eigen/QR index ac5b026935..80838e3bdd 100644 --- a/ground/gcs/src/libs/eigen/Eigen/QR +++ b/ground/gcs/src/libs/eigen/Eigen/QR @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_QR_MODULE_H #define EIGEN_QR_MODULE_H @@ -15,31 +22,26 @@ * * This module provides various QR decompositions * This module also provides some MatrixBase methods, including: - * - MatrixBase::qr(), + * - MatrixBase::householderQr() + * - MatrixBase::colPivHouseholderQr() + * - MatrixBase::fullPivHouseholderQr() * * \code * #include * \endcode */ -#include "src/misc/Solve.h" #include "src/QR/HouseholderQR.h" #include "src/QR/FullPivHouseholderQR.h" #include "src/QR/ColPivHouseholderQR.h" +#include "src/QR/CompleteOrthogonalDecomposition.h" #ifdef EIGEN_USE_LAPACKE -#include "src/QR/HouseholderQR_MKL.h" -#include "src/QR/ColPivHouseholderQR_MKL.h" -#endif - -#ifdef EIGEN2_SUPPORT -#include "src/Eigen2Support/QR.h" +#include "src/misc/lapacke.h" +#include "src/QR/HouseholderQR_LAPACKE.h" +#include "src/QR/ColPivHouseholderQR_LAPACKE.h" #endif #include "src/Core/util/ReenableStupidWarnings.h" -#ifdef EIGEN2_SUPPORT -#include "Eigenvalues" -#endif - #endif // EIGEN_QR_MODULE_H /* vim: set filetype=cpp et sw=2 ts=2 ai: */ diff --git a/ground/gcs/src/libs/eigen/Eigen/QtAlignedMalloc b/ground/gcs/src/libs/eigen/Eigen/QtAlignedMalloc index 46f7d83b70..c6571f1291 100644 --- a/ground/gcs/src/libs/eigen/Eigen/QtAlignedMalloc +++ b/ground/gcs/src/libs/eigen/Eigen/QtAlignedMalloc @@ -1,3 +1,9 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #ifndef EIGEN_QTMALLOC_MODULE_H #define EIGEN_QTMALLOC_MODULE_H @@ -8,7 +14,7 @@ #include "src/Core/util/DisableStupidWarnings.h" -void *qMalloc(size_t size) +void *qMalloc(std::size_t size) { return Eigen::internal::aligned_malloc(size); } @@ -18,7 +24,7 @@ void qFree(void *ptr) Eigen::internal::aligned_free(ptr); } -void *qRealloc(void *ptr, size_t size) +void *qRealloc(void *ptr, std::size_t size) { void* newPtr = Eigen::internal::aligned_malloc(size); memcpy(newPtr, ptr, size); diff --git a/ground/gcs/src/libs/eigen/Eigen/SPQRSupport b/ground/gcs/src/libs/eigen/Eigen/SPQRSupport index 7f1eb4770b..f70390c176 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SPQRSupport +++ b/ground/gcs/src/libs/eigen/Eigen/SPQRSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SPQRSUPPORT_MODULE_H #define EIGEN_SPQRSUPPORT_MODULE_H @@ -21,8 +28,6 @@ * */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" #include "src/CholmodSupport/CholmodSupport.h" #include "src/SPQRSupport/SuiteSparseQRSupport.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/SVD b/ground/gcs/src/libs/eigen/Eigen/SVD index fd310017ad..86143c23d7 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SVD +++ b/ground/gcs/src/libs/eigen/Eigen/SVD @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SVD_MODULE_H #define EIGEN_SVD_MODULE_H @@ -12,23 +19,26 @@ * * * This module provides SVD decomposition for matrices (both real and complex). - * This decomposition is accessible via the following MatrixBase method: + * Two decomposition algorithms are provided: + * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones. + * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems. + * These decompositions are accessible via the respective classes and following MatrixBase methods: * - MatrixBase::jacobiSvd() + * - MatrixBase::bdcSvd() * * \code * #include * \endcode */ -#include "src/misc/Solve.h" +#include "src/misc/RealSvd2x2.h" +#include "src/SVD/UpperBidiagonalization.h" +#include "src/SVD/SVDBase.h" #include "src/SVD/JacobiSVD.h" +#include "src/SVD/BDCSVD.h" #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) -#include "src/SVD/JacobiSVD_MKL.h" -#endif -#include "src/SVD/UpperBidiagonalization.h" - -#ifdef EIGEN2_SUPPORT -#include "src/Eigen2Support/SVD.h" +#include "src/misc/lapacke.h" +#include "src/SVD/JacobiSVD_LAPACKE.h" #endif #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/Sparse b/ground/gcs/src/libs/eigen/Eigen/Sparse index 7cc9c09133..136e681a1f 100644 --- a/ground/gcs/src/libs/eigen/Eigen/Sparse +++ b/ground/gcs/src/libs/eigen/Eigen/Sparse @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SPARSE_MODULE_H #define EIGEN_SPARSE_MODULE_H @@ -11,14 +18,16 @@ * - \ref SparseQR_Module * - \ref IterativeLinearSolvers_Module * - * \code - * #include - * \endcode + \code + #include + \endcode */ #include "SparseCore" #include "OrderingMethods" +#ifndef EIGEN_MPL2_ONLY #include "SparseCholesky" +#endif #include "SparseLU" #include "SparseQR" #include "IterativeLinearSolvers" diff --git a/ground/gcs/src/libs/eigen/Eigen/SparseCholesky b/ground/gcs/src/libs/eigen/Eigen/SparseCholesky index 9f5056aa1a..b6a320c402 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SparseCholesky +++ b/ground/gcs/src/libs/eigen/Eigen/SparseCholesky @@ -34,8 +34,6 @@ #error The SparseCholesky module has nothing to offer in MPL2 only mode #endif -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" #include "src/SparseCholesky/SimplicialCholesky.h" #ifndef EIGEN_MPL2_ONLY diff --git a/ground/gcs/src/libs/eigen/Eigen/SparseCore b/ground/gcs/src/libs/eigen/Eigen/SparseCore index 24bcf0156b..76966c4c4c 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SparseCore +++ b/ground/gcs/src/libs/eigen/Eigen/SparseCore @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SPARSECORE_MODULE_H #define EIGEN_SPARSECORE_MODULE_H @@ -26,37 +33,35 @@ * This module depends on: Core. */ -namespace Eigen { - -/** The type used to identify a general sparse storage. */ -struct Sparse {}; - -} - #include "src/SparseCore/SparseUtil.h" #include "src/SparseCore/SparseMatrixBase.h" +#include "src/SparseCore/SparseAssign.h" #include "src/SparseCore/CompressedStorage.h" #include "src/SparseCore/AmbiVector.h" +#include "src/SparseCore/SparseCompressedBase.h" #include "src/SparseCore/SparseMatrix.h" +#include "src/SparseCore/SparseMap.h" #include "src/SparseCore/MappedSparseMatrix.h" #include "src/SparseCore/SparseVector.h" -#include "src/SparseCore/SparseBlock.h" -#include "src/SparseCore/SparseTranspose.h" +#include "src/SparseCore/SparseRef.h" #include "src/SparseCore/SparseCwiseUnaryOp.h" #include "src/SparseCore/SparseCwiseBinaryOp.h" +#include "src/SparseCore/SparseTranspose.h" +#include "src/SparseCore/SparseBlock.h" #include "src/SparseCore/SparseDot.h" -#include "src/SparseCore/SparsePermutation.h" #include "src/SparseCore/SparseRedux.h" -#include "src/SparseCore/SparseFuzzy.h" +#include "src/SparseCore/SparseView.h" +#include "src/SparseCore/SparseDiagonalProduct.h" #include "src/SparseCore/ConservativeSparseSparseProduct.h" #include "src/SparseCore/SparseSparseProductWithPruning.h" #include "src/SparseCore/SparseProduct.h" #include "src/SparseCore/SparseDenseProduct.h" -#include "src/SparseCore/SparseDiagonalProduct.h" -#include "src/SparseCore/SparseTriangularView.h" #include "src/SparseCore/SparseSelfAdjointView.h" +#include "src/SparseCore/SparseTriangularView.h" #include "src/SparseCore/TriangularSolver.h" -#include "src/SparseCore/SparseView.h" +#include "src/SparseCore/SparsePermutation.h" +#include "src/SparseCore/SparseFuzzy.h" +#include "src/SparseCore/SparseSolverBase.h" #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/SparseLU b/ground/gcs/src/libs/eigen/Eigen/SparseLU index 8527a49bd8..38b38b531d 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SparseLU +++ b/ground/gcs/src/libs/eigen/Eigen/SparseLU @@ -20,9 +20,6 @@ * Please, see the documentation of the SparseLU class for more details. */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - // Ordering interface #include "OrderingMethods" diff --git a/ground/gcs/src/libs/eigen/Eigen/SparseQR b/ground/gcs/src/libs/eigen/Eigen/SparseQR index 4ee42065ee..a6f3b7f7d7 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SparseQR +++ b/ground/gcs/src/libs/eigen/Eigen/SparseQR @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SPARSEQR_MODULE_H #define EIGEN_SPARSEQR_MODULE_H @@ -21,9 +28,6 @@ * */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - #include "OrderingMethods" #include "src/SparseCore/SparseColEtree.h" #include "src/SparseQR/SparseQR.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/StdDeque b/ground/gcs/src/libs/eigen/Eigen/StdDeque index f27234778f..bc68397be2 100644 --- a/ground/gcs/src/libs/eigen/Eigen/StdDeque +++ b/ground/gcs/src/libs/eigen/Eigen/StdDeque @@ -14,7 +14,7 @@ #include "Core" #include -#if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */ +#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) diff --git a/ground/gcs/src/libs/eigen/Eigen/StdList b/ground/gcs/src/libs/eigen/Eigen/StdList index 225c1e18f8..4c6262c08c 100644 --- a/ground/gcs/src/libs/eigen/Eigen/StdList +++ b/ground/gcs/src/libs/eigen/Eigen/StdList @@ -13,7 +13,7 @@ #include "Core" #include -#if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */ +#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) diff --git a/ground/gcs/src/libs/eigen/Eigen/StdVector b/ground/gcs/src/libs/eigen/Eigen/StdVector index 6b22627f6f..0c4697ad5b 100644 --- a/ground/gcs/src/libs/eigen/Eigen/StdVector +++ b/ground/gcs/src/libs/eigen/Eigen/StdVector @@ -14,7 +14,7 @@ #include "Core" #include -#if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */ +#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) diff --git a/ground/gcs/src/libs/eigen/Eigen/SuperLUSupport b/ground/gcs/src/libs/eigen/Eigen/SuperLUSupport index 575e14fbc2..59312a82db 100644 --- a/ground/gcs/src/libs/eigen/Eigen/SuperLUSupport +++ b/ground/gcs/src/libs/eigen/Eigen/SuperLUSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H #define EIGEN_SUPERLUSUPPORT_MODULE_H @@ -36,6 +43,8 @@ namespace Eigen { struct SluMatrix; } * - class SuperLU: a supernodal sequential LU factorization. * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). * + * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. + * * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. * * \code @@ -48,12 +57,8 @@ namespace Eigen { struct SluMatrix; } * */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - #include "src/SuperLUSupport/SuperLUSupport.h" - #include "src/Core/util/ReenableStupidWarnings.h" #endif // EIGEN_SUPERLUSUPPORT_MODULE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/UmfPackSupport b/ground/gcs/src/libs/eigen/Eigen/UmfPackSupport index 7b1b660649..00eec80875 100644 --- a/ground/gcs/src/libs/eigen/Eigen/UmfPackSupport +++ b/ground/gcs/src/libs/eigen/Eigen/UmfPackSupport @@ -1,3 +1,10 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H #define EIGEN_UMFPACKSUPPORT_MODULE_H @@ -26,9 +33,6 @@ extern "C" { * */ -#include "src/misc/Solve.h" -#include "src/misc/SparseSolve.h" - #include "src/UmfPackSupport/UmfPackSupport.h" #include "src/Core/util/ReenableStupidWarnings.h" diff --git a/ground/gcs/src/libs/eigen/Eigen/src/CMakeLists.txt b/ground/gcs/src/libs/eigen/Eigen/src/CMakeLists.txt deleted file mode 100644 index c326f374d9..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -file(GLOB Eigen_src_subdirectories "*") -escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") -foreach(f ${Eigen_src_subdirectories}) - if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" ) - add_subdirectory(${f}) - endif() -endforeach() diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/CMakeLists.txt b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/CMakeLists.txt deleted file mode 100644 index d01488b41a..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -FILE(GLOB Eigen_Cholesky_SRCS "*.h") - -INSTALL(FILES - ${Eigen_Cholesky_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Cholesky COMPONENT Devel - ) diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LDLT.h b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LDLT.h index abd30bd916..fcee7b2e3f 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LDLT.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LDLT.h @@ -13,7 +13,7 @@ #ifndef EIGEN_LDLT_H #define EIGEN_LDLT_H -namespace Eigen { +namespace Eigen { namespace internal { template struct LDLT_Traits; @@ -28,8 +28,8 @@ namespace internal { * * \brief Robust Cholesky decomposition of a matrix with pivoting * - * \param MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition - * \param UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper. + * \tparam _MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition + * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper. * The other triangular part won't be read. * * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite @@ -43,7 +43,9 @@ namespace internal { * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky * decomposition to determine whether a system of equations has a solution. * - * \sa MatrixBase::ldlt(), class LLT + * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism. + * + * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT */ template class LDLT { @@ -52,15 +54,15 @@ template class LDLT enum { RowsAtCompileTime = MatrixType::RowsAtCompileTime, ColsAtCompileTime = MatrixType::ColsAtCompileTime, - Options = MatrixType::Options & ~RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here! MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, UpLo = _UpLo }; typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; - typedef typename MatrixType::Index Index; - typedef Matrix TmpMatrixType; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef typename MatrixType::StorageIndex StorageIndex; + typedef Matrix TmpMatrixType; typedef Transpositions TranspositionType; typedef PermutationMatrix PermutationType; @@ -72,11 +74,11 @@ template class LDLT * The default constructor is useful in cases in which the user intends to * perform decompositions via LDLT::compute(const MatrixType&). */ - LDLT() - : m_matrix(), - m_transpositions(), + LDLT() + : m_matrix(), + m_transpositions(), m_sign(internal::ZeroSign), - m_isInitialized(false) + m_isInitialized(false) {} /** \brief Default Constructor with memory preallocation @@ -85,7 +87,7 @@ template class LDLT * according to the specified problem \a size. * \sa LDLT() */ - LDLT(Index size) + explicit LDLT(Index size) : m_matrix(size, size), m_transpositions(size), m_temporary(size), @@ -96,16 +98,35 @@ template class LDLT /** \brief Constructor with decomposition * * This calculates the decomposition for the input \a matrix. + * * \sa LDLT(Index size) */ - LDLT(const MatrixType& matrix) + template + explicit LDLT(const EigenBase& matrix) : m_matrix(matrix.rows(), matrix.cols()), m_transpositions(matrix.rows()), m_temporary(matrix.rows()), m_sign(internal::ZeroSign), m_isInitialized(false) { - compute(matrix); + compute(matrix.derived()); + } + + /** \brief Constructs a LDLT factorization from a given matrix + * + * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref. + * + * \sa LDLT(const EigenBase&) + */ + template + explicit LDLT(EigenBase& matrix) + : m_matrix(matrix.derived()), + m_transpositions(matrix.rows()), + m_temporary(matrix.rows()), + m_sign(internal::ZeroSign), + m_isInitialized(false) + { + compute(matrix.derived()); } /** Clear any existing decomposition @@ -151,13 +172,6 @@ template class LDLT eigen_assert(m_isInitialized && "LDLT is not initialized."); return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign; } - - #ifdef EIGEN2_SUPPORT - inline bool isPositiveDefinite() const - { - return isPositive(); - } - #endif /** \returns true if the matrix is negative (semidefinite) */ inline bool isNegative(void) const @@ -173,37 +187,38 @@ template class LDLT * \note_about_checking_solutions * * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$ - * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$, + * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$, * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function * computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular. * - * \sa MatrixBase::ldlt() + * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt() */ template - inline const internal::solve_retval + inline const Solve solve(const MatrixBase& b) const { eigen_assert(m_isInitialized && "LDLT is not initialized."); eigen_assert(m_matrix.rows()==b.rows() && "LDLT::solve(): invalid number of rows of the right hand side matrix b"); - return internal::solve_retval(*this, b.derived()); + return Solve(*this, b.derived()); } - #ifdef EIGEN2_SUPPORT - template - bool solve(const MatrixBase& b, ResultType *result) const - { - *result = this->solve(b); - return true; - } - #endif - template bool solveInPlace(MatrixBase &bAndX) const; - LDLT& compute(const MatrixType& matrix); + template + LDLT& compute(const EigenBase& matrix); + + /** \returns an estimate of the reciprocal condition number of the matrix of + * which \c *this is the LDLT decomposition. + */ + RealScalar rcond() const + { + eigen_assert(m_isInitialized && "LDLT is not initialized."); + return internal::rcond_estimate_helper(m_l1_norm, *this); + } template LDLT& rankUpdate(const MatrixBase& w, const RealScalar& alpha=1); @@ -220,6 +235,13 @@ template class LDLT MatrixType reconstructedMatrix() const; + /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint. + * + * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as: + * \code x = decomposition.adjoint().solve(b) \endcode + */ + const LDLT& adjoint() const { return *this; }; + inline Index rows() const { return m_matrix.rows(); } inline Index cols() const { return m_matrix.cols(); } @@ -231,11 +253,17 @@ template class LDLT ComputationInfo info() const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return Success; + return m_info; } + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC + void _solve_impl(const RhsType &rhs, DstType &dst) const; + #endif + protected: - + static void check_template_parameters() { EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); @@ -248,10 +276,12 @@ template class LDLT * is not stored), and the diagonal entries correspond to D. */ MatrixType m_matrix; + RealScalar m_l1_norm; TranspositionType m_transpositions; TmpMatrixType m_temporary; internal::SignMatrix m_sign; bool m_isInitialized; + ComputationInfo m_info; }; namespace internal { @@ -266,15 +296,17 @@ template<> struct ldlt_inplace using std::abs; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - typedef typename MatrixType::Index Index; + typedef typename TranspositionType::StorageIndex IndexType; eigen_assert(mat.rows()==mat.cols()); const Index size = mat.rows(); + bool found_zero_pivot = false; + bool ret = true; if (size <= 1) { transpositions.setIdentity(); - if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef; - else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef; + if (numext::real(mat.coeff(0,0)) > static_cast(0) ) sign = PositiveSemiDef; + else if (numext::real(mat.coeff(0,0)) < static_cast(0)) sign = NegativeSemiDef; else sign = ZeroSign; return true; } @@ -286,7 +318,7 @@ template<> struct ldlt_inplace mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner); index_of_biggest_in_corner += k; - transpositions.coeffRef(k) = index_of_biggest_in_corner; + transpositions.coeffRef(k) = IndexType(index_of_biggest_in_corner); if(k != index_of_biggest_in_corner) { // apply the transposition while taking care to consider only @@ -295,7 +327,7 @@ template<> struct ldlt_inplace mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k)); mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s)); std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner)); - for(int i=k+1;i struct ldlt_inplace if(rs>0) A21.noalias() -= A20 * temp.head(k); } - + // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot - // was smaller than the cutoff value. However, soince LDLT is not rank-revealing - // we should only make sure we do not introduce INF or NaN values. - // LAPACK also uses 0 as the cutoff value. + // was smaller than the cutoff value. However, since LDLT is not rank-revealing + // we should only make sure that we do not introduce INF or NaN values. + // Remark that LAPACK also uses 0 as the cutoff value. RealScalar realAkk = numext::real(mat.coeffRef(k,k)); - if((rs>0) && (abs(realAkk) > RealScalar(0))) + bool pivot_is_valid = (abs(realAkk) > RealScalar(0)); + + if(k==0 && !pivot_is_valid) + { + // The entire diagonal is zero, there is nothing more to do + // except filling the transpositions, and checking whether the matrix is zero. + sign = ZeroSign; + for(Index j = 0; j0) && pivot_is_valid) A21 /= realAkk; + if(found_zero_pivot && pivot_is_valid) ret = false; // factorization failed + else if(!pivot_is_valid) found_zero_pivot = true; + if (sign == PositiveSemiDef) { - if (realAkk < 0) sign = Indefinite; + if (realAkk < static_cast(0)) sign = Indefinite; } else if (sign == NegativeSemiDef) { - if (realAkk > 0) sign = Indefinite; + if (realAkk > static_cast(0)) sign = Indefinite; } else if (sign == ZeroSign) { - if (realAkk > 0) sign = PositiveSemiDef; - else if (realAkk < 0) sign = NegativeSemiDef; + if (realAkk > static_cast(0)) sign = PositiveSemiDef; + else if (realAkk < static_cast(0)) sign = NegativeSemiDef; } } - return true; + return ret; } // Reference for the algorithm: Davis and Hager, "Multiple Rank @@ -356,7 +406,6 @@ template<> struct ldlt_inplace using numext::isfinite; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - typedef typename MatrixType::Index Index; const Index size = mat.rows(); eigen_assert(mat.cols() == size && w.size()==size); @@ -420,16 +469,16 @@ template struct LDLT_Traits { typedef const TriangularView MatrixL; typedef const TriangularView MatrixU; - static inline MatrixL getL(const MatrixType& m) { return m; } - static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); } + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); } }; template struct LDLT_Traits { typedef const TriangularView MatrixL; typedef const TriangularView MatrixU; - static inline MatrixL getL(const MatrixType& m) { return m.adjoint(); } - static inline MatrixU getU(const MatrixType& m) { return m; } + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); } }; } // end namespace internal @@ -437,21 +486,35 @@ template struct LDLT_Traits /** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix */ template -LDLT& LDLT::compute(const MatrixType& a) +template +LDLT& LDLT::compute(const EigenBase& a) { check_template_parameters(); - + eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); - m_matrix = a; + m_matrix = a.derived(); + + // Compute matrix L1 norm = max abs column sum. + m_l1_norm = RealScalar(0); + // TODO move this code to SelfAdjointView + for (Index col = 0; col < size; ++col) { + RealScalar abs_col_sum; + if (_UpLo == Lower) + abs_col_sum = m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>(); + else + abs_col_sum = m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>(); + if (abs_col_sum > m_l1_norm) + m_l1_norm = abs_col_sum; + } m_transpositions.resize(size); m_isInitialized = false; m_temporary.resize(size); m_sign = internal::ZeroSign; - internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); + m_info = internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign) ? Success : NumericalIssue; m_isInitialized = true; return *this; @@ -466,18 +529,19 @@ template template LDLT& LDLT::rankUpdate(const MatrixBase& w, const typename LDLT::RealScalar& sigma) { + typedef typename TranspositionType::StorageIndex IndexType; const Index size = w.rows(); if (m_isInitialized) { eigen_assert(m_matrix.rows()==size); } else - { + { m_matrix.resize(size,size); m_matrix.setZero(); m_transpositions.resize(size); for (Index i = 0; i < size; i++) - m_transpositions.coeffRef(i) = i; + m_transpositions.coeffRef(i) = IndexType(i); m_temporary.resize(size); m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef; m_isInitialized = true; @@ -488,53 +552,45 @@ LDLT& LDLT::rankUpdate(const MatrixBase -struct solve_retval, Rhs> - : solve_retval_base, Rhs> +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +template +void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const { - typedef LDLT<_MatrixType,_UpLo> LDLTType; - EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs) - - template void evalTo(Dest& dst) const + eigen_assert(rhs.rows() == rows()); + // dst = P b + dst = m_transpositions * rhs; + + // dst = L^-1 (P b) + matrixL().solveInPlace(dst); + + // dst = D^-1 (L^-1 P b) + // more precisely, use pseudo-inverse of D (see bug 241) + using std::abs; + const typename Diagonal::RealReturnType vecD(vectorD()); + // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon + // as motivated by LAPACK's xGELSS: + // RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits::epsilon(),RealScalar(1) / NumTraits::highest()); + // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest + // diagonal element is not well justified and leads to numerical issues in some cases. + // Moreover, Lapack's xSYTRS routines use 0 for the tolerance. + RealScalar tolerance = RealScalar(1) / NumTraits::highest(); + + for (Index i = 0; i < vecD.size(); ++i) { - eigen_assert(rhs().rows() == dec().matrixLDLT().rows()); - // dst = P b - dst = dec().transpositionsP() * rhs(); - - // dst = L^-1 (P b) - dec().matrixL().solveInPlace(dst); - - // dst = D^-1 (L^-1 P b) - // more precisely, use pseudo-inverse of D (see bug 241) - using std::abs; - using std::max; - typedef typename LDLTType::MatrixType MatrixType; - typedef typename LDLTType::RealScalar RealScalar; - const typename Diagonal::RealReturnType vectorD(dec().vectorD()); - // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon - // as motivated by LAPACK's xGELSS: - // RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits::epsilon(),RealScalar(1) / NumTraits::highest()); - // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest - // diagonal element is not well justified and to numerical issues in some cases. - // Moreover, Lapack's xSYTRS routines use 0 for the tolerance. - RealScalar tolerance = RealScalar(1) / NumTraits::highest(); - - for (Index i = 0; i < vectorD.size(); ++i) { - if(abs(vectorD(i)) > tolerance) - dst.row(i) /= vectorD(i); - else - dst.row(i).setZero(); - } + if(abs(vecD(i)) > tolerance) + dst.row(i) /= vecD(i); + else + dst.row(i).setZero(); + } - // dst = L^-T (D^-1 L^-1 P b) - dec().matrixU().solveInPlace(dst); + // dst = L^-T (D^-1 L^-1 P b) + matrixU().solveInPlace(dst); - // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b - dst = dec().transpositionsP().transpose() * dst; - } -}; + // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b + dst = m_transpositions.transpose() * dst; } +#endif /** \internal use x = ldlt_object.solve(x); * @@ -588,6 +644,7 @@ MatrixType LDLT::reconstructedMatrix() const /** \cholesky_module * \returns the Cholesky decomposition with full pivoting without square root of \c *this + * \sa MatrixBase::ldlt() */ template inline const LDLT::PlainObject, UpLo> @@ -598,6 +655,7 @@ SelfAdjointView::ldlt() const /** \cholesky_module * \returns the Cholesky decomposition with full pivoting without square root of \c *this + * \sa SelfAdjointView::ldlt() */ template inline const LDLT::PlainObject> diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT.h b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT.h index 7c11a2dc29..87ca8d4236 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT.h @@ -10,7 +10,7 @@ #ifndef EIGEN_LLT_H #define EIGEN_LLT_H -namespace Eigen { +namespace Eigen { namespace internal{ template struct LLT_Traits; @@ -22,8 +22,8 @@ template struct LLT_Traits; * * \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features * - * \param MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition - * \param UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper. + * \tparam _MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition + * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper. * The other triangular part won't be read. * * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite @@ -40,8 +40,10 @@ template struct LLT_Traits; * * Example: \include LLT_example.cpp * Output: \verbinclude LLT_example.out - * - * \sa MatrixBase::llt(), class LDLT + * + * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism. + * + * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT */ /* HEY THIS DOX IS DISABLED BECAUSE THERE's A BUG EITHER HERE OR IN LDLT ABOUT THAT (OR BOTH) * Note that during the decomposition, only the upper triangular part of A is considered. Therefore, @@ -54,12 +56,12 @@ template class LLT enum { RowsAtCompileTime = MatrixType::RowsAtCompileTime, ColsAtCompileTime = MatrixType::ColsAtCompileTime, - Options = MatrixType::Options, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime }; typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; - typedef typename MatrixType::Index Index; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 + typedef typename MatrixType::StorageIndex StorageIndex; enum { PacketSize = internal::packet_traits::size, @@ -83,14 +85,30 @@ template class LLT * according to the specified problem \a size. * \sa LLT() */ - LLT(Index size) : m_matrix(size, size), + explicit LLT(Index size) : m_matrix(size, size), m_isInitialized(false) {} - LLT(const MatrixType& matrix) + template + explicit LLT(const EigenBase& matrix) : m_matrix(matrix.rows(), matrix.cols()), m_isInitialized(false) { - compute(matrix); + compute(matrix.derived()); + } + + /** \brief Constructs a LDLT factorization from a given matrix + * + * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when + * \c MatrixType is a Eigen::Ref. + * + * \sa LLT(const EigenBase&) + */ + template + explicit LLT(EigenBase& matrix) + : m_matrix(matrix.derived()), + m_isInitialized(false) + { + compute(matrix.derived()); } /** \returns a view of the upper triangular matrix U */ @@ -115,33 +133,33 @@ template class LLT * Example: \include LLT_solve.cpp * Output: \verbinclude LLT_solve.out * - * \sa solveInPlace(), MatrixBase::llt() + * \sa solveInPlace(), MatrixBase::llt(), SelfAdjointView::llt() */ template - inline const internal::solve_retval + inline const Solve solve(const MatrixBase& b) const { eigen_assert(m_isInitialized && "LLT is not initialized."); eigen_assert(m_matrix.rows()==b.rows() && "LLT::solve(): invalid number of rows of the right hand side matrix b"); - return internal::solve_retval(*this, b.derived()); + return Solve(*this, b.derived()); } - #ifdef EIGEN2_SUPPORT - template - bool solve(const MatrixBase& b, ResultType *result) const - { - *result = this->solve(b); - return true; - } - - bool isPositiveDefinite() const { return true; } - #endif - template void solveInPlace(MatrixBase &bAndX) const; - LLT& compute(const MatrixType& matrix); + template + LLT& compute(const EigenBase& matrix); + + /** \returns an estimate of the reciprocal condition number of the matrix of + * which \c *this is the Cholesky decomposition. + */ + RealScalar rcond() const + { + eigen_assert(m_isInitialized && "LLT is not initialized."); + eigen_assert(m_info == Success && "LLT failed because matrix appears to be negative"); + return internal::rcond_estimate_helper(m_l1_norm, *this); + } /** \returns the LLT decomposition matrix * @@ -167,24 +185,38 @@ template class LLT return m_info; } + /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint. + * + * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as: + * \code x = decomposition.adjoint().solve(b) \endcode + */ + const LLT& adjoint() const { return *this; }; + inline Index rows() const { return m_matrix.rows(); } inline Index cols() const { return m_matrix.cols(); } template LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1); + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC + void _solve_impl(const RhsType &rhs, DstType &dst) const; + #endif + protected: - + static void check_template_parameters() { EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); } - + /** \internal * Used to compute and store L * The strict upper part is not used and even not initialized. */ MatrixType m_matrix; + RealScalar m_l1_norm; bool m_isInitialized; ComputationInfo m_info; }; @@ -194,12 +226,11 @@ namespace internal { template struct llt_inplace; template -static typename MatrixType::Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) +static Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) { using std::sqrt; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - typedef typename MatrixType::Index Index; typedef typename MatrixType::ColXpr ColXpr; typedef typename internal::remove_all::type ColXprCleaned; typedef typename ColXprCleaned::SegmentReturnType ColXprSegment; @@ -268,11 +299,10 @@ template struct llt_inplace { typedef typename NumTraits::Real RealScalar; template - static typename MatrixType::Index unblocked(MatrixType& mat) + static Index unblocked(MatrixType& mat) { using std::sqrt; - typedef typename MatrixType::Index Index; - + eigen_assert(mat.rows()==mat.cols()); const Index size = mat.rows(); for(Index k = 0; k < size; ++k) @@ -295,9 +325,8 @@ template struct llt_inplace } template - static typename MatrixType::Index blocked(MatrixType& m) + static Index blocked(MatrixType& m) { - typedef typename MatrixType::Index Index; eigen_assert(m.rows()==m.cols()); Index size = m.rows(); if(size<32) @@ -322,36 +351,36 @@ template struct llt_inplace Index ret; if((ret=unblocked(A11))>=0) return k+ret; if(rs>0) A11.adjoint().template triangularView().template solveInPlace(A21); - if(rs>0) A22.template selfadjointView().rankUpdate(A21,-1); // bottleneck + if(rs>0) A22.template selfadjointView().rankUpdate(A21,typename NumTraits::Literal(-1)); // bottleneck } return -1; } template - static typename MatrixType::Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) { return Eigen::internal::llt_rank_update_lower(mat, vec, sigma); } }; - + template struct llt_inplace { typedef typename NumTraits::Real RealScalar; template - static EIGEN_STRONG_INLINE typename MatrixType::Index unblocked(MatrixType& mat) + static EIGEN_STRONG_INLINE Index unblocked(MatrixType& mat) { Transpose matt(mat); return llt_inplace::unblocked(matt); } template - static EIGEN_STRONG_INLINE typename MatrixType::Index blocked(MatrixType& mat) + static EIGEN_STRONG_INLINE Index blocked(MatrixType& mat) { Transpose matt(mat); return llt_inplace::blocked(matt); } template - static typename MatrixType::Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma) { Transpose matt(mat); return llt_inplace::rankUpdate(matt, vec.conjugate(), sigma); @@ -362,8 +391,8 @@ template struct LLT_Traits { typedef const TriangularView MatrixL; typedef const TriangularView MatrixU; - static inline MatrixL getL(const MatrixType& m) { return m; } - static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); } + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); } static bool inplace_decomposition(MatrixType& m) { return llt_inplace::blocked(m)==-1; } }; @@ -372,8 +401,8 @@ template struct LLT_Traits { typedef const TriangularView MatrixL; typedef const TriangularView MatrixU; - static inline MatrixL getL(const MatrixType& m) { return m.adjoint(); } - static inline MatrixU getU(const MatrixType& m) { return m; } + static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); } + static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); } static bool inplace_decomposition(MatrixType& m) { return llt_inplace::blocked(m)==-1; } }; @@ -388,14 +417,28 @@ template struct LLT_Traits * Output: \verbinclude TutorialLinAlgComputeTwice.out */ template -LLT& LLT::compute(const MatrixType& a) +template +LLT& LLT::compute(const EigenBase& a) { check_template_parameters(); - + eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); m_matrix.resize(size, size); - m_matrix = a; + m_matrix = a.derived(); + + // Compute matrix L1 norm = max abs column sum. + m_l1_norm = RealScalar(0); + // TODO move this code to SelfAdjointView + for (Index col = 0; col < size; ++col) { + RealScalar abs_col_sum; + if (_UpLo == Lower) + abs_col_sum = m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>(); + else + abs_col_sum = m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>(); + if (abs_col_sum > m_l1_norm) + m_l1_norm = abs_col_sum; + } m_isInitialized = true; bool ok = Traits::inplace_decomposition(m_matrix); @@ -423,33 +466,24 @@ LLT<_MatrixType,_UpLo> LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, c return *this; } - -namespace internal { -template -struct solve_retval, Rhs> - : solve_retval_base, Rhs> -{ - typedef LLT<_MatrixType,UpLo> LLTType; - EIGEN_MAKE_SOLVE_HELPERS(LLTType,Rhs) - template void evalTo(Dest& dst) const - { - dst = rhs(); - dec().solveInPlace(dst); - } -}; +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +template +void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const +{ + dst = rhs; + solveInPlace(dst); } +#endif /** \internal use x = llt_object.solve(x); - * + * * This is the \em in-place version of solve(). * * \param bAndX represents both the right-hand side matrix b and result x. * - * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD. - * - * This version avoids a copy when the right hand side matrix b is not - * needed anymore. + * This version avoids a copy when the right hand side matrix b is not needed anymore. * * \sa LLT::solve(), MatrixBase::llt() */ @@ -475,6 +509,7 @@ MatrixType LLT::reconstructedMatrix() const /** \cholesky_module * \returns the LLT decomposition of \c *this + * \sa SelfAdjointView::llt() */ template inline const LLT::PlainObject> @@ -485,6 +520,7 @@ MatrixBase::llt() const /** \cholesky_module * \returns the LLT decomposition of \c *this + * \sa SelfAdjointView::llt() */ template inline const LLT::PlainObject, UpLo> diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT_LAPACKE.h b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT_LAPACKE.h new file mode 100644 index 0000000000..bc6489e69a --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT_LAPACKE.h @@ -0,0 +1,99 @@ +/* + Copyright (c) 2011, Intel Corporation. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of Intel Corporation nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + ******************************************************************************** + * Content : Eigen bindings to LAPACKe + * LLt decomposition based on LAPACKE_?potrf function. + ******************************************************************************** +*/ + +#ifndef EIGEN_LLT_LAPACKE_H +#define EIGEN_LLT_LAPACKE_H + +namespace Eigen { + +namespace internal { + +template struct lapacke_llt; + +#define EIGEN_LAPACKE_LLT(EIGTYPE, BLASTYPE, LAPACKE_PREFIX) \ +template<> struct lapacke_llt \ +{ \ + template \ + static inline Index potrf(MatrixType& m, char uplo) \ + { \ + lapack_int matrix_order; \ + lapack_int size, lda, info, StorageOrder; \ + EIGTYPE* a; \ + eigen_assert(m.rows()==m.cols()); \ + /* Set up parameters for ?potrf */ \ + size = convert_index(m.rows()); \ + StorageOrder = MatrixType::Flags&RowMajorBit?RowMajor:ColMajor; \ + matrix_order = StorageOrder==RowMajor ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ + a = &(m.coeffRef(0,0)); \ + lda = convert_index(m.outerStride()); \ +\ + info = LAPACKE_##LAPACKE_PREFIX##potrf( matrix_order, uplo, size, (BLASTYPE*)a, lda ); \ + info = (info==0) ? -1 : info>0 ? info-1 : size; \ + return info; \ + } \ +}; \ +template<> struct llt_inplace \ +{ \ + template \ + static Index blocked(MatrixType& m) \ + { \ + return lapacke_llt::potrf(m, 'L'); \ + } \ + template \ + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \ + { return Eigen::internal::llt_rank_update_lower(mat, vec, sigma); } \ +}; \ +template<> struct llt_inplace \ +{ \ + template \ + static Index blocked(MatrixType& m) \ + { \ + return lapacke_llt::potrf(m, 'U'); \ + } \ + template \ + static Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \ + { \ + Transpose matt(mat); \ + return llt_inplace::rankUpdate(matt, vec.conjugate(), sigma); \ + } \ +}; + +EIGEN_LAPACKE_LLT(double, double, d) +EIGEN_LAPACKE_LLT(float, float, s) +EIGEN_LAPACKE_LLT(dcomplex, lapack_complex_double, z) +EIGEN_LAPACKE_LLT(scomplex, lapack_complex_float, c) + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_LLT_LAPACKE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT_MKL.h b/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT_MKL.h deleted file mode 100644 index 66675d7476..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/Cholesky/LLT_MKL.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - Copyright (c) 2011, Intel Corporation. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, - are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - * Neither the name of Intel Corporation nor the names of its contributors may - be used to endorse or promote products derived from this software without - specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR - ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - ******************************************************************************** - * Content : Eigen bindings to Intel(R) MKL - * LLt decomposition based on LAPACKE_?potrf function. - ******************************************************************************** -*/ - -#ifndef EIGEN_LLT_MKL_H -#define EIGEN_LLT_MKL_H - -#include "Eigen/src/Core/util/MKL_support.h" -#include - -namespace Eigen { - -namespace internal { - -template struct mkl_llt; - -#define EIGEN_MKL_LLT(EIGTYPE, MKLTYPE, MKLPREFIX) \ -template<> struct mkl_llt \ -{ \ - template \ - static inline typename MatrixType::Index potrf(MatrixType& m, char uplo) \ - { \ - lapack_int matrix_order; \ - lapack_int size, lda, info, StorageOrder; \ - EIGTYPE* a; \ - eigen_assert(m.rows()==m.cols()); \ - /* Set up parameters for ?potrf */ \ - size = m.rows(); \ - StorageOrder = MatrixType::Flags&RowMajorBit?RowMajor:ColMajor; \ - matrix_order = StorageOrder==RowMajor ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ - a = &(m.coeffRef(0,0)); \ - lda = m.outerStride(); \ -\ - info = LAPACKE_##MKLPREFIX##potrf( matrix_order, uplo, size, (MKLTYPE*)a, lda ); \ - info = (info==0) ? -1 : info>0 ? info-1 : size; \ - return info; \ - } \ -}; \ -template<> struct llt_inplace \ -{ \ - template \ - static typename MatrixType::Index blocked(MatrixType& m) \ - { \ - return mkl_llt::potrf(m, 'L'); \ - } \ - template \ - static typename MatrixType::Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \ - { return Eigen::internal::llt_rank_update_lower(mat, vec, sigma); } \ -}; \ -template<> struct llt_inplace \ -{ \ - template \ - static typename MatrixType::Index blocked(MatrixType& m) \ - { \ - return mkl_llt::potrf(m, 'U'); \ - } \ - template \ - static typename MatrixType::Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \ - { \ - Transpose matt(mat); \ - return llt_inplace::rankUpdate(matt, vec.conjugate(), sigma); \ - } \ -}; - -EIGEN_MKL_LLT(double, double, d) -EIGEN_MKL_LLT(float, float, s) -EIGEN_MKL_LLT(dcomplex, MKL_Complex16, z) -EIGEN_MKL_LLT(scomplex, MKL_Complex8, c) - -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_LLT_MKL_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CMakeLists.txt b/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CMakeLists.txt deleted file mode 100644 index 814dfa6131..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -FILE(GLOB Eigen_CholmodSupport_SRCS "*.h") - -INSTALL(FILES - ${Eigen_CholmodSupport_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/CholmodSupport COMPONENT Devel - ) diff --git a/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CholmodSupport.h index 99dbe171c3..5719720238 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -14,46 +14,52 @@ namespace Eigen { namespace internal { -template -void cholmod_configure_matrix(CholmodType& mat) -{ - if (internal::is_same::value) - { - mat.xtype = CHOLMOD_REAL; - mat.dtype = CHOLMOD_SINGLE; - } - else if (internal::is_same::value) - { +template struct cholmod_configure_matrix; + +template<> struct cholmod_configure_matrix { + template + static void run(CholmodType& mat) { mat.xtype = CHOLMOD_REAL; mat.dtype = CHOLMOD_DOUBLE; } - else if (internal::is_same >::value) - { - mat.xtype = CHOLMOD_COMPLEX; - mat.dtype = CHOLMOD_SINGLE; - } - else if (internal::is_same >::value) - { +}; + +template<> struct cholmod_configure_matrix > { + template + static void run(CholmodType& mat) { mat.xtype = CHOLMOD_COMPLEX; mat.dtype = CHOLMOD_DOUBLE; } - else - { - eigen_assert(false && "Scalar type not supported by CHOLMOD"); - } -} +}; + +// Other scalar types are not yet suppotred by Cholmod +// template<> struct cholmod_configure_matrix { +// template +// static void run(CholmodType& mat) { +// mat.xtype = CHOLMOD_REAL; +// mat.dtype = CHOLMOD_SINGLE; +// } +// }; +// +// template<> struct cholmod_configure_matrix > { +// template +// static void run(CholmodType& mat) { +// mat.xtype = CHOLMOD_COMPLEX; +// mat.dtype = CHOLMOD_SINGLE; +// } +// }; } // namespace internal /** Wraps the Eigen sparse matrix \a mat into a Cholmod sparse matrix object. * Note that the data are shared. */ -template -cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) +template +cholmod_sparse viewAsCholmod(Ref > mat) { cholmod_sparse res; res.nzmax = mat.nonZeros(); - res.nrow = mat.rows();; + res.nrow = mat.rows(); res.ncol = mat.cols(); res.p = mat.outerIndexPtr(); res.i = mat.innerIndexPtr(); @@ -74,11 +80,11 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) res.dtype = 0; res.stype = -1; - if (internal::is_same<_Index,int>::value) + if (internal::is_same<_StorageIndex,int>::value) { res.itype = CHOLMOD_INT; } - else if (internal::is_same<_Index,SuiteSparse_long>::value) + else if (internal::is_same<_StorageIndex,long>::value) { res.itype = CHOLMOD_LONG; } @@ -88,7 +94,7 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) } // setup res.xtype - internal::cholmod_configure_matrix<_Scalar>(res); + internal::cholmod_configure_matrix<_Scalar>::run(res); res.stype = 0; @@ -98,16 +104,23 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) template const cholmod_sparse viewAsCholmod(const SparseMatrix<_Scalar,_Options,_Index>& mat) { - cholmod_sparse res = viewAsCholmod(mat.const_cast_derived()); + cholmod_sparse res = viewAsCholmod(Ref >(mat.const_cast_derived())); + return res; +} + +template +const cholmod_sparse viewAsCholmod(const SparseVector<_Scalar,_Options,_Index>& mat) +{ + cholmod_sparse res = viewAsCholmod(Ref >(mat.const_cast_derived())); return res; } /** Returns a view of the Eigen sparse matrix \a mat as Cholmod sparse matrix. * The data are not copied but shared. */ template -cholmod_sparse viewAsCholmod(const SparseSelfAdjointView, UpLo>& mat) +cholmod_sparse viewAsCholmod(const SparseSelfAdjointView, UpLo>& mat) { - cholmod_sparse res = viewAsCholmod(mat.matrix().const_cast_derived()); + cholmod_sparse res = viewAsCholmod(Ref >(mat.matrix().const_cast_derived())); if(UpLo==Upper) res.stype = 1; if(UpLo==Lower) res.stype = -1; @@ -131,19 +144,19 @@ cholmod_dense viewAsCholmod(MatrixBase& mat) res.x = (void*)(mat.derived().data()); res.z = 0; - internal::cholmod_configure_matrix(res); + internal::cholmod_configure_matrix::run(res); return res; } /** Returns a view of the Cholmod sparse matrix \a cm as an Eigen sparse matrix. * The data are not copied but shared. */ -template -MappedSparseMatrix viewAsEigen(cholmod_sparse& cm) +template +MappedSparseMatrix viewAsEigen(cholmod_sparse& cm) { - return MappedSparseMatrix - (cm.nrow, cm.ncol, static_cast(cm.p)[cm.ncol], - static_cast(cm.p), static_cast(cm.i),static_cast(cm.x) ); + return MappedSparseMatrix + (cm.nrow, cm.ncol, static_cast(cm.p)[cm.ncol], + static_cast(cm.p), static_cast(cm.i),static_cast(cm.x) ); } enum CholmodMode { @@ -157,29 +170,39 @@ enum CholmodMode { * \sa class CholmodSupernodalLLT, class CholmodSimplicialLDLT, class CholmodSimplicialLLT */ template -class CholmodBase : internal::noncopyable +class CholmodBase : public SparseSolverBase { + protected: + typedef SparseSolverBase Base; + using Base::derived; + using Base::m_isInitialized; public: typedef _MatrixType MatrixType; enum { UpLo = _UpLo }; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; typedef MatrixType CholMatrixType; - typedef typename MatrixType::Index Index; + typedef typename MatrixType::StorageIndex StorageIndex; + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime, + MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime + }; public: CholmodBase() - : m_cholmodFactor(0), m_info(Success), m_isInitialized(false) + : m_cholmodFactor(0), m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false) { - m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0); + EIGEN_STATIC_ASSERT((internal::is_same::value), CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY); + m_shiftOffset[0] = m_shiftOffset[1] = 0.0; cholmod_start(&m_cholmod); } - CholmodBase(const MatrixType& matrix) - : m_cholmodFactor(0), m_info(Success), m_isInitialized(false) + explicit CholmodBase(const MatrixType& matrix) + : m_cholmodFactor(0), m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false) { - m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0); + EIGEN_STATIC_ASSERT((internal::is_same::value), CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY); + m_shiftOffset[0] = m_shiftOffset[1] = 0.0; cholmod_start(&m_cholmod); compute(matrix); } @@ -191,11 +214,8 @@ class CholmodBase : internal::noncopyable cholmod_finish(&m_cholmod); } - inline Index cols() const { return m_cholmodFactor->n; } - inline Index rows() const { return m_cholmodFactor->n; } - - Derived& derived() { return *static_cast(this); } - const Derived& derived() const { return *static_cast(this); } + inline StorageIndex cols() const { return internal::convert_index(m_cholmodFactor->n); } + inline StorageIndex rows() const { return internal::convert_index(m_cholmodFactor->n); } /** \brief Reports whether previous computation was successful. * @@ -216,34 +236,6 @@ class CholmodBase : internal::noncopyable return derived(); } - /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A. - * - * \sa compute() - */ - template - inline const internal::solve_retval - solve(const MatrixBase& b) const - { - eigen_assert(m_isInitialized && "LLT is not initialized."); - eigen_assert(rows()==b.rows() - && "CholmodDecomposition::solve(): invalid number of rows of the right hand side matrix b"); - return internal::solve_retval(*this, b.derived()); - } - - /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A. - * - * \sa compute() - */ - template - inline const internal::sparse_solve_retval - solve(const SparseMatrixBase& b) const - { - eigen_assert(m_isInitialized && "LLT is not initialized."); - eigen_assert(rows()==b.rows() - && "CholmodDecomposition::solve(): invalid number of rows of the right hand side matrix b"); - return internal::sparse_solve_retval(*this, b.derived()); - } - /** Performs a symbolic decomposition on the sparsity pattern of \a matrix. * * This function is particularly useful when solving for several problems having the same structure. @@ -277,7 +269,7 @@ class CholmodBase : internal::noncopyable eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); cholmod_sparse A = viewAsCholmod(matrix.template selfadjointView()); cholmod_factorize_p(&A, m_shiftOffset, 0, 0, m_cholmodFactor, &m_cholmod); - + // If the factorization failed, minor is the column at which it did. On success minor == n. this->m_info = (m_cholmodFactor->minor == m_cholmodFactor->n ? Success : NumericalIssue); m_factorizationIsOk = true; @@ -290,20 +282,22 @@ class CholmodBase : internal::noncopyable #ifndef EIGEN_PARSED_BY_DOXYGEN /** \internal */ template - void _solve(const MatrixBase &b, MatrixBase &dest) const + void _solve_impl(const MatrixBase &b, MatrixBase &dest) const { eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); const Index size = m_cholmodFactor->n; EIGEN_UNUSED_VARIABLE(size); eigen_assert(size==b.rows()); + + // Cholmod needs column-major stoarge without inner-stride, which corresponds to the default behavior of Ref. + Ref > b_ref(b.derived()); - // note: cd stands for Cholmod Dense - Rhs& b_ref(b.const_cast_derived()); cholmod_dense b_cd = viewAsCholmod(b_ref); cholmod_dense* x_cd = cholmod_solve(CHOLMOD_A, m_cholmodFactor, &b_cd, &m_cholmod); if(!x_cd) { this->m_info = NumericalIssue; + return; } // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = Matrix::Map(reinterpret_cast(x_cd->x),b.rows(),b.cols()); @@ -311,8 +305,8 @@ class CholmodBase : internal::noncopyable } /** \internal */ - template - void _solve(const SparseMatrix &b, SparseMatrix &dest) const + template + void _solve_impl(const SparseMatrixBase &b, SparseMatrixBase &dest) const { eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); const Index size = m_cholmodFactor->n; @@ -320,14 +314,16 @@ class CholmodBase : internal::noncopyable eigen_assert(size==b.rows()); // note: cs stands for Cholmod Sparse - cholmod_sparse b_cs = viewAsCholmod(b); + Ref > b_ref(b.const_cast_derived()); + cholmod_sparse b_cs = viewAsCholmod(b_ref); cholmod_sparse* x_cs = cholmod_spsolve(CHOLMOD_A, m_cholmodFactor, &b_cs, &m_cholmod); if(!x_cs) { this->m_info = NumericalIssue; + return; } // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) - dest = viewAsEigen(*x_cs); + dest.derived() = viewAsEigen(*x_cs); cholmod_free_sparse(&x_cs, &m_cholmod); } #endif // EIGEN_PARSED_BY_DOXYGEN @@ -344,10 +340,61 @@ class CholmodBase : internal::noncopyable */ Derived& setShift(const RealScalar& offset) { - m_shiftOffset[0] = offset; + m_shiftOffset[0] = double(offset); return derived(); } + /** \returns the determinant of the underlying matrix from the current factorization */ + Scalar determinant() const + { + using std::exp; + return exp(logDeterminant()); + } + + /** \returns the log determinant of the underlying matrix from the current factorization */ + Scalar logDeterminant() const + { + using std::log; + using numext::real; + eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); + + RealScalar logDet = 0; + Scalar *x = static_cast(m_cholmodFactor->x); + if (m_cholmodFactor->is_super) + { + // Supernodal factorization stored as a packed list of dense column-major blocs, + // as described by the following structure: + + // super[k] == index of the first column of the j-th super node + StorageIndex *super = static_cast(m_cholmodFactor->super); + // pi[k] == offset to the description of row indices + StorageIndex *pi = static_cast(m_cholmodFactor->pi); + // px[k] == offset to the respective dense block + StorageIndex *px = static_cast(m_cholmodFactor->px); + + Index nb_super_nodes = m_cholmodFactor->nsuper; + for (Index k=0; k < nb_super_nodes; ++k) + { + StorageIndex ncols = super[k + 1] - super[k]; + StorageIndex nrows = pi[k + 1] - pi[k]; + + Map, 0, InnerStride<> > sk(x + px[k], ncols, InnerStride<>(nrows+1)); + logDet += sk.real().log().sum(); + } + } + else + { + // Simplicial factorization stored as standard CSC matrix. + StorageIndex *p = static_cast(m_cholmodFactor->p); + Index size = m_cholmodFactor->n; + for (Index k=0; kis_ll) + logDet *= 2.0; + return logDet; + }; + template void dumpMemory(Stream& /*s*/) {} @@ -355,9 +402,8 @@ class CholmodBase : internal::noncopyable protected: mutable cholmod_common m_cholmod; cholmod_factor* m_cholmodFactor; - RealScalar m_shiftOffset[2]; + double m_shiftOffset[2]; mutable ComputationInfo m_info; - bool m_isInitialized; int m_factorizationIsOk; int m_analysisIsOk; }; @@ -376,9 +422,13 @@ class CholmodBase : internal::noncopyable * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower * or Upper. Default is Lower. * + * \implsparsesolverconcept + * * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. * - * \sa \ref TutorialSparseDirectSolvers, class CholmodSupernodalLLT, class SimplicialLLT + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept, class CholmodSupernodalLLT, class SimplicialLLT */ template class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLLT<_MatrixType, _UpLo> > @@ -395,7 +445,7 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl CholmodSimplicialLLT(const MatrixType& matrix) : Base() { init(); - Base::compute(matrix); + this->compute(matrix); } ~CholmodSimplicialLLT() {} @@ -423,9 +473,13 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower * or Upper. Default is Lower. * + * \implsparsesolverconcept + * * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. * - * \sa \ref TutorialSparseDirectSolvers, class CholmodSupernodalLLT, class SimplicialLDLT + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept, class CholmodSupernodalLLT, class SimplicialLDLT */ template class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLDLT<_MatrixType, _UpLo> > @@ -442,7 +496,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp CholmodSimplicialLDLT(const MatrixType& matrix) : Base() { init(); - Base::compute(matrix); + this->compute(matrix); } ~CholmodSimplicialLDLT() {} @@ -468,9 +522,13 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower * or Upper. Default is Lower. * + * \implsparsesolverconcept + * * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. * - * \sa \ref TutorialSparseDirectSolvers + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept */ template class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSupernodalLLT<_MatrixType, _UpLo> > @@ -487,7 +545,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper CholmodSupernodalLLT(const MatrixType& matrix) : Base() { init(); - Base::compute(matrix); + this->compute(matrix); } ~CholmodSupernodalLLT() {} @@ -515,9 +573,13 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower * or Upper. Default is Lower. * + * \implsparsesolverconcept + * * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed. * - * \sa \ref TutorialSparseDirectSolvers + * \warning Only double precision real and complex scalar types are supported by Cholmod. + * + * \sa \ref TutorialSparseSolverConcept */ template class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecomposition<_MatrixType, _UpLo> > @@ -534,7 +596,7 @@ class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecom CholmodDecomposition(const MatrixType& matrix) : Base() { init(); - Base::compute(matrix); + this->compute(matrix); } ~CholmodDecomposition() {} @@ -572,36 +634,6 @@ class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecom } }; -namespace internal { - -template -struct solve_retval, Rhs> - : solve_retval_base, Rhs> -{ - typedef CholmodBase<_MatrixType,_UpLo,Derived> Dec; - EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs) - - template void evalTo(Dest& dst) const - { - dec()._solve(rhs(),dst); - } -}; - -template -struct sparse_solve_retval, Rhs> - : sparse_solve_retval_base, Rhs> -{ - typedef CholmodBase<_MatrixType,_UpLo,Derived> Dec; - EIGEN_MAKE_SPARSE_SOLVE_HELPERS(Dec,Rhs) - - template void evalTo(Dest& dst) const - { - dec()._solve(rhs(),dst); - } -}; - -} // end namespace internal - } // end namespace Eigen #endif // EIGEN_CHOLMODSUPPORT_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Array.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Array.h index 0b9c38c821..0d34269fd4 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Array.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Array.h @@ -12,7 +12,16 @@ namespace Eigen { -/** \class Array +namespace internal { +template +struct traits > : traits > +{ + typedef ArrayXpr XprKind; + typedef ArrayBase > XprBase; +}; +} + +/** \class Array * \ingroup Core_Module * * \brief General-purpose arrays with easy API for coefficient-wise operations @@ -24,20 +33,14 @@ namespace Eigen { * API for the %Matrix class provides easy access to linear-algebra * operations. * + * See documentation of class Matrix for detailed information on the template parameters + * storage layout. + * * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_ARRAY_PLUGIN. + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAY_PLUGIN. * - * \sa \ref TutorialArrayClass, \ref TopicClassHierarchy + * \sa \blank \ref TutorialArrayClass, \ref TopicClassHierarchy */ -namespace internal { -template -struct traits > : traits > -{ - typedef ArrayXpr XprKind; - typedef ArrayBase > XprBase; -}; -} - template class Array : public PlainObjectBase > @@ -69,11 +72,27 @@ class Array * the usage of 'using'. This should be done only for operator=. */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const EigenBase &other) { return Base::operator=(other); } + /** Set all the entries to \a value. + * \sa DenseBase::setConstant(), DenseBase::fill() + */ + /* This overload is needed because the usage of + * using Base::operator=; + * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped + * the usage of 'using'. This should be done only for operator=. + */ + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Array& operator=(const Scalar &value) + { + Base::setConstant(value); + return *this; + } + /** Copies the value of the expression \a other into \c *this with automatic resizing. * * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized), @@ -84,7 +103,8 @@ class Array * remain row-vectors and vectors remain vectors. */ template - EIGEN_STRONG_INLINE Array& operator=(const ArrayBase& other) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Array& operator=(const DenseBase& other) { return Base::_set(other); } @@ -92,11 +112,12 @@ class Array /** This is a special case of the templated operator=. Its purpose is to * prevent a default operator= from hiding the templated operator=. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array& operator=(const Array& other) { return Base::_set(other); } - + /** Default constructor. * * For fixed-size matrices, does nothing. @@ -107,6 +128,7 @@ class Array * * \sa resize(Index,Index) */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array() : Base() { Base::_check_template_params(); @@ -116,6 +138,7 @@ class Array #ifndef EIGEN_PARSED_BY_DOXYGEN // FIXME is it still needed ?? /** \internal */ + EIGEN_DEVICE_FUNC Array(internal::constructor_without_unaligned_array_assert) : Base(internal::constructor_without_unaligned_array_assert()) { @@ -124,56 +147,64 @@ class Array } #endif -#ifdef EIGEN_HAVE_RVALUE_REFERENCES - Array(Array&& other) +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + Array(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) : Base(std::move(other)) { Base::_check_template_params(); if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) Base::_set_noalias(other); } - Array& operator=(Array&& other) + EIGEN_DEVICE_FUNC + Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) { other.swap(*this); return *this; } #endif - /** Constructs a vector or row-vector with given dimension. \only_for_vectors - * - * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, - * it is redundant to pass the dimension here, so it makes more sense to use the default - * constructor Matrix() instead. - */ - EIGEN_STRONG_INLINE explicit Array(Index dim) - : Base(dim, RowsAtCompileTime == 1 ? 1 : dim, ColsAtCompileTime == 1 ? 1 : dim) + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE explicit Array(const T& x) { Base::_check_template_params(); - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Array) - eigen_assert(dim >= 0); - eigen_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim); - EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + Base::template _init1(x); } - #ifndef EIGEN_PARSED_BY_DOXYGEN template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1) { Base::_check_template_params(); this->template _init2(val0, val1); } #else - /** constructs an uninitialized matrix with \a rows rows and \a cols columns. + /** \brief Constructs a fixed-sized array initialized with coefficients starting at \a data */ + EIGEN_DEVICE_FUNC explicit Array(const Scalar *data); + /** Constructs a vector or row-vector with given dimension. \only_for_vectors * - * This is useful for dynamic-size matrices. For fixed-size matrices, + * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, + * it is redundant to pass the dimension here, so it makes more sense to use the default + * constructor Array() instead. + */ + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE explicit Array(Index dim); + /** constructs an initialized 1x1 Array with the given coefficient */ + Array(const Scalar& value); + /** constructs an uninitialized array with \a rows rows and \a cols columns. + * + * This is useful for dynamic-size arrays. For fixed-size arrays, * it is redundant to pass these parameters, so one should use the default constructor - * Matrix() instead. */ + * Array() instead. */ Array(Index rows, Index cols); /** constructs an initialized 2D vector with given coefficients */ Array(const Scalar& val0, const Scalar& val1); #endif /** constructs an initialized 3D vector with given coefficients */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2) { Base::_check_template_params(); @@ -183,6 +214,7 @@ class Array m_storage.data()[2] = val2; } /** constructs an initialized 4D vector with given coefficients */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, const Scalar& val3) { Base::_check_template_params(); @@ -193,51 +225,21 @@ class Array m_storage.data()[3] = val3; } - explicit Array(const Scalar *data); - - /** Constructor copying the value of the expression \a other */ - template - EIGEN_STRONG_INLINE Array(const ArrayBase& other) - : Base(other.rows() * other.cols(), other.rows(), other.cols()) - { - Base::_check_template_params(); - Base::_set_noalias(other); - } /** Copy constructor */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const Array& other) - : Base(other.rows() * other.cols(), other.rows(), other.cols()) - { - Base::_check_template_params(); - Base::_set_noalias(other); - } - /** Copy constructor with in-place evaluation */ - template - EIGEN_STRONG_INLINE Array(const ReturnByValue& other) - { - Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); - other.evalTo(*this); - } + : Base(other) + { } /** \sa MatrixBase::operator=(const EigenBase&) */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Array(const EigenBase &other) - : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) - { - Base::_check_template_params(); - Base::_resize_to_match(other); - *this = other; - } - - /** Override MatrixBase::swap() since for dynamic-sized matrices of same type it is enough to swap the - * data pointers. - */ - template - void swap(ArrayBase const & other) - { this->_swap(other.derived()); } + : Base(other.derived()) + { } - inline Index innerStride() const { return 1; } - inline Index outerStride() const { return this->innerSize(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return 1; } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return this->innerSize(); } #ifdef EIGEN_ARRAY_PLUGIN #include EIGEN_ARRAY_PLUGIN diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayBase.h index 33ff553712..f0232f65ef 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayBase.h @@ -32,7 +32,7 @@ template class MatrixWrapper; * \tparam Derived is the derived type, e.g., an array or an expression type. * * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN. + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN. * * \sa class MatrixBase, \ref TopicClassHierarchy */ @@ -47,13 +47,11 @@ template class ArrayBase typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; typedef DenseBase Base; - using Base::operator*; using Base::RowsAtCompileTime; using Base::ColsAtCompileTime; using Base::SizeAtCompileTime; @@ -62,8 +60,7 @@ template class ArrayBase using Base::MaxSizeAtCompileTime; using Base::IsVectorAtCompileTime; using Base::Flags; - using Base::CoeffReadCost; - + using Base::derived; using Base::const_cast_derived; using Base::rows; @@ -83,25 +80,14 @@ template class ArrayBase #endif // not EIGEN_PARSED_BY_DOXYGEN #ifndef EIGEN_PARSED_BY_DOXYGEN - /** \internal the plain matrix type corresponding to this expression. Note that is not necessarily - * exactly the return type of eval(): in the case of plain matrices, the return type of eval() is a const - * reference to a matrix, not a matrix! It is however guaranteed that the return type of eval() is either - * PlainObject or const PlainObject&. - */ - typedef Array::Scalar, - internal::traits::RowsAtCompileTime, - internal::traits::ColsAtCompileTime, - AutoAlign | (internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor), - internal::traits::MaxRowsAtCompileTime, - internal::traits::MaxColsAtCompileTime - > PlainObject; - + typedef typename Base::PlainObject PlainObject; /** \internal Represents a matrix with all coefficients equal to one another*/ - typedef CwiseNullaryOp,Derived> ConstantReturnType; + typedef CwiseNullaryOp,PlainObject> ConstantReturnType; #endif // not EIGEN_PARSED_BY_DOXYGEN #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase +#define EIGEN_DOC_UNARY_ADDONS(X,Y) # include "../plugins/CommonCwiseUnaryOps.h" # include "../plugins/MatrixCwiseUnaryOps.h" # include "../plugins/ArrayCwiseUnaryOps.h" @@ -112,44 +98,62 @@ template class ArrayBase # include EIGEN_ARRAYBASE_PLUGIN # endif #undef EIGEN_CURRENT_STORAGE_BASE_CLASS +#undef EIGEN_DOC_UNARY_ADDONS /** Special case of the template operator=, in order to prevent the compiler * from generating a default operator= (issue hit with g++ 4.1) */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const ArrayBase& other) { - return internal::assign_selector::run(derived(), other.derived()); + internal::call_assignment(derived(), other.derived()); + return derived(); } - - Derived& operator+=(const Scalar& scalar) - { return *this = derived() + scalar; } - Derived& operator-=(const Scalar& scalar) - { return *this = derived() - scalar; } + + /** Set all the entries to \a value. + * \sa DenseBase::setConstant(), DenseBase::fill() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator=(const Scalar &value) + { Base::setConstant(value); return derived(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator+=(const Scalar& scalar); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator-=(const Scalar& scalar); template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const ArrayBase& other); template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const ArrayBase& other); template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const ArrayBase& other); template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const ArrayBase& other); public: + EIGEN_DEVICE_FUNC ArrayBase& array() { return *this; } + EIGEN_DEVICE_FUNC const ArrayBase& array() const { return *this; } /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array * \sa MatrixBase::array() */ - MatrixWrapper matrix() { return derived(); } - const MatrixWrapper matrix() const { return derived(); } + EIGEN_DEVICE_FUNC + MatrixWrapper matrix() { return MatrixWrapper(derived()); } + EIGEN_DEVICE_FUNC + const MatrixWrapper matrix() const { return MatrixWrapper(derived()); } // template // inline void evalTo(Dest& dst) const { dst = matrix(); } protected: + EIGEN_DEVICE_FUNC ArrayBase() : Base() {} private: @@ -174,8 +178,7 @@ template EIGEN_STRONG_INLINE Derived & ArrayBase::operator-=(const ArrayBase &other) { - SelfCwiseBinaryOp, Derived, OtherDerived> tmp(derived()); - tmp = other.derived(); + call_assignment(derived(), other.derived(), internal::sub_assign_op()); return derived(); } @@ -188,8 +191,7 @@ template EIGEN_STRONG_INLINE Derived & ArrayBase::operator+=(const ArrayBase& other) { - SelfCwiseBinaryOp, Derived, OtherDerived> tmp(derived()); - tmp = other.derived(); + call_assignment(derived(), other.derived(), internal::add_assign_op()); return derived(); } @@ -202,8 +204,7 @@ template EIGEN_STRONG_INLINE Derived & ArrayBase::operator*=(const ArrayBase& other) { - SelfCwiseBinaryOp, Derived, OtherDerived> tmp(derived()); - tmp = other.derived(); + call_assignment(derived(), other.derived(), internal::mul_assign_op()); return derived(); } @@ -216,8 +217,7 @@ template EIGEN_STRONG_INLINE Derived & ArrayBase::operator/=(const ArrayBase& other) { - SelfCwiseBinaryOp, Derived, OtherDerived> tmp(derived()); - tmp = other.derived(); + call_assignment(derived(), other.derived(), internal::div_assign_op()); return derived(); } diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayWrapper.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayWrapper.h index b4641e2a01..a04521a161 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayWrapper.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/ArrayWrapper.h @@ -44,6 +44,7 @@ class ArrayWrapper : public ArrayBase > typedef ArrayBase Base; EIGEN_DENSE_PUBLIC_INTERFACE(ArrayWrapper) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ArrayWrapper) + typedef typename internal::remove_all::type NestedExpression; typedef typename internal::conditional< internal::is_lvalue::value, @@ -51,76 +52,45 @@ class ArrayWrapper : public ArrayBase > const Scalar >::type ScalarWithConstIfNotLvalue; - typedef typename internal::nested::type NestedExpressionType; + typedef typename internal::ref_selector::non_const_type NestedExpressionType; - inline ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {} + using Base::coeffRef; + EIGEN_DEVICE_FUNC + explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); } - inline ScalarWithConstIfNotLvalue* data() { return m_expression.const_cast_derived().data(); } + EIGEN_DEVICE_FUNC + inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_expression.data(); } - inline CoeffReturnType coeff(Index rowId, Index colId) const - { - return m_expression.coeff(rowId, colId); - } - - inline Scalar& coeffRef(Index rowId, Index colId) - { - return m_expression.const_cast_derived().coeffRef(rowId, colId); - } - + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { - return m_expression.const_cast_derived().coeffRef(rowId, colId); - } - - inline CoeffReturnType coeff(Index index) const - { - return m_expression.coeff(index); - } - - inline Scalar& coeffRef(Index index) - { - return m_expression.const_cast_derived().coeffRef(index); + return m_expression.coeffRef(rowId, colId); } + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { - return m_expression.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index rowId, Index colId) const - { - return m_expression.template packet(rowId, colId); - } - - template - inline void writePacket(Index rowId, Index colId, const PacketScalar& val) - { - m_expression.const_cast_derived().template writePacket(rowId, colId, val); - } - - template - inline const PacketScalar packet(Index index) const - { - return m_expression.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& val) - { - m_expression.const_cast_derived().template writePacket(index, val); + return m_expression.coeffRef(index); } template + EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const { dst = m_expression; } const typename internal::remove_all::type& + EIGEN_DEVICE_FUNC nestedExpression() const { return m_expression; @@ -128,10 +98,12 @@ class ArrayWrapper : public ArrayBase > /** Forwards the resizing request to the nested expression * \sa DenseBase::resize(Index) */ - void resize(Index newSize) { m_expression.const_cast_derived().resize(newSize); } + EIGEN_DEVICE_FUNC + void resize(Index newSize) { m_expression.resize(newSize); } /** Forwards the resizing request to the nested expression * \sa DenseBase::resize(Index,Index)*/ - void resize(Index nbRows, Index nbCols) { m_expression.const_cast_derived().resize(nbRows,nbCols); } + EIGEN_DEVICE_FUNC + void resize(Index rows, Index cols) { m_expression.resize(rows,cols); } protected: NestedExpressionType m_expression; @@ -169,6 +141,7 @@ class MatrixWrapper : public MatrixBase > typedef MatrixBase > Base; EIGEN_DENSE_PUBLIC_INTERFACE(MatrixWrapper) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MatrixWrapper) + typedef typename internal::remove_all::type NestedExpression; typedef typename internal::conditional< internal::is_lvalue::value, @@ -176,72 +149,40 @@ class MatrixWrapper : public MatrixBase > const Scalar >::type ScalarWithConstIfNotLvalue; - typedef typename internal::nested::type NestedExpressionType; + typedef typename internal::ref_selector::non_const_type NestedExpressionType; - inline MatrixWrapper(ExpressionType& a_matrix) : m_expression(a_matrix) {} + using Base::coeffRef; + EIGEN_DEVICE_FUNC + explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {} + + EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); } - inline ScalarWithConstIfNotLvalue* data() { return m_expression.const_cast_derived().data(); } + EIGEN_DEVICE_FUNC + inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_expression.data(); } - inline CoeffReturnType coeff(Index rowId, Index colId) const - { - return m_expression.coeff(rowId, colId); - } - - inline Scalar& coeffRef(Index rowId, Index colId) - { - return m_expression.const_cast_derived().coeffRef(rowId, colId); - } - + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { return m_expression.derived().coeffRef(rowId, colId); } - inline CoeffReturnType coeff(Index index) const - { - return m_expression.coeff(index); - } - - inline Scalar& coeffRef(Index index) - { - return m_expression.const_cast_derived().coeffRef(index); - } - + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { - return m_expression.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index rowId, Index colId) const - { - return m_expression.template packet(rowId, colId); - } - - template - inline void writePacket(Index rowId, Index colId, const PacketScalar& val) - { - m_expression.const_cast_derived().template writePacket(rowId, colId, val); - } - - template - inline const PacketScalar packet(Index index) const - { - return m_expression.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& val) - { - m_expression.const_cast_derived().template writePacket(index, val); + return m_expression.coeffRef(index); } + EIGEN_DEVICE_FUNC const typename internal::remove_all::type& nestedExpression() const { @@ -250,10 +191,12 @@ class MatrixWrapper : public MatrixBase > /** Forwards the resizing request to the nested expression * \sa DenseBase::resize(Index) */ - void resize(Index newSize) { m_expression.const_cast_derived().resize(newSize); } + EIGEN_DEVICE_FUNC + void resize(Index newSize) { m_expression.resize(newSize); } /** Forwards the resizing request to the nested expression * \sa DenseBase::resize(Index,Index)*/ - void resize(Index nbRows, Index nbCols) { m_expression.const_cast_derived().resize(nbRows,nbCols); } + EIGEN_DEVICE_FUNC + void resize(Index rows, Index cols) { m_expression.resize(rows,cols); } protected: NestedExpressionType m_expression; diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign.h index f481731727..53806ba33c 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign.h @@ -14,478 +14,6 @@ namespace Eigen { -namespace internal { - -/*************************************************************************** -* Part 1 : the logic deciding a strategy for traversal and unrolling * -***************************************************************************/ - -template -struct assign_traits -{ -public: - enum { - DstIsAligned = Derived::Flags & AlignedBit, - DstHasDirectAccess = Derived::Flags & DirectAccessBit, - SrcIsAligned = OtherDerived::Flags & AlignedBit, - JointAlignment = bool(DstIsAligned) && bool(SrcIsAligned) ? Aligned : Unaligned - }; - -private: - enum { - InnerSize = int(Derived::IsVectorAtCompileTime) ? int(Derived::SizeAtCompileTime) - : int(Derived::Flags)&RowMajorBit ? int(Derived::ColsAtCompileTime) - : int(Derived::RowsAtCompileTime), - InnerMaxSize = int(Derived::IsVectorAtCompileTime) ? int(Derived::MaxSizeAtCompileTime) - : int(Derived::Flags)&RowMajorBit ? int(Derived::MaxColsAtCompileTime) - : int(Derived::MaxRowsAtCompileTime), - MaxSizeAtCompileTime = Derived::SizeAtCompileTime, - PacketSize = packet_traits::size - }; - - enum { - StorageOrdersAgree = (int(Derived::IsRowMajor) == int(OtherDerived::IsRowMajor)), - MightVectorize = StorageOrdersAgree - && (int(Derived::Flags) & int(OtherDerived::Flags) & ActualPacketAccessBit), - MayInnerVectorize = MightVectorize && int(InnerSize)!=Dynamic && int(InnerSize)%int(PacketSize)==0 - && int(DstIsAligned) && int(SrcIsAligned), - MayLinearize = StorageOrdersAgree && (int(Derived::Flags) & int(OtherDerived::Flags) & LinearAccessBit), - MayLinearVectorize = MightVectorize && MayLinearize && DstHasDirectAccess - && (DstIsAligned || MaxSizeAtCompileTime == Dynamic), - /* If the destination isn't aligned, we have to do runtime checks and we don't unroll, - so it's only good for large enough sizes. */ - MaySliceVectorize = MightVectorize && DstHasDirectAccess - && (int(InnerMaxSize)==Dynamic || int(InnerMaxSize)>=3*PacketSize) - /* slice vectorization can be slow, so we only want it if the slices are big, which is - indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block - in a fixed-size matrix */ - }; - -public: - enum { - Traversal = int(MayInnerVectorize) ? int(InnerVectorizedTraversal) - : int(MayLinearVectorize) ? int(LinearVectorizedTraversal) - : int(MaySliceVectorize) ? int(SliceVectorizedTraversal) - : int(MayLinearize) ? int(LinearTraversal) - : int(DefaultTraversal), - Vectorized = int(Traversal) == InnerVectorizedTraversal - || int(Traversal) == LinearVectorizedTraversal - || int(Traversal) == SliceVectorizedTraversal - }; - -private: - enum { - UnrollingLimit = EIGEN_UNROLLING_LIMIT * (Vectorized ? int(PacketSize) : 1), - MayUnrollCompletely = int(Derived::SizeAtCompileTime) != Dynamic - && int(OtherDerived::CoeffReadCost) != Dynamic - && int(Derived::SizeAtCompileTime) * int(OtherDerived::CoeffReadCost) <= int(UnrollingLimit), - MayUnrollInner = int(InnerSize) != Dynamic - && int(OtherDerived::CoeffReadCost) != Dynamic - && int(InnerSize) * int(OtherDerived::CoeffReadCost) <= int(UnrollingLimit) - }; - -public: - enum { - Unrolling = (int(Traversal) == int(InnerVectorizedTraversal) || int(Traversal) == int(DefaultTraversal)) - ? ( - int(MayUnrollCompletely) ? int(CompleteUnrolling) - : int(MayUnrollInner) ? int(InnerUnrolling) - : int(NoUnrolling) - ) - : int(Traversal) == int(LinearVectorizedTraversal) - ? ( bool(MayUnrollCompletely) && bool(DstIsAligned) ? int(CompleteUnrolling) : int(NoUnrolling) ) - : int(Traversal) == int(LinearTraversal) - ? ( bool(MayUnrollCompletely) ? int(CompleteUnrolling) : int(NoUnrolling) ) - : int(NoUnrolling) - }; - -#ifdef EIGEN_DEBUG_ASSIGN - static void debug() - { - EIGEN_DEBUG_VAR(DstIsAligned) - EIGEN_DEBUG_VAR(SrcIsAligned) - EIGEN_DEBUG_VAR(JointAlignment) - EIGEN_DEBUG_VAR(InnerSize) - EIGEN_DEBUG_VAR(InnerMaxSize) - EIGEN_DEBUG_VAR(PacketSize) - EIGEN_DEBUG_VAR(StorageOrdersAgree) - EIGEN_DEBUG_VAR(MightVectorize) - EIGEN_DEBUG_VAR(MayLinearize) - EIGEN_DEBUG_VAR(MayInnerVectorize) - EIGEN_DEBUG_VAR(MayLinearVectorize) - EIGEN_DEBUG_VAR(MaySliceVectorize) - EIGEN_DEBUG_VAR(Traversal) - EIGEN_DEBUG_VAR(UnrollingLimit) - EIGEN_DEBUG_VAR(MayUnrollCompletely) - EIGEN_DEBUG_VAR(MayUnrollInner) - EIGEN_DEBUG_VAR(Unrolling) - } -#endif -}; - -/*************************************************************************** -* Part 2 : meta-unrollers -***************************************************************************/ - -/************************ -*** Default traversal *** -************************/ - -template -struct assign_DefaultTraversal_CompleteUnrolling -{ - enum { - outer = Index / Derived1::InnerSizeAtCompileTime, - inner = Index % Derived1::InnerSizeAtCompileTime - }; - - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - dst.copyCoeffByOuterInner(outer, inner, src); - assign_DefaultTraversal_CompleteUnrolling::run(dst, src); - } -}; - -template -struct assign_DefaultTraversal_CompleteUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &) {} -}; - -template -struct assign_DefaultTraversal_InnerUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src, typename Derived1::Index outer) - { - dst.copyCoeffByOuterInner(outer, Index, src); - assign_DefaultTraversal_InnerUnrolling::run(dst, src, outer); - } -}; - -template -struct assign_DefaultTraversal_InnerUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &, typename Derived1::Index) {} -}; - -/*********************** -*** Linear traversal *** -***********************/ - -template -struct assign_LinearTraversal_CompleteUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - dst.copyCoeff(Index, src); - assign_LinearTraversal_CompleteUnrolling::run(dst, src); - } -}; - -template -struct assign_LinearTraversal_CompleteUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &) {} -}; - -/************************** -*** Inner vectorization *** -**************************/ - -template -struct assign_innervec_CompleteUnrolling -{ - enum { - outer = Index / Derived1::InnerSizeAtCompileTime, - inner = Index % Derived1::InnerSizeAtCompileTime, - JointAlignment = assign_traits::JointAlignment - }; - - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - dst.template copyPacketByOuterInner(outer, inner, src); - assign_innervec_CompleteUnrolling::size, Stop>::run(dst, src); - } -}; - -template -struct assign_innervec_CompleteUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &) {} -}; - -template -struct assign_innervec_InnerUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src, typename Derived1::Index outer) - { - dst.template copyPacketByOuterInner(outer, Index, src); - assign_innervec_InnerUnrolling::size, Stop>::run(dst, src, outer); - } -}; - -template -struct assign_innervec_InnerUnrolling -{ - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &, typename Derived1::Index) {} -}; - -/*************************************************************************** -* Part 3 : implementation of all cases -***************************************************************************/ - -template::Traversal, - int Unrolling = assign_traits::Unrolling, - int Version = Specialized> -struct assign_impl; - -/************************ -*** Default traversal *** -************************/ - -template -struct assign_impl -{ - static inline void run(Derived1 &, const Derived2 &) { } -}; - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static inline void run(Derived1 &dst, const Derived2 &src) - { - const Index innerSize = dst.innerSize(); - const Index outerSize = dst.outerSize(); - for(Index outer = 0; outer < outerSize; ++outer) - for(Index inner = 0; inner < innerSize; ++inner) - dst.copyCoeffByOuterInner(outer, inner, src); - } -}; - -template -struct assign_impl -{ - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - assign_DefaultTraversal_CompleteUnrolling - ::run(dst, src); - } -}; - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - const Index outerSize = dst.outerSize(); - for(Index outer = 0; outer < outerSize; ++outer) - assign_DefaultTraversal_InnerUnrolling - ::run(dst, src, outer); - } -}; - -/*********************** -*** Linear traversal *** -***********************/ - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static inline void run(Derived1 &dst, const Derived2 &src) - { - const Index size = dst.size(); - for(Index i = 0; i < size; ++i) - dst.copyCoeff(i, src); - } -}; - -template -struct assign_impl -{ - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - assign_LinearTraversal_CompleteUnrolling - ::run(dst, src); - } -}; - -/************************** -*** Inner vectorization *** -**************************/ - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static inline void run(Derived1 &dst, const Derived2 &src) - { - const Index innerSize = dst.innerSize(); - const Index outerSize = dst.outerSize(); - const Index packetSize = packet_traits::size; - for(Index outer = 0; outer < outerSize; ++outer) - for(Index inner = 0; inner < innerSize; inner+=packetSize) - dst.template copyPacketByOuterInner(outer, inner, src); - } -}; - -template -struct assign_impl -{ - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - assign_innervec_CompleteUnrolling - ::run(dst, src); - } -}; - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - const Index outerSize = dst.outerSize(); - for(Index outer = 0; outer < outerSize; ++outer) - assign_innervec_InnerUnrolling - ::run(dst, src, outer); - } -}; - -/*************************** -*** Linear vectorization *** -***************************/ - -template -struct unaligned_assign_impl -{ - template - static EIGEN_STRONG_INLINE void run(const Derived&, OtherDerived&, typename Derived::Index, typename Derived::Index) {} -}; - -template <> -struct unaligned_assign_impl -{ - // MSVC must not inline this functions. If it does, it fails to optimize the - // packet access path. -#ifdef _MSC_VER - template - static EIGEN_DONT_INLINE void run(const Derived& src, OtherDerived& dst, typename Derived::Index start, typename Derived::Index end) -#else - template - static EIGEN_STRONG_INLINE void run(const Derived& src, OtherDerived& dst, typename Derived::Index start, typename Derived::Index end) -#endif - { - for (typename Derived::Index index = start; index < end; ++index) - dst.copyCoeff(index, src); - } -}; - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - const Index size = dst.size(); - typedef packet_traits PacketTraits; - enum { - packetSize = PacketTraits::size, - dstAlignment = PacketTraits::AlignedOnScalar ? Aligned : int(assign_traits::DstIsAligned) , - srcAlignment = assign_traits::JointAlignment - }; - const Index alignedStart = assign_traits::DstIsAligned ? 0 - : internal::first_aligned(&dst.coeffRef(0), size); - const Index alignedEnd = alignedStart + ((size-alignedStart)/packetSize)*packetSize; - - unaligned_assign_impl::DstIsAligned!=0>::run(src,dst,0,alignedStart); - - for(Index index = alignedStart; index < alignedEnd; index += packetSize) - { - dst.template copyPacket(index, src); - } - - unaligned_assign_impl<>::run(src,dst,alignedEnd,size); - } -}; - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src) - { - enum { size = Derived1::SizeAtCompileTime, - packetSize = packet_traits::size, - alignedSize = (size/packetSize)*packetSize }; - - assign_innervec_CompleteUnrolling::run(dst, src); - assign_DefaultTraversal_CompleteUnrolling::run(dst, src); - } -}; - -/************************** -*** Slice vectorization *** -***************************/ - -template -struct assign_impl -{ - typedef typename Derived1::Index Index; - static inline void run(Derived1 &dst, const Derived2 &src) - { - typedef typename Derived1::Scalar Scalar; - typedef packet_traits PacketTraits; - enum { - packetSize = PacketTraits::size, - alignable = PacketTraits::AlignedOnScalar, - dstIsAligned = assign_traits::DstIsAligned, - dstAlignment = alignable ? Aligned : int(dstIsAligned), - srcAlignment = assign_traits::JointAlignment - }; - const Scalar *dst_ptr = &dst.coeffRef(0,0); - if((!bool(dstIsAligned)) && (size_t(dst_ptr) % sizeof(Scalar))>0) - { - // the pointer is not aligend-on scalar, so alignment is not possible - return assign_impl::run(dst, src); - } - const Index packetAlignedMask = packetSize - 1; - const Index innerSize = dst.innerSize(); - const Index outerSize = dst.outerSize(); - const Index alignedStep = alignable ? (packetSize - dst.outerStride() % packetSize) & packetAlignedMask : 0; - Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); - - for(Index outer = 0; outer < outerSize; ++outer) - { - const Index alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask); - // do the non-vectorizable part of the assignment - for(Index inner = 0; inner(outer, inner, src); - - // do the non-vectorizable part of the assignment - for(Index inner = alignedEnd; inner((alignedStart+alignedStep)%packetSize, innerSize); - } - } -}; - -} // end namespace internal - -/*************************************************************************** -* Part 4 : implementation of DenseBase methods -***************************************************************************/ - template template EIGEN_STRONG_INLINE Derived& DenseBase @@ -499,90 +27,62 @@ EIGEN_STRONG_INLINE Derived& DenseBase EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived) EIGEN_STATIC_ASSERT(SameType,YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) -#ifdef EIGEN_DEBUG_ASSIGN - internal::assign_traits::debug(); -#endif eigen_assert(rows() == other.rows() && cols() == other.cols()); - internal::assign_impl::Traversal) - : int(InvalidTraversal)>::run(derived(),other.derived()); -#ifndef EIGEN_NO_DEBUG - checkTransposeAliasing(other.derived()); -#endif + internal::call_assignment_no_alias(derived(),other.derived()); + return derived(); } -namespace internal { - -template::Flags) & EvalBeforeAssigningBit) != 0, - bool NeedToTranspose = ((int(Derived::RowsAtCompileTime) == 1 && int(OtherDerived::ColsAtCompileTime) == 1) - | // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&". - // revert to || as soon as not needed anymore. - (int(Derived::ColsAtCompileTime) == 1 && int(OtherDerived::RowsAtCompileTime) == 1)) - && int(Derived::SizeAtCompileTime) != 1> -struct assign_selector; - -template -struct assign_selector { - static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.derived()); } - template - static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, const ActualOtherDerived& other) { other.evalTo(dst); return dst; } -}; -template -struct assign_selector { - static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.eval()); } -}; -template -struct assign_selector { - static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose()); } - template - static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, const ActualOtherDerived& other) { Transpose dstTrans(dst); other.evalTo(dstTrans); return dst; } -}; -template -struct assign_selector { - static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose().eval()); } -}; - -} // end namespace internal - template template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) { - return internal::assign_selector::run(derived(), other.derived()); + internal::call_assignment(derived(), other.derived()); + return derived(); } template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) { - return internal::assign_selector::run(derived(), other.derived()); + internal::call_assignment(derived(), other.derived()); + return derived(); } template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const MatrixBase& other) { - return internal::assign_selector::run(derived(), other.derived()); + internal::call_assignment(derived(), other.derived()); + return derived(); } template template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const DenseBase& other) { - return internal::assign_selector::run(derived(), other.derived()); + internal::call_assignment(derived(), other.derived()); + return derived(); } template template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const EigenBase& other) { - return internal::assign_selector::evalTo(derived(), other.derived()); + internal::call_assignment(derived(), other.derived()); + return derived(); } template template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const ReturnByValue& other) { - return internal::assign_selector::evalTo(derived(), other.derived()); + other.derived().evalTo(derived()); + return derived(); } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/AssignEvaluator.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/AssignEvaluator.h new file mode 100644 index 0000000000..b0ec7b7cad --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/AssignEvaluator.h @@ -0,0 +1,935 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob +// Copyright (C) 2011-2014 Gael Guennebaud +// Copyright (C) 2011-2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_ASSIGN_EVALUATOR_H +#define EIGEN_ASSIGN_EVALUATOR_H + +namespace Eigen { + +// This implementation is based on Assign.h + +namespace internal { + +/*************************************************************************** +* Part 1 : the logic deciding a strategy for traversal and unrolling * +***************************************************************************/ + +// copy_using_evaluator_traits is based on assign_traits + +template +struct copy_using_evaluator_traits +{ + typedef typename DstEvaluator::XprType Dst; + typedef typename Dst::Scalar DstScalar; + + enum { + DstFlags = DstEvaluator::Flags, + SrcFlags = SrcEvaluator::Flags + }; + +public: + enum { + DstAlignment = DstEvaluator::Alignment, + SrcAlignment = SrcEvaluator::Alignment, + DstHasDirectAccess = DstFlags & DirectAccessBit, + JointAlignment = EIGEN_PLAIN_ENUM_MIN(DstAlignment,SrcAlignment) + }; + +private: + enum { + InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime) + : int(DstFlags)&RowMajorBit ? int(Dst::ColsAtCompileTime) + : int(Dst::RowsAtCompileTime), + InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime) + : int(DstFlags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime) + : int(Dst::MaxRowsAtCompileTime), + OuterStride = int(outer_stride_at_compile_time::ret), + MaxSizeAtCompileTime = Dst::SizeAtCompileTime + }; + + // TODO distinguish between linear traversal and inner-traversals + typedef typename find_best_packet::type LinearPacketType; + typedef typename find_best_packet::type InnerPacketType; + + enum { + LinearPacketSize = unpacket_traits::size, + InnerPacketSize = unpacket_traits::size + }; + +public: + enum { + LinearRequiredAlignment = unpacket_traits::alignment, + InnerRequiredAlignment = unpacket_traits::alignment + }; + +private: + enum { + DstIsRowMajor = DstFlags&RowMajorBit, + SrcIsRowMajor = SrcFlags&RowMajorBit, + StorageOrdersAgree = (int(DstIsRowMajor) == int(SrcIsRowMajor)), + MightVectorize = bool(StorageOrdersAgree) + && (int(DstFlags) & int(SrcFlags) & ActualPacketAccessBit) + && bool(functor_traits::PacketAccess), + MayInnerVectorize = MightVectorize + && int(InnerSize)!=Dynamic && int(InnerSize)%int(InnerPacketSize)==0 + && int(OuterStride)!=Dynamic && int(OuterStride)%int(InnerPacketSize)==0 + && (EIGEN_UNALIGNED_VECTORIZE || int(JointAlignment)>=int(InnerRequiredAlignment)), + MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit), + MayLinearVectorize = bool(MightVectorize) && MayLinearize && DstHasDirectAccess + && (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)) || MaxSizeAtCompileTime == Dynamic), + /* If the destination isn't aligned, we have to do runtime checks and we don't unroll, + so it's only good for large enough sizes. */ + MaySliceVectorize = bool(MightVectorize) && bool(DstHasDirectAccess) + && (int(InnerMaxSize)==Dynamic || int(InnerMaxSize)>=(EIGEN_UNALIGNED_VECTORIZE?InnerPacketSize:(3*InnerPacketSize))) + /* slice vectorization can be slow, so we only want it if the slices are big, which is + indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block + in a fixed-size matrix + However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */ + }; + +public: + enum { + Traversal = int(MayLinearVectorize) && (LinearPacketSize>InnerPacketSize) ? int(LinearVectorizedTraversal) + : int(MayInnerVectorize) ? int(InnerVectorizedTraversal) + : int(MayLinearVectorize) ? int(LinearVectorizedTraversal) + : int(MaySliceVectorize) ? int(SliceVectorizedTraversal) + : int(MayLinearize) ? int(LinearTraversal) + : int(DefaultTraversal), + Vectorized = int(Traversal) == InnerVectorizedTraversal + || int(Traversal) == LinearVectorizedTraversal + || int(Traversal) == SliceVectorizedTraversal + }; + + typedef typename conditional::type PacketType; + +private: + enum { + ActualPacketSize = int(Traversal)==LinearVectorizedTraversal ? LinearPacketSize + : Vectorized ? InnerPacketSize + : 1, + UnrollingLimit = EIGEN_UNROLLING_LIMIT * ActualPacketSize, + MayUnrollCompletely = int(Dst::SizeAtCompileTime) != Dynamic + && int(Dst::SizeAtCompileTime) * (int(DstEvaluator::CoeffReadCost)+int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit), + MayUnrollInner = int(InnerSize) != Dynamic + && int(InnerSize) * (int(DstEvaluator::CoeffReadCost)+int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit) + }; + +public: + enum { + Unrolling = (int(Traversal) == int(InnerVectorizedTraversal) || int(Traversal) == int(DefaultTraversal)) + ? ( + int(MayUnrollCompletely) ? int(CompleteUnrolling) + : int(MayUnrollInner) ? int(InnerUnrolling) + : int(NoUnrolling) + ) + : int(Traversal) == int(LinearVectorizedTraversal) + ? ( bool(MayUnrollCompletely) && ( EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment))) + ? int(CompleteUnrolling) + : int(NoUnrolling) ) + : int(Traversal) == int(LinearTraversal) + ? ( bool(MayUnrollCompletely) ? int(CompleteUnrolling) + : int(NoUnrolling) ) +#if EIGEN_UNALIGNED_VECTORIZE + : int(Traversal) == int(SliceVectorizedTraversal) + ? ( bool(MayUnrollInner) ? int(InnerUnrolling) + : int(NoUnrolling) ) +#endif + : int(NoUnrolling) + }; + +#ifdef EIGEN_DEBUG_ASSIGN + static void debug() + { + std::cerr << "DstXpr: " << typeid(typename DstEvaluator::XprType).name() << std::endl; + std::cerr << "SrcXpr: " << typeid(typename SrcEvaluator::XprType).name() << std::endl; + std::cerr.setf(std::ios::hex, std::ios::basefield); + std::cerr << "DstFlags" << " = " << DstFlags << " (" << demangle_flags(DstFlags) << " )" << std::endl; + std::cerr << "SrcFlags" << " = " << SrcFlags << " (" << demangle_flags(SrcFlags) << " )" << std::endl; + std::cerr.unsetf(std::ios::hex); + EIGEN_DEBUG_VAR(DstAlignment) + EIGEN_DEBUG_VAR(SrcAlignment) + EIGEN_DEBUG_VAR(LinearRequiredAlignment) + EIGEN_DEBUG_VAR(InnerRequiredAlignment) + EIGEN_DEBUG_VAR(JointAlignment) + EIGEN_DEBUG_VAR(InnerSize) + EIGEN_DEBUG_VAR(InnerMaxSize) + EIGEN_DEBUG_VAR(LinearPacketSize) + EIGEN_DEBUG_VAR(InnerPacketSize) + EIGEN_DEBUG_VAR(ActualPacketSize) + EIGEN_DEBUG_VAR(StorageOrdersAgree) + EIGEN_DEBUG_VAR(MightVectorize) + EIGEN_DEBUG_VAR(MayLinearize) + EIGEN_DEBUG_VAR(MayInnerVectorize) + EIGEN_DEBUG_VAR(MayLinearVectorize) + EIGEN_DEBUG_VAR(MaySliceVectorize) + std::cerr << "Traversal" << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl; + EIGEN_DEBUG_VAR(SrcEvaluator::CoeffReadCost) + EIGEN_DEBUG_VAR(UnrollingLimit) + EIGEN_DEBUG_VAR(MayUnrollCompletely) + EIGEN_DEBUG_VAR(MayUnrollInner) + std::cerr << "Unrolling" << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl; + std::cerr << std::endl; + } +#endif +}; + +/*************************************************************************** +* Part 2 : meta-unrollers +***************************************************************************/ + +/************************ +*** Default traversal *** +************************/ + +template +struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling +{ + // FIXME: this is not very clean, perhaps this information should be provided by the kernel? + typedef typename Kernel::DstEvaluatorType DstEvaluatorType; + typedef typename DstEvaluatorType::XprType DstXprType; + + enum { + outer = Index / DstXprType::InnerSizeAtCompileTime, + inner = Index % DstXprType::InnerSizeAtCompileTime + }; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + kernel.assignCoeffByOuterInner(outer, inner); + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { } +}; + +template +struct copy_using_evaluator_DefaultTraversal_InnerUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, Index outer) + { + kernel.assignCoeffByOuterInner(outer, Index_); + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); + } +}; + +template +struct copy_using_evaluator_DefaultTraversal_InnerUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) { } +}; + +/*********************** +*** Linear traversal *** +***********************/ + +template +struct copy_using_evaluator_LinearTraversal_CompleteUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) + { + kernel.assignCoeff(Index); + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_LinearTraversal_CompleteUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { } +}; + +/************************** +*** Inner vectorization *** +**************************/ + +template +struct copy_using_evaluator_innervec_CompleteUnrolling +{ + // FIXME: this is not very clean, perhaps this information should be provided by the kernel? + typedef typename Kernel::DstEvaluatorType DstEvaluatorType; + typedef typename DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { + outer = Index / DstXprType::InnerSizeAtCompileTime, + inner = Index % DstXprType::InnerSizeAtCompileTime, + SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, + DstAlignment = Kernel::AssignmentTraits::DstAlignment + }; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + kernel.template assignPacketByOuterInner(outer, inner); + enum { NextIndex = Index + unpacket_traits::size }; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + } +}; + +template +struct copy_using_evaluator_innervec_CompleteUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { } +}; + +template +struct copy_using_evaluator_innervec_InnerUnrolling +{ + typedef typename Kernel::PacketType PacketType; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, Index outer) + { + kernel.template assignPacketByOuterInner(outer, Index_); + enum { NextIndex = Index_ + unpacket_traits::size }; + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); + } +}; + +template +struct copy_using_evaluator_innervec_InnerUnrolling +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &, Index) { } +}; + +/*************************************************************************** +* Part 3 : implementation of all cases +***************************************************************************/ + +// dense_assignment_loop is based on assign_impl + +template +struct dense_assignment_loop; + +/************************ +*** Default traversal *** +************************/ + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel &kernel) + { + for(Index outer = 0; outer < kernel.outerSize(); ++outer) { + for(Index inner = 0; inner < kernel.innerSize(); ++inner) { + kernel.assignCoeffByOuterInner(outer, inner); + } + } + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + + const Index outerSize = kernel.outerSize(); + for(Index outer = 0; outer < outerSize; ++outer) + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); + } +}; + +/*************************** +*** Linear vectorization *** +***************************/ + + +// The goal of unaligned_dense_assignment_loop is simply to factorize the handling +// of the non vectorizable beginning and ending parts + +template +struct unaligned_dense_assignment_loop +{ + // if IsAligned = true, then do nothing + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index, Index) {} +}; + +template <> +struct unaligned_dense_assignment_loop +{ + // MSVC must not inline this functions. If it does, it fails to optimize the + // packet access path. + // FIXME check which version exhibits this issue +#if EIGEN_COMP_MSVC + template + static EIGEN_DONT_INLINE void run(Kernel &kernel, + Index start, + Index end) +#else + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, + Index start, + Index end) +#endif + { + for (Index index = start; index < end; ++index) + kernel.assignCoeff(index); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + const Index size = kernel.size(); + typedef typename Kernel::Scalar Scalar; + typedef typename Kernel::PacketType PacketType; + enum { + requestedAlignment = Kernel::AssignmentTraits::LinearRequiredAlignment, + packetSize = unpacket_traits::size, + dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment)>=int(requestedAlignment), + dstAlignment = packet_traits::AlignedOnScalar ? int(requestedAlignment) + : int(Kernel::AssignmentTraits::DstAlignment), + srcAlignment = Kernel::AssignmentTraits::JointAlignment + }; + const Index alignedStart = dstIsAligned ? 0 : internal::first_aligned(kernel.dstDataPtr(), size); + const Index alignedEnd = alignedStart + ((size-alignedStart)/packetSize)*packetSize; + + unaligned_dense_assignment_loop::run(kernel, 0, alignedStart); + + for(Index index = alignedStart; index < alignedEnd; index += packetSize) + kernel.template assignPacket(index); + + unaligned_dense_assignment_loop<>::run(kernel, alignedEnd, size); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { size = DstXprType::SizeAtCompileTime, + packetSize =unpacket_traits::size, + alignedSize = (size/packetSize)*packetSize }; + + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); + } +}; + +/************************** +*** Inner vectorization *** +**************************/ + +template +struct dense_assignment_loop +{ + typedef typename Kernel::PacketType PacketType; + enum { + SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, + DstAlignment = Kernel::AssignmentTraits::DstAlignment + }; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + const Index innerSize = kernel.innerSize(); + const Index outerSize = kernel.outerSize(); + const Index packetSize = unpacket_traits::size; + for(Index outer = 0; outer < outerSize; ++outer) + for(Index inner = 0; inner < innerSize; inner+=packetSize) + kernel.template assignPacketByOuterInner(outer, inner); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::AssignmentTraits Traits; + const Index outerSize = kernel.outerSize(); + for(Index outer = 0; outer < outerSize; ++outer) + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); + } +}; + +/*********************** +*** Linear traversal *** +***********************/ + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + const Index size = kernel.size(); + for(Index i = 0; i < size; ++i) + kernel.assignCoeff(i); + } +}; + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } +}; + +/************************** +*** Slice vectorization *** +***************************/ + +template +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::Scalar Scalar; + typedef typename Kernel::PacketType PacketType; + enum { + packetSize = unpacket_traits::size, + requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), + alignable = packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment)>=sizeof(Scalar), + dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment)>=int(requestedAlignment), + dstAlignment = alignable ? int(requestedAlignment) + : int(Kernel::AssignmentTraits::DstAlignment) + }; + const Scalar *dst_ptr = kernel.dstDataPtr(); + if((!bool(dstIsAligned)) && (UIntPtr(dst_ptr) % sizeof(Scalar))>0) + { + // the pointer is not aligend-on scalar, so alignment is not possible + return dense_assignment_loop::run(kernel); + } + const Index packetAlignedMask = packetSize - 1; + const Index innerSize = kernel.innerSize(); + const Index outerSize = kernel.outerSize(); + const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; + Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); + + for(Index outer = 0; outer < outerSize; ++outer) + { + const Index alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask); + // do the non-vectorizable part of the assignment + for(Index inner = 0; inner(outer, inner); + + // do the non-vectorizable part of the assignment + for(Index inner = alignedEnd; inner +struct dense_assignment_loop +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel) + { + typedef typename Kernel::DstEvaluatorType::XprType DstXprType; + typedef typename Kernel::PacketType PacketType; + + enum { size = DstXprType::InnerSizeAtCompileTime, + packetSize =unpacket_traits::size, + vectorizableSize = (size/packetSize)*packetSize }; + + for(Index outer = 0; outer < kernel.outerSize(); ++outer) + { + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); + } + } +}; +#endif + + +/*************************************************************************** +* Part 4 : Generic dense assignment kernel +***************************************************************************/ + +// This class generalize the assignment of a coefficient (or packet) from one dense evaluator +// to another dense writable evaluator. +// It is parametrized by the two evaluators, and the actual assignment functor. +// This abstraction level permits to keep the evaluation loops as simple and as generic as possible. +// One can customize the assignment using this generic dense_assignment_kernel with different +// functors, or by completely overloading it, by-passing a functor. +template +class generic_dense_assignment_kernel +{ +protected: + typedef typename DstEvaluatorTypeT::XprType DstXprType; + typedef typename SrcEvaluatorTypeT::XprType SrcXprType; +public: + + typedef DstEvaluatorTypeT DstEvaluatorType; + typedef SrcEvaluatorTypeT SrcEvaluatorType; + typedef typename DstEvaluatorType::Scalar Scalar; + typedef copy_using_evaluator_traits AssignmentTraits; + typedef typename AssignmentTraits::PacketType PacketType; + + + EIGEN_DEVICE_FUNC generic_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr) + : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) + { + #ifdef EIGEN_DEBUG_ASSIGN + AssignmentTraits::debug(); + #endif + } + + EIGEN_DEVICE_FUNC Index size() const { return m_dstExpr.size(); } + EIGEN_DEVICE_FUNC Index innerSize() const { return m_dstExpr.innerSize(); } + EIGEN_DEVICE_FUNC Index outerSize() const { return m_dstExpr.outerSize(); } + EIGEN_DEVICE_FUNC Index rows() const { return m_dstExpr.rows(); } + EIGEN_DEVICE_FUNC Index cols() const { return m_dstExpr.cols(); } + EIGEN_DEVICE_FUNC Index outerStride() const { return m_dstExpr.outerStride(); } + + EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() { return m_dst; } + EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const { return m_src; } + + /// Assign src(row,col) to dst(row,col) through the assignment functor. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) + { + m_functor.assignCoeff(m_dst.coeffRef(row,col), m_src.coeff(row,col)); + } + + /// \sa assignCoeff(Index,Index) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index index) + { + m_functor.assignCoeff(m_dst.coeffRef(index), m_src.coeff(index)); + } + + /// \sa assignCoeff(Index,Index) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) + { + Index row = rowIndexByOuterInner(outer, inner); + Index col = colIndexByOuterInner(outer, inner); + assignCoeff(row, col); + } + + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index row, Index col) + { + m_functor.template assignPacket(&m_dst.coeffRef(row,col), m_src.template packet(row,col)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index index) + { + m_functor.template assignPacket(&m_dst.coeffRef(index), m_src.template packet(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner) + { + Index row = rowIndexByOuterInner(outer, inner); + Index col = colIndexByOuterInner(outer, inner); + assignPacket(row, col); + } + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) + { + typedef typename DstEvaluatorType::ExpressionTraits Traits; + return int(Traits::RowsAtCompileTime) == 1 ? 0 + : int(Traits::ColsAtCompileTime) == 1 ? inner + : int(DstEvaluatorType::Flags)&RowMajorBit ? outer + : inner; + } + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) + { + typedef typename DstEvaluatorType::ExpressionTraits Traits; + return int(Traits::ColsAtCompileTime) == 1 ? 0 + : int(Traits::RowsAtCompileTime) == 1 ? inner + : int(DstEvaluatorType::Flags)&RowMajorBit ? inner + : outer; + } + + EIGEN_DEVICE_FUNC const Scalar* dstDataPtr() const + { + return m_dstExpr.data(); + } + +protected: + DstEvaluatorType& m_dst; + const SrcEvaluatorType& m_src; + const Functor &m_functor; + // TODO find a way to avoid the needs of the original expression + DstXprType& m_dstExpr; +}; + +/*************************************************************************** +* Part 5 : Entry point for dense rectangular assignment +***************************************************************************/ + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void resize_if_allowed(DstXprType &dst, const SrcXprType& src, const Functor &/*func*/) +{ + EIGEN_ONLY_USED_FOR_DEBUG(dst); + EIGEN_ONLY_USED_FOR_DEBUG(src); + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void resize_if_allowed(DstXprType &dst, const SrcXprType& src, const internal::assign_op &/*func*/) +{ + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if(((dst.rows()!=dstRows) || (dst.cols()!=dstCols))) + dst.resize(dstRows, dstCols); + eigen_assert(dst.rows() == dstRows && dst.cols() == dstCols); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src, const Functor &func) +{ + typedef evaluator DstEvaluatorType; + typedef evaluator SrcEvaluatorType; + + SrcEvaluatorType srcEvaluator(src); + + // NOTE To properly handle A = (A*A.transpose())/s with A rectangular, + // we need to resize the destination after the source evaluator has been created. + resize_if_allowed(dst, src, func); + + DstEvaluatorType dstEvaluator(dst); + + typedef generic_dense_assignment_kernel Kernel; + Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived()); + + dense_assignment_loop::run(kernel); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src) +{ + call_dense_assignment_loop(dst, src, internal::assign_op()); +} + +/*************************************************************************** +* Part 6 : Generic assignment +***************************************************************************/ + +// Based on the respective shapes of the destination and source, +// the class AssignmentKind determine the kind of assignment mechanism. +// AssignmentKind must define a Kind typedef. +template struct AssignmentKind; + +// Assignement kind defined in this file: +struct Dense2Dense {}; +struct EigenBase2EigenBase {}; + +template struct AssignmentKind { typedef EigenBase2EigenBase Kind; }; +template<> struct AssignmentKind { typedef Dense2Dense Kind; }; + +// This is the main assignment class +template< typename DstXprType, typename SrcXprType, typename Functor, + typename Kind = typename AssignmentKind< typename evaluator_traits::Shape , typename evaluator_traits::Shape >::Kind, + typename EnableIf = void> +struct Assignment; + + +// The only purpose of this call_assignment() function is to deal with noalias() / "assume-aliasing" and automatic transposition. +// Indeed, I (Gael) think that this concept of "assume-aliasing" was a mistake, and it makes thing quite complicated. +// So this intermediate function removes everything related to "assume-aliasing" such that Assignment +// does not has to bother about these annoying details. + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment(Dst& dst, const Src& src) +{ + call_assignment(dst, src, internal::assign_op()); +} +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment(const Dst& dst, const Src& src) +{ + call_assignment(dst, src, internal::assign_op()); +} + +// Deal with "assume-aliasing" +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if< evaluator_assume_aliasing::value, void*>::type = 0) +{ + typename plain_matrix_type::type tmp(src); + call_assignment_no_alias(dst, tmp, func); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if::value, void*>::type = 0) +{ + call_assignment_no_alias(dst, src, func); +} + +// by-pass "assume-aliasing" +// When there is no aliasing, we require that 'dst' has been properly resized +template class StorageBase, typename Src, typename Func> +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment(NoAlias& dst, const Src& src, const Func& func) +{ + call_assignment_no_alias(dst.expression(), src, func); +} + + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment_no_alias(Dst& dst, const Src& src, const Func& func) +{ + enum { + NeedToTranspose = ( (int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1) + || (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1) + ) && int(Dst::SizeAtCompileTime) != 1 + }; + + typedef typename internal::conditional, Dst>::type ActualDstTypeCleaned; + typedef typename internal::conditional, Dst&>::type ActualDstType; + ActualDstType actualDst(dst); + + // TODO check whether this is the right place to perform these checks: + EIGEN_STATIC_ASSERT_LVALUE(Dst) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src) + EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename ActualDstTypeCleaned::Scalar,typename Src::Scalar); + + Assignment::run(actualDst, src, func); +} +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment_no_alias(Dst& dst, const Src& src) +{ + call_assignment_no_alias(dst, src, internal::assign_op()); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src, const Func& func) +{ + // TODO check whether this is the right place to perform these checks: + EIGEN_STATIC_ASSERT_LVALUE(Dst) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Dst,Src) + EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename Dst::Scalar,typename Src::Scalar); + + Assignment::run(dst, src, func); +} +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src) +{ + call_assignment_no_alias_no_transpose(dst, src, internal::assign_op()); +} + +// forward declaration +template void check_for_aliasing(const Dst &dst, const Src &src); + +// Generic Dense to Dense assignment +// Note that the last template argument "Weak" is needed to make it possible to perform +// both partial specialization+SFINAE without ambiguous specialization +template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak> +struct Assignment +{ + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const Functor &func) + { +#ifndef EIGEN_NO_DEBUG + internal::check_for_aliasing(dst, src); +#endif + + call_dense_assignment_loop(dst, src, func); + } +}; + +// Generic assignment through evalTo. +// TODO: not sure we have to keep that one, but it helps porting current code to new evaluator mechanism. +// Note that the last template argument "Weak" is needed to make it possible to perform +// both partial specialization+SFINAE without ambiguous specialization +template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak> +struct Assignment +{ + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + src.evalTo(dst); + } + + // NOTE The following two functions are templated to avoid their instanciation if not needed + // This is needed because some expressions supports evalTo only and/or have 'void' as scalar type. + template + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op &/*func*/) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + src.addTo(dst); + } + + template + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op &/*func*/) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + src.subTo(dst); + } +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_ASSIGN_EVALUATOR_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign_MKL.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign_MKL.h index 7772951b91..6c2ab92648 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign_MKL.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Assign_MKL.h @@ -1,6 +1,7 @@ /* Copyright (c) 2011, Intel Corporation. All rights reserved. - + Copyright (C) 2015 Gael Guennebaud + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -37,17 +38,13 @@ namespace Eigen { namespace internal { -template struct vml_call -{ enum { IsSupported = 0 }; }; - -template +template class vml_assign_traits { private: enum { DstHasDirectAccess = Dst::Flags & DirectAccessBit, SrcHasDirectAccess = Src::Flags & DirectAccessBit, - StorageOrdersAgree = (int(Dst::IsRowMajor) == int(Src::IsRowMajor)), InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime) : int(Dst::Flags)&RowMajorBit ? int(Dst::ColsAtCompileTime) @@ -57,165 +54,120 @@ class vml_assign_traits : int(Dst::MaxRowsAtCompileTime), MaxSizeAtCompileTime = Dst::SizeAtCompileTime, - MightEnableVml = vml_call::IsSupported && StorageOrdersAgree && DstHasDirectAccess && SrcHasDirectAccess - && Src::InnerStrideAtCompileTime==1 && Dst::InnerStrideAtCompileTime==1, + MightEnableVml = StorageOrdersAgree && DstHasDirectAccess && SrcHasDirectAccess && Src::InnerStrideAtCompileTime==1 && Dst::InnerStrideAtCompileTime==1, MightLinearize = MightEnableVml && (int(Dst::Flags) & int(Src::Flags) & LinearAccessBit), VmlSize = MightLinearize ? MaxSizeAtCompileTime : InnerMaxSize, - LargeEnough = VmlSize==Dynamic || VmlSize>=EIGEN_MKL_VML_THRESHOLD, - MayEnableVml = MightEnableVml && LargeEnough, - MayLinearize = MayEnableVml && MightLinearize + LargeEnough = VmlSize==Dynamic || VmlSize>=EIGEN_MKL_VML_THRESHOLD }; public: enum { - Traversal = MayLinearize ? LinearVectorizedTraversal - : MayEnableVml ? InnerVectorizedTraversal - : DefaultTraversal + EnableVml = MightEnableVml && LargeEnough, + Traversal = MightLinearize ? LinearTraversal : DefaultTraversal }; }; -template::Traversal > -struct vml_assign_impl - : assign_impl,Traversal,Unrolling,BuiltIn> -{ -}; - -template -struct vml_assign_impl -{ - typedef typename Derived1::Scalar Scalar; - typedef typename Derived1::Index Index; - static inline void run(Derived1& dst, const CwiseUnaryOp& src) - { - // in case we want to (or have to) skip VML at runtime we can call: - // assign_impl,Traversal,Unrolling,BuiltIn>::run(dst,src); - const Index innerSize = dst.innerSize(); - const Index outerSize = dst.outerSize(); - for(Index outer = 0; outer < outerSize; ++outer) { - const Scalar *src_ptr = src.IsRowMajor ? &(src.nestedExpression().coeffRef(outer,0)) : - &(src.nestedExpression().coeffRef(0, outer)); - Scalar *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer)); - vml_call::run(src.functor(), innerSize, src_ptr, dst_ptr ); - } - } -}; - -template -struct vml_assign_impl -{ - static inline void run(Derived1& dst, const CwiseUnaryOp& src) - { - // in case we want to (or have to) skip VML at runtime we can call: - // assign_impl,Traversal,Unrolling,BuiltIn>::run(dst,src); - vml_call::run(src.functor(), dst.size(), src.nestedExpression().data(), dst.data() ); - } -}; - -// Macroses - -#define EIGEN_MKL_VML_SPECIALIZE_ASSIGN(TRAVERSAL,UNROLLING) \ - template \ - struct assign_impl, TRAVERSAL, UNROLLING, Specialized> { \ - static inline void run(Derived1 &dst, const Eigen::CwiseUnaryOp &src) { \ - vml_assign_impl::run(dst, src); \ - } \ - }; - -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(DefaultTraversal,NoUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(DefaultTraversal,CompleteUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(DefaultTraversal,InnerUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(LinearTraversal,NoUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(LinearTraversal,CompleteUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(InnerVectorizedTraversal,NoUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(InnerVectorizedTraversal,CompleteUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(InnerVectorizedTraversal,InnerUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(LinearVectorizedTraversal,CompleteUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(LinearVectorizedTraversal,NoUnrolling) -EIGEN_MKL_VML_SPECIALIZE_ASSIGN(SliceVectorizedTraversal,NoUnrolling) - - +#define EIGEN_PP_EXPAND(ARG) ARG #if !defined (EIGEN_FAST_MATH) || (EIGEN_FAST_MATH != 1) -#define EIGEN_MKL_VML_MODE VML_HA +#define EIGEN_VMLMODE_EXPAND_LA , VML_HA #else -#define EIGEN_MKL_VML_MODE VML_LA +#define EIGEN_VMLMODE_EXPAND_LA , VML_LA #endif -#define EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE) \ - template<> struct vml_call< scalar_##EIGENOP##_op > { \ - enum { IsSupported = 1 }; \ - static inline void run( const scalar_##EIGENOP##_op& /*func*/, \ - int size, const EIGENTYPE* src, EIGENTYPE* dst) { \ - VMLOP(size, (const VMLTYPE*)src, (VMLTYPE*)dst); \ - } \ +#define EIGEN_VMLMODE_EXPAND__ + +#define EIGEN_VMLMODE_PREFIX_LA vm +#define EIGEN_VMLMODE_PREFIX__ v +#define EIGEN_VMLMODE_PREFIX(VMLMODE) EIGEN_CAT(EIGEN_VMLMODE_PREFIX_,VMLMODE) + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE) \ + template< typename DstXprType, typename SrcXprNested> \ + struct Assignment, SrcXprNested>, assign_op, \ + Dense2Dense, typename enable_if::EnableVml>::type> { \ + typedef CwiseUnaryOp, SrcXprNested> SrcXprType; \ + static void run(DstXprType &dst, const SrcXprType &src, const assign_op &/*func*/) { \ + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); \ + if(vml_assign_traits::Traversal==LinearTraversal) { \ + VMLOP(dst.size(), (const VMLTYPE*)src.nestedExpression().data(), \ + (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) ); \ + } else { \ + const Index outerSize = dst.outerSize(); \ + for(Index outer = 0; outer < outerSize; ++outer) { \ + const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.nestedExpression().coeffRef(outer,0)) : \ + &(src.nestedExpression().coeffRef(0, outer)); \ + EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer)); \ + VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr, \ + (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE)); \ + } \ + } \ + } \ + }; \ + + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),s##VMLOP), float, float, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),d##VMLOP), double, double, VMLMODE) + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),c##VMLOP), scomplex, MKL_Complex8, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),z##VMLOP), dcomplex, MKL_Complex16, VMLMODE) + +#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE) \ + EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE) + + +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sin, Sin, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(asin, Asin, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sinh, Sinh, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cos, Cos, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(acos, Acos, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cosh, Cosh, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tan, Tan, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(atan, Atan, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tanh, Tanh, LA) +// EIGEN_MKL_VML_DECLARE_UNARY_CALLS(abs, Abs, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(exp, Exp, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log, Ln, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log10, Log10, LA) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sqrt, Sqrt, _) + +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(square, Sqr, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(arg, Arg, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(round, Round, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(floor, Floor, _) +EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(ceil, Ceil, _) + +#define EIGEN_MKL_VML_DECLARE_POW_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE) \ + template< typename DstXprType, typename SrcXprNested, typename Plain> \ + struct Assignment, SrcXprNested, \ + const CwiseNullaryOp,Plain> >, assign_op, \ + Dense2Dense, typename enable_if::EnableVml>::type> { \ + typedef CwiseBinaryOp, SrcXprNested, \ + const CwiseNullaryOp,Plain> > SrcXprType; \ + static void run(DstXprType &dst, const SrcXprType &src, const assign_op &/*func*/) { \ + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); \ + VMLTYPE exponent = reinterpret_cast(src.rhs().functor().m_other); \ + if(vml_assign_traits::Traversal==LinearTraversal) \ + { \ + VMLOP( dst.size(), (const VMLTYPE*)src.lhs().data(), exponent, \ + (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) ); \ + } else { \ + const Index outerSize = dst.outerSize(); \ + for(Index outer = 0; outer < outerSize; ++outer) { \ + const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.lhs().coeffRef(outer,0)) : \ + &(src.lhs().coeffRef(0, outer)); \ + EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer)); \ + VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr, exponent, \ + (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE)); \ + } \ + } \ + } \ }; - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALL_LA(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE) \ - template<> struct vml_call< scalar_##EIGENOP##_op > { \ - enum { IsSupported = 1 }; \ - static inline void run( const scalar_##EIGENOP##_op& /*func*/, \ - int size, const EIGENTYPE* src, EIGENTYPE* dst) { \ - MKL_INT64 vmlMode = EIGEN_MKL_VML_MODE; \ - VMLOP(size, (const VMLTYPE*)src, (VMLTYPE*)dst, vmlMode); \ - } \ - }; - -#define EIGEN_MKL_VML_DECLARE_POW_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE) \ - template<> struct vml_call< scalar_##EIGENOP##_op > { \ - enum { IsSupported = 1 }; \ - static inline void run( const scalar_##EIGENOP##_op& func, \ - int size, const EIGENTYPE* src, EIGENTYPE* dst) { \ - EIGENTYPE exponent = func.m_exponent; \ - MKL_INT64 vmlMode = EIGEN_MKL_VML_MODE; \ - VMLOP(&size, (const VMLTYPE*)src, (const VMLTYPE*)&exponent, \ - (VMLTYPE*)dst, &vmlMode); \ - } \ - }; - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, vs##VMLOP, float, float) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, vd##VMLOP, double, double) - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_COMPLEX(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, vc##VMLOP, scomplex, MKL_Complex8) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, vz##VMLOP, dcomplex, MKL_Complex16) - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALLS_COMPLEX(EIGENOP, VMLOP) - - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL_LA(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL_LA(EIGENOP, vms##VMLOP, float, float) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL_LA(EIGENOP, vmd##VMLOP, double, double) - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_COMPLEX_LA(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL_LA(EIGENOP, vmc##VMLOP, scomplex, MKL_Complex8) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALL_LA(EIGENOP, vmz##VMLOP, dcomplex, MKL_Complex16) - -#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL_LA(EIGENOP, VMLOP) \ - EIGEN_MKL_VML_DECLARE_UNARY_CALLS_COMPLEX_LA(EIGENOP, VMLOP) - - -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(sin, Sin) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(asin, Asin) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(cos, Cos) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(acos, Acos) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(tan, Tan) -//EIGEN_MKL_VML_DECLARE_UNARY_CALLS(abs, Abs) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(exp, Exp) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(log, Ln) -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(sqrt, Sqrt) - -EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(square, Sqr) - -// The vm*powx functions are not avaibale in the windows version of MKL. -#ifndef _WIN32 -EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmspowx_, float, float) -EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmdpowx_, double, double) -EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmcpowx_, scomplex, MKL_Complex8) -EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmzpowx_, dcomplex, MKL_Complex16) -#endif + +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmsPowx, float, float, LA) +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmdPowx, double, double, LA) +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmcPowx, scomplex, MKL_Complex8, LA) +EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmzPowx, dcomplex, MKL_Complex16, LA) } // end namespace internal diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/BandMatrix.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/BandMatrix.h index ffd7fe8b30..4978c91405 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/BandMatrix.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/BandMatrix.h @@ -32,7 +32,7 @@ class BandMatrixBase : public EigenBase }; typedef typename internal::traits::Scalar Scalar; typedef Matrix DenseMatrixType; - typedef typename DenseMatrixType::Index Index; + typedef typename DenseMatrixType::StorageIndex StorageIndex; typedef typename internal::traits::CoefficientsType CoefficientsType; typedef EigenBase Base; @@ -161,15 +161,15 @@ class BandMatrixBase : public EigenBase * * \brief Represents a rectangular matrix with a banded storage * - * \param _Scalar Numeric type, i.e. float, double, int - * \param Rows Number of rows, or \b Dynamic - * \param Cols Number of columns, or \b Dynamic - * \param Supers Number of super diagonal - * \param Subs Number of sub diagonal - * \param _Options A combination of either \b #RowMajor or \b #ColMajor, and of \b #SelfAdjoint - * The former controls \ref TopicStorageOrders "storage order", and defaults to - * column-major. The latter controls whether the matrix represents a selfadjoint - * matrix in which case either Supers of Subs have to be null. + * \tparam _Scalar Numeric type, i.e. float, double, int + * \tparam _Rows Number of rows, or \b Dynamic + * \tparam _Cols Number of columns, or \b Dynamic + * \tparam _Supers Number of super diagonal + * \tparam _Subs Number of sub diagonal + * \tparam _Options A combination of either \b #RowMajor or \b #ColMajor, and of \b #SelfAdjoint + * The former controls \ref TopicStorageOrders "storage order", and defaults to + * column-major. The latter controls whether the matrix represents a selfadjoint + * matrix in which case either Supers of Subs have to be null. * * \sa class TridiagonalMatrix */ @@ -179,7 +179,7 @@ struct traits > { typedef _Scalar Scalar; typedef Dense StorageKind; - typedef DenseIndex Index; + typedef Eigen::Index StorageIndex; enum { CoeffReadCost = NumTraits::ReadCost, RowsAtCompileTime = _Rows, @@ -201,10 +201,10 @@ class BandMatrix : public BandMatrixBase::Scalar Scalar; - typedef typename internal::traits::Index Index; + typedef typename internal::traits::StorageIndex StorageIndex; typedef typename internal::traits::CoefficientsType CoefficientsType; - inline BandMatrix(Index rows=Rows, Index cols=Cols, Index supers=Supers, Index subs=Subs) + explicit inline BandMatrix(Index rows=Rows, Index cols=Cols, Index supers=Supers, Index subs=Subs) : m_coeffs(1+supers+subs,cols), m_rows(rows), m_supers(supers), m_subs(subs) { @@ -241,7 +241,7 @@ struct traits::CoeffReadCost, RowsAtCompileTime = _Rows, @@ -264,9 +264,9 @@ class BandMatrixWrapper : public BandMatrixBase::Scalar Scalar; typedef typename internal::traits::CoefficientsType CoefficientsType; - typedef typename internal::traits::Index Index; + typedef typename internal::traits::StorageIndex StorageIndex; - inline BandMatrixWrapper(const CoefficientsType& coeffs, Index rows=_Rows, Index cols=_Cols, Index supers=_Supers, Index subs=_Subs) + explicit inline BandMatrixWrapper(const CoefficientsType& coeffs, Index rows=_Rows, Index cols=_Cols, Index supers=_Supers, Index subs=_Subs) : m_coeffs(coeffs), m_rows(rows), m_supers(supers), m_subs(subs) { @@ -302,9 +302,9 @@ class BandMatrixWrapper : public BandMatrixBase class TridiagonalMatrix : public BandMatrix { typedef BandMatrix Base; - typedef typename Base::Index Index; + typedef typename Base::StorageIndex StorageIndex; public: - TridiagonalMatrix(Index size = Size) : Base(size,size,Options&SelfAdjoint?0:1,1) {} + explicit TridiagonalMatrix(Index size = Size) : Base(size,size,Options&SelfAdjoint?0:1,1) {} inline typename Base::template DiagonalIntReturnType<1>::Type super() { return Base::template diagonal<1>(); } @@ -327,6 +327,25 @@ class TridiagonalMatrix : public BandMatrix +struct evaluator_traits > + : public evaluator_traits_base > +{ + typedef BandShape Shape; +}; + +template +struct evaluator_traits > + : public evaluator_traits_base > +{ + typedef BandShape Shape; +}; + +template<> struct AssignmentKind { typedef EigenBase2EigenBase Kind; }; + } // end namespace internal } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Block.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Block.h index 8278944432..11de45c2ec 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Block.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Block.h @@ -13,38 +13,6 @@ namespace Eigen { -/** \class Block - * \ingroup Core_Module - * - * \brief Expression of a fixed-size or dynamic-size block - * - * \param XprType the type of the expression in which we are taking a block - * \param BlockRows the number of rows of the block we are taking at compile time (optional) - * \param BlockCols the number of columns of the block we are taking at compile time (optional) - * - * This class represents an expression of either a fixed-size or dynamic-size block. It is the return - * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block(Index,Index) and - * most of the time this is the only way it is used. - * - * However, if you want to directly maniputate block expressions, - * for instance if you want to write a function returning such an expression, you - * will need to use this class. - * - * Here is an example illustrating the dynamic case: - * \include class_Block.cpp - * Output: \verbinclude class_Block.out - * - * \note Even though this expression has dynamic size, in the case where \a XprType - * has fixed size, this expression inherits a fixed maximal size which means that evaluating - * it does not cause a dynamic memory allocation. - * - * Here is an example illustrating the fixed-size case: - * \include class_FixedBlock.cpp - * Output: \verbinclude class_FixedBlock.out - * - * \sa DenseBase::block(Index,Index,Index,Index), DenseBase::block(Index,Index), class VectorBlock - */ - namespace internal { template struct traits > : traits @@ -52,7 +20,7 @@ struct traits > : traits::Scalar Scalar; typedef typename traits::StorageKind StorageKind; typedef typename traits::XprKind XprKind; - typedef typename nested::type XprTypeNested; + typedef typename ref_selector::type XprTypeNested; typedef typename remove_reference::type _XprTypeNested; enum{ MatrixRows = traits::RowsAtCompileTime, @@ -65,10 +33,10 @@ struct traits > : traits::MaxColsAtCompileTime), + XprTypeIsRowMajor = (int(traits::Flags)&RowMajorBit) != 0, - IsDense = is_same::value, - IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 - : (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 + IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 + : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 : XprTypeIsRowMajor, HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), @@ -78,18 +46,16 @@ struct traits > : traits::ret) : int(inner_stride_at_compile_time::ret), - MaskPacketAccessBit = (InnerSize == Dynamic || (InnerSize % packet_traits::size) == 0) - && (InnerStrideAtCompileTime == 1) - ? PacketAccessBit : 0, - MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0, - FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (traits::Flags&LinearAccessBit))) ? LinearAccessBit : 0, + + // FIXME, this traits is rather specialized for dense object and it needs to be cleaned further FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, - Flags0 = traits::Flags & ( (HereditaryBits & ~RowMajorBit) | - DirectAccessBit | - MaskPacketAccessBit | - MaskAlignedBit), - Flags = Flags0 | FlagsLinearAccessBit | FlagsLvalueBit | FlagsRowMajorBit + Flags = (traits::Flags & (DirectAccessBit | (InnerPanel?CompressedAccessBit:0))) | FlagsLvalueBit | FlagsRowMajorBit, + // FIXME DirectAccessBit should not be handled by expressions + // + // Alignment is needed by MapBase's assertions + // We can sefely set it to false here. Internal alignment errors will be detected by an eigen_internal_assert in the respective evaluator + Alignment = 0 }; }; @@ -100,6 +66,40 @@ template class BlockImpl; +/** \class Block + * \ingroup Core_Module + * + * \brief Expression of a fixed-size or dynamic-size block + * + * \tparam XprType the type of the expression in which we are taking a block + * \tparam BlockRows the number of rows of the block we are taking at compile time (optional) + * \tparam BlockCols the number of columns of the block we are taking at compile time (optional) + * \tparam InnerPanel is true, if the block maps to a set of rows of a row major matrix or + * to set of columns of a column major matrix (optional). The parameter allows to determine + * at compile time whether aligned access is possible on the block expression. + * + * This class represents an expression of either a fixed-size or dynamic-size block. It is the return + * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block(Index,Index) and + * most of the time this is the only way it is used. + * + * However, if you want to directly maniputate block expressions, + * for instance if you want to write a function returning such an expression, you + * will need to use this class. + * + * Here is an example illustrating the dynamic case: + * \include class_Block.cpp + * Output: \verbinclude class_Block.out + * + * \note Even though this expression has dynamic size, in the case where \a XprType + * has fixed size, this expression inherits a fixed maximal size which means that evaluating + * it does not cause a dynamic memory allocation. + * + * Here is an example illustrating the fixed-size case: + * \include class_FixedBlock.cpp + * Output: \verbinclude class_FixedBlock.out + * + * \sa DenseBase::block(Index,Index,Index,Index), DenseBase::block(Index,Index), class VectorBlock + */ template class Block : public BlockImpl::StorageKind> { @@ -109,9 +109,12 @@ template class typedef Impl Base; EIGEN_GENERIC_PUBLIC_INTERFACE(Block) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block) + + typedef typename internal::remove_all::type NestedExpression; /** Column or Row constructor */ + EIGEN_DEVICE_FUNC inline Block(XprType& xpr, Index i) : Impl(xpr,i) { eigen_assert( (i>=0) && ( @@ -121,25 +124,27 @@ template class /** Fixed-size constructor */ - inline Block(XprType& xpr, Index a_startRow, Index a_startCol) - : Impl(xpr, a_startRow, a_startCol) + EIGEN_DEVICE_FUNC + inline Block(XprType& xpr, Index startRow, Index startCol) + : Impl(xpr, startRow, startCol) { EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE) - eigen_assert(a_startRow >= 0 && BlockRows >= 1 && a_startRow + BlockRows <= xpr.rows() - && a_startCol >= 0 && BlockCols >= 1 && a_startCol + BlockCols <= xpr.cols()); + eigen_assert(startRow >= 0 && BlockRows >= 0 && startRow + BlockRows <= xpr.rows() + && startCol >= 0 && BlockCols >= 0 && startCol + BlockCols <= xpr.cols()); } /** Dynamic-size constructor */ + EIGEN_DEVICE_FUNC inline Block(XprType& xpr, - Index a_startRow, Index a_startCol, + Index startRow, Index startCol, Index blockRows, Index blockCols) - : Impl(xpr, a_startRow, a_startCol, blockRows, blockCols) + : Impl(xpr, startRow, startCol, blockRows, blockCols) { eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows) && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols)); - eigen_assert(a_startRow >= 0 && blockRows >= 0 && a_startRow <= xpr.rows() - blockRows - && a_startCol >= 0 && blockCols >= 0 && a_startCol <= xpr.cols() - blockCols); + eigen_assert(startRow >= 0 && blockRows >= 0 && startRow <= xpr.rows() - blockRows + && startCol >= 0 && blockCols >= 0 && startCol <= xpr.cols() - blockCols); } }; @@ -150,14 +155,15 @@ class BlockImpl : public internal::BlockImpl_dense { typedef internal::BlockImpl_dense Impl; - typedef typename XprType::Index Index; + typedef typename XprType::StorageIndex StorageIndex; public: typedef Impl Base; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - inline BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {} - inline BlockImpl(XprType& xpr, Index a_startRow, Index a_startCol) : Impl(xpr, a_startRow, a_startCol) {} - inline BlockImpl(XprType& xpr, Index a_startRow, Index a_startCol, Index blockRows, Index blockCols) - : Impl(xpr, a_startRow, a_startCol, blockRows, blockCols) {} + EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {} + EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index startRow, Index startCol) : Impl(xpr, startRow, startCol) {} + EIGEN_DEVICE_FUNC + inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) + : Impl(xpr, startRow, startCol, blockRows, blockCols) {} }; namespace internal { @@ -167,16 +173,18 @@ template >::type { typedef Block BlockType; + typedef typename internal::ref_selector::non_const_type XprTypeNested; public: typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(BlockType) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense) - class InnerIterator; + // class InnerIterator; // FIXME apparently never used /** Column or Row constructor */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index i) : m_xpr(xpr), // It is a row if and only if BlockRows==1 and BlockCols==XprType::ColsAtCompileTime, @@ -191,75 +199,76 @@ template inline PacketScalar packet(Index rowId, Index colId) const { - return m_xpr.template packet - (rowId + m_startRow.value(), colId + m_startCol.value()); + return m_xpr.template packet(rowId + m_startRow.value(), colId + m_startCol.value()); } template inline void writePacket(Index rowId, Index colId, const PacketScalar& val) { - m_xpr.const_cast_derived().template writePacket - (rowId + m_startRow.value(), colId + m_startCol.value(), val); + m_xpr.template writePacket(rowId + m_startRow.value(), colId + m_startCol.value(), val); } template @@ -273,40 +282,46 @@ template inline void writePacket(Index index, const PacketScalar& val) { - m_xpr.const_cast_derived().template writePacket + m_xpr.template writePacket (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), val); } #ifdef EIGEN_PARSED_BY_DOXYGEN /** \sa MapBase::data() */ - inline const Scalar* data() const; - inline Index innerStride() const; - inline Index outerStride() const; + EIGEN_DEVICE_FUNC inline const Scalar* data() const; + EIGEN_DEVICE_FUNC inline Index innerStride() const; + EIGEN_DEVICE_FUNC inline Index outerStride() const; #endif - const typename internal::remove_all::type& nestedExpression() const + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& nestedExpression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + XprType& nestedExpression() { return m_xpr; } - Index startRow() const + EIGEN_DEVICE_FUNC + StorageIndex startRow() const { return m_startRow.value(); } - Index startCol() const + EIGEN_DEVICE_FUNC + StorageIndex startCol() const { return m_startCol.value(); } protected: - const typename XprType::Nested m_xpr; - const internal::variable_if_dynamic m_startRow; - const internal::variable_if_dynamic m_startCol; - const internal::variable_if_dynamic m_blockRows; - const internal::variable_if_dynamic m_blockCols; + XprTypeNested m_xpr; + const internal::variable_if_dynamic m_startRow; + const internal::variable_if_dynamic m_startCol; + const internal::variable_if_dynamic m_blockRows; + const internal::variable_if_dynamic m_blockCols; }; /** \internal Internal implementation of dense Blocks in the direct access case.*/ @@ -315,6 +330,10 @@ class BlockImpl_dense : public MapBase > { typedef Block BlockType; + typedef typename internal::ref_selector::non_const_type XprTypeNested; + enum { + XprTypeIsRowMajor = (int(traits::Flags)&RowMajorBit) != 0 + }; public: typedef MapBase Base; @@ -323,42 +342,52 @@ class BlockImpl_dense /** Column or Row constructor */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index i) - : Base(internal::const_cast_ptr(&xpr.coeffRef( - (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0, - (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0)), + : Base(xpr.data() + i * ( ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor)) + || ((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && ( XprTypeIsRowMajor)) ? xpr.innerStride() : xpr.outerStride()), BlockRows==1 ? 1 : xpr.rows(), BlockCols==1 ? 1 : xpr.cols()), - m_xpr(xpr) + m_xpr(xpr), + m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), + m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0) { init(); } /** Fixed-size constructor */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol) - : Base(internal::const_cast_ptr(&xpr.coeffRef(startRow,startCol))), m_xpr(xpr) + : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol)), + m_xpr(xpr), m_startRow(startRow), m_startCol(startCol) { init(); } /** Dynamic-size constructor */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) - : Base(internal::const_cast_ptr(&xpr.coeffRef(startRow,startCol)), blockRows, blockCols), - m_xpr(xpr) + : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol), blockRows, blockCols), + m_xpr(xpr), m_startRow(startRow), m_startCol(startCol) { init(); } - const typename internal::remove_all::type& nestedExpression() const + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& nestedExpression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + XprType& nestedExpression() { return m_xpr; } /** \sa MapBase::innerStride() */ + EIGEN_DEVICE_FUNC inline Index innerStride() const { return internal::traits::HasSameStorageOrderAsXprType @@ -367,11 +396,24 @@ class BlockImpl_dense } /** \sa MapBase::outerStride() */ + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_outerStride; } + EIGEN_DEVICE_FUNC + StorageIndex startRow() const + { + return m_startRow.value(); + } + + EIGEN_DEVICE_FUNC + StorageIndex startCol() const + { + return m_startCol.value(); + } + #ifndef __SUNPRO_CC // FIXME sunstudio is not friendly with the above friend... // META-FIXME there is no 'friend' keyword around here. Is this obsolete? @@ -380,6 +422,7 @@ class BlockImpl_dense #ifndef EIGEN_PARSED_BY_DOXYGEN /** \internal used by allowAligned() */ + EIGEN_DEVICE_FUNC inline BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols) : Base(data, blockRows, blockCols), m_xpr(xpr) { @@ -388,6 +431,7 @@ class BlockImpl_dense #endif protected: + EIGEN_DEVICE_FUNC void init() { m_outerStride = internal::traits::HasSameStorageOrderAsXprType @@ -395,7 +439,9 @@ class BlockImpl_dense : m_xpr.innerStride(); } - typename XprType::Nested m_xpr; + XprTypeNested m_xpr; + const internal::variable_if_dynamic m_startRow; + const internal::variable_if_dynamic m_startCol; Index m_outerStride; }; diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/BooleanRedux.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/BooleanRedux.h index be9f48a8c7..8409d8749a 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/BooleanRedux.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/BooleanRedux.h @@ -17,9 +17,10 @@ namespace internal { template struct all_unroller { + typedef typename Derived::ExpressionTraits Traits; enum { - col = (UnrollCount-1) / Derived::RowsAtCompileTime, - row = (UnrollCount-1) % Derived::RowsAtCompileTime + col = (UnrollCount-1) / Traits::RowsAtCompileTime, + row = (UnrollCount-1) % Traits::RowsAtCompileTime }; static inline bool run(const Derived &mat) @@ -43,11 +44,12 @@ struct all_unroller template struct any_unroller { + typedef typename Derived::ExpressionTraits Traits; enum { - col = (UnrollCount-1) / Derived::RowsAtCompileTime, - row = (UnrollCount-1) % Derived::RowsAtCompileTime + col = (UnrollCount-1) / Traits::RowsAtCompileTime, + row = (UnrollCount-1) % Traits::RowsAtCompileTime }; - + static inline bool run(const Derived &mat) { return any_unroller::run(mat) || mat.coeff(row, col); @@ -78,19 +80,19 @@ struct any_unroller template inline bool DenseBase::all() const { + typedef internal::evaluator Evaluator; enum { unroll = SizeAtCompileTime != Dynamic - && CoeffReadCost != Dynamic - && NumTraits::AddCost != Dynamic - && SizeAtCompileTime * (CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT + && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT }; + Evaluator evaluator(derived()); if(unroll) - return internal::all_unroller::run(derived()); + return internal::all_unroller::run(evaluator); else { for(Index j = 0; j < cols(); ++j) for(Index i = 0; i < rows(); ++i) - if (!coeff(i, j)) return false; + if (!evaluator.coeff(i, j)) return false; return true; } } @@ -102,19 +104,19 @@ inline bool DenseBase::all() const template inline bool DenseBase::any() const { + typedef internal::evaluator Evaluator; enum { unroll = SizeAtCompileTime != Dynamic - && CoeffReadCost != Dynamic - && NumTraits::AddCost != Dynamic - && SizeAtCompileTime * (CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT + && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT }; + Evaluator evaluator(derived()); if(unroll) - return internal::any_unroller::run(derived()); + return internal::any_unroller::run(evaluator); else { for(Index j = 0; j < cols(); ++j) for(Index i = 0; i < rows(); ++i) - if (coeff(i, j)) return true; + if (evaluator.coeff(i, j)) return true; return false; } } @@ -124,7 +126,7 @@ inline bool DenseBase::any() const * \sa all(), any() */ template -inline typename DenseBase::Index DenseBase::count() const +inline Eigen::Index DenseBase::count() const { return derived().template cast().template cast().sum(); } @@ -136,7 +138,11 @@ inline typename DenseBase::Index DenseBase::count() const template inline bool DenseBase::hasNaN() const { +#if EIGEN_COMP_MSVC || (defined __FAST_MATH__) + return derived().array().isNaN().any(); +#else return !((derived().array()==derived().array()).all()); +#endif } /** \returns true if \c *this contains only finite numbers, i.e., no NaN and no +/-INF values. @@ -146,7 +152,11 @@ inline bool DenseBase::hasNaN() const template inline bool DenseBase::allFinite() const { +#if EIGEN_COMP_MSVC || (defined __FAST_MATH__) + return derived().array().isFinite().all(); +#else return !((derived()-derived()).hasNaN()); +#endif } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CMakeLists.txt b/ground/gcs/src/libs/eigen/Eigen/src/Core/CMakeLists.txt deleted file mode 100644 index 2346fc2bbc..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -FILE(GLOB Eigen_Core_SRCS "*.h") - -INSTALL(FILES - ${Eigen_Core_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Core COMPONENT Devel - ) - -ADD_SUBDIRECTORY(products) -ADD_SUBDIRECTORY(util) -ADD_SUBDIRECTORY(arch) diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CommaInitializer.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CommaInitializer.h index a036d8c3bc..d218e98143 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CommaInitializer.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CommaInitializer.h @@ -22,14 +22,14 @@ namespace Eigen { * the return type of MatrixBase::operator<<, and most of the time this is the only * way it is used. * - * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() + * \sa \blank \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() */ template struct CommaInitializer { typedef typename XprType::Scalar Scalar; - typedef typename XprType::Index Index; + EIGEN_DEVICE_FUNC inline CommaInitializer(XprType& xpr, const Scalar& s) : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) { @@ -37,6 +37,7 @@ struct CommaInitializer } template + EIGEN_DEVICE_FUNC inline CommaInitializer(XprType& xpr, const DenseBase& other) : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows()) { @@ -46,6 +47,7 @@ struct CommaInitializer /* Copy/Move constructor which transfers ownership. This is crucial in * absence of return value optimization to avoid assertions during destruction. */ // FIXME in C++11 mode this could be replaced by a proper RValue constructor + EIGEN_DEVICE_FUNC inline CommaInitializer(const CommaInitializer& o) : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) { // Mark original object as finished. In absence of R-value references we need to const_cast: @@ -55,6 +57,7 @@ struct CommaInitializer } /* inserts a scalar value in the target matrix */ + EIGEN_DEVICE_FUNC CommaInitializer& operator,(const Scalar& s) { if (m_col==m_xpr.cols()) @@ -74,11 +77,10 @@ struct CommaInitializer /* inserts a matrix expression in the target matrix */ template + EIGEN_DEVICE_FUNC CommaInitializer& operator,(const DenseBase& other) { - if(other.cols()==0 || other.rows()==0) - return *this; - if (m_col==m_xpr.cols()) + if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows)) { m_row+=m_currentBlockRows; m_col = 0; @@ -86,24 +88,22 @@ struct CommaInitializer eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows() && "Too many rows passed to comma initializer (operator<<)"); } - eigen_assert(m_col - (m_row, m_col) = other; - else - m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other; + m_xpr.template block + (m_row, m_col, other.rows(), other.cols()) = other; m_col += other.cols(); return *this; } + EIGEN_DEVICE_FUNC inline ~CommaInitializer() +#if defined VERIFY_RAISES_ASSERT && (!defined EIGEN_NO_ASSERTION_CHECKING) && defined EIGEN_EXCEPTIONS + EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception) +#endif { - eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows() - && m_col == m_xpr.cols() - && "Too few coefficients passed to comma initializer (operator<<)"); + finished(); } /** \returns the built matrix once all its coefficients have been set. @@ -113,9 +113,15 @@ struct CommaInitializer * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); * \endcode */ - inline XprType& finished() { return m_xpr; } + EIGEN_DEVICE_FUNC + inline XprType& finished() { + eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0) + && m_col == m_xpr.cols() + && "Too few coefficients passed to comma initializer (operator<<)"); + return m_xpr; + } - XprType& m_xpr; // target expression + XprType& m_xpr; // target expression Index m_row; // current row id Index m_col; // current col id Index m_currentBlockRows; // current block height diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ConditionEstimator.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ConditionEstimator.h new file mode 100644 index 0000000000..aa7efdc765 --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/ConditionEstimator.h @@ -0,0 +1,175 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Rasmus Munk Larsen (rmlarsen@google.com) +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CONDITIONESTIMATOR_H +#define EIGEN_CONDITIONESTIMATOR_H + +namespace Eigen { + +namespace internal { + +template +struct rcond_compute_sign { + static inline Vector run(const Vector& v) { + const RealVector v_abs = v.cwiseAbs(); + return (v_abs.array() == static_cast(0)) + .select(Vector::Ones(v.size()), v.cwiseQuotient(v_abs)); + } +}; + +// Partial specialization to avoid elementwise division for real vectors. +template +struct rcond_compute_sign { + static inline Vector run(const Vector& v) { + return (v.array() < static_cast(0)) + .select(-Vector::Ones(v.size()), Vector::Ones(v.size())); + } +}; + +/** + * \returns an estimate of ||inv(matrix)||_1 given a decomposition of + * \a matrix that implements .solve() and .adjoint().solve() methods. + * + * This function implements Algorithms 4.1 and 5.1 from + * http://www.maths.manchester.ac.uk/~higham/narep/narep135.pdf + * which also forms the basis for the condition number estimators in + * LAPACK. Since at most 10 calls to the solve method of dec are + * performed, the total cost is O(dims^2), as opposed to O(dims^3) + * needed to compute the inverse matrix explicitly. + * + * The most common usage is in estimating the condition number + * ||matrix||_1 * ||inv(matrix)||_1. The first term ||matrix||_1 can be + * computed directly in O(n^2) operations. + * + * Supports the following decompositions: FullPivLU, PartialPivLU, LDLT, and + * LLT. + * + * \sa FullPivLU, PartialPivLU, LDLT, LLT. + */ +template +typename Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition& dec) +{ + typedef typename Decomposition::MatrixType MatrixType; + typedef typename Decomposition::Scalar Scalar; + typedef typename Decomposition::RealScalar RealScalar; + typedef typename internal::plain_col_type::type Vector; + typedef typename internal::plain_col_type::type RealVector; + const bool is_complex = (NumTraits::IsComplex != 0); + + eigen_assert(dec.rows() == dec.cols()); + const Index n = dec.rows(); + if (n == 0) + return 0; + + // Disable Index to float conversion warning +#ifdef __INTEL_COMPILER + #pragma warning push + #pragma warning ( disable : 2259 ) +#endif + Vector v = dec.solve(Vector::Ones(n) / Scalar(n)); +#ifdef __INTEL_COMPILER + #pragma warning pop +#endif + + // lower_bound is a lower bound on + // ||inv(matrix)||_1 = sup_v ||inv(matrix) v||_1 / ||v||_1 + // and is the objective maximized by the ("super-") gradient ascent + // algorithm below. + RealScalar lower_bound = v.template lpNorm<1>(); + if (n == 1) + return lower_bound; + + // Gradient ascent algorithm follows: We know that the optimum is achieved at + // one of the simplices v = e_i, so in each iteration we follow a + // super-gradient to move towards the optimal one. + RealScalar old_lower_bound = lower_bound; + Vector sign_vector(n); + Vector old_sign_vector; + Index v_max_abs_index = -1; + Index old_v_max_abs_index = v_max_abs_index; + for (int k = 0; k < 4; ++k) + { + sign_vector = internal::rcond_compute_sign::run(v); + if (k > 0 && !is_complex && sign_vector == old_sign_vector) { + // Break if the solution stagnated. + break; + } + // v_max_abs_index = argmax |real( inv(matrix)^T * sign_vector )| + v = dec.adjoint().solve(sign_vector); + v.real().cwiseAbs().maxCoeff(&v_max_abs_index); + if (v_max_abs_index == old_v_max_abs_index) { + // Break if the solution stagnated. + break; + } + // Move to the new simplex e_j, where j = v_max_abs_index. + v = dec.solve(Vector::Unit(n, v_max_abs_index)); // v = inv(matrix) * e_j. + lower_bound = v.template lpNorm<1>(); + if (lower_bound <= old_lower_bound) { + // Break if the gradient step did not increase the lower_bound. + break; + } + if (!is_complex) { + old_sign_vector = sign_vector; + } + old_v_max_abs_index = v_max_abs_index; + old_lower_bound = lower_bound; + } + // The following calculates an independent estimate of ||matrix||_1 by + // multiplying matrix by a vector with entries of slowly increasing + // magnitude and alternating sign: + // v_i = (-1)^{i} (1 + (i / (dim-1))), i = 0,...,dim-1. + // This improvement to Hager's algorithm above is due to Higham. It was + // added to make the algorithm more robust in certain corner cases where + // large elements in the matrix might otherwise escape detection due to + // exact cancellation (especially when op and op_adjoint correspond to a + // sequence of backsubstitutions and permutations), which could cause + // Hager's algorithm to vastly underestimate ||matrix||_1. + Scalar alternating_sign(RealScalar(1)); + for (Index i = 0; i < n; ++i) { + // The static_cast is needed when Scalar is a complex and RealScalar implements expression templates + v[i] = alternating_sign * static_cast(RealScalar(1) + (RealScalar(i) / (RealScalar(n - 1)))); + alternating_sign = -alternating_sign; + } + v = dec.solve(v); + const RealScalar alternate_lower_bound = (2 * v.template lpNorm<1>()) / (3 * RealScalar(n)); + return numext::maxi(lower_bound, alternate_lower_bound); +} + +/** \brief Reciprocal condition number estimator. + * + * Computing a decomposition of a dense matrix takes O(n^3) operations, while + * this method estimates the condition number quickly and reliably in O(n^2) + * operations. + * + * \returns an estimate of the reciprocal condition number + * (1 / (||matrix||_1 * ||inv(matrix)||_1)) of matrix, given ||matrix||_1 and + * its decomposition. Supports the following decompositions: FullPivLU, + * PartialPivLU, LDLT, and LLT. + * + * \sa FullPivLU, PartialPivLU, LDLT, LLT. + */ +template +typename Decomposition::RealScalar +rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Decomposition& dec) +{ + typedef typename Decomposition::RealScalar RealScalar; + eigen_assert(dec.rows() == dec.cols()); + if (dec.rows() == 0) return RealScalar(1); + if (matrix_norm == RealScalar(0)) return RealScalar(0); + if (dec.rows() == 1) return RealScalar(1); + const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec); + return (inverse_matrix_norm == RealScalar(0) ? RealScalar(0) + : (RealScalar(1) / inverse_matrix_norm) / matrix_norm); +} + +} // namespace internal + +} // namespace Eigen + +#endif diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreEvaluators.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreEvaluators.h new file mode 100644 index 0000000000..f7c1effca7 --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreEvaluators.h @@ -0,0 +1,1671 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob +// Copyright (C) 2011-2014 Gael Guennebaud +// Copyright (C) 2011-2012 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + + +#ifndef EIGEN_COREEVALUATORS_H +#define EIGEN_COREEVALUATORS_H + +namespace Eigen { + +namespace internal { + +// This class returns the evaluator kind from the expression storage kind. +// Default assumes index based accessors +template +struct storage_kind_to_evaluator_kind { + typedef IndexBased Kind; +}; + +// This class returns the evaluator shape from the expression storage kind. +// It can be Dense, Sparse, Triangular, Diagonal, SelfAdjoint, Band, etc. +template struct storage_kind_to_shape; + +template<> struct storage_kind_to_shape { typedef DenseShape Shape; }; +template<> struct storage_kind_to_shape { typedef SolverShape Shape; }; +template<> struct storage_kind_to_shape { typedef PermutationShape Shape; }; +template<> struct storage_kind_to_shape { typedef TranspositionsShape Shape; }; + +// Evaluators have to be specialized with respect to various criteria such as: +// - storage/structure/shape +// - scalar type +// - etc. +// Therefore, we need specialization of evaluator providing additional template arguments for each kind of evaluators. +// We currently distinguish the following kind of evaluators: +// - unary_evaluator for expressions taking only one arguments (CwiseUnaryOp, CwiseUnaryView, Transpose, MatrixWrapper, ArrayWrapper, Reverse, Replicate) +// - binary_evaluator for expression taking two arguments (CwiseBinaryOp) +// - ternary_evaluator for expression taking three arguments (CwiseTernaryOp) +// - product_evaluator for linear algebra products (Product); special case of binary_evaluator because it requires additional tags for dispatching. +// - mapbase_evaluator for Map, Block, Ref +// - block_evaluator for Block (special dispatching to a mapbase_evaluator or unary_evaluator) + +template< typename T, + typename Arg1Kind = typename evaluator_traits::Kind, + typename Arg2Kind = typename evaluator_traits::Kind, + typename Arg3Kind = typename evaluator_traits::Kind, + typename Arg1Scalar = typename traits::Scalar, + typename Arg2Scalar = typename traits::Scalar, + typename Arg3Scalar = typename traits::Scalar> struct ternary_evaluator; + +template< typename T, + typename LhsKind = typename evaluator_traits::Kind, + typename RhsKind = typename evaluator_traits::Kind, + typename LhsScalar = typename traits::Scalar, + typename RhsScalar = typename traits::Scalar> struct binary_evaluator; + +template< typename T, + typename Kind = typename evaluator_traits::Kind, + typename Scalar = typename T::Scalar> struct unary_evaluator; + +// evaluator_traits contains traits for evaluator + +template +struct evaluator_traits_base +{ + // by default, get evaluator kind and shape from storage + typedef typename storage_kind_to_evaluator_kind::StorageKind>::Kind Kind; + typedef typename storage_kind_to_shape::StorageKind>::Shape Shape; +}; + +// Default evaluator traits +template +struct evaluator_traits : public evaluator_traits_base +{ +}; + +template::Shape > +struct evaluator_assume_aliasing { + static const bool value = false; +}; + +// By default, we assume a unary expression: +template +struct evaluator : public unary_evaluator +{ + typedef unary_evaluator Base; + EIGEN_DEVICE_FUNC explicit evaluator(const T& xpr) : Base(xpr) {} +}; + + +// TODO: Think about const-correctness +template +struct evaluator + : evaluator +{ + EIGEN_DEVICE_FUNC + explicit evaluator(const T& xpr) : evaluator(xpr) {} +}; + +// ---------- base class for all evaluators ---------- + +template +struct evaluator_base : public noncopyable +{ + // TODO that's not very nice to have to propagate all these traits. They are currently only needed to handle outer,inner indices. + typedef traits ExpressionTraits; + + enum { + Alignment = 0 + }; +}; + +// -------------------- Matrix and Array -------------------- +// +// evaluator is a common base class for the +// Matrix and Array evaluators. +// Here we directly specialize evaluator. This is not really a unary expression, and it is, by definition, dense, +// so no need for more sophisticated dispatching. + +template +struct evaluator > + : evaluator_base +{ + typedef PlainObjectBase PlainObjectType; + typedef typename PlainObjectType::Scalar Scalar; + typedef typename PlainObjectType::CoeffReturnType CoeffReturnType; + + enum { + IsRowMajor = PlainObjectType::IsRowMajor, + IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime, + RowsAtCompileTime = PlainObjectType::RowsAtCompileTime, + ColsAtCompileTime = PlainObjectType::ColsAtCompileTime, + + CoeffReadCost = NumTraits::ReadCost, + Flags = traits::EvaluatorFlags, + Alignment = traits::Alignment + }; + + EIGEN_DEVICE_FUNC evaluator() + : m_data(0), + m_outerStride(IsVectorAtCompileTime ? 0 + : int(IsRowMajor) ? ColsAtCompileTime + : RowsAtCompileTime) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC explicit evaluator(const PlainObjectType& m) + : m_data(m.data()), m_outerStride(IsVectorAtCompileTime ? 0 : m.outerStride()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + if (IsRowMajor) + return m_data[row * m_outerStride.value() + col]; + else + return m_data[row + col * m_outerStride.value()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_data[index]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + if (IsRowMajor) + return const_cast(m_data)[row * m_outerStride.value() + col]; + else + return const_cast(m_data)[row + col * m_outerStride.value()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return const_cast(m_data)[index]; + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + if (IsRowMajor) + return ploadt(m_data + row * m_outerStride.value() + col); + else + return ploadt(m_data + row + col * m_outerStride.value()); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return ploadt(m_data + index); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index row, Index col, const PacketType& x) + { + if (IsRowMajor) + return pstoret + (const_cast(m_data) + row * m_outerStride.value() + col, x); + else + return pstoret + (const_cast(m_data) + row + col * m_outerStride.value(), x); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketType& x) + { + return pstoret(const_cast(m_data) + index, x); + } + +protected: + const Scalar *m_data; + + // We do not need to know the outer stride for vectors + variable_if_dynamic m_outerStride; +}; + +template +struct evaluator > + : evaluator > > +{ + typedef Matrix XprType; + + EIGEN_DEVICE_FUNC evaluator() {} + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& m) + : evaluator >(m) + { } +}; + +template +struct evaluator > + : evaluator > > +{ + typedef Array XprType; + + EIGEN_DEVICE_FUNC evaluator() {} + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& m) + : evaluator >(m) + { } +}; + +// -------------------- Transpose -------------------- + +template +struct unary_evaluator, IndexBased> + : evaluator_base > +{ + typedef Transpose XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = evaluator::Flags ^ RowMajorBit, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& t) : m_argImpl(t.nestedExpression()) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_argImpl.coeff(col, row); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_argImpl.coeff(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_argImpl.coeffRef(col, row); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename XprType::Scalar& coeffRef(Index index) + { + return m_argImpl.coeffRef(index); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + return m_argImpl.template packet(col, row); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return m_argImpl.template packet(index); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index row, Index col, const PacketType& x) + { + m_argImpl.template writePacket(col, row, x); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketType& x) + { + m_argImpl.template writePacket(index, x); + } + +protected: + evaluator m_argImpl; +}; + +// -------------------- CwiseNullaryOp -------------------- +// Like Matrix and Array, this is not really a unary expression, so we directly specialize evaluator. +// Likewise, there is not need to more sophisticated dispatching here. + +template::value, + bool has_unary = has_unary_operator::value, + bool has_binary = has_binary_operator::value> +struct nullary_wrapper +{ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { return op(i,j); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { return op(i); } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { return op.template packetOp(i,j); } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { return op.template packetOp(i); } +}; + +template +struct nullary_wrapper +{ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType=0, IndexType=0) const { return op(); } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType=0, IndexType=0) const { return op.template packetOp(); } +}; + +template +struct nullary_wrapper +{ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j=0) const { return op(i,j); } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j=0) const { return op.template packetOp(i,j); } +}; + +// We need the following specialization for vector-only functors assigned to a runtime vector, +// for instance, using linspace and assigning a RowVectorXd to a MatrixXd or even a row of a MatrixXd. +// In this case, i==0 and j is used for the actual iteration. +template +struct nullary_wrapper +{ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { + eigen_assert(i==0 || j==0); + return op(i+j); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { + eigen_assert(i==0 || j==0); + return op.template packetOp(i+j); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { return op(i); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { return op.template packetOp(i); } +}; + +template +struct nullary_wrapper {}; + +#if 0 && EIGEN_COMP_MSVC>0 +// Disable this ugly workaround. This is now handled in traits::match, +// but this piece of code might still become handly if some other weird compilation +// erros pop up again. + +// MSVC exhibits a weird compilation error when +// compiling: +// Eigen::MatrixXf A = MatrixXf::Random(3,3); +// Ref R = 2.f*A; +// and that has_*ary_operator> have not been instantiated yet. +// The "problem" is that evaluator<2.f*A> is instantiated by traits::match<2.f*A> +// and at that time has_*ary_operator returns true regardless of T. +// Then nullary_wrapper is badly instantiated as nullary_wrapper<.,.,true,true,true>. +// The trick is thus to defer the proper instantiation of nullary_wrapper when coeff(), +// and packet() are really instantiated as implemented below: + +// This is a simple wrapper around Index to enforce the re-instantiation of +// has_*ary_operator when needed. +template struct nullary_wrapper_workaround_msvc { + nullary_wrapper_workaround_msvc(const T&); + operator T()const; +}; + +template +struct nullary_wrapper +{ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i, IndexType j) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().operator()(op,i,j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar operator()(const NullaryOp& op, IndexType i) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().operator()(op,i); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i, IndexType j) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().template packetOp(op,i,j); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T packetOp(const NullaryOp& op, IndexType i) const { + return nullary_wrapper >::value, + has_unary_operator >::value, + has_binary_operator >::value>().template packetOp(op,i); + } +}; +#endif // MSVC workaround + +template +struct evaluator > + : evaluator_base > +{ + typedef CwiseNullaryOp XprType; + typedef typename internal::remove_all::type PlainObjectTypeCleaned; + + enum { + CoeffReadCost = internal::functor_traits::Cost, + + Flags = (evaluator::Flags + & ( HereditaryBits + | (functor_has_linear_access::ret ? LinearAccessBit : 0) + | (functor_traits::PacketAccess ? PacketAccessBit : 0))) + | (functor_traits::IsRepeatable ? 0 : EvalBeforeNestingBit), + Alignment = AlignedMax + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& n) + : m_functor(n.functor()), m_wrapper() + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(IndexType row, IndexType col) const + { + return m_wrapper(m_functor, row, col); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(IndexType index) const + { + return m_wrapper(m_functor,index); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(IndexType row, IndexType col) const + { + return m_wrapper.template packetOp(m_functor, row, col); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(IndexType index) const + { + return m_wrapper.template packetOp(m_functor, index); + } + +protected: + const NullaryOp m_functor; + const internal::nullary_wrapper m_wrapper; +}; + +// -------------------- CwiseUnaryOp -------------------- + +template +struct unary_evaluator, IndexBased > + : evaluator_base > +{ + typedef CwiseUnaryOp XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost + functor_traits::Cost, + + Flags = evaluator::Flags + & (HereditaryBits | LinearAccessBit | (functor_traits::PacketAccess ? PacketAccessBit : 0)), + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + explicit unary_evaluator(const XprType& op) + : m_functor(op.functor()), + m_argImpl(op.nestedExpression()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_functor(m_argImpl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_functor(m_argImpl.coeff(index)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + return m_functor.packetOp(m_argImpl.template packet(row, col)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return m_functor.packetOp(m_argImpl.template packet(index)); + } + +protected: + const UnaryOp m_functor; + evaluator m_argImpl; +}; + +// -------------------- CwiseTernaryOp -------------------- + +// this is a ternary expression +template +struct evaluator > + : public ternary_evaluator > +{ + typedef CwiseTernaryOp XprType; + typedef ternary_evaluator > Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct ternary_evaluator, IndexBased, IndexBased> + : evaluator_base > +{ + typedef CwiseTernaryOp XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost + evaluator::CoeffReadCost + evaluator::CoeffReadCost + functor_traits::Cost, + + Arg1Flags = evaluator::Flags, + Arg2Flags = evaluator::Flags, + Arg3Flags = evaluator::Flags, + SameType = is_same::value && is_same::value, + StorageOrdersAgree = (int(Arg1Flags)&RowMajorBit)==(int(Arg2Flags)&RowMajorBit) && (int(Arg1Flags)&RowMajorBit)==(int(Arg3Flags)&RowMajorBit), + Flags0 = (int(Arg1Flags) | int(Arg2Flags) | int(Arg3Flags)) & ( + HereditaryBits + | (int(Arg1Flags) & int(Arg2Flags) & int(Arg3Flags) & + ( (StorageOrdersAgree ? LinearAccessBit : 0) + | (functor_traits::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0) + ) + ) + ), + Flags = (Flags0 & ~RowMajorBit) | (Arg1Flags & RowMajorBit), + Alignment = EIGEN_PLAIN_ENUM_MIN( + EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment, evaluator::Alignment), + evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC explicit ternary_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_arg1Impl(xpr.arg1()), + m_arg2Impl(xpr.arg2()), + m_arg3Impl(xpr.arg3()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_functor(m_arg1Impl.coeff(row, col), m_arg2Impl.coeff(row, col), m_arg3Impl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_functor(m_arg1Impl.coeff(index), m_arg2Impl.coeff(index), m_arg3Impl.coeff(index)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + return m_functor.packetOp(m_arg1Impl.template packet(row, col), + m_arg2Impl.template packet(row, col), + m_arg3Impl.template packet(row, col)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return m_functor.packetOp(m_arg1Impl.template packet(index), + m_arg2Impl.template packet(index), + m_arg3Impl.template packet(index)); + } + +protected: + const TernaryOp m_functor; + evaluator m_arg1Impl; + evaluator m_arg2Impl; + evaluator m_arg3Impl; +}; + +// -------------------- CwiseBinaryOp -------------------- + +// this is a binary expression +template +struct evaluator > + : public binary_evaluator > +{ + typedef CwiseBinaryOp XprType; + typedef binary_evaluator > Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct binary_evaluator, IndexBased, IndexBased> + : evaluator_base > +{ + typedef CwiseBinaryOp XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost + evaluator::CoeffReadCost + functor_traits::Cost, + + LhsFlags = evaluator::Flags, + RhsFlags = evaluator::Flags, + SameType = is_same::value, + StorageOrdersAgree = (int(LhsFlags)&RowMajorBit)==(int(RhsFlags)&RowMajorBit), + Flags0 = (int(LhsFlags) | int(RhsFlags)) & ( + HereditaryBits + | (int(LhsFlags) & int(RhsFlags) & + ( (StorageOrdersAgree ? LinearAccessBit : 0) + | (functor_traits::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0) + ) + ) + ), + Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit), + Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment,evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC explicit binary_evaluator(const XprType& xpr) + : m_functor(xpr.functor()), + m_lhsImpl(xpr.lhs()), + m_rhsImpl(xpr.rhs()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_functor(m_lhsImpl.coeff(row, col), m_rhsImpl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_functor(m_lhsImpl.coeff(index), m_rhsImpl.coeff(index)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + return m_functor.packetOp(m_lhsImpl.template packet(row, col), + m_rhsImpl.template packet(row, col)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return m_functor.packetOp(m_lhsImpl.template packet(index), + m_rhsImpl.template packet(index)); + } + +protected: + const BinaryOp m_functor; + evaluator m_lhsImpl; + evaluator m_rhsImpl; +}; + +// -------------------- CwiseUnaryView -------------------- + +template +struct unary_evaluator, IndexBased> + : evaluator_base > +{ + typedef CwiseUnaryView XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost + functor_traits::Cost, + + Flags = (evaluator::Flags & (HereditaryBits | LinearAccessBit | DirectAccessBit)), + + Alignment = 0 // FIXME it is not very clear why alignment is necessarily lost... + }; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) + : m_unaryOp(op.functor()), + m_argImpl(op.nestedExpression()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits::Cost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_unaryOp(m_argImpl.coeff(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_unaryOp(m_argImpl.coeff(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_unaryOp(m_argImpl.coeffRef(row, col)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return m_unaryOp(m_argImpl.coeffRef(index)); + } + +protected: + const UnaryOp m_unaryOp; + evaluator m_argImpl; +}; + +// -------------------- Map -------------------- + +// FIXME perhaps the PlainObjectType could be provided by Derived::PlainObject ? +// but that might complicate template specialization +template +struct mapbase_evaluator; + +template +struct mapbase_evaluator : evaluator_base +{ + typedef Derived XprType; + typedef typename XprType::PointerType PointerType; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + IsRowMajor = XprType::RowsAtCompileTime, + ColsAtCompileTime = XprType::ColsAtCompileTime, + CoeffReadCost = NumTraits::ReadCost + }; + + EIGEN_DEVICE_FUNC explicit mapbase_evaluator(const XprType& map) + : m_data(const_cast(map.data())), + m_innerStride(map.innerStride()), + m_outerStride(map.outerStride()) + { + EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(evaluator::Flags&PacketAccessBit, internal::inner_stride_at_compile_time::ret==1), + PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_data[col * colStride() + row * rowStride()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_data[index * m_innerStride.value()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_data[col * colStride() + row * rowStride()]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return m_data[index * m_innerStride.value()]; + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + PointerType ptr = m_data + row * rowStride() + col * colStride(); + return internal::ploadt(ptr); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return internal::ploadt(m_data + index * m_innerStride.value()); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index row, Index col, const PacketType& x) + { + PointerType ptr = m_data + row * rowStride() + col * colStride(); + return internal::pstoret(ptr, x); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketType& x) + { + internal::pstoret(m_data + index * m_innerStride.value(), x); + } +protected: + EIGEN_DEVICE_FUNC + inline Index rowStride() const { return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value(); } + EIGEN_DEVICE_FUNC + inline Index colStride() const { return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value(); } + + PointerType m_data; + const internal::variable_if_dynamic m_innerStride; + const internal::variable_if_dynamic m_outerStride; +}; + +template +struct evaluator > + : public mapbase_evaluator, PlainObjectType> +{ + typedef Map XprType; + typedef typename XprType::Scalar Scalar; + // TODO: should check for smaller packet types once we can handle multi-sized packet types + typedef typename packet_traits::type PacketScalar; + + enum { + InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0 + ? int(PlainObjectType::InnerStrideAtCompileTime) + : int(StrideType::InnerStrideAtCompileTime), + OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0 + ? int(PlainObjectType::OuterStrideAtCompileTime) + : int(StrideType::OuterStrideAtCompileTime), + HasNoInnerStride = InnerStrideAtCompileTime == 1, + HasNoOuterStride = StrideType::OuterStrideAtCompileTime == 0, + HasNoStride = HasNoInnerStride && HasNoOuterStride, + IsDynamicSize = PlainObjectType::SizeAtCompileTime==Dynamic, + + PacketAccessMask = bool(HasNoInnerStride) ? ~int(0) : ~int(PacketAccessBit), + LinearAccessMask = bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime) ? ~int(0) : ~int(LinearAccessBit), + Flags = int( evaluator::Flags) & (LinearAccessMask&PacketAccessMask), + + Alignment = int(MapOptions)&int(AlignedMask) + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& map) + : mapbase_evaluator(map) + { } +}; + +// -------------------- Ref -------------------- + +template +struct evaluator > + : public mapbase_evaluator, PlainObjectType> +{ + typedef Ref XprType; + + enum { + Flags = evaluator >::Flags, + Alignment = evaluator >::Alignment + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& ref) + : mapbase_evaluator(ref) + { } +}; + +// -------------------- Block -------------------- + +template::ret> struct block_evaluator; + +template +struct evaluator > + : block_evaluator +{ + typedef Block XprType; + typedef typename XprType::Scalar Scalar; + // TODO: should check for smaller packet types once we can handle multi-sized packet types + typedef typename packet_traits::type PacketScalar; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + + RowsAtCompileTime = traits::RowsAtCompileTime, + ColsAtCompileTime = traits::ColsAtCompileTime, + MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = traits::MaxColsAtCompileTime, + + ArgTypeIsRowMajor = (int(evaluator::Flags)&RowMajorBit) != 0, + IsRowMajor = (MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1) ? 1 + : (MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1) ? 0 + : ArgTypeIsRowMajor, + HasSameStorageOrderAsArgType = (IsRowMajor == ArgTypeIsRowMajor), + InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), + InnerStrideAtCompileTime = HasSameStorageOrderAsArgType + ? int(inner_stride_at_compile_time::ret) + : int(outer_stride_at_compile_time::ret), + OuterStrideAtCompileTime = HasSameStorageOrderAsArgType + ? int(outer_stride_at_compile_time::ret) + : int(inner_stride_at_compile_time::ret), + MaskPacketAccessBit = (InnerStrideAtCompileTime == 1) ? PacketAccessBit : 0, + + FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (evaluator::Flags&LinearAccessBit))) ? LinearAccessBit : 0, + FlagsRowMajorBit = XprType::Flags&RowMajorBit, + Flags0 = evaluator::Flags & ( (HereditaryBits & ~RowMajorBit) | + DirectAccessBit | + MaskPacketAccessBit), + Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit, + + PacketAlignment = unpacket_traits::alignment, + Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0, + Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment, Alignment0) + }; + typedef block_evaluator block_evaluator_type; + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& block) : block_evaluator_type(block) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } +}; + +// no direct-access => dispatch to a unary evaluator +template +struct block_evaluator + : unary_evaluator > +{ + typedef Block XprType; + + EIGEN_DEVICE_FUNC explicit block_evaluator(const XprType& block) + : unary_evaluator(block) + {} +}; + +template +struct unary_evaluator, IndexBased> + : evaluator_base > +{ + typedef Block XprType; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& block) + : m_argImpl(block.nestedExpression()), + m_startRow(block.startRow()), + m_startCol(block.startCol()) + { } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + RowsAtCompileTime = XprType::RowsAtCompileTime + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_argImpl.coeff(m_startRow.value() + row, m_startCol.value() + col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_argImpl.coeffRef(m_startRow.value() + row, m_startCol.value() + col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + return m_argImpl.template packet(m_startRow.value() + row, m_startCol.value() + col); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return packet(RowsAtCompileTime == 1 ? 0 : index, + RowsAtCompileTime == 1 ? index : 0); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index row, Index col, const PacketType& x) + { + return m_argImpl.template writePacket(m_startRow.value() + row, m_startCol.value() + col, x); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketType& x) + { + return writePacket(RowsAtCompileTime == 1 ? 0 : index, + RowsAtCompileTime == 1 ? index : 0, + x); + } + +protected: + evaluator m_argImpl; + const variable_if_dynamic m_startRow; + const variable_if_dynamic m_startCol; +}; + +// TODO: This evaluator does not actually use the child evaluator; +// all action is via the data() as returned by the Block expression. + +template +struct block_evaluator + : mapbase_evaluator, + typename Block::PlainObject> +{ + typedef Block XprType; + typedef typename XprType::Scalar Scalar; + + EIGEN_DEVICE_FUNC explicit block_evaluator(const XprType& block) + : mapbase_evaluator(block) + { + // TODO: for the 3.3 release, this should be turned to an internal assertion, but let's keep it as is for the beta lifetime + eigen_assert(((internal::UIntPtr(block.data()) % EIGEN_PLAIN_ENUM_MAX(1,evaluator::Alignment)) == 0) && "data is not aligned"); + } +}; + + +// -------------------- Select -------------------- +// NOTE shall we introduce a ternary_evaluator? + +// TODO enable vectorization for Select +template +struct evaluator > + : evaluator_base > +{ + typedef Select XprType; + enum { + CoeffReadCost = evaluator::CoeffReadCost + + EIGEN_PLAIN_ENUM_MAX(evaluator::CoeffReadCost, + evaluator::CoeffReadCost), + + Flags = (unsigned int)evaluator::Flags & evaluator::Flags & HereditaryBits, + + Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator::Alignment, evaluator::Alignment) + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& select) + : m_conditionImpl(select.conditionMatrix()), + m_thenImpl(select.thenMatrix()), + m_elseImpl(select.elseMatrix()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + if (m_conditionImpl.coeff(row, col)) + return m_thenImpl.coeff(row, col); + else + return m_elseImpl.coeff(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + if (m_conditionImpl.coeff(index)) + return m_thenImpl.coeff(index); + else + return m_elseImpl.coeff(index); + } + +protected: + evaluator m_conditionImpl; + evaluator m_thenImpl; + evaluator m_elseImpl; +}; + + +// -------------------- Replicate -------------------- + +template +struct unary_evaluator > + : evaluator_base > +{ + typedef Replicate XprType; + typedef typename XprType::CoeffReturnType CoeffReturnType; + enum { + Factor = (RowFactor==Dynamic || ColFactor==Dynamic) ? Dynamic : RowFactor*ColFactor + }; + typedef typename internal::nested_eval::type ArgTypeNested; + typedef typename internal::remove_all::type ArgTypeNestedCleaned; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + LinearAccessMask = XprType::IsVectorAtCompileTime ? LinearAccessBit : 0, + Flags = (evaluator::Flags & (HereditaryBits|LinearAccessMask) & ~RowMajorBit) | (traits::Flags & RowMajorBit), + + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& replicate) + : m_arg(replicate.nestedExpression()), + m_argImpl(m_arg), + m_rows(replicate.nestedExpression().rows()), + m_cols(replicate.nestedExpression().cols()) + {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + // try to avoid using modulo; this is a pure optimization strategy + const Index actual_row = internal::traits::RowsAtCompileTime==1 ? 0 + : RowFactor==1 ? row + : row % m_rows.value(); + const Index actual_col = internal::traits::ColsAtCompileTime==1 ? 0 + : ColFactor==1 ? col + : col % m_cols.value(); + + return m_argImpl.coeff(actual_row, actual_col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + // try to avoid using modulo; this is a pure optimization strategy + const Index actual_index = internal::traits::RowsAtCompileTime==1 + ? (ColFactor==1 ? index : index%m_cols.value()) + : (RowFactor==1 ? index : index%m_rows.value()); + + return m_argImpl.coeff(actual_index); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + const Index actual_row = internal::traits::RowsAtCompileTime==1 ? 0 + : RowFactor==1 ? row + : row % m_rows.value(); + const Index actual_col = internal::traits::ColsAtCompileTime==1 ? 0 + : ColFactor==1 ? col + : col % m_cols.value(); + + return m_argImpl.template packet(actual_row, actual_col); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + const Index actual_index = internal::traits::RowsAtCompileTime==1 + ? (ColFactor==1 ? index : index%m_cols.value()) + : (RowFactor==1 ? index : index%m_rows.value()); + + return m_argImpl.template packet(actual_index); + } + +protected: + const ArgTypeNested m_arg; + evaluator m_argImpl; + const variable_if_dynamic m_rows; + const variable_if_dynamic m_cols; +}; + + +// -------------------- PartialReduxExpr -------------------- + +template< typename ArgType, typename MemberOp, int Direction> +struct evaluator > + : evaluator_base > +{ + typedef PartialReduxExpr XprType; + typedef typename internal::nested_eval::type ArgTypeNested; + typedef typename internal::remove_all::type ArgTypeNestedCleaned; + typedef typename ArgType::Scalar InputScalar; + typedef typename XprType::Scalar Scalar; + enum { + TraversalSize = Direction==int(Vertical) ? int(ArgType::RowsAtCompileTime) : int(ArgType::ColsAtCompileTime) + }; + typedef typename MemberOp::template Cost CostOpType; + enum { + CoeffReadCost = TraversalSize==Dynamic ? HugeCost + : TraversalSize * evaluator::CoeffReadCost + int(CostOpType::value), + + Flags = (traits::Flags&RowMajorBit) | (evaluator::Flags&(HereditaryBits&(~RowMajorBit))) | LinearAccessBit, + + Alignment = 0 // FIXME this will need to be improved once PartialReduxExpr is vectorized + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType xpr) + : m_arg(xpr.nestedExpression()), m_functor(xpr.functor()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize==Dynamic ? HugeCost : int(CostOpType::value)); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const Scalar coeff(Index i, Index j) const + { + if (Direction==Vertical) + return m_functor(m_arg.col(j)); + else + return m_functor(m_arg.row(i)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const Scalar coeff(Index index) const + { + if (Direction==Vertical) + return m_functor(m_arg.col(index)); + else + return m_functor(m_arg.row(index)); + } + +protected: + typename internal::add_const_on_value_type::type m_arg; + const MemberOp m_functor; +}; + + +// -------------------- MatrixWrapper and ArrayWrapper -------------------- +// +// evaluator_wrapper_base is a common base class for the +// MatrixWrapper and ArrayWrapper evaluators. + +template +struct evaluator_wrapper_base + : evaluator_base +{ + typedef typename remove_all::type ArgType; + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = evaluator::Flags, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC explicit evaluator_wrapper_base(const ArgType& arg) : m_argImpl(arg) {} + + typedef typename ArgType::Scalar Scalar; + typedef typename ArgType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_argImpl.coeff(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_argImpl.coeff(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_argImpl.coeffRef(row, col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return m_argImpl.coeffRef(index); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + return m_argImpl.template packet(row, col); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + return m_argImpl.template packet(index); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index row, Index col, const PacketType& x) + { + m_argImpl.template writePacket(row, col, x); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketType& x) + { + m_argImpl.template writePacket(index, x); + } + +protected: + evaluator m_argImpl; +}; + +template +struct unary_evaluator > + : evaluator_wrapper_base > +{ + typedef MatrixWrapper XprType; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& wrapper) + : evaluator_wrapper_base >(wrapper.nestedExpression()) + { } +}; + +template +struct unary_evaluator > + : evaluator_wrapper_base > +{ + typedef ArrayWrapper XprType; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& wrapper) + : evaluator_wrapper_base >(wrapper.nestedExpression()) + { } +}; + + +// -------------------- Reverse -------------------- + +// defined in Reverse.h: +template struct reverse_packet_cond; + +template +struct unary_evaluator > + : evaluator_base > +{ + typedef Reverse XprType; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + IsRowMajor = XprType::IsRowMajor, + IsColMajor = !IsRowMajor, + ReverseRow = (Direction == Vertical) || (Direction == BothDirections), + ReverseCol = (Direction == Horizontal) || (Direction == BothDirections), + ReversePacket = (Direction == BothDirections) + || ((Direction == Vertical) && IsColMajor) + || ((Direction == Horizontal) && IsRowMajor), + + CoeffReadCost = evaluator::CoeffReadCost, + + // let's enable LinearAccess only with vectorization because of the product overhead + // FIXME enable DirectAccess with negative strides? + Flags0 = evaluator::Flags, + LinearAccess = ( (Direction==BothDirections) && (int(Flags0)&PacketAccessBit) ) + || ((ReverseRow && XprType::ColsAtCompileTime==1) || (ReverseCol && XprType::RowsAtCompileTime==1)) + ? LinearAccessBit : 0, + + Flags = int(Flags0) & (HereditaryBits | PacketAccessBit | LinearAccess), + + Alignment = 0 // FIXME in some rare cases, Alignment could be preserved, like a Vector4f. + }; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& reverse) + : m_argImpl(reverse.nestedExpression()), + m_rows(ReverseRow ? reverse.nestedExpression().rows() : 1), + m_cols(ReverseCol ? reverse.nestedExpression().cols() : 1) + { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index col) const + { + return m_argImpl.coeff(ReverseRow ? m_rows.value() - row - 1 : row, + ReverseCol ? m_cols.value() - col - 1 : col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_argImpl.coeff(m_rows.value() * m_cols.value() - index - 1); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index col) + { + return m_argImpl.coeffRef(ReverseRow ? m_rows.value() - row - 1 : row, + ReverseCol ? m_cols.value() - col - 1 : col); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return m_argImpl.coeffRef(m_rows.value() * m_cols.value() - index - 1); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index row, Index col) const + { + enum { + PacketSize = unpacket_traits::size, + OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, + OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1 + }; + typedef internal::reverse_packet_cond reverse_packet; + return reverse_packet::run(m_argImpl.template packet( + ReverseRow ? m_rows.value() - row - OffsetRow : row, + ReverseCol ? m_cols.value() - col - OffsetCol : col)); + } + + template + EIGEN_STRONG_INLINE + PacketType packet(Index index) const + { + enum { PacketSize = unpacket_traits::size }; + return preverse(m_argImpl.template packet(m_rows.value() * m_cols.value() - index - PacketSize)); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index row, Index col, const PacketType& x) + { + // FIXME we could factorize some code with packet(i,j) + enum { + PacketSize = unpacket_traits::size, + OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, + OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1 + }; + typedef internal::reverse_packet_cond reverse_packet; + m_argImpl.template writePacket( + ReverseRow ? m_rows.value() - row - OffsetRow : row, + ReverseCol ? m_cols.value() - col - OffsetCol : col, + reverse_packet::run(x)); + } + + template + EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketType& x) + { + enum { PacketSize = unpacket_traits::size }; + m_argImpl.template writePacket + (m_rows.value() * m_cols.value() - index - PacketSize, preverse(x)); + } + +protected: + evaluator m_argImpl; + + // If we do not reverse rows, then we do not need to know the number of rows; same for columns + // Nonetheless, in this case it is important to set to 1 such that the coeff(index) method works fine for vectors. + const variable_if_dynamic m_rows; + const variable_if_dynamic m_cols; +}; + + +// -------------------- Diagonal -------------------- + +template +struct evaluator > + : evaluator_base > +{ + typedef Diagonal XprType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + + Flags = (unsigned int)(evaluator::Flags & (HereditaryBits | DirectAccessBit) & ~RowMajorBit) | LinearAccessBit, + + Alignment = 0 + }; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& diagonal) + : m_argImpl(diagonal.nestedExpression()), + m_index(diagonal.index()) + { } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index row, Index) const + { + return m_argImpl.coeff(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + CoeffReturnType coeff(Index index) const + { + return m_argImpl.coeff(index + rowOffset(), index + colOffset()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index row, Index) + { + return m_argImpl.coeffRef(row + rowOffset(), row + colOffset()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Scalar& coeffRef(Index index) + { + return m_argImpl.coeffRef(index + rowOffset(), index + colOffset()); + } + +protected: + evaluator m_argImpl; + const internal::variable_if_dynamicindex m_index; + +private: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rowOffset() const { return m_index.value() > 0 ? 0 : -m_index.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index colOffset() const { return m_index.value() > 0 ? m_index.value() : 0; } +}; + + +//---------------------------------------------------------------------- +// deprecated code +//---------------------------------------------------------------------- + +// -------------------- EvalToTemp -------------------- + +// expression class for evaluating nested expression to a temporary + +template class EvalToTemp; + +template +struct traits > + : public traits +{ }; + +template +class EvalToTemp + : public dense_xpr_base >::type +{ + public: + + typedef typename dense_xpr_base::type Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(EvalToTemp) + + explicit EvalToTemp(const ArgType& arg) + : m_arg(arg) + { } + + const ArgType& arg() const + { + return m_arg; + } + + Index rows() const + { + return m_arg.rows(); + } + + Index cols() const + { + return m_arg.cols(); + } + + private: + const ArgType& m_arg; +}; + +template +struct evaluator > + : public evaluator +{ + typedef EvalToTemp XprType; + typedef typename ArgType::PlainObject PlainObject; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) + : m_result(xpr.arg()) + { + ::new (static_cast(this)) Base(m_result); + } + + // This constructor is used when nesting an EvalTo evaluator in another evaluator + EIGEN_DEVICE_FUNC evaluator(const ArgType& arg) + : m_result(arg) + { + ::new (static_cast(this)) Base(m_result); + } + +protected: + PlainObject m_result; +}; + +} // namespace internal + +} // end namespace Eigen + +#endif // EIGEN_COREEVALUATORS_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreIterators.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreIterators.h index 6da4683d2c..4eb42b93af 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreIterators.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CoreIterators.h @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2008-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -15,47 +15,113 @@ namespace Eigen { /* This file contains the respective InnerIterator definition of the expressions defined in Eigen/Core */ -/** \ingroup SparseCore_Module - * \class InnerIterator - * \brief An InnerIterator allows to loop over the element of a sparse (or dense) matrix or expression - * - * todo +namespace internal { + +template +class inner_iterator_selector; + +} + +/** \class InnerIterator + * \brief An InnerIterator allows to loop over the element of any matrix expression. + * + * \warning To be used with care because an evaluator is constructed every time an InnerIterator iterator is constructed. + * + * TODO: add a usage example */ +template +class InnerIterator +{ +protected: + typedef internal::inner_iterator_selector::Kind> IteratorType; + typedef internal::evaluator EvaluatorType; + typedef typename internal::traits::Scalar Scalar; +public: + /** Construct an iterator over the \a outerId -th row or column of \a xpr */ + InnerIterator(const XprType &xpr, const Index &outerId) + : m_eval(xpr), m_iter(m_eval, outerId, xpr.innerSize()) + {} + + /// \returns the value of the current coefficient. + EIGEN_STRONG_INLINE Scalar value() const { return m_iter.value(); } + /** Increment the iterator \c *this to the next non-zero coefficient. + * Explicit zeros are not skipped over. To skip explicit zeros, see class SparseView + */ + EIGEN_STRONG_INLINE InnerIterator& operator++() { m_iter.operator++(); return *this; } + /// \returns the column or row index of the current coefficient. + EIGEN_STRONG_INLINE Index index() const { return m_iter.index(); } + /// \returns the row index of the current coefficient. + EIGEN_STRONG_INLINE Index row() const { return m_iter.row(); } + /// \returns the column index of the current coefficient. + EIGEN_STRONG_INLINE Index col() const { return m_iter.col(); } + /// \returns \c true if the iterator \c *this still references a valid coefficient. + EIGEN_STRONG_INLINE operator bool() const { return m_iter; } + +protected: + EvaluatorType m_eval; + IteratorType m_iter; +private: + // If you get here, then you're not using the right InnerIterator type, e.g.: + // SparseMatrix A; + // SparseMatrix::InnerIterator it(A,0); + template InnerIterator(const EigenBase&,Index outer); +}; + +namespace internal { -// generic version for dense matrix and expressions -template class DenseBase::InnerIterator +// Generic inner iterator implementation for dense objects +template +class inner_iterator_selector { - protected: - typedef typename Derived::Scalar Scalar; - typedef typename Derived::Index Index; - - enum { IsRowMajor = (Derived::Flags&RowMajorBit)==RowMajorBit }; - public: - EIGEN_STRONG_INLINE InnerIterator(const Derived& expr, Index outer) - : m_expression(expr), m_inner(0), m_outer(outer), m_end(expr.innerSize()) - {} - - EIGEN_STRONG_INLINE Scalar value() const - { - return (IsRowMajor) ? m_expression.coeff(m_outer, m_inner) - : m_expression.coeff(m_inner, m_outer); - } - - EIGEN_STRONG_INLINE InnerIterator& operator++() { m_inner++; return *this; } - - EIGEN_STRONG_INLINE Index index() const { return m_inner; } - inline Index row() const { return IsRowMajor ? m_outer : index(); } - inline Index col() const { return IsRowMajor ? index() : m_outer; } - - EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; } - - protected: - const Derived& m_expression; - Index m_inner; - const Index m_outer; - const Index m_end; +protected: + typedef evaluator EvaluatorType; + typedef typename traits::Scalar Scalar; + enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit }; + +public: + EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &innerSize) + : m_eval(eval), m_inner(0), m_outer(outerId), m_end(innerSize) + {} + + EIGEN_STRONG_INLINE Scalar value() const + { + return (IsRowMajor) ? m_eval.coeff(m_outer, m_inner) + : m_eval.coeff(m_inner, m_outer); + } + + EIGEN_STRONG_INLINE inner_iterator_selector& operator++() { m_inner++; return *this; } + + EIGEN_STRONG_INLINE Index index() const { return m_inner; } + inline Index row() const { return IsRowMajor ? m_outer : index(); } + inline Index col() const { return IsRowMajor ? index() : m_outer; } + + EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; } + +protected: + const EvaluatorType& m_eval; + Index m_inner; + const Index m_outer; + const Index m_end; }; +// For iterator-based evaluator, inner-iterator is already implemented as +// evaluator<>::InnerIterator +template +class inner_iterator_selector + : public evaluator::InnerIterator +{ +protected: + typedef typename evaluator::InnerIterator Base; + typedef evaluator EvaluatorType; + +public: + EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &/*innerSize*/) + : Base(eval, outerId) + {} +}; + +} // end namespace internal + } // end namespace Eigen #endif // EIGEN_COREITERATORS_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseBinaryOp.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseBinaryOp.h index 519a866e60..a36765e396 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseBinaryOp.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseBinaryOp.h @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2008-2009 Gael Guennebaud +// Copyright (C) 2008-2014 Gael Guennebaud // Copyright (C) 2006-2008 Benoit Jacob // // This Source Code Form is subject to the terms of the Mozilla @@ -13,26 +13,6 @@ namespace Eigen { -/** \class CwiseBinaryOp - * \ingroup Core_Module - * - * \brief Generic expression where a coefficient-wise binary operator is applied to two expressions - * - * \param BinaryOp template functor implementing the operator - * \param Lhs the type of the left-hand side - * \param Rhs the type of the right-hand side - * - * This class represents an expression where a coefficient-wise binary operator is applied to two expressions. - * It is the return type of binary operators, by which we mean only those binary operators where - * both the left-hand side and the right-hand side are Eigen expressions. - * For example, the return type of matrix1+matrix2 is a CwiseBinaryOp. - * - * Most of the time, this is the only way that it is used, so you typically don't have to name - * CwiseBinaryOp types explicitly. - * - * \sa MatrixBase::binaryExpr(const MatrixBase &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp - */ - namespace internal { template struct traits > @@ -52,77 +32,75 @@ struct traits > // we still want to handle the case when the result type is different. typedef typename result_of< BinaryOp( - typename Lhs::Scalar, - typename Rhs::Scalar + const typename Lhs::Scalar&, + const typename Rhs::Scalar& ) >::type Scalar; - typedef typename promote_storage_type::StorageKind, - typename traits::StorageKind>::ret StorageKind; - typedef typename promote_index_type::Index, - typename traits::Index>::type Index; + typedef typename cwise_promote_storage_type::StorageKind, + typename traits::StorageKind, + BinaryOp>::ret StorageKind; + typedef typename promote_index_type::StorageIndex, + typename traits::StorageIndex>::type StorageIndex; typedef typename Lhs::Nested LhsNested; typedef typename Rhs::Nested RhsNested; typedef typename remove_reference::type _LhsNested; typedef typename remove_reference::type _RhsNested; enum { - LhsCoeffReadCost = _LhsNested::CoeffReadCost, - RhsCoeffReadCost = _RhsNested::CoeffReadCost, - LhsFlags = _LhsNested::Flags, - RhsFlags = _RhsNested::Flags, - SameType = is_same::value, - StorageOrdersAgree = (int(Lhs::Flags)&RowMajorBit)==(int(Rhs::Flags)&RowMajorBit), - Flags0 = (int(LhsFlags) | int(RhsFlags)) & ( - HereditaryBits - | (int(LhsFlags) & int(RhsFlags) & - ( AlignedBit - | (StorageOrdersAgree ? LinearAccessBit : 0) - | (functor_traits::PacketAccess && StorageOrdersAgree && SameType ? PacketAccessBit : 0) - ) - ) - ), - Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit), - Cost0 = EIGEN_ADD_COST(LhsCoeffReadCost,RhsCoeffReadCost), - CoeffReadCost = EIGEN_ADD_COST(Cost0,functor_traits::Cost) + Flags = cwise_promote_storage_order::StorageKind,typename traits::StorageKind,_LhsNested::Flags & RowMajorBit,_RhsNested::Flags & RowMajorBit>::value }; }; } // end namespace internal -// we require Lhs and Rhs to have the same scalar type. Currently there is no example of a binary functor -// that would take two operands of different types. If there were such an example, then this check should be -// moved to the BinaryOp functors, on a per-case basis. This would however require a change in the BinaryOp functors, as -// currently they take only one typename Scalar template parameter. -// It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized paths. -// So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to -// add together a float matrix and a double matrix. -#define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP,LHS,RHS) \ - EIGEN_STATIC_ASSERT((internal::functor_is_product_like::ret \ - ? int(internal::scalar_product_traits::Defined) \ - : int(internal::is_same::value)), \ - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - template class CwiseBinaryOpImpl; -template -class CwiseBinaryOp : internal::no_assignment_operator, +/** \class CwiseBinaryOp + * \ingroup Core_Module + * + * \brief Generic expression where a coefficient-wise binary operator is applied to two expressions + * + * \tparam BinaryOp template functor implementing the operator + * \tparam LhsType the type of the left-hand side + * \tparam RhsType the type of the right-hand side + * + * This class represents an expression where a coefficient-wise binary operator is applied to two expressions. + * It is the return type of binary operators, by which we mean only those binary operators where + * both the left-hand side and the right-hand side are Eigen expressions. + * For example, the return type of matrix1+matrix2 is a CwiseBinaryOp. + * + * Most of the time, this is the only way that it is used, so you typically don't have to name + * CwiseBinaryOp types explicitly. + * + * \sa MatrixBase::binaryExpr(const MatrixBase &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp + */ +template +class CwiseBinaryOp : public CwiseBinaryOpImpl< - BinaryOp, Lhs, Rhs, - typename internal::promote_storage_type::StorageKind, - typename internal::traits::StorageKind>::ret> + BinaryOp, LhsType, RhsType, + typename internal::cwise_promote_storage_type::StorageKind, + typename internal::traits::StorageKind, + BinaryOp>::ret>, + internal::no_assignment_operator { public: + + typedef typename internal::remove_all::type Functor; + typedef typename internal::remove_all::type Lhs; + typedef typename internal::remove_all::type Rhs; typedef typename CwiseBinaryOpImpl< - BinaryOp, Lhs, Rhs, - typename internal::promote_storage_type::StorageKind, - typename internal::traits::StorageKind>::ret>::Base Base; + BinaryOp, LhsType, RhsType, + typename internal::cwise_promote_storage_type::StorageKind, + typename internal::traits::StorageKind, + BinaryOp>::ret>::Base Base; EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp) - typedef typename internal::nested::type LhsNested; - typedef typename internal::nested::type RhsNested; + typedef typename internal::ref_selector::type LhsNested; + typedef typename internal::ref_selector::type RhsNested; typedef typename internal::remove_reference::type _LhsNested; typedef typename internal::remove_reference::type _RhsNested; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, const BinaryOp& func = BinaryOp()) : m_lhs(aLhs), m_rhs(aRhs), m_functor(func) { @@ -132,6 +110,7 @@ class CwiseBinaryOp : internal::no_assignment_operator, eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols()); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { // return the fixed size type if available to enable compile time optimizations if (internal::traits::type>::RowsAtCompileTime==Dynamic) @@ -139,6 +118,7 @@ class CwiseBinaryOp : internal::no_assignment_operator, else return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { // return the fixed size type if available to enable compile time optimizations if (internal::traits::type>::ColsAtCompileTime==Dynamic) @@ -148,10 +128,13 @@ class CwiseBinaryOp : internal::no_assignment_operator, } /** \returns the left hand side nested expression */ + EIGEN_DEVICE_FUNC const _LhsNested& lhs() const { return m_lhs; } /** \returns the right hand side nested expression */ + EIGEN_DEVICE_FUNC const _RhsNested& rhs() const { return m_rhs; } /** \returns the functor representing the binary operation */ + EIGEN_DEVICE_FUNC const BinaryOp& functor() const { return m_functor; } protected: @@ -160,41 +143,13 @@ class CwiseBinaryOp : internal::no_assignment_operator, const BinaryOp m_functor; }; -template -class CwiseBinaryOpImpl - : public internal::dense_xpr_base >::type +// Generic API dispatcher +template +class CwiseBinaryOpImpl + : public internal::generic_xpr_base >::type { - typedef CwiseBinaryOp Derived; - public: - - typedef typename internal::dense_xpr_base >::type Base; - EIGEN_DENSE_PUBLIC_INTERFACE( Derived ) - - EIGEN_STRONG_INLINE const Scalar coeff(Index rowId, Index colId) const - { - return derived().functor()(derived().lhs().coeff(rowId, colId), - derived().rhs().coeff(rowId, colId)); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const - { - return derived().functor().packetOp(derived().lhs().template packet(rowId, colId), - derived().rhs().template packet(rowId, colId)); - } - - EIGEN_STRONG_INLINE const Scalar coeff(Index index) const - { - return derived().functor()(derived().lhs().coeff(index), - derived().rhs().coeff(index)); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index index) const - { - return derived().functor().packetOp(derived().lhs().template packet(index), - derived().rhs().template packet(index)); - } +public: + typedef typename internal::generic_xpr_base >::type Base; }; /** replaces \c *this by \c *this - \a other. @@ -206,8 +161,7 @@ template EIGEN_STRONG_INLINE Derived & MatrixBase::operator-=(const MatrixBase &other) { - SelfCwiseBinaryOp, Derived, OtherDerived> tmp(derived()); - tmp = other.derived(); + call_assignment(derived(), other.derived(), internal::sub_assign_op()); return derived(); } @@ -220,11 +174,11 @@ template EIGEN_STRONG_INLINE Derived & MatrixBase::operator+=(const MatrixBase& other) { - SelfCwiseBinaryOp, Derived, OtherDerived> tmp(derived()); - tmp = other.derived(); + call_assignment(derived(), other.derived(), internal::add_assign_op()); return derived(); } } // end namespace Eigen #endif // EIGEN_CWISE_BINARY_OP_H + diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseNullaryOp.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseNullaryOp.h index a93bab2d0f..dd498f758d 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseNullaryOp.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseNullaryOp.h @@ -12,13 +12,24 @@ namespace Eigen { +namespace internal { +template +struct traits > : traits +{ + enum { + Flags = traits::Flags & RowMajorBit + }; +}; + +} // namespace internal + /** \class CwiseNullaryOp * \ingroup Core_Module * * \brief Generic expression of a matrix where all coefficients are defined by a functor * - * \param NullaryOp template functor implementing the operator - * \param PlainObjectType the underlying plain matrix/array type + * \tparam NullaryOp template functor implementing the operator + * \tparam PlainObjectType the underlying plain matrix/array type * * This class represents an expression of a generic nullary operator. * It is the return type of the Ones(), Zero(), Constant(), Identity() and Random() methods, @@ -27,68 +38,49 @@ namespace Eigen { * However, if you want to write a function returning such an expression, you * will need to use this class. * - * \sa class CwiseUnaryOp, class CwiseBinaryOp, DenseBase::NullaryExpr() + * The functor NullaryOp must expose one of the following method: + + + + +
\c operator()() if the procedural generation does not depend on the coefficient entries (e.g., random numbers)
\c operator()(Index i)if the procedural generation makes sense for vectors only and that it depends on the coefficient index \c i (e.g., linspace)
\c operator()(Index i,Index j)if the procedural generation depends on the matrix coordinates \c i, \c j (e.g., to generate a checkerboard with 0 and 1)
+ * It is also possible to expose the last two operators if the generation makes sense for matrices but can be optimized for vectors. + * + * See DenseBase::NullaryExpr(Index,const CustomNullaryOp&) for an example binding + * C++11 random number generators. + * + * A nullary expression can also be used to implement custom sophisticated matrix manipulations + * that cannot be covered by the existing set of natively supported matrix manipulations. + * See this \ref TopicCustomizing_NullaryExpr "page" for some examples and additional explanations + * on the behavior of CwiseNullaryOp. + * + * \sa class CwiseUnaryOp, class CwiseBinaryOp, DenseBase::NullaryExpr */ - -namespace internal { template -struct traits > : traits -{ - enum { - Flags = (traits::Flags - & ( HereditaryBits - | (functor_has_linear_access::ret ? LinearAccessBit : 0) - | (functor_traits::PacketAccess ? PacketAccessBit : 0))) - | (functor_traits::IsRepeatable ? 0 : EvalBeforeNestingBit), - CoeffReadCost = functor_traits::Cost - }; -}; -} - -template -class CwiseNullaryOp : internal::no_assignment_operator, - public internal::dense_xpr_base< CwiseNullaryOp >::type +class CwiseNullaryOp : public internal::dense_xpr_base< CwiseNullaryOp >::type, internal::no_assignment_operator { public: typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(CwiseNullaryOp) - CwiseNullaryOp(Index nbRows, Index nbCols, const NullaryOp& func = NullaryOp()) - : m_rows(nbRows), m_cols(nbCols), m_functor(func) + EIGEN_DEVICE_FUNC + CwiseNullaryOp(Index rows, Index cols, const NullaryOp& func = NullaryOp()) + : m_rows(rows), m_cols(cols), m_functor(func) { - eigen_assert(nbRows >= 0 - && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == nbRows) - && nbCols >= 0 - && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == nbCols)); + eigen_assert(rows >= 0 + && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) + && cols >= 0 + && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_rows.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_cols.value(); } - EIGEN_STRONG_INLINE const Scalar coeff(Index rowId, Index colId) const - { - return m_functor(rowId, colId); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const - { - return m_functor.packetOp(rowId, colId); - } - - EIGEN_STRONG_INLINE const Scalar coeff(Index index) const - { - return m_functor(index); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index index) const - { - return m_functor.packetOp(index); - } - /** \returns the functor representing the nullary operation */ + EIGEN_DEVICE_FUNC const NullaryOp& functor() const { return m_functor; } protected: @@ -113,10 +105,10 @@ class CwiseNullaryOp : internal::no_assignment_operator, */ template template -EIGEN_STRONG_INLINE const CwiseNullaryOp +EIGEN_STRONG_INLINE const CwiseNullaryOp::PlainObject> DenseBase::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func) { - return CwiseNullaryOp(rows, cols, func); + return CwiseNullaryOp(rows, cols, func); } /** \returns an expression of a matrix defined by a custom functor \a func @@ -132,16 +124,19 @@ DenseBase::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& f * * The template parameter \a CustomNullaryOp is the type of the functor. * + * Here is an example with C++11 random generators: \include random_cpp11.cpp + * Output: \verbinclude random_cpp11.out + * * \sa class CwiseNullaryOp */ template template -EIGEN_STRONG_INLINE const CwiseNullaryOp +EIGEN_STRONG_INLINE const CwiseNullaryOp::PlainObject> DenseBase::NullaryExpr(Index size, const CustomNullaryOp& func) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - if(RowsAtCompileTime == 1) return CwiseNullaryOp(1, size, func); - else return CwiseNullaryOp(size, 1, func); + if(RowsAtCompileTime == 1) return CwiseNullaryOp(1, size, func); + else return CwiseNullaryOp(size, 1, func); } /** \returns an expression of a matrix defined by a custom functor \a func @@ -155,19 +150,19 @@ DenseBase::NullaryExpr(Index size, const CustomNullaryOp& func) */ template template -EIGEN_STRONG_INLINE const CwiseNullaryOp +EIGEN_STRONG_INLINE const CwiseNullaryOp::PlainObject> DenseBase::NullaryExpr(const CustomNullaryOp& func) { - return CwiseNullaryOp(RowsAtCompileTime, ColsAtCompileTime, func); + return CwiseNullaryOp(RowsAtCompileTime, ColsAtCompileTime, func); } /** \returns an expression of a constant matrix of value \a value * - * The parameters \a nbRows and \a nbCols are the number of rows and of columns of + * The parameters \a rows and \a cols are the number of rows and of columns of * the returned matrix. Must be compatible with this DenseBase type. * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, - * it is redundant to pass \a nbRows and \a nbCols as arguments, so Zero() should be used + * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used * instead. * * The template parameter \a CustomNullaryOp is the type of the functor. @@ -176,9 +171,9 @@ DenseBase::NullaryExpr(const CustomNullaryOp& func) */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Constant(Index nbRows, Index nbCols, const Scalar& value) +DenseBase::Constant(Index rows, Index cols, const Scalar& value) { - return DenseBase::NullaryExpr(nbRows, nbCols, internal::scalar_constant_op(value)); + return DenseBase::NullaryExpr(rows, cols, internal::scalar_constant_op(value)); } /** \returns an expression of a constant matrix of value \a value @@ -220,46 +215,33 @@ DenseBase::Constant(const Scalar& value) return DenseBase::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_constant_op(value)); } -/** - * \brief Sets a linearly space vector. - * - * The function generates 'size' equally spaced values in the closed interval [low,high]. - * This particular version of LinSpaced() uses sequential access, i.e. vector access is - * assumed to be a(0), a(1), ..., a(size). This assumption allows for better vectorization - * and yields faster code than the random access version. +/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) * - * When size is set to 1, a vector of length 1 containing 'high' is returned. - * - * \only_for_vectors - * - * Example: \include DenseBase_LinSpaced_seq.cpp - * Output: \verbinclude DenseBase_LinSpaced_seq.out - * - * \sa setLinSpaced(Index,const Scalar&,const Scalar&), LinSpaced(Index,Scalar,Scalar), CwiseNullaryOp + * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&) */ template -EIGEN_STRONG_INLINE const typename DenseBase::SequentialLinSpacedReturnType +EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); + return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); } -/** - * \copydoc DenseBase::LinSpaced(Sequential_t, Index, const Scalar&, const Scalar&) - * Special version for fixed size types which does not require the size parameter. +/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) + * + * \sa LinSpaced(Scalar,Scalar) */ template -EIGEN_STRONG_INLINE const typename DenseBase::SequentialLinSpacedReturnType +EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType DenseBase::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) - return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op(low,high,Derived::SizeAtCompileTime)); + return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op(low,high,Derived::SizeAtCompileTime)); } /** - * \brief Sets a linearly space vector. + * \brief Sets a linearly spaced vector. * * The function generates 'size' equally spaced values in the closed interval [low,high]. * When size is set to 1, a vector of length 1 containing 'high' is returned. @@ -269,14 +251,24 @@ DenseBase::LinSpaced(Sequential_t, const Scalar& low, const Scalar& hig * Example: \include DenseBase_LinSpaced.cpp * Output: \verbinclude DenseBase_LinSpaced.out * - * \sa setLinSpaced(Index,const Scalar&,const Scalar&), LinSpaced(Sequential_t,Index,const Scalar&,const Scalar&,Index), CwiseNullaryOp + * For integer scalar types, an even spacing is possible if and only if the length of the range, + * i.e., \c high-low is a scalar multiple of \c size-1, or if \c size is a scalar multiple of the + * number of values \c high-low+1 (meaning each value can be repeated the same number of time). + * If one of these two considions is not satisfied, then \c high is lowered to the largest value + * satisfying one of this constraint. + * Here are some examples: + * + * Example: \include DenseBase_LinSpacedInt.cpp + * Output: \verbinclude DenseBase_LinSpacedInt.out + * + * \sa setLinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp */ template EIGEN_STRONG_INLINE const typename DenseBase::RandomAccessLinSpacedReturnType DenseBase::LinSpaced(Index size, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); + return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); } /** @@ -289,7 +281,7 @@ DenseBase::LinSpaced(const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) - return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op(low,high,Derived::SizeAtCompileTime)); + return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op(low,high,Derived::SizeAtCompileTime)); } /** \returns true if all coefficients in this matrix are approximately equal to \a val, to within precision \a prec */ @@ -297,9 +289,10 @@ template bool DenseBase::isApproxToConstant (const Scalar& val, const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); for(Index j = 0; j < cols(); ++j) for(Index i = 0; i < rows(); ++i) - if(!internal::isApprox(this->coeff(i, j), val, prec)) + if(!internal::isApprox(self.coeff(i, j), val, prec)) return false; return true; } @@ -324,7 +317,7 @@ EIGEN_STRONG_INLINE void DenseBase::fill(const Scalar& val) setConstant(val); } -/** Sets all coefficients in this expression to \a value. +/** Sets all coefficients in this expression to value \a val. * * \sa fill(), setConstant(Index,const Scalar&), setConstant(Index,Index,const Scalar&), setZero(), setOnes(), Constant(), class CwiseNullaryOp, setZero(), setOnes() */ @@ -334,7 +327,7 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setConstant(const Scalar& val) return derived() = Constant(rows(), cols(), val); } -/** Resizes to the given \a size, and sets all coefficients in this expression to the given \a value. +/** Resizes to the given \a size, and sets all coefficients in this expression to the given value \a val. * * \only_for_vectors * @@ -351,10 +344,10 @@ PlainObjectBase::setConstant(Index size, const Scalar& val) return setConstant(val); } -/** Resizes to the given size, and sets all coefficients in this expression to the given \a value. +/** Resizes to the given size, and sets all coefficients in this expression to the given value \a val. * - * \param nbRows the new number of rows - * \param nbCols the new number of columns + * \param rows the new number of rows + * \param cols the new number of columns * \param val the value to which all coefficients are set * * Example: \include Matrix_setConstant_int_int.cpp @@ -364,14 +357,14 @@ PlainObjectBase::setConstant(Index size, const Scalar& val) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setConstant(Index nbRows, Index nbCols, const Scalar& val) +PlainObjectBase::setConstant(Index rows, Index cols, const Scalar& val) { - resize(nbRows, nbCols); + resize(rows, cols); return setConstant(val); } /** - * \brief Sets a linearly space vector. + * \brief Sets a linearly spaced vector. * * The function generates 'size' equally spaced values in the closed interval [low,high]. * When size is set to 1, a vector of length 1 containing 'high' is returned. @@ -381,24 +374,30 @@ PlainObjectBase::setConstant(Index nbRows, Index nbCols, const Scalar& * Example: \include DenseBase_setLinSpaced.cpp * Output: \verbinclude DenseBase_setLinSpaced.out * - * \sa CwiseNullaryOp + * For integer scalar types, do not miss the explanations on the definition + * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink. + * + * \sa LinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp */ template EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(Index newSize, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op(low,high,newSize)); + return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op(low,high,newSize)); } /** - * \brief Sets a linearly space vector. + * \brief Sets a linearly spaced vector. * - * The function fill *this with equally spaced values in the closed interval [low,high]. + * The function fills \c *this with equally spaced values in the closed interval [low,high]. * When size is set to 1, a vector of length 1 containing 'high' is returned. * * \only_for_vectors * - * \sa setLinSpaced(Index, const Scalar&, const Scalar&), CwiseNullaryOp + * For integer scalar types, do not miss the explanations on the definition + * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink. + * + * \sa LinSpaced(Index,const Scalar&,const Scalar&), setLinSpaced(Index, const Scalar&, const Scalar&), CwiseNullaryOp */ template EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(const Scalar& low, const Scalar& high) @@ -425,9 +424,9 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(const Scalar& low, */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Zero(Index nbRows, Index nbCols) +DenseBase::Zero(Index rows, Index cols) { - return Constant(nbRows, nbCols, Scalar(0)); + return Constant(rows, cols, Scalar(0)); } /** \returns an expression of a zero vector. @@ -481,9 +480,10 @@ DenseBase::Zero() template bool DenseBase::isZero(const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); for(Index j = 0; j < cols(); ++j) for(Index i = 0; i < rows(); ++i) - if(!internal::isMuchSmallerThan(this->coeff(i, j), static_cast(1), prec)) + if(!internal::isMuchSmallerThan(self.coeff(i, j), static_cast(1), prec)) return false; return true; } @@ -520,8 +520,8 @@ PlainObjectBase::setZero(Index newSize) /** Resizes to the given size, and sets all coefficients in this expression to zero. * - * \param nbRows the new number of rows - * \param nbCols the new number of columns + * \param rows the new number of rows + * \param cols the new number of columns * * Example: \include Matrix_setZero_int_int.cpp * Output: \verbinclude Matrix_setZero_int_int.out @@ -530,9 +530,9 @@ PlainObjectBase::setZero(Index newSize) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setZero(Index nbRows, Index nbCols) +PlainObjectBase::setZero(Index rows, Index cols) { - resize(nbRows, nbCols); + resize(rows, cols); return setConstant(Scalar(0)); } @@ -540,7 +540,7 @@ PlainObjectBase::setZero(Index nbRows, Index nbCols) /** \returns an expression of a matrix where all coefficients equal one. * - * The parameters \a nbRows and \a nbCols are the number of rows and of columns of + * The parameters \a rows and \a cols are the number of rows and of columns of * the returned matrix. Must be compatible with this MatrixBase type. * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, @@ -554,9 +554,9 @@ PlainObjectBase::setZero(Index nbRows, Index nbCols) */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Ones(Index nbRows, Index nbCols) +DenseBase::Ones(Index rows, Index cols) { - return Constant(nbRows, nbCols, Scalar(1)); + return Constant(rows, cols, Scalar(1)); } /** \returns an expression of a vector where all coefficients equal one. @@ -646,8 +646,8 @@ PlainObjectBase::setOnes(Index newSize) /** Resizes to the given size, and sets all coefficients in this expression to one. * - * \param nbRows the new number of rows - * \param nbCols the new number of columns + * \param rows the new number of rows + * \param cols the new number of columns * * Example: \include Matrix_setOnes_int_int.cpp * Output: \verbinclude Matrix_setOnes_int_int.out @@ -656,9 +656,9 @@ PlainObjectBase::setOnes(Index newSize) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setOnes(Index nbRows, Index nbCols) +PlainObjectBase::setOnes(Index rows, Index cols) { - resize(nbRows, nbCols); + resize(rows, cols); return setConstant(Scalar(1)); } @@ -666,7 +666,7 @@ PlainObjectBase::setOnes(Index nbRows, Index nbCols) /** \returns an expression of the identity matrix (not necessarily square). * - * The parameters \a nbRows and \a nbCols are the number of rows and of columns of + * The parameters \a rows and \a cols are the number of rows and of columns of * the returned matrix. Must be compatible with this MatrixBase type. * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, @@ -680,9 +680,9 @@ PlainObjectBase::setOnes(Index nbRows, Index nbCols) */ template EIGEN_STRONG_INLINE const typename MatrixBase::IdentityReturnType -MatrixBase::Identity(Index nbRows, Index nbCols) +MatrixBase::Identity(Index rows, Index cols) { - return DenseBase::NullaryExpr(nbRows, nbCols, internal::scalar_identity_op()); + return DenseBase::NullaryExpr(rows, cols, internal::scalar_identity_op()); } /** \returns an expression of the identity matrix (not necessarily square). @@ -716,18 +716,19 @@ template bool MatrixBase::isIdentity (const RealScalar& prec) const { + typename internal::nested_eval::type self(derived()); for(Index j = 0; j < cols(); ++j) { for(Index i = 0; i < rows(); ++i) { if(i == j) { - if(!internal::isApprox(this->coeff(i, j), static_cast(1), prec)) + if(!internal::isApprox(self.coeff(i, j), static_cast(1), prec)) return false; } else { - if(!internal::isMuchSmallerThan(this->coeff(i, j), static_cast(1), prec)) + if(!internal::isMuchSmallerThan(self.coeff(i, j), static_cast(1), prec)) return false; } } @@ -740,6 +741,7 @@ namespace internal { template=16)> struct setIdentity_impl { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Derived& run(Derived& m) { return m = Derived::Identity(m.rows(), m.cols()); @@ -749,11 +751,11 @@ struct setIdentity_impl template struct setIdentity_impl { - typedef typename Derived::Index Index; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Derived& run(Derived& m) { m.setZero(); - const Index size = (std::min)(m.rows(), m.cols()); + const Index size = numext::mini(m.rows(), m.cols()); for(Index i = 0; i < size; ++i) m.coeffRef(i,i) = typename Derived::Scalar(1); return m; } @@ -776,8 +778,8 @@ EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity() /** \brief Resizes to the given size, and writes the identity expression (not necessarily square) into *this. * - * \param nbRows the new number of rows - * \param nbCols the new number of columns + * \param rows the new number of rows + * \param cols the new number of columns * * Example: \include Matrix_setIdentity_int_int.cpp * Output: \verbinclude Matrix_setIdentity_int_int.out @@ -785,9 +787,9 @@ EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity() * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity() */ template -EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity(Index nbRows, Index nbCols) +EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity(Index rows, Index cols) { - derived().resize(nbRows, nbCols); + derived().resize(rows, cols); return setIdentity(); } diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseTernaryOp.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseTernaryOp.h new file mode 100644 index 0000000000..9f3576fece --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseTernaryOp.h @@ -0,0 +1,197 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2016 Eugene Brevdo +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CWISE_TERNARY_OP_H +#define EIGEN_CWISE_TERNARY_OP_H + +namespace Eigen { + +namespace internal { +template +struct traits > { + // we must not inherit from traits since it has + // the potential to cause problems with MSVC + typedef typename remove_all::type Ancestor; + typedef typename traits::XprKind XprKind; + enum { + RowsAtCompileTime = traits::RowsAtCompileTime, + ColsAtCompileTime = traits::ColsAtCompileTime, + MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = traits::MaxColsAtCompileTime + }; + + // even though we require Arg1, Arg2, and Arg3 to have the same scalar type + // (see CwiseTernaryOp constructor), + // we still want to handle the case when the result type is different. + typedef typename result_of::type Scalar; + + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::StorageIndex StorageIndex; + + typedef typename Arg1::Nested Arg1Nested; + typedef typename Arg2::Nested Arg2Nested; + typedef typename Arg3::Nested Arg3Nested; + typedef typename remove_reference::type _Arg1Nested; + typedef typename remove_reference::type _Arg2Nested; + typedef typename remove_reference::type _Arg3Nested; + enum { Flags = _Arg1Nested::Flags & RowMajorBit }; +}; +} // end namespace internal + +template +class CwiseTernaryOpImpl; + +/** \class CwiseTernaryOp + * \ingroup Core_Module + * + * \brief Generic expression where a coefficient-wise ternary operator is + * applied to two expressions + * + * \tparam TernaryOp template functor implementing the operator + * \tparam Arg1Type the type of the first argument + * \tparam Arg2Type the type of the second argument + * \tparam Arg3Type the type of the third argument + * + * This class represents an expression where a coefficient-wise ternary + * operator is applied to three expressions. + * It is the return type of ternary operators, by which we mean only those + * ternary operators where + * all three arguments are Eigen expressions. + * For example, the return type of betainc(matrix1, matrix2, matrix3) is a + * CwiseTernaryOp. + * + * Most of the time, this is the only way that it is used, so you typically + * don't have to name + * CwiseTernaryOp types explicitly. + * + * \sa MatrixBase::ternaryExpr(const MatrixBase &, const + * MatrixBase &, const CustomTernaryOp &) const, class CwiseBinaryOp, + * class CwiseUnaryOp, class CwiseNullaryOp + */ +template +class CwiseTernaryOp : public CwiseTernaryOpImpl< + TernaryOp, Arg1Type, Arg2Type, Arg3Type, + typename internal::traits::StorageKind>, + internal::no_assignment_operator +{ + public: + typedef typename internal::remove_all::type Arg1; + typedef typename internal::remove_all::type Arg2; + typedef typename internal::remove_all::type Arg3; + + typedef typename CwiseTernaryOpImpl< + TernaryOp, Arg1Type, Arg2Type, Arg3Type, + typename internal::traits::StorageKind>::Base Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseTernaryOp) + + typedef typename internal::ref_selector::type Arg1Nested; + typedef typename internal::ref_selector::type Arg2Nested; + typedef typename internal::ref_selector::type Arg3Nested; + typedef typename internal::remove_reference::type _Arg1Nested; + typedef typename internal::remove_reference::type _Arg2Nested; + typedef typename internal::remove_reference::type _Arg3Nested; + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE CwiseTernaryOp(const Arg1& a1, const Arg2& a2, + const Arg3& a3, + const TernaryOp& func = TernaryOp()) + : m_arg1(a1), m_arg2(a2), m_arg3(a3), m_functor(func) { + // require the sizes to match + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg2) + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg3) + + // The index types should match + EIGEN_STATIC_ASSERT((internal::is_same< + typename internal::traits::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + EIGEN_STATIC_ASSERT((internal::is_same< + typename internal::traits::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + + eigen_assert(a1.rows() == a2.rows() && a1.cols() == a2.cols() && + a1.rows() == a3.rows() && a1.cols() == a3.cols()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index rows() const { + // return the fixed size type if available to enable compile time + // optimizations + if (internal::traits::type>:: + RowsAtCompileTime == Dynamic && + internal::traits::type>:: + RowsAtCompileTime == Dynamic) + return m_arg3.rows(); + else if (internal::traits::type>:: + RowsAtCompileTime == Dynamic && + internal::traits::type>:: + RowsAtCompileTime == Dynamic) + return m_arg2.rows(); + else + return m_arg1.rows(); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index cols() const { + // return the fixed size type if available to enable compile time + // optimizations + if (internal::traits::type>:: + ColsAtCompileTime == Dynamic && + internal::traits::type>:: + ColsAtCompileTime == Dynamic) + return m_arg3.cols(); + else if (internal::traits::type>:: + ColsAtCompileTime == Dynamic && + internal::traits::type>:: + ColsAtCompileTime == Dynamic) + return m_arg2.cols(); + else + return m_arg1.cols(); + } + + /** \returns the first argument nested expression */ + EIGEN_DEVICE_FUNC + const _Arg1Nested& arg1() const { return m_arg1; } + /** \returns the first argument nested expression */ + EIGEN_DEVICE_FUNC + const _Arg2Nested& arg2() const { return m_arg2; } + /** \returns the third argument nested expression */ + EIGEN_DEVICE_FUNC + const _Arg3Nested& arg3() const { return m_arg3; } + /** \returns the functor representing the ternary operation */ + EIGEN_DEVICE_FUNC + const TernaryOp& functor() const { return m_functor; } + + protected: + Arg1Nested m_arg1; + Arg2Nested m_arg2; + Arg3Nested m_arg3; + const TernaryOp m_functor; +}; + +// Generic API dispatcher +template +class CwiseTernaryOpImpl + : public internal::generic_xpr_base< + CwiseTernaryOp >::type { + public: + typedef typename internal::generic_xpr_base< + CwiseTernaryOp >::type Base; +}; + +} // end namespace Eigen + +#endif // EIGEN_CWISE_TERNARY_OP_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryOp.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryOp.h index f7ee60e987..1d2dd19f2b 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryOp.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryOp.h @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2008-2014 Gael Guennebaud // Copyright (C) 2006-2008 Benoit Jacob // // This Source Code Form is subject to the terms of the Mozilla @@ -13,41 +13,18 @@ namespace Eigen { -/** \class CwiseUnaryOp - * \ingroup Core_Module - * - * \brief Generic expression where a coefficient-wise unary operator is applied to an expression - * - * \param UnaryOp template functor implementing the operator - * \param XprType the type of the expression to which we are applying the unary operator - * - * This class represents an expression where a unary operator is applied to an expression. - * It is the return type of all operations taking exactly 1 input expression, regardless of the - * presence of other inputs such as scalars. For example, the operator* in the expression 3*matrix - * is considered unary, because only the right-hand side is an expression, and its - * return type is a specialization of CwiseUnaryOp. - * - * Most of the time, this is the only way that it is used, so you typically don't have to name - * CwiseUnaryOp types explicitly. - * - * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp - */ - namespace internal { template struct traits > : traits { typedef typename result_of< - UnaryOp(typename XprType::Scalar) + UnaryOp(const typename XprType::Scalar&) >::type Scalar; typedef typename XprType::Nested XprTypeNested; typedef typename remove_reference::type _XprTypeNested; enum { - Flags = _XprTypeNested::Flags & ( - HereditaryBits | LinearAccessBit | AlignedBit - | (functor_traits::PacketAccess ? PacketAccessBit : 0)), - CoeffReadCost = EIGEN_ADD_COST(_XprTypeNested::CoeffReadCost, functor_traits::Cost) + Flags = _XprTypeNested::Flags & RowMajorBit }; }; } @@ -55,70 +32,70 @@ struct traits > template class CwiseUnaryOpImpl; +/** \class CwiseUnaryOp + * \ingroup Core_Module + * + * \brief Generic expression where a coefficient-wise unary operator is applied to an expression + * + * \tparam UnaryOp template functor implementing the operator + * \tparam XprType the type of the expression to which we are applying the unary operator + * + * This class represents an expression where a unary operator is applied to an expression. + * It is the return type of all operations taking exactly 1 input expression, regardless of the + * presence of other inputs such as scalars. For example, the operator* in the expression 3*matrix + * is considered unary, because only the right-hand side is an expression, and its + * return type is a specialization of CwiseUnaryOp. + * + * Most of the time, this is the only way that it is used, so you typically don't have to name + * CwiseUnaryOp types explicitly. + * + * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp + */ template -class CwiseUnaryOp : internal::no_assignment_operator, - public CwiseUnaryOpImpl::StorageKind> +class CwiseUnaryOp : public CwiseUnaryOpImpl::StorageKind>, internal::no_assignment_operator { public: typedef typename CwiseUnaryOpImpl::StorageKind>::Base Base; EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp) + typedef typename internal::ref_selector::type XprTypeNested; + typedef typename internal::remove_all::type NestedExpression; - inline CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp()) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + explicit CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp()) : m_xpr(xpr), m_functor(func) {} - EIGEN_STRONG_INLINE Index rows() const { return m_xpr.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return m_xpr.cols(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index rows() const { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index cols() const { return m_xpr.cols(); } /** \returns the functor representing the unary operation */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryOp& functor() const { return m_functor; } /** \returns the nested expression */ - const typename internal::remove_all::type& + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const typename internal::remove_all::type& nestedExpression() const { return m_xpr; } /** \returns the nested expression */ - typename internal::remove_all::type& - nestedExpression() { return m_xpr.const_cast_derived(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename internal::remove_all::type& + nestedExpression() { return m_xpr; } protected: - typename XprType::Nested m_xpr; + XprTypeNested m_xpr; const UnaryOp m_functor; }; -// This is the generic implementation for dense storage. -// It can be used for any expression types implementing the dense concept. -template -class CwiseUnaryOpImpl - : public internal::dense_xpr_base >::type +// Generic API dispatcher +template +class CwiseUnaryOpImpl + : public internal::generic_xpr_base >::type { - public: - - typedef CwiseUnaryOp Derived; - typedef typename internal::dense_xpr_base >::type Base; - EIGEN_DENSE_PUBLIC_INTERFACE(Derived) - - EIGEN_STRONG_INLINE const Scalar coeff(Index rowId, Index colId) const - { - return derived().functor()(derived().nestedExpression().coeff(rowId, colId)); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const - { - return derived().functor().packetOp(derived().nestedExpression().template packet(rowId, colId)); - } - - EIGEN_STRONG_INLINE const Scalar coeff(Index index) const - { - return derived().functor()(derived().nestedExpression().coeff(index)); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index index) const - { - return derived().functor().packetOp(derived().nestedExpression().template packet(index)); - } +public: + typedef typename internal::generic_xpr_base >::type Base; }; } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryView.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryView.h index f3b2ffeb6f..2710330562 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryView.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryView.h @@ -12,33 +12,19 @@ namespace Eigen { -/** \class CwiseUnaryView - * \ingroup Core_Module - * - * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector - * - * \param ViewOp template functor implementing the view - * \param MatrixType the type of the matrix we are applying the unary operator - * - * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector. - * It is the return type of real() and imag(), and most of the time this is the only way it is used. - * - * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp - */ - namespace internal { template struct traits > : traits { typedef typename result_of< - ViewOp(typename traits::Scalar) + ViewOp(const typename traits::Scalar&) >::type Scalar; typedef typename MatrixType::Nested MatrixTypeNested; typedef typename remove_all::type _MatrixTypeNested; enum { - Flags = (traits<_MatrixTypeNested>::Flags & (HereditaryBits | LvalueBit | LinearAccessBit | DirectAccessBit)), - CoeffReadCost = EIGEN_ADD_COST(traits<_MatrixTypeNested>::CoeffReadCost, functor_traits::Cost), + FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, + Flags = traits<_MatrixTypeNested>::Flags & (RowMajorBit | FlagsLvalueBit | DirectAccessBit), // FIXME DirectAccessBit should not be handled by expressions MatrixTypeInnerStride = inner_stride_at_compile_time::ret, // need to cast the sizeof's from size_t to int explicitly, otherwise: // "error: no integral type can represent all of the enumerator values @@ -55,6 +41,19 @@ struct traits > template class CwiseUnaryViewImpl; +/** \class CwiseUnaryView + * \ingroup Core_Module + * + * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector + * + * \tparam ViewOp template functor implementing the view + * \tparam MatrixType the type of the matrix we are applying the unary operator + * + * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector. + * It is the return type of real() and imag(), and most of the time this is the only way it is used. + * + * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp + */ template class CwiseUnaryView : public CwiseUnaryViewImpl::StorageKind> { @@ -62,8 +61,10 @@ class CwiseUnaryView : public CwiseUnaryViewImpl::StorageKind>::Base Base; EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView) + typedef typename internal::ref_selector::non_const_type MatrixTypeNested; + typedef typename internal::remove_all::type NestedExpression; - inline CwiseUnaryView(const MatrixType& mat, const ViewOp& func = ViewOp()) + explicit inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp()) : m_matrix(mat), m_functor(func) {} EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView) @@ -75,19 +76,27 @@ class CwiseUnaryView : public CwiseUnaryViewImpl::type& + const typename internal::remove_all::type& nestedExpression() const { return m_matrix; } /** \returns the nested expression */ - typename internal::remove_all::type& + typename internal::remove_reference::type& nestedExpression() { return m_matrix.const_cast_derived(); } protected: - // FIXME changed from MatrixType::Nested because of a weird compilation error with sun CC - typename internal::nested::type m_matrix; + MatrixTypeNested m_matrix; ViewOp m_functor; }; +// Generic API dispatcher +template +class CwiseUnaryViewImpl + : public internal::generic_xpr_base >::type +{ +public: + typedef typename internal::generic_xpr_base >::type Base; +}; + template class CwiseUnaryViewImpl : public internal::dense_xpr_base< CwiseUnaryView >::type @@ -100,38 +109,18 @@ class CwiseUnaryViewImpl EIGEN_DENSE_PUBLIC_INTERFACE(Derived) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl) - inline Scalar* data() { return &coeffRef(0); } - inline const Scalar* data() const { return &coeff(0); } + EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); } - inline Index innerStride() const + EIGEN_DEVICE_FUNC inline Index innerStride() const { return derived().nestedExpression().innerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); } - inline Index outerStride() const + EIGEN_DEVICE_FUNC inline Index outerStride() const { return derived().nestedExpression().outerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); } - - EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const - { - return derived().functor()(derived().nestedExpression().coeff(row, col)); - } - - EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const - { - return derived().functor()(derived().nestedExpression().coeff(index)); - } - - EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) - { - return derived().functor()(const_cast_derived().nestedExpression().coeffRef(row, col)); - } - - EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) - { - return derived().functor()(const_cast_derived().nestedExpression().coeffRef(index)); - } }; } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseBase.h index 4b371b075b..46fe5193c2 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseBase.h @@ -34,37 +34,45 @@ static inline void check_DenseIndex_is_signed() { * \tparam Derived is the derived type, e.g., a matrix type or an expression. * * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_DENSEBASE_PLUGIN. + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_DENSEBASE_PLUGIN. * - * \sa \ref TopicClassHierarchy + * \sa \blank \ref TopicClassHierarchy */ template class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN - : public internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real, - DenseCoeffsBase > -#else : public DenseCoeffsBase +#else + : public DenseCoeffsBase #endif // not EIGEN_PARSED_BY_DOXYGEN { public: - class InnerIterator; + /** Inner iterator type to iterate over the coefficients of a row or column. + * \sa class InnerIterator + */ + typedef Eigen::InnerIterator InnerIterator; typedef typename internal::traits::StorageKind StorageKind; - /** \brief The type of indices - * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. - * \sa \ref TopicPreprocessorDirectives. - */ - typedef typename internal::traits::Index Index; + /** + * \brief The type used to store indices + * \details This typedef is relevant for types that store multiple indices such as + * PermutationMatrix or Transpositions, otherwise it defaults to Eigen::Index + * \sa \blank \ref TopicPreprocessorDirectives, Eigen::Index, SparseMatrixBase. + */ + typedef typename internal::traits::StorageIndex StorageIndex; + /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex, etc. */ typedef typename internal::traits::Scalar Scalar; - typedef typename internal::packet_traits::type PacketScalar; + + /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex, etc. + * + * It is an alias for the Scalar type */ + typedef Scalar value_type; + typedef typename NumTraits::Real RealScalar; - typedef internal::special_scalar_op_base > Base; + typedef DenseCoeffsBase Base; - using Base::operator*; using Base::derived; using Base::const_cast_derived; using Base::rows; @@ -74,16 +82,6 @@ template class DenseBase using Base::colIndexByOuterInner; using Base::coeff; using Base::coeffByOuterInner; - using Base::packet; - using Base::packetByOuterInner; - using Base::writePacket; - using Base::writePacketByOuterInner; - using Base::coeffRef; - using Base::coeffRefByOuterInner; - using Base::copyCoeff; - using Base::copyCoeffByOuterInner; - using Base::copyPacket; - using Base::copyPacketByOuterInner; using Base::operator(); using Base::operator[]; using Base::x; @@ -169,19 +167,46 @@ template class DenseBase InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime) : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime), - CoeffReadCost = internal::traits::CoeffReadCost, - /**< This is a rough measure of how expensive it is to read one coefficient from - * this expression. - */ - InnerStrideAtCompileTime = internal::inner_stride_at_compile_time::ret, OuterStrideAtCompileTime = internal::outer_stride_at_compile_time::ret }; + + typedef typename internal::find_best_packet::type PacketScalar; - enum { ThisConstantIsPrivateInPlainObjectBase }; + enum { IsPlainObjectBase = 0 }; + + /** The plain matrix type corresponding to this expression. + * \sa PlainObject */ + typedef Matrix::Scalar, + internal::traits::RowsAtCompileTime, + internal::traits::ColsAtCompileTime, + AutoAlign | (internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor), + internal::traits::MaxRowsAtCompileTime, + internal::traits::MaxColsAtCompileTime + > PlainMatrix; + + /** The plain array type corresponding to this expression. + * \sa PlainObject */ + typedef Array::Scalar, + internal::traits::RowsAtCompileTime, + internal::traits::ColsAtCompileTime, + AutoAlign | (internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor), + internal::traits::MaxRowsAtCompileTime, + internal::traits::MaxColsAtCompileTime + > PlainArray; + + /** \brief The plain matrix or array type corresponding to this expression. + * + * This is not necessarily exactly the return type of eval(). In the case of plain matrices, + * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed + * that the return type of eval() is either PlainObject or const PlainObject&. + */ + typedef typename internal::conditional::XprKind,MatrixXpr >::value, + PlainMatrix, PlainArray>::type PlainObject; /** \returns the number of nonzero coefficients which is in practice the number * of stored coefficients. */ + EIGEN_DEVICE_FUNC inline Index nonZeros() const { return size(); } /** \returns the outer size. @@ -189,6 +214,7 @@ template class DenseBase * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a * column-major matrix, and the number of rows for a row-major matrix. */ + EIGEN_DEVICE_FUNC Index outerSize() const { return IsVectorAtCompileTime ? 1 @@ -200,6 +226,7 @@ template class DenseBase * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a * column-major matrix, and the number of columns for a row-major matrix. */ + EIGEN_DEVICE_FUNC Index innerSize() const { return IsVectorAtCompileTime ? this->size() @@ -210,6 +237,7 @@ template class DenseBase * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does * nothing else. */ + EIGEN_DEVICE_FUNC void resize(Index newSize) { EIGEN_ONLY_USED_FOR_DEBUG(newSize); @@ -220,22 +248,22 @@ template class DenseBase * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does * nothing else. */ - void resize(Index nbRows, Index nbCols) + EIGEN_DEVICE_FUNC + void resize(Index rows, Index cols) { - EIGEN_ONLY_USED_FOR_DEBUG(nbRows); - EIGEN_ONLY_USED_FOR_DEBUG(nbCols); - eigen_assert(nbRows == this->rows() && nbCols == this->cols() + EIGEN_ONLY_USED_FOR_DEBUG(rows); + EIGEN_ONLY_USED_FOR_DEBUG(cols); + eigen_assert(rows == this->rows() && cols == this->cols() && "DenseBase::resize() does not actually allow to resize."); } #ifndef EIGEN_PARSED_BY_DOXYGEN - /** \internal Represents a matrix with all coefficients equal to one another*/ - typedef CwiseNullaryOp,Derived> ConstantReturnType; - /** \internal Represents a vector with linearly spaced coefficients that allows sequential access only. */ - typedef CwiseNullaryOp,Derived> SequentialLinSpacedReturnType; + typedef CwiseNullaryOp,PlainObject> ConstantReturnType; + /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */ + typedef CwiseNullaryOp,PlainObject> SequentialLinSpacedReturnType; /** \internal Represents a vector with linearly spaced coefficients that allows random access. */ - typedef CwiseNullaryOp,Derived> RandomAccessLinSpacedReturnType; + typedef CwiseNullaryOp,PlainObject> RandomAccessLinSpacedReturnType; /** \internal the return type of MatrixBase::eigenvalues() */ typedef Matrix::Scalar>::Real, internal::traits::ColsAtCompileTime, 1> EigenvaluesReturnType; @@ -243,120 +271,133 @@ template class DenseBase /** Copies \a other into *this. \returns a reference to *this. */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other); /** Special case of the template operator=, in order to prevent the compiler * from generating a default operator= (issue hit with g++ 4.1) */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other); template + EIGEN_DEVICE_FUNC Derived& operator=(const EigenBase &other); template + EIGEN_DEVICE_FUNC Derived& operator+=(const EigenBase &other); template + EIGEN_DEVICE_FUNC Derived& operator-=(const EigenBase &other); template + EIGEN_DEVICE_FUNC Derived& operator=(const ReturnByValue& func); - /** \internal Copies \a other into *this without evaluating other. \returns a reference to *this. */ + /** \ínternal + * Copies \a other into *this without evaluating other. \returns a reference to *this. + * \deprecated */ template + EIGEN_DEVICE_FUNC Derived& lazyAssign(const DenseBase& other); - /** \internal Evaluates \a other into *this. \returns a reference to *this. */ - template - Derived& lazyAssign(const ReturnByValue& other); - + EIGEN_DEVICE_FUNC CommaInitializer operator<< (const Scalar& s); + /** \deprecated it now returns \c *this */ template - const Flagged flagged() const; + EIGEN_DEPRECATED + const Derived& flagged() const + { return derived(); } template + EIGEN_DEVICE_FUNC CommaInitializer operator<< (const DenseBase& other); - Eigen::Transpose transpose(); - typedef typename internal::add_const >::type ConstTransposeReturnType; + typedef Transpose TransposeReturnType; + EIGEN_DEVICE_FUNC + TransposeReturnType transpose(); + typedef typename internal::add_const >::type ConstTransposeReturnType; + EIGEN_DEVICE_FUNC ConstTransposeReturnType transpose() const; + EIGEN_DEVICE_FUNC void transposeInPlace(); -#ifndef EIGEN_NO_DEBUG - protected: - template - void checkTransposeAliasing(const OtherDerived& other) const; - public: -#endif - - static const ConstantReturnType + EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index rows, Index cols, const Scalar& value); - static const ConstantReturnType + EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index size, const Scalar& value); - static const ConstantReturnType + EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(const Scalar& value); - static const SequentialLinSpacedReturnType + EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high); - static const RandomAccessLinSpacedReturnType + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Index size, const Scalar& low, const Scalar& high); - static const SequentialLinSpacedReturnType + EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType LinSpaced(Sequential_t, const Scalar& low, const Scalar& high); - static const RandomAccessLinSpacedReturnType + EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(const Scalar& low, const Scalar& high); - template - static const CwiseNullaryOp + template EIGEN_DEVICE_FUNC + static const CwiseNullaryOp NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func); - template - static const CwiseNullaryOp + template EIGEN_DEVICE_FUNC + static const CwiseNullaryOp NullaryExpr(Index size, const CustomNullaryOp& func); - template - static const CwiseNullaryOp + template EIGEN_DEVICE_FUNC + static const CwiseNullaryOp NullaryExpr(const CustomNullaryOp& func); - static const ConstantReturnType Zero(Index rows, Index cols); - static const ConstantReturnType Zero(Index size); - static const ConstantReturnType Zero(); - static const ConstantReturnType Ones(Index rows, Index cols); - static const ConstantReturnType Ones(Index size); - static const ConstantReturnType Ones(); - - void fill(const Scalar& value); - Derived& setConstant(const Scalar& value); - Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high); - Derived& setLinSpaced(const Scalar& low, const Scalar& high); - Derived& setZero(); - Derived& setOnes(); - Derived& setRandom(); - - template + EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size); + EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(); + EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size); + EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(); + + EIGEN_DEVICE_FUNC void fill(const Scalar& value); + EIGEN_DEVICE_FUNC Derived& setConstant(const Scalar& value); + EIGEN_DEVICE_FUNC Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high); + EIGEN_DEVICE_FUNC Derived& setLinSpaced(const Scalar& low, const Scalar& high); + EIGEN_DEVICE_FUNC Derived& setZero(); + EIGEN_DEVICE_FUNC Derived& setOnes(); + EIGEN_DEVICE_FUNC Derived& setRandom(); + + template EIGEN_DEVICE_FUNC bool isApprox(const DenseBase& other, const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isMuchSmallerThan(const RealScalar& other, const RealScalar& prec = NumTraits::dummy_precision()) const; - template + template EIGEN_DEVICE_FUNC bool isMuchSmallerThan(const DenseBase& other, const RealScalar& prec = NumTraits::dummy_precision()) const; - bool isApproxToConstant(const Scalar& value, const RealScalar& prec = NumTraits::dummy_precision()) const; - bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits::dummy_precision()) const; - bool isZero(const RealScalar& prec = NumTraits::dummy_precision()) const; - bool isOnes(const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isApproxToConstant(const Scalar& value, const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits::dummy_precision()) const; + EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits::dummy_precision()) const; inline bool hasNaN() const; inline bool allFinite() const; - inline Derived& operator*=(const Scalar& other); - inline Derived& operator/=(const Scalar& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator*=(const Scalar& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator/=(const Scalar& other); typedef typename internal::add_const_on_value_type::type>::type EvalReturnType; /** \returns the matrix or vector obtained by evaluating this expression. * * Notice that in the case of a plain matrix or vector (not an expression) this function just returns * a const reference, in order to avoid a useless copy. + * + * \warning Be carefull with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page \endlink. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EvalReturnType eval() const { // Even though MSVC does not honor strong inlining when the return type @@ -364,61 +405,78 @@ template class DenseBase // size types on MSVC. return typename internal::eval::type(derived()); } - + /** swaps *this with the expression \a other. * */ template - void swap(const DenseBase& other, - int = OtherDerived::ThisConstantIsPrivateInPlainObjectBase) + EIGEN_DEVICE_FUNC + void swap(const DenseBase& other) { - SwapWrapper(derived()).lazyAssign(other.derived()); + EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + eigen_assert(rows()==other.rows() && cols()==other.cols()); + call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op()); } /** swaps *this with the matrix or array \a other. * */ template + EIGEN_DEVICE_FUNC void swap(PlainObjectBase& other) { - SwapWrapper(derived()).lazyAssign(other.derived()); + eigen_assert(rows()==other.rows() && cols()==other.cols()); + call_assignment(derived(), other.derived(), internal::swap_assign_op()); } + EIGEN_DEVICE_FUNC inline const NestByValue nestByValue() const; + EIGEN_DEVICE_FUNC inline const ForceAlignedAccess forceAlignedAccess() const; + EIGEN_DEVICE_FUNC inline ForceAlignedAccess forceAlignedAccess(); + template EIGEN_DEVICE_FUNC + inline const typename internal::conditional,Derived&>::type forceAlignedAccessIf() const; + template EIGEN_DEVICE_FUNC + inline typename internal::conditional,Derived&>::type forceAlignedAccessIf(); - inline const NestByValue nestByValue() const; - inline const ForceAlignedAccess forceAlignedAccess() const; - inline ForceAlignedAccess forceAlignedAccess(); - template inline const typename internal::conditional,Derived&>::type forceAlignedAccessIf() const; - template inline typename internal::conditional,Derived&>::type forceAlignedAccessIf(); - - Scalar sum() const; - Scalar mean() const; - Scalar trace() const; + EIGEN_DEVICE_FUNC Scalar sum() const; + EIGEN_DEVICE_FUNC Scalar mean() const; + EIGEN_DEVICE_FUNC Scalar trace() const; - Scalar prod() const; + EIGEN_DEVICE_FUNC Scalar prod() const; - typename internal::traits::Scalar minCoeff() const; - typename internal::traits::Scalar maxCoeff() const; + EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff() const; + EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff() const; - template + template EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff(IndexType* row, IndexType* col) const; - template + template EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff(IndexType* row, IndexType* col) const; - template + template EIGEN_DEVICE_FUNC typename internal::traits::Scalar minCoeff(IndexType* index) const; - template + template EIGEN_DEVICE_FUNC typename internal::traits::Scalar maxCoeff(IndexType* index) const; template - typename internal::result_of::Scalar)>::type - redux(const BinaryOp& func) const; + EIGEN_DEVICE_FUNC + Scalar redux(const BinaryOp& func) const; template + EIGEN_DEVICE_FUNC void visit(Visitor& func) const; - inline const WithFormat format(const IOFormat& fmt) const; + /** \returns a WithFormat proxy object allowing to print a matrix the with given + * format \a fmt. + * + * See class IOFormat for some examples. + * + * \sa class IOFormat, class WithFormat + */ + inline const WithFormat format(const IOFormat& fmt) const + { + return WithFormat(derived(), fmt); + } /** \returns the unique coefficient of a 1x1 expression */ + EIGEN_DEVICE_FUNC CoeffReturnType value() const { EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) @@ -426,8 +484,8 @@ template class DenseBase return derived().coeff(0,0); } - bool all(void) const; - bool any(void) const; + bool all() const; + bool any() const; Index count() const; typedef VectorwiseOp RowwiseReturnType; @@ -435,14 +493,35 @@ template class DenseBase typedef VectorwiseOp ColwiseReturnType; typedef const VectorwiseOp ConstColwiseReturnType; - ConstRowwiseReturnType rowwise() const; - RowwiseReturnType rowwise(); - ConstColwiseReturnType colwise() const; - ColwiseReturnType colwise(); + /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations + * + * Example: \include MatrixBase_rowwise.cpp + * Output: \verbinclude MatrixBase_rowwise.out + * + * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting + */ + //Code moved here due to a CUDA compiler bug + EIGEN_DEVICE_FUNC inline ConstRowwiseReturnType rowwise() const { + return ConstRowwiseReturnType(derived()); + } + EIGEN_DEVICE_FUNC RowwiseReturnType rowwise(); + + /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations + * + * Example: \include MatrixBase_colwise.cpp + * Output: \verbinclude MatrixBase_colwise.out + * + * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting + */ + EIGEN_DEVICE_FUNC inline ConstColwiseReturnType colwise() const { + return ConstColwiseReturnType(derived()); + } + EIGEN_DEVICE_FUNC ColwiseReturnType colwise(); - static const CwiseNullaryOp,Derived> Random(Index rows, Index cols); - static const CwiseNullaryOp,Derived> Random(Index size); - static const CwiseNullaryOp,Derived> Random(); + typedef CwiseNullaryOp,PlainObject> RandomReturnType; + static const RandomReturnType Random(Index rows, Index cols); + static const RandomReturnType Random(Index size); + static const RandomReturnType Random(); template const Select @@ -460,45 +539,56 @@ template class DenseBase template RealScalar lpNorm() const; template - inline const Replicate replicate() const; - - typedef Replicate ReplicateReturnType; - inline const ReplicateReturnType replicate(Index rowFacor,Index colFactor) const; + EIGEN_DEVICE_FUNC + const Replicate replicate() const; + /** + * \return an expression of the replication of \c *this + * + * Example: \include MatrixBase_replicate_int_int.cpp + * Output: \verbinclude MatrixBase_replicate_int_int.out + * + * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate + */ + //Code moved here due to a CUDA compiler bug + EIGEN_DEVICE_FUNC + const Replicate replicate(Index rowFactor, Index colFactor) const + { + return Replicate(derived(), rowFactor, colFactor); + } typedef Reverse ReverseReturnType; typedef const Reverse ConstReverseReturnType; - ReverseReturnType reverse(); - ConstReverseReturnType reverse() const; - void reverseInPlace(); + EIGEN_DEVICE_FUNC ReverseReturnType reverse(); + /** This is the const version of reverse(). */ + //Code moved here due to a CUDA compiler bug + EIGEN_DEVICE_FUNC ConstReverseReturnType reverse() const + { + return ConstReverseReturnType(derived()); + } + EIGEN_DEVICE_FUNC void reverseInPlace(); #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase +#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL +#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) # include "../plugins/BlockMethods.h" # ifdef EIGEN_DENSEBASE_PLUGIN # include EIGEN_DENSEBASE_PLUGIN # endif #undef EIGEN_CURRENT_STORAGE_BASE_CLASS - -#ifdef EIGEN2_SUPPORT - - Block corner(CornerType type, Index cRows, Index cCols); - const Block corner(CornerType type, Index cRows, Index cCols) const; - template - Block corner(CornerType type); - template - const Block corner(CornerType type) const; - -#endif // EIGEN2_SUPPORT - +#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL +#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF // disable the use of evalTo for dense objects with a nice compilation error - template inline void evalTo(Dest& ) const + template + EIGEN_DEVICE_FUNC + inline void evalTo(Dest& ) const { EIGEN_STATIC_ASSERT((internal::is_same::value),THE_EVAL_EVALTO_FUNCTION_SHOULD_NEVER_BE_CALLED_FOR_DENSE_OBJECTS); } protected: /** Default constructor. Do nothing. */ - DenseBase() + EIGEN_DEVICE_FUNC DenseBase() { /* Just checks for self-consistency of the flags. * Only do it when debugging Eigen, as this borders on paranoiac and could slow compilation down @@ -511,9 +601,9 @@ template class DenseBase } private: - explicit DenseBase(int); - DenseBase(int,int); - template explicit DenseBase(const DenseBase&); + EIGEN_DEVICE_FUNC explicit DenseBase(int); + EIGEN_DEVICE_FUNC DenseBase(int,int); + template EIGEN_DEVICE_FUNC explicit DenseBase(const DenseBase&); }; } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseCoeffsBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseCoeffsBase.h index 3c890f2159..c4af48ab69 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseCoeffsBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseCoeffsBase.h @@ -35,7 +35,6 @@ class DenseCoeffsBase : public EigenBase { public: typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; @@ -61,6 +60,7 @@ class DenseCoeffsBase : public EigenBase using Base::size; using Base::derived; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) const { return int(Derived::RowsAtCompileTime) == 1 ? 0 @@ -69,6 +69,7 @@ class DenseCoeffsBase : public EigenBase : inner; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) const { return int(Derived::ColsAtCompileTime) == 1 ? 0 @@ -91,13 +92,15 @@ class DenseCoeffsBase : public EigenBase * * \sa operator()(Index,Index) const, coeffRef(Index,Index), coeff(Index) const */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { eigen_internal_assert(row >= 0 && row < rows() - && col >= 0 && col < cols()); - return derived().coeff(row, col); + && col >= 0 && col < cols()); + return internal::evaluator(derived()).coeff(row,col); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffByOuterInner(Index outer, Index inner) const { return coeff(rowIndexByOuterInner(outer, inner), @@ -108,11 +111,12 @@ class DenseCoeffsBase : public EigenBase * * \sa operator()(Index,Index), operator[](Index) */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator()(Index row, Index col) const { eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); - return derived().coeff(row, col); + return coeff(row, col); } /** Short version: don't use this function, use @@ -130,11 +134,14 @@ class DenseCoeffsBase : public EigenBase * \sa operator[](Index) const, coeffRef(Index), coeff(Index,Index) const */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + EIGEN_STATIC_ASSERT(internal::evaluator::Flags & LinearAccessBit, + THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) eigen_internal_assert(index >= 0 && index < size()); - return derived().coeff(index); + return internal::evaluator(derived()).coeff(index); } @@ -146,15 +153,14 @@ class DenseCoeffsBase : public EigenBase * z() const, w() const */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator[](Index index) const { - #ifndef EIGEN2_SUPPORT EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime, THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD) - #endif eigen_assert(index >= 0 && index < size()); - return derived().coeff(index); + return coeff(index); } /** \returns the coefficient at given index. @@ -167,32 +173,49 @@ class DenseCoeffsBase : public EigenBase * z() const, w() const */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType operator()(Index index) const { eigen_assert(index >= 0 && index < size()); - return derived().coeff(index); + return coeff(index); } /** equivalent to operator[](0). */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType x() const { return (*this)[0]; } /** equivalent to operator[](1). */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType - y() const { return (*this)[1]; } + y() const + { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=2, OUT_OF_RANGE_ACCESS); + return (*this)[1]; + } /** equivalent to operator[](2). */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType - z() const { return (*this)[2]; } + z() const + { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=3, OUT_OF_RANGE_ACCESS); + return (*this)[2]; + } /** equivalent to operator[](3). */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType - w() const { return (*this)[3]; } + w() const + { + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=4, OUT_OF_RANGE_ACCESS); + return (*this)[3]; + } /** \internal * \returns the packet of coefficients starting at the given row and column. It is your responsibility @@ -207,9 +230,9 @@ class DenseCoeffsBase : public EigenBase template EIGEN_STRONG_INLINE PacketReturnType packet(Index row, Index col) const { - eigen_internal_assert(row >= 0 && row < rows() - && col >= 0 && col < cols()); - return derived().template packet(row,col); + typedef typename internal::packet_traits::type DefaultPacketType; + eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return internal::evaluator(derived()).template packet(row,col); } @@ -234,8 +257,11 @@ class DenseCoeffsBase : public EigenBase template EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const { + EIGEN_STATIC_ASSERT(internal::evaluator::Flags & LinearAccessBit, + THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) + typedef typename internal::packet_traits::type DefaultPacketType; eigen_internal_assert(index >= 0 && index < size()); - return derived().template packet(index); + return internal::evaluator(derived()).template packet(index); } protected: @@ -278,7 +304,6 @@ class DenseCoeffsBase : public DenseCoeffsBase Base; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; @@ -311,13 +336,15 @@ class DenseCoeffsBase : public DenseCoeffsBase= 0 && row < rows() - && col >= 0 && col < cols()); - return derived().coeffRef(row, col); + && col >= 0 && col < cols()); + return internal::evaluator(derived()).coeffRef(row,col); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRefByOuterInner(Index outer, Index inner) { @@ -330,12 +357,13 @@ class DenseCoeffsBase : public DenseCoeffsBase= 0 && row < rows() && col >= 0 && col < cols()); - return derived().coeffRef(row, col); + return coeffRef(row, col); } @@ -354,11 +382,14 @@ class DenseCoeffsBase : public DenseCoeffsBase::Flags & LinearAccessBit, + THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) eigen_internal_assert(index >= 0 && index < size()); - return derived().coeffRef(index); + return internal::evaluator(derived()).coeffRef(index); } /** \returns a reference to the coefficient at given index. @@ -368,15 +399,14 @@ class DenseCoeffsBase : public DenseCoeffsBase= 0 && index < size()); - return derived().coeffRef(index); + return coeffRef(index); } /** \returns a reference to the coefficient at given index. @@ -388,167 +418,49 @@ class DenseCoeffsBase : public DenseCoeffsBase= 0 && index < size()); - return derived().coeffRef(index); + return coeffRef(index); } /** equivalent to operator[](0). */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& x() { return (*this)[0]; } /** equivalent to operator[](1). */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& - y() { return (*this)[1]; } - - /** equivalent to operator[](2). */ - - EIGEN_STRONG_INLINE Scalar& - z() { return (*this)[2]; } - - /** equivalent to operator[](3). */ - - EIGEN_STRONG_INLINE Scalar& - w() { return (*this)[3]; } - - /** \internal - * Stores the given packet of coefficients, at the given row and column of this expression. It is your responsibility - * to ensure that a packet really starts there. This method is only available on expressions having the - * PacketAccessBit. - * - * The \a LoadMode parameter may have the value \a #Aligned or \a #Unaligned. Its effect is to select - * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets - * starting at an address which is a multiple of the packet size. - */ - - template - EIGEN_STRONG_INLINE void writePacket - (Index row, Index col, const typename internal::packet_traits::type& val) + y() { - eigen_internal_assert(row >= 0 && row < rows() - && col >= 0 && col < cols()); - derived().template writePacket(row,col,val); + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=2, OUT_OF_RANGE_ACCESS); + return (*this)[1]; } + /** equivalent to operator[](2). */ - /** \internal */ - template - EIGEN_STRONG_INLINE void writePacketByOuterInner - (Index outer, Index inner, const typename internal::packet_traits::type& val) - { - writePacket(rowIndexByOuterInner(outer, inner), - colIndexByOuterInner(outer, inner), - val); - } - - /** \internal - * Stores the given packet of coefficients, at the given index in this expression. It is your responsibility - * to ensure that a packet really starts there. This method is only available on expressions having the - * PacketAccessBit and the LinearAccessBit. - * - * The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select - * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets - * starting at an address which is a multiple of the packet size. - */ - template - EIGEN_STRONG_INLINE void writePacket - (Index index, const typename internal::packet_traits::type& val) - { - eigen_internal_assert(index >= 0 && index < size()); - derived().template writePacket(index,val); - } - -#ifndef EIGEN_PARSED_BY_DOXYGEN - - /** \internal Copies the coefficient at position (row,col) of other into *this. - * - * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code - * with usual assignments. - * - * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox. - */ - - template - EIGEN_STRONG_INLINE void copyCoeff(Index row, Index col, const DenseBase& other) - { - eigen_internal_assert(row >= 0 && row < rows() - && col >= 0 && col < cols()); - derived().coeffRef(row, col) = other.derived().coeff(row, col); - } - - /** \internal Copies the coefficient at the given index of other into *this. - * - * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code - * with usual assignments. - * - * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox. - */ - - template - EIGEN_STRONG_INLINE void copyCoeff(Index index, const DenseBase& other) - { - eigen_internal_assert(index >= 0 && index < size()); - derived().coeffRef(index) = other.derived().coeff(index); - } - - - template - EIGEN_STRONG_INLINE void copyCoeffByOuterInner(Index outer, Index inner, const DenseBase& other) - { - const Index row = rowIndexByOuterInner(outer,inner); - const Index col = colIndexByOuterInner(outer,inner); - // derived() is important here: copyCoeff() may be reimplemented in Derived! - derived().copyCoeff(row, col, other); - } - - /** \internal Copies the packet at position (row,col) of other into *this. - * - * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code - * with usual assignments. - * - * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox. - */ - - template - EIGEN_STRONG_INLINE void copyPacket(Index row, Index col, const DenseBase& other) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& + z() { - eigen_internal_assert(row >= 0 && row < rows() - && col >= 0 && col < cols()); - derived().template writePacket(row, col, - other.derived().template packet(row, col)); + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=3, OUT_OF_RANGE_ACCESS); + return (*this)[2]; } - /** \internal Copies the packet at the given index of other into *this. - * - * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code - * with usual assignments. - * - * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox. - */ - - template - EIGEN_STRONG_INLINE void copyPacket(Index index, const DenseBase& other) - { - eigen_internal_assert(index >= 0 && index < size()); - derived().template writePacket(index, - other.derived().template packet(index)); - } + /** equivalent to operator[](3). */ - /** \internal */ - template - EIGEN_STRONG_INLINE void copyPacketByOuterInner(Index outer, Index inner, const DenseBase& other) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& + w() { - const Index row = rowIndexByOuterInner(outer,inner); - const Index col = colIndexByOuterInner(outer,inner); - // derived() is important here: copyCoeff() may be reimplemented in Derived! - derived().template copyPacket< OtherDerived, StoreMode, LoadMode>(row, col, other); + EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime==-1 || Derived::SizeAtCompileTime>=4, OUT_OF_RANGE_ACCESS); + return (*this)[3]; } -#endif - }; /** \brief Base class providing direct read-only coefficient access to matrices and arrays. @@ -560,7 +472,7 @@ class DenseCoeffsBase : public DenseCoeffsBase which defines functions to access entries read-only using * \c operator() . * - * \sa \ref TopicClassHierarchy + * \sa \blank \ref TopicClassHierarchy */ template class DenseCoeffsBase : public DenseCoeffsBase @@ -568,7 +480,6 @@ class DenseCoeffsBase : public DenseCoeffsBase Base; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; typedef typename NumTraits::Real RealScalar; @@ -581,6 +492,7 @@ class DenseCoeffsBase : public DenseCoeffsBase : public DenseCoeffsBase : public DenseCoeffsBase : public DenseCoeffsBase : public DenseCoeffsBase which defines functions to access entries read/write using * \c operator(). * - * \sa \ref TopicClassHierarchy + * \sa \blank \ref TopicClassHierarchy */ template class DenseCoeffsBase @@ -639,7 +554,6 @@ class DenseCoeffsBase public: typedef DenseCoeffsBase Base; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; typedef typename NumTraits::Real RealScalar; @@ -652,6 +566,7 @@ class DenseCoeffsBase * * \sa outerStride(), rowStride(), colStride() */ + EIGEN_DEVICE_FUNC inline Index innerStride() const { return derived().innerStride(); @@ -662,6 +577,7 @@ class DenseCoeffsBase * * \sa innerStride(), rowStride(), colStride() */ + EIGEN_DEVICE_FUNC inline Index outerStride() const { return derived().outerStride(); @@ -677,6 +593,7 @@ class DenseCoeffsBase * * \sa innerStride(), outerStride(), colStride() */ + EIGEN_DEVICE_FUNC inline Index rowStride() const { return Derived::IsRowMajor ? outerStride() : innerStride(); @@ -686,6 +603,7 @@ class DenseCoeffsBase * * \sa innerStride(), outerStride(), rowStride() */ + EIGEN_DEVICE_FUNC inline Index colStride() const { return Derived::IsRowMajor ? innerStride() : outerStride(); @@ -694,33 +612,42 @@ class DenseCoeffsBase namespace internal { -template +template struct first_aligned_impl { - static inline typename Derived::Index run(const Derived&) + static inline Index run(const Derived&) { return 0; } }; -template -struct first_aligned_impl +template +struct first_aligned_impl { - static inline typename Derived::Index run(const Derived& m) + static inline Index run(const Derived& m) { - return internal::first_aligned(&m.const_cast_derived().coeffRef(0,0), m.size()); + return internal::first_aligned(m.data(), m.size()); } }; -/** \internal \returns the index of the first element of the array that is well aligned for vectorization. +/** \internal \returns the index of the first element of the array stored by \a m that is properly aligned with respect to \a Alignment for vectorization. + * + * \tparam Alignment requested alignment in Bytes. * * There is also the variant first_aligned(const Scalar*, Integer) defined in Memory.h. See it for more * documentation. */ +template +static inline Index first_aligned(const DenseBase& m) +{ + enum { ReturnZero = (int(evaluator::Alignment) >= Alignment) || !(Derived::Flags & DirectAccessBit) }; + return first_aligned_impl::run(m.derived()); +} + template -static inline typename Derived::Index first_aligned(const Derived& m) +static inline Index first_default_aligned(const DenseBase& m) { - return first_aligned_impl - - ::run(m); + typedef typename Derived::Scalar Scalar; + typedef typename packet_traits::type DefaultPacketType; + return internal::first_aligned::alignment),Derived>(m); } template::ret> diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseStorage.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseStorage.h index 568493cbae..7958feeb9c 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseStorage.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/DenseStorage.h @@ -3,7 +3,7 @@ // // Copyright (C) 2008 Gael Guennebaud // Copyright (C) 2006-2009 Benoit Jacob -// Copyright (C) 2010 Hauke Heibel +// Copyright (C) 2010-2013 Hauke Heibel // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -13,9 +13,9 @@ #define EIGEN_MATRIXSTORAGE_H #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN - #define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN EIGEN_DENSE_STORAGE_CTOR_PLUGIN; + #define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X) X; EIGEN_DENSE_STORAGE_CTOR_PLUGIN; #else - #define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN + #define EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(X) #endif namespace Eigen { @@ -24,7 +24,9 @@ namespace internal { struct constructor_without_unaligned_array_assert {}; -template void check_static_allocation_size() +template +EIGEN_DEVICE_FUNC +void check_static_allocation_size() { // if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit #if EIGEN_STACK_ALLOCATION_LIMIT @@ -38,18 +40,19 @@ template void check_static_allocation_size() */ template + : compute_default_alignment::value > struct plain_array { T array[Size]; - plain_array() + EIGEN_DEVICE_FUNC + plain_array() { check_static_allocation_size(); } - plain_array(constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC + plain_array(constructor_without_unaligned_array_assert) { check_static_allocation_size(); } @@ -64,29 +67,88 @@ struct plain_array template EIGEN_ALWAYS_INLINE PtrType eigen_unaligned_array_assert_workaround_gcc47(PtrType array) { return array; } #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ - eigen_assert((reinterpret_cast(eigen_unaligned_array_assert_workaround_gcc47(array)) & sizemask) == 0 \ + eigen_assert((internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (sizemask)) == 0 \ && "this assertion is explained here: " \ "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \ " **** READ THIS WEB PAGE !!! ****"); #else #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ - eigen_assert((reinterpret_cast(array) & sizemask) == 0 \ + eigen_assert((internal::UIntPtr(array) & (sizemask)) == 0 \ && "this assertion is explained here: " \ "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \ " **** READ THIS WEB PAGE !!! ****"); #endif +template +struct plain_array +{ + EIGEN_ALIGN_TO_BOUNDARY(8) T array[Size]; + + EIGEN_DEVICE_FUNC + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(7); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + plain_array(constructor_without_unaligned_array_assert) + { + check_static_allocation_size(); + } +}; + template struct plain_array { - EIGEN_USER_ALIGN16 T array[Size]; + EIGEN_ALIGN_TO_BOUNDARY(16) T array[Size]; + EIGEN_DEVICE_FUNC plain_array() { - EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf); + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(15); check_static_allocation_size(); } + EIGEN_DEVICE_FUNC + plain_array(constructor_without_unaligned_array_assert) + { + check_static_allocation_size(); + } +}; + +template +struct plain_array +{ + EIGEN_ALIGN_TO_BOUNDARY(32) T array[Size]; + + EIGEN_DEVICE_FUNC + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(31); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC + plain_array(constructor_without_unaligned_array_assert) + { + check_static_allocation_size(); + } +}; + +template +struct plain_array +{ + EIGEN_ALIGN_TO_BOUNDARY(64) T array[Size]; + + EIGEN_DEVICE_FUNC + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(63); + check_static_allocation_size(); + } + + EIGEN_DEVICE_FUNC plain_array(constructor_without_unaligned_array_assert) { check_static_allocation_size(); @@ -96,9 +158,9 @@ struct plain_array template struct plain_array { - EIGEN_USER_ALIGN16 T array[1]; - plain_array() {} - plain_array(constructor_without_unaligned_array_assert) {} + T array[1]; + EIGEN_DEVICE_FUNC plain_array() {} + EIGEN_DEVICE_FUNC plain_array(constructor_without_unaligned_array_assert) {} }; } // end namespace internal @@ -122,41 +184,54 @@ template class DenseSt { internal::plain_array m_data; public: - DenseStorage() {} - DenseStorage(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC DenseStorage() { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size) + } + EIGEN_DEVICE_FUNC + explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()) {} - DenseStorage(const DenseStorage& other) : m_data(other.m_data) {} + EIGEN_DEVICE_FUNC + DenseStorage(const DenseStorage& other) : m_data(other.m_data) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size) + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) - { + { if (this != &other) m_data = other.m_data; - return *this; + return *this; } - DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} - void swap(DenseStorage& other) { std::swap(m_data,other.m_data); } - static DenseIndex rows(void) {return _Rows;} - static DenseIndex cols(void) {return _Cols;} - void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} - void resize(DenseIndex,DenseIndex,DenseIndex) {} - const T *data() const { return m_data.array; } - T *data() { return m_data.array; } + EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size==rows*cols && rows==_Rows && cols==_Cols); + EIGEN_UNUSED_VARIABLE(size); + EIGEN_UNUSED_VARIABLE(rows); + EIGEN_UNUSED_VARIABLE(cols); + } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); } + EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;} + EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC T *data() { return m_data.array; } }; // null matrix template class DenseStorage { public: - DenseStorage() {} - DenseStorage(internal::constructor_without_unaligned_array_assert) {} - DenseStorage(const DenseStorage&) {} - DenseStorage& operator=(const DenseStorage&) { return *this; } - DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} - void swap(DenseStorage& ) {} - static DenseIndex rows(void) {return _Rows;} - static DenseIndex cols(void) {return _Cols;} - void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} - void resize(DenseIndex,DenseIndex,DenseIndex) {} - const T *data() const { return 0; } - T *data() { return 0; } + EIGEN_DEVICE_FUNC DenseStorage() {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) {} + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage&) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage&) { return *this; } + EIGEN_DEVICE_FUNC DenseStorage(Index,Index,Index) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& ) {} + EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;} + EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {} + EIGEN_DEVICE_FUNC const T *data() const { return 0; } + EIGEN_DEVICE_FUNC T *data() { return 0; } }; // more specializations for null matrices; these are necessary to resolve ambiguities @@ -173,74 +248,74 @@ template class DenseStorage class DenseStorage { internal::plain_array m_data; - DenseIndex m_rows; - DenseIndex m_cols; + Index m_rows; + Index m_cols; public: - DenseStorage() : m_rows(0), m_cols(0) {} - DenseStorage(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {} - DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows), m_cols(other.m_cols) {} - DenseStorage& operator=(const DenseStorage& other) - { + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows), m_cols(other.m_cols) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { if (this != &other) { m_data = other.m_data; m_rows = other.m_rows; m_cols = other.m_cols; } - return *this; + return *this; } - DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) : m_rows(nbRows), m_cols(nbCols) {} - void swap(DenseStorage& other) + EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } - DenseIndex rows() const {return m_rows;} - DenseIndex cols() const {return m_cols;} - void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) { m_rows = nbRows; m_cols = nbCols; } - void resize(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) { m_rows = nbRows; m_cols = nbCols; } - const T *data() const { return m_data.array; } - T *data() { return m_data.array; } + EIGEN_DEVICE_FUNC Index rows() const {return m_rows;} + EIGEN_DEVICE_FUNC Index cols() const {return m_cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; } + EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; } + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC T *data() { return m_data.array; } }; // dynamic-size matrix with fixed-size storage and fixed width template class DenseStorage { internal::plain_array m_data; - DenseIndex m_rows; + Index m_rows; public: - DenseStorage() : m_rows(0) {} - DenseStorage(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {} - DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {} - DenseStorage& operator=(const DenseStorage& other) + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { if (this != &other) { m_data = other.m_data; m_rows = other.m_rows; } - return *this; + return *this; } - DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {} - void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } - DenseIndex rows(void) const {return m_rows;} - DenseIndex cols(void) const {return _Cols;} - void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } - void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } - const T *data() const { return m_data.array; } - T *data() { return m_data.array; } + EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index) : m_rows(rows) {} + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {return _Cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index) { m_rows = rows; } + EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index) { m_rows = rows; } + EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; } + EIGEN_DEVICE_FUNC T *data() { return m_data.array; } }; // dynamic-size matrix with fixed-size storage and fixed height template class DenseStorage { internal::plain_array m_data; - DenseIndex m_cols; + Index m_cols; public: - DenseStorage() : m_cols(0) {} - DenseStorage(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC DenseStorage() : m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {} - DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {} - DenseStorage& operator=(const DenseStorage& other) + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {} + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) { if (this != &other) { @@ -249,38 +324,62 @@ template class DenseStorage class DenseStorage { T *m_data; - DenseIndex m_rows; - DenseIndex m_cols; + Index m_rows; + Index m_cols; public: - DenseStorage() : m_data(0), m_rows(0), m_cols(0) {} - DenseStorage(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC DenseStorage() : m_data(0), m_rows(0), m_cols(0) {} + EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0), m_cols(0) {} - DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) - : m_data(internal::conditional_aligned_new_auto(size)), m_rows(nbRows), m_cols(nbCols) - { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } -#ifdef EIGEN_HAVE_RVALUE_REFERENCES - DenseStorage(DenseStorage&& other) + EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) + : m_data(internal::conditional_aligned_new_auto(size)), m_rows(rows), m_cols(cols) + { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size==rows*cols && rows>=0 && cols >=0); + } + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) + : m_data(internal::conditional_aligned_new_auto(other.m_rows*other.m_cols)) + , m_rows(other.m_rows) + , m_cols(other.m_cols) + { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows*m_cols) + internal::smart_copy(other.m_data, other.m_data+other.m_rows*other.m_cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + DenseStorage tmp(other); + this->swap(tmp); + } + return *this; + } +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)) , m_rows(std::move(other.m_rows)) , m_cols(std::move(other.m_cols)) { other.m_data = nullptr; + other.m_rows = 0; + other.m_cols = 0; } - DenseStorage& operator=(DenseStorage&& other) + EIGEN_DEVICE_FUNC + DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT { using std::swap; swap(m_data, other.m_data); @@ -289,18 +388,18 @@ template class DenseStorage(m_data, m_rows*m_cols); } - void swap(DenseStorage& other) + EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } - DenseIndex rows(void) const {return m_rows;} - DenseIndex cols(void) const {return m_cols;} - void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) + EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {return m_cols;} + void conservativeResize(Index size, Index rows, Index cols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*m_cols); - m_rows = nbRows; - m_cols = nbCols; + m_rows = rows; + m_cols = cols; } - void resize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) + EIGEN_DEVICE_FUNC void resize(Index size, Index rows, Index cols) { if(size != m_rows*m_cols) { @@ -309,36 +408,56 @@ template class DenseStorage(size); else m_data = 0; - EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) } - m_rows = nbRows; - m_cols = nbCols; + m_rows = rows; + m_cols = cols; } - const T *data() const { return m_data; } - T *data() { return m_data; } - private: - DenseStorage(const DenseStorage&); - DenseStorage& operator=(const DenseStorage&); + EIGEN_DEVICE_FUNC const T *data() const { return m_data; } + EIGEN_DEVICE_FUNC T *data() { return m_data; } }; // matrix with dynamic width and fixed height (so that matrix has dynamic size). template class DenseStorage { T *m_data; - DenseIndex m_cols; + Index m_cols; public: - DenseStorage() : m_data(0), m_cols(0) {} - DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {} - DenseStorage(DenseIndex size, DenseIndex, DenseIndex nbCols) : m_data(internal::conditional_aligned_new_auto(size)), m_cols(nbCols) - { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } -#ifdef EIGEN_HAVE_RVALUE_REFERENCES - DenseStorage(DenseStorage&& other) + EIGEN_DEVICE_FUNC DenseStorage() : m_data(0), m_cols(0) {} + explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {} + EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) : m_data(internal::conditional_aligned_new_auto(size)), m_cols(cols) + { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size==rows*cols && rows==_Rows && cols >=0); + EIGEN_UNUSED_VARIABLE(rows); + } + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) + : m_data(internal::conditional_aligned_new_auto(_Rows*other.m_cols)) + , m_cols(other.m_cols) + { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_cols*_Rows) + internal::smart_copy(other.m_data, other.m_data+_Rows*m_cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + DenseStorage tmp(other); + this->swap(tmp); + } + return *this; + } +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)) , m_cols(std::move(other.m_cols)) { other.m_data = nullptr; + other.m_cols = 0; } - DenseStorage& operator=(DenseStorage&& other) + EIGEN_DEVICE_FUNC + DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT { using std::swap; swap(m_data, other.m_data); @@ -346,16 +465,16 @@ template class DenseStorage(m_data, _Rows*m_cols); } - void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } - static DenseIndex rows(void) {return _Rows;} - DenseIndex cols(void) const {return m_cols;} - void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols) + EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;} + EIGEN_DEVICE_FUNC Index cols(void) const {return m_cols;} + EIGEN_DEVICE_FUNC void conservativeResize(Index size, Index, Index cols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, _Rows*m_cols); - m_cols = nbCols; + m_cols = cols; } - EIGEN_STRONG_INLINE void resize(DenseIndex size, DenseIndex, DenseIndex nbCols) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index, Index cols) { if(size != _Rows*m_cols) { @@ -364,35 +483,55 @@ template class DenseStorage(size); else m_data = 0; - EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) } - m_cols = nbCols; + m_cols = cols; } - const T *data() const { return m_data; } - T *data() { return m_data; } - private: - DenseStorage(const DenseStorage&); - DenseStorage& operator=(const DenseStorage&); + EIGEN_DEVICE_FUNC const T *data() const { return m_data; } + EIGEN_DEVICE_FUNC T *data() { return m_data; } }; // matrix with dynamic height and fixed width (so that matrix has dynamic size). template class DenseStorage { T *m_data; - DenseIndex m_rows; + Index m_rows; public: - DenseStorage() : m_data(0), m_rows(0) {} - DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {} - DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex) : m_data(internal::conditional_aligned_new_auto(size)), m_rows(nbRows) - { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } -#ifdef EIGEN_HAVE_RVALUE_REFERENCES - DenseStorage(DenseStorage&& other) + EIGEN_DEVICE_FUNC DenseStorage() : m_data(0), m_rows(0) {} + explicit DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {} + EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) : m_data(internal::conditional_aligned_new_auto(size)), m_rows(rows) + { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) + eigen_internal_assert(size==rows*cols && rows>=0 && cols == _Cols); + EIGEN_UNUSED_VARIABLE(cols); + } + EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) + : m_data(internal::conditional_aligned_new_auto(other.m_rows*_Cols)) + , m_rows(other.m_rows) + { + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = m_rows*_Cols) + internal::smart_copy(other.m_data, other.m_data+other.m_rows*_Cols, m_data); + } + EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + DenseStorage tmp(other); + this->swap(tmp); + } + return *this; + } +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT : m_data(std::move(other.m_data)) , m_rows(std::move(other.m_rows)) { other.m_data = nullptr; + other.m_rows = 0; } - DenseStorage& operator=(DenseStorage&& other) + EIGEN_DEVICE_FUNC + DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT { using std::swap; swap(m_data, other.m_data); @@ -400,16 +539,16 @@ template class DenseStorage(m_data, _Cols*m_rows); } - void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } - DenseIndex rows(void) const {return m_rows;} - static DenseIndex cols(void) {return _Cols;} - void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex) + EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } + EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;} + EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;} + void conservativeResize(Index size, Index rows, Index) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*_Cols); - m_rows = nbRows; + m_rows = rows; } - EIGEN_STRONG_INLINE void resize(DenseIndex size, DenseIndex nbRows, DenseIndex) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index size, Index rows, Index) { if(size != m_rows*_Cols) { @@ -418,15 +557,12 @@ template class DenseStorage(size); else m_data = 0; - EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN + EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({}) } - m_rows = nbRows; + m_rows = rows; } - const T *data() const { return m_data; } - T *data() { return m_data; } - private: - DenseStorage(const DenseStorage&); - DenseStorage& operator=(const DenseStorage&); + EIGEN_DEVICE_FUNC const T *data() const { return m_data; } + EIGEN_DEVICE_FUNC T *data() { return m_data; } }; } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Diagonal.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Diagonal.h index 68cf6d4b04..49e7112571 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Diagonal.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Diagonal.h @@ -21,7 +21,7 @@ namespace Eigen { * \param MatrixType the type of the object in which we are taking a sub/main/super diagonal * \param DiagIndex the index of the sub/super diagonal. The default is 0 and it means the main diagonal. * A positive value means a superdiagonal, a negative value means a subdiagonal. - * You can also use Dynamic so the index can be set at runtime. + * You can also use DynamicIndex so the index can be set at runtime. * * The matrix is not required to be square. * @@ -37,7 +37,7 @@ template struct traits > : traits { - typedef typename nested::type MatrixTypeNested; + typedef typename ref_selector::type MatrixTypeNested; typedef typename remove_reference::type _MatrixTypeNested; typedef typename MatrixType::StorageKind StorageKind; enum { @@ -52,8 +52,7 @@ struct traits > MatrixType::MaxColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))), MaxColsAtCompileTime = 1, MaskLvalueBit = is_lvalue::value ? LvalueBit : 0, - Flags = (unsigned int)_MatrixTypeNested::Flags & (HereditaryBits | LinearAccessBit | MaskLvalueBit | DirectAccessBit) & ~RowMajorBit, - CoeffReadCost = _MatrixTypeNested::CoeffReadCost, + Flags = (unsigned int)_MatrixTypeNested::Flags & (RowMajorBit | MaskLvalueBit | DirectAccessBit) & ~RowMajorBit, // FIXME DirectAccessBit should not be handled by expressions MatrixTypeOuterStride = outer_stride_at_compile_time::ret, InnerStrideAtCompileTime = MatrixTypeOuterStride == Dynamic ? Dynamic : MatrixTypeOuterStride+1, OuterStrideAtCompileTime = 0 @@ -70,20 +69,28 @@ template class Diagonal typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal) - inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index) {} + EIGEN_DEVICE_FUNC + explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index) {} EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal) + EIGEN_DEVICE_FUNC inline Index rows() const - { return m_index.value()<0 ? (std::min)(m_matrix.cols(),m_matrix.rows()+m_index.value()) : (std::min)(m_matrix.rows(),m_matrix.cols()-m_index.value()); } + { + return m_index.value()<0 ? numext::mini(m_matrix.cols(),m_matrix.rows()+m_index.value()) + : numext::mini(m_matrix.rows(),m_matrix.cols()-m_index.value()); + } + EIGEN_DEVICE_FUNC inline Index cols() const { return 1; } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_matrix.outerStride() + 1; } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return 0; @@ -95,62 +102,75 @@ template class Diagonal const Scalar >::type ScalarWithConstIfNotLvalue; - inline ScalarWithConstIfNotLvalue* data() { return &(m_matrix.const_cast_derived().coeffRef(rowOffset(), colOffset())); } - inline const Scalar* data() const { return &(m_matrix.const_cast_derived().coeffRef(rowOffset(), colOffset())); } + EIGEN_DEVICE_FUNC + inline ScalarWithConstIfNotLvalue* data() { return &(m_matrix.coeffRef(rowOffset(), colOffset())); } + EIGEN_DEVICE_FUNC + inline const Scalar* data() const { return &(m_matrix.coeffRef(rowOffset(), colOffset())); } + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index) { EIGEN_STATIC_ASSERT_LVALUE(MatrixType) - return m_matrix.const_cast_derived().coeffRef(row+rowOffset(), row+colOffset()); + return m_matrix.coeffRef(row+rowOffset(), row+colOffset()); } + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index row, Index) const { - return m_matrix.const_cast_derived().coeffRef(row+rowOffset(), row+colOffset()); + return m_matrix.coeffRef(row+rowOffset(), row+colOffset()); } + EIGEN_DEVICE_FUNC inline CoeffReturnType coeff(Index row, Index) const { return m_matrix.coeff(row+rowOffset(), row+colOffset()); } + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index idx) { EIGEN_STATIC_ASSERT_LVALUE(MatrixType) - return m_matrix.const_cast_derived().coeffRef(idx+rowOffset(), idx+colOffset()); + return m_matrix.coeffRef(idx+rowOffset(), idx+colOffset()); } + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index idx) const { - return m_matrix.const_cast_derived().coeffRef(idx+rowOffset(), idx+colOffset()); + return m_matrix.coeffRef(idx+rowOffset(), idx+colOffset()); } + EIGEN_DEVICE_FUNC inline CoeffReturnType coeff(Index idx) const { return m_matrix.coeff(idx+rowOffset(), idx+colOffset()); } - const typename internal::remove_all::type& + EIGEN_DEVICE_FUNC + inline const typename internal::remove_all::type& nestedExpression() const { return m_matrix; } - int index() const + EIGEN_DEVICE_FUNC + inline Index index() const { return m_index.value(); } protected: - typename MatrixType::Nested m_matrix; + typename internal::ref_selector::non_const_type m_matrix; const internal::variable_if_dynamicindex m_index; private: // some compilers may fail to optimize std::max etc in case of compile-time constants... + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index absDiagIndex() const { return m_index.value()>0 ? m_index.value() : -m_index.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rowOffset() const { return m_index.value()>0 ? 0 : -m_index.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index colOffset() const { return m_index.value()>0 ? m_index.value() : 0; } - // triger a compile time error is someone try to call packet + // trigger a compile-time error if someone try to call packet template typename MatrixType::PacketReturnType packet(Index) const; template typename MatrixType::PacketReturnType packet(Index,Index) const; }; @@ -167,7 +187,7 @@ template inline typename MatrixBase::DiagonalReturnType MatrixBase::diagonal() { - return derived(); + return DiagonalReturnType(derived()); } /** This is the const version of diagonal(). */ @@ -216,20 +236,20 @@ MatrixBase::diagonal(Index index) const * * \sa MatrixBase::diagonal(), class Diagonal */ template -template -inline typename MatrixBase::template DiagonalIndexReturnType::Type +template +inline typename MatrixBase::template DiagonalIndexReturnType::Type MatrixBase::diagonal() { - return derived(); + return typename DiagonalIndexReturnType::Type(derived()); } /** This is the const version of diagonal(). */ template -template -inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type +template +inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type MatrixBase::diagonal() const { - return derived(); + return typename ConstDiagonalIndexReturnType::Type(derived()); } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalMatrix.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalMatrix.h index e6c220f419..ecfdce8efa 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalMatrix.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalMatrix.h @@ -22,7 +22,7 @@ class DiagonalBase : public EigenBase typedef typename DiagonalVectorType::Scalar Scalar; typedef typename DiagonalVectorType::RealScalar RealScalar; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; + typedef typename internal::traits::StorageIndex StorageIndex; enum { RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, @@ -30,79 +30,61 @@ class DiagonalBase : public EigenBase MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, IsVectorAtCompileTime = 0, - Flags = 0 + Flags = NoPreferredStorageOrderBit }; typedef Matrix DenseMatrixType; typedef DenseMatrixType DenseType; typedef DiagonalMatrix PlainObject; + EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast(this); } + EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast(this); } + EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const { return derived(); } - template - void evalTo(MatrixBase &other) const; - template - void addTo(MatrixBase &other) const - { other.diagonal() += diagonal(); } - template - void subTo(MatrixBase &other) const - { other.diagonal() -= diagonal(); } - + + EIGEN_DEVICE_FUNC inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); } + EIGEN_DEVICE_FUNC inline DiagonalVectorType& diagonal() { return derived().diagonal(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return diagonal().size(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return diagonal().size(); } - /** \returns the diagonal matrix product of \c *this by the matrix \a matrix. - */ template - const DiagonalProduct + EIGEN_DEVICE_FUNC + const Product operator*(const MatrixBase &matrix) const { - return DiagonalProduct(matrix.derived(), derived()); + return Product(derived(),matrix.derived()); } - inline const DiagonalWrapper, const DiagonalVectorType> > + typedef DiagonalWrapper, const DiagonalVectorType> > InverseReturnType; + EIGEN_DEVICE_FUNC + inline const InverseReturnType inverse() const { - return diagonal().cwiseInverse(); + return InverseReturnType(diagonal().cwiseInverse()); } - inline const DiagonalWrapper, const DiagonalVectorType> > + EIGEN_DEVICE_FUNC + inline const DiagonalWrapper operator*(const Scalar& scalar) const { - return diagonal() * scalar; + return DiagonalWrapper(diagonal() * scalar); } - friend inline const DiagonalWrapper, const DiagonalVectorType> > + EIGEN_DEVICE_FUNC + friend inline const DiagonalWrapper operator*(const Scalar& scalar, const DiagonalBase& other) { - return other.diagonal() * scalar; - } - - #ifdef EIGEN2_SUPPORT - template - bool isApprox(const DiagonalBase& other, typename NumTraits::Real precision = NumTraits::dummy_precision()) const - { - return diagonal().isApprox(other.diagonal(), precision); + return DiagonalWrapper(scalar * other.diagonal()); } - template - bool isApprox(const MatrixBase& other, typename NumTraits::Real precision = NumTraits::dummy_precision()) const - { - return toDenseMatrix().isApprox(other, precision); - } - #endif }; -template -template -void DiagonalBase::evalTo(MatrixBase &other) const -{ - other.setZero(); - other.diagonal() = diagonal(); -} #endif /** \class DiagonalMatrix @@ -124,10 +106,9 @@ struct traits > : traits > { typedef Matrix<_Scalar,SizeAtCompileTime,1,0,MaxSizeAtCompileTime,1> DiagonalVectorType; - typedef Dense StorageKind; - typedef DenseIndex Index; + typedef DiagonalShape StorageKind; enum { - Flags = LvalueBit + Flags = LvalueBit | NoPreferredStorageOrderBit }; }; } @@ -141,7 +122,7 @@ class DiagonalMatrix typedef const DiagonalMatrix& Nested; typedef _Scalar Scalar; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; + typedef typename internal::traits::StorageIndex StorageIndex; #endif protected: @@ -151,24 +132,31 @@ class DiagonalMatrix public: /** const version of diagonal(). */ + EIGEN_DEVICE_FUNC inline const DiagonalVectorType& diagonal() const { return m_diagonal; } /** \returns a reference to the stored vector of diagonal coefficients. */ + EIGEN_DEVICE_FUNC inline DiagonalVectorType& diagonal() { return m_diagonal; } /** Default constructor without initialization */ + EIGEN_DEVICE_FUNC inline DiagonalMatrix() {} /** Constructs a diagonal matrix with given dimension */ - inline DiagonalMatrix(Index dim) : m_diagonal(dim) {} + EIGEN_DEVICE_FUNC + explicit inline DiagonalMatrix(Index dim) : m_diagonal(dim) {} /** 2D constructor. */ + EIGEN_DEVICE_FUNC inline DiagonalMatrix(const Scalar& x, const Scalar& y) : m_diagonal(x,y) {} /** 3D constructor. */ + EIGEN_DEVICE_FUNC inline DiagonalMatrix(const Scalar& x, const Scalar& y, const Scalar& z) : m_diagonal(x,y,z) {} /** Copy constructor. */ template + EIGEN_DEVICE_FUNC inline DiagonalMatrix(const DiagonalBase& other) : m_diagonal(other.diagonal()) {} #ifndef EIGEN_PARSED_BY_DOXYGEN @@ -178,11 +166,13 @@ class DiagonalMatrix /** generic constructor from expression of the diagonal coefficients */ template + EIGEN_DEVICE_FUNC explicit inline DiagonalMatrix(const MatrixBase& other) : m_diagonal(other) {} /** Copy operator. */ template + EIGEN_DEVICE_FUNC DiagonalMatrix& operator=(const DiagonalBase& other) { m_diagonal = other.diagonal(); @@ -193,6 +183,7 @@ class DiagonalMatrix /** This is a special case of the templated operator=. Its purpose is to * prevent a default operator= from hiding the templated operator=. */ + EIGEN_DEVICE_FUNC DiagonalMatrix& operator=(const DiagonalMatrix& other) { m_diagonal = other.diagonal(); @@ -201,14 +192,19 @@ class DiagonalMatrix #endif /** Resizes to given size. */ + EIGEN_DEVICE_FUNC inline void resize(Index size) { m_diagonal.resize(size); } /** Sets all coefficients to zero. */ + EIGEN_DEVICE_FUNC inline void setZero() { m_diagonal.setZero(); } /** Resizes and sets all coefficients to zero. */ + EIGEN_DEVICE_FUNC inline void setZero(Index size) { m_diagonal.setZero(size); } /** Sets this matrix to be the identity matrix of the current size. */ + EIGEN_DEVICE_FUNC inline void setIdentity() { m_diagonal.setOnes(); } /** Sets this matrix to be the identity matrix of the given size. */ + EIGEN_DEVICE_FUNC inline void setIdentity(Index size) { m_diagonal.setOnes(size); } }; @@ -232,14 +228,15 @@ struct traits > { typedef _DiagonalVectorType DiagonalVectorType; typedef typename DiagonalVectorType::Scalar Scalar; - typedef typename DiagonalVectorType::Index Index; - typedef typename DiagonalVectorType::StorageKind StorageKind; + typedef typename DiagonalVectorType::StorageIndex StorageIndex; + typedef DiagonalShape StorageKind; + typedef typename traits::XprKind XprKind; enum { RowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, ColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, - MaxRowsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, - MaxColsAtCompileTime = DiagonalVectorType::SizeAtCompileTime, - Flags = traits::Flags & LvalueBit + MaxRowsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = DiagonalVectorType::MaxSizeAtCompileTime, + Flags = (traits::Flags & LvalueBit) | NoPreferredStorageOrderBit }; }; } @@ -255,9 +252,11 @@ class DiagonalWrapper #endif /** Constructor from expression of diagonal coefficients to wrap. */ - inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {} + EIGEN_DEVICE_FUNC + explicit inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {} /** \returns a const reference to the wrapped expression of diagonal coefficients. */ + EIGEN_DEVICE_FUNC const DiagonalVectorType& diagonal() const { return m_diagonal; } protected: @@ -277,7 +276,7 @@ template inline const DiagonalWrapper MatrixBase::asDiagonal() const { - return derived(); + return DiagonalWrapper(derived()); } /** \returns true if *this is approximately equal to a diagonal matrix, @@ -291,12 +290,11 @@ MatrixBase::asDiagonal() const template bool MatrixBase::isDiagonal(const RealScalar& prec) const { - using std::abs; if(cols() != rows()) return false; RealScalar maxAbsOnDiagonal = static_cast(-1); for(Index j = 0; j < cols(); ++j) { - RealScalar absOnDiagonal = abs(coeff(j,j)); + RealScalar absOnDiagonal = numext::abs(coeff(j,j)); if(absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal; } for(Index j = 0; j < cols(); ++j) @@ -308,6 +306,38 @@ bool MatrixBase::isDiagonal(const RealScalar& prec) const return true; } +namespace internal { + +template<> struct storage_kind_to_shape { typedef DiagonalShape Shape; }; + +struct Diagonal2Dense {}; + +template<> struct AssignmentKind { typedef Diagonal2Dense Kind; }; + +// Diagonal matrix to Dense assignment +template< typename DstXprType, typename SrcXprType, typename Functor> +struct Assignment +{ + static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &/*func*/) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + dst.setZero(); + dst.diagonal() = src.diagonal(); + } + + static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op &/*func*/) + { dst.diagonal() += src.diagonal(); } + + static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op &/*func*/) + { dst.diagonal() -= src.diagonal(); } +}; + +} // namespace internal + } // end namespace Eigen #endif // EIGEN_DIAGONALMATRIX_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalProduct.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalProduct.h index cc6b536e19..d372b938f6 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalProduct.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/DiagonalProduct.h @@ -13,117 +13,14 @@ namespace Eigen { -namespace internal { -template -struct traits > - : traits -{ - typedef typename scalar_product_traits::ReturnType Scalar; - enum { - RowsAtCompileTime = MatrixType::RowsAtCompileTime, - ColsAtCompileTime = MatrixType::ColsAtCompileTime, - MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, - MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, - - _StorageOrder = MatrixType::Flags & RowMajorBit ? RowMajor : ColMajor, - _ScalarAccessOnDiag = !((int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheLeft) - ||(int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheRight)), - _SameTypes = is_same::value, - // FIXME currently we need same types, but in the future the next rule should be the one - //_Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))), - _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))), - _LinearAccessMask = (RowsAtCompileTime==1 || ColsAtCompileTime==1) ? LinearAccessBit : 0, - - Flags = ((HereditaryBits|_LinearAccessMask|AlignedBit) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0),//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit), - Cost0 = EIGEN_ADD_COST(NumTraits::MulCost, MatrixType::CoeffReadCost), - CoeffReadCost = EIGEN_ADD_COST(Cost0,DiagonalType::DiagonalVectorType::CoeffReadCost) - }; -}; -} - -template -class DiagonalProduct : internal::no_assignment_operator, - public MatrixBase > -{ - public: - - typedef MatrixBase Base; - EIGEN_DENSE_PUBLIC_INTERFACE(DiagonalProduct) - - inline DiagonalProduct(const MatrixType& matrix, const DiagonalType& diagonal) - : m_matrix(matrix), m_diagonal(diagonal) - { - eigen_assert(diagonal.diagonal().size() == (ProductOrder == OnTheLeft ? matrix.rows() : matrix.cols())); - } - - EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); } - - EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const - { - return m_diagonal.diagonal().coeff(ProductOrder == OnTheLeft ? row : col) * m_matrix.coeff(row, col); - } - - EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const - { - enum { - StorageOrder = int(MatrixType::Flags) & RowMajorBit ? RowMajor : ColMajor - }; - return coeff(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const - { - enum { - StorageOrder = Flags & RowMajorBit ? RowMajor : ColMajor - }; - const Index indexInDiagonalVector = ProductOrder == OnTheLeft ? row : col; - return packet_impl(row,col,indexInDiagonalVector,typename internal::conditional< - ((int(StorageOrder) == RowMajor && int(ProductOrder) == OnTheLeft) - ||(int(StorageOrder) == ColMajor && int(ProductOrder) == OnTheRight)), internal::true_type, internal::false_type>::type()); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet(Index idx) const - { - enum { - StorageOrder = int(MatrixType::Flags) & RowMajorBit ? RowMajor : ColMajor - }; - return packet(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx); - } - - protected: - template - EIGEN_STRONG_INLINE PacketScalar packet_impl(Index row, Index col, Index id, internal::true_type) const - { - return internal::pmul(m_matrix.template packet(row, col), - internal::pset1(m_diagonal.diagonal().coeff(id))); - } - - template - EIGEN_STRONG_INLINE PacketScalar packet_impl(Index row, Index col, Index id, internal::false_type) const - { - enum { - InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime, - DiagonalVectorPacketLoadMode = (LoadMode == Aligned && (((InnerSize%16) == 0) || (int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit)==AlignedBit) ? Aligned : Unaligned) - }; - return internal::pmul(m_matrix.template packet(row, col), - m_diagonal.diagonal().template packet(id)); - } - - typename MatrixType::Nested m_matrix; - typename DiagonalType::Nested m_diagonal; -}; - /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. */ template template -inline const DiagonalProduct +inline const Product MatrixBase::operator*(const DiagonalBase &a_diagonal) const { - return DiagonalProduct(derived(), a_diagonal.derived()); + return Product(derived(),a_diagonal.derived()); } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Dot.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Dot.h index 9d7651f1fd..06ef18b8be 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Dot.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Dot.h @@ -28,26 +28,31 @@ template struct dot_nocheck { - typedef typename scalar_product_traits::Scalar,typename traits::Scalar>::ReturnType ResScalar; + typedef scalar_conj_product_op::Scalar,typename traits::Scalar> conj_prod; + typedef typename conj_prod::result_type ResScalar; + EIGEN_DEVICE_FUNC static inline ResScalar run(const MatrixBase& a, const MatrixBase& b) { - return a.template binaryExpr::Scalar,typename traits::Scalar> >(b).sum(); + return a.template binaryExpr(b).sum(); } }; template struct dot_nocheck { - typedef typename scalar_product_traits::Scalar,typename traits::Scalar>::ReturnType ResScalar; + typedef scalar_conj_product_op::Scalar,typename traits::Scalar> conj_prod; + typedef typename conj_prod::result_type ResScalar; + EIGEN_DEVICE_FUNC static inline ResScalar run(const MatrixBase& a, const MatrixBase& b) { - return a.transpose().template binaryExpr::Scalar,typename traits::Scalar> >(b).sum(); + return a.transpose().template binaryExpr(b).sum(); } }; } // end namespace internal -/** \returns the dot product of *this with other. +/** \fn MatrixBase::dot + * \returns the dot product of *this with other. * * \only_for_vectors * @@ -59,55 +64,30 @@ struct dot_nocheck */ template template -typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType +EIGEN_DEVICE_FUNC +typename ScalarBinaryOpTraits::Scalar,typename internal::traits::Scalar>::ReturnType MatrixBase::dot(const MatrixBase& other) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) +#if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG)) typedef internal::scalar_conj_product_op func; EIGEN_CHECK_BINARY_COMPATIBILIY(func,Scalar,typename OtherDerived::Scalar); - +#endif + eigen_assert(size() == other.size()); return internal::dot_nocheck::run(*this, other); } -#ifdef EIGEN2_SUPPORT -/** \returns the dot product of *this with other, with the Eigen2 convention that the dot product is linear in the first variable - * (conjugating the second variable). Of course this only makes a difference in the complex case. - * - * This method is only available in EIGEN2_SUPPORT mode. - * - * \only_for_vectors - * - * \sa dot() - */ -template -template -typename internal::traits::Scalar -MatrixBase::eigen2_dot(const MatrixBase& other) const -{ - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) - EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - - eigen_assert(size() == other.size()); - - return internal::dot_nocheck::run(other,*this); -} -#endif - - //---------- implementation of L2 norm and related functions ---------- /** \returns, for vectors, the squared \em l2 norm of \c *this, and for matrices the Frobenius norm. * In both cases, it consists in the sum of the square of all the matrix entries. * For vectors, this is also equals to the dot product of \c *this with itself. * - * \sa dot(), norm() + * \sa dot(), norm(), lpNorm() */ template EIGEN_STRONG_INLINE typename NumTraits::Scalar>::Real MatrixBase::squaredNorm() const @@ -119,16 +99,18 @@ EIGEN_STRONG_INLINE typename NumTraits::Scala * In both cases, it consists in the square root of the sum of the square of all the matrix entries. * For vectors, this is also equals to the square root of the dot product of \c *this with itself. * - * \sa dot(), squaredNorm() + * \sa lpNorm(), dot(), squaredNorm() */ template inline typename NumTraits::Scalar>::Real MatrixBase::norm() const { - using std::sqrt; - return sqrt(squaredNorm()); + return numext::sqrt(squaredNorm()); } -/** \returns an expression of the quotient of *this by its own norm. +/** \returns an expression of the quotient of \c *this by its own norm. + * + * \warning If the input vector is too small (i.e., this->norm()==0), + * then this function returns a copy of the input. * * \only_for_vectors * @@ -138,22 +120,77 @@ template inline const typename MatrixBase::PlainObject MatrixBase::normalized() const { - typedef typename internal::nested::type Nested; - typedef typename internal::remove_reference::type _Nested; + typedef typename internal::nested_eval::type _Nested; _Nested n(derived()); - return n / n.norm(); + RealScalar z = n.squaredNorm(); + // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU + if(z>RealScalar(0)) + return n / numext::sqrt(z); + else + return n; } /** Normalizes the vector, i.e. divides it by its own norm. * * \only_for_vectors * + * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged. + * * \sa norm(), normalized() */ template inline void MatrixBase::normalize() { - *this /= norm(); + RealScalar z = squaredNorm(); + // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU + if(z>RealScalar(0)) + derived() /= numext::sqrt(z); +} + +/** \returns an expression of the quotient of \c *this by its own norm while avoiding underflow and overflow. + * + * \only_for_vectors + * + * This method is analogue to the normalized() method, but it reduces the risk of + * underflow and overflow when computing the norm. + * + * \warning If the input vector is too small (i.e., this->norm()==0), + * then this function returns a copy of the input. + * + * \sa stableNorm(), stableNormalize(), normalized() + */ +template +inline const typename MatrixBase::PlainObject +MatrixBase::stableNormalized() const +{ + typedef typename internal::nested_eval::type _Nested; + _Nested n(derived()); + RealScalar w = n.cwiseAbs().maxCoeff(); + RealScalar z = (n/w).squaredNorm(); + if(z>RealScalar(0)) + return n / (numext::sqrt(z)*w); + else + return n; +} + +/** Normalizes the vector while avoid underflow and overflow + * + * \only_for_vectors + * + * This method is analogue to the normalize() method, but it reduces the risk of + * underflow and overflow when computing the norm. + * + * \warning If the input vector is too small (i.e., this->norm()==0), then \c *this is left unchanged. + * + * \sa stableNorm(), stableNormalized(), normalize() + */ +template +inline void MatrixBase::stableNormalize() +{ + RealScalar w = cwiseAbs().maxCoeff(); + RealScalar z = (derived()/w).squaredNorm(); + if(z>RealScalar(0)) + derived() /= numext::sqrt(z)*w; } //---------- implementation of other norms ---------- @@ -164,9 +201,10 @@ template struct lpNorm_selector { typedef typename NumTraits::Scalar>::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const MatrixBase& m) { - using std::pow; + EIGEN_USING_STD_MATH(pow) return pow(m.cwiseAbs().array().pow(p).sum(), RealScalar(1)/p); } }; @@ -174,6 +212,7 @@ struct lpNorm_selector template struct lpNorm_selector { + EIGEN_DEVICE_FUNC static inline typename NumTraits::Scalar>::Real run(const MatrixBase& m) { return m.cwiseAbs().sum(); @@ -183,6 +222,7 @@ struct lpNorm_selector template struct lpNorm_selector { + EIGEN_DEVICE_FUNC static inline typename NumTraits::Scalar>::Real run(const MatrixBase& m) { return m.norm(); @@ -192,23 +232,35 @@ struct lpNorm_selector template struct lpNorm_selector { - static inline typename NumTraits::Scalar>::Real run(const MatrixBase& m) + typedef typename NumTraits::Scalar>::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const MatrixBase& m) { + if(Derived::SizeAtCompileTime==0 || (Derived::SizeAtCompileTime==Dynamic && m.size()==0)) + return RealScalar(0); return m.cwiseAbs().maxCoeff(); } }; } // end namespace internal -/** \returns the \f$ \ell^p \f$ norm of *this, that is, returns the p-th root of the sum of the p-th powers of the absolute values - * of the coefficients of *this. If \a p is the special value \a Eigen::Infinity, this function returns the \f$ \ell^\infty \f$ - * norm, that is the maximum of the absolute values of the coefficients of *this. +/** \returns the \b coefficient-wise \f$ \ell^p \f$ norm of \c *this, that is, returns the p-th root of the sum of the p-th powers of the absolute values + * of the coefficients of \c *this. If \a p is the special value \a Eigen::Infinity, this function returns the \f$ \ell^\infty \f$ + * norm, that is the maximum of the absolute values of the coefficients of \c *this. + * + * In all cases, if \c *this is empty, then the value 0 is returned. + * + * \note For matrices, this function does not compute the operator-norm. That is, if \c *this is a matrix, then its coefficients are interpreted as a 1D vector. Nonetheless, you can easily compute the 1-norm and \f$\infty\f$-norm matrix operator norms using \link TutorialReductionsVisitorsBroadcastingReductionsNorm partial reductions \endlink. * * \sa norm() */ template template +#ifndef EIGEN_PARSED_BY_DOXYGEN inline typename NumTraits::Scalar>::Real +#else +MatrixBase::RealScalar +#endif MatrixBase::lpNorm() const { return internal::lpNorm_selector::run(*this); @@ -227,8 +279,8 @@ template bool MatrixBase::isOrthogonal (const MatrixBase& other, const RealScalar& prec) const { - typename internal::nested::type nested(derived()); - typename internal::nested::type otherNested(other.derived()); + typename internal::nested_eval::type nested(derived()); + typename internal::nested_eval::type otherNested(other.derived()); return numext::abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm(); } @@ -246,13 +298,13 @@ bool MatrixBase::isOrthogonal template bool MatrixBase::isUnitary(const RealScalar& prec) const { - typename Derived::Nested nested(derived()); + typename internal::nested_eval::type self(derived()); for(Index i = 0; i < cols(); ++i) { - if(!internal::isApprox(nested.col(i).squaredNorm(), static_cast(1), prec)) + if(!internal::isApprox(self.col(i).squaredNorm(), static_cast(1), prec)) return false; for(Index j = 0; j < i; ++j) - if(!internal::isMuchSmallerThan(nested.col(i).dot(nested.col(j)), static_cast(1), prec)) + if(!internal::isMuchSmallerThan(self.col(i).dot(self.col(j)), static_cast(1), prec)) return false; } return true; diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/EigenBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/EigenBase.h index fadb45852f..f76995af90 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/EigenBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/EigenBase.h @@ -13,7 +13,9 @@ namespace Eigen { -/** Common base class for all classes T such that MatrixBase has an operator=(T) and a constructor MatrixBase(T). +/** \class EigenBase + * + * Common base class for all classes T such that MatrixBase has an operator=(T) and a constructor MatrixBase(T). * * In other words, an EigenBase object is an object that can be copied into a MatrixBase. * @@ -21,39 +23,57 @@ namespace Eigen { * * Notice that this class is trivial, it is only used to disambiguate overloaded functions. * - * \sa \ref TopicClassHierarchy + * \sa \blank \ref TopicClassHierarchy */ template struct EigenBase { // typedef typename internal::plain_matrix_type::type PlainObject; - + + /** \brief The interface type of indices + * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. + * \deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. + * \sa StorageIndex, \ref TopicPreprocessorDirectives. + */ + typedef Eigen::Index Index; + + // FIXME is it needed? typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; /** \returns a reference to the derived object */ + EIGEN_DEVICE_FUNC Derived& derived() { return *static_cast(this); } /** \returns a const reference to the derived object */ + EIGEN_DEVICE_FUNC const Derived& derived() const { return *static_cast(this); } + EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { return *static_cast(const_cast(this)); } + EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } /** \returns the number of rows. \sa cols(), RowsAtCompileTime */ + EIGEN_DEVICE_FUNC inline Index rows() const { return derived().rows(); } /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/ + EIGEN_DEVICE_FUNC inline Index cols() const { return derived().cols(); } /** \returns the number of coefficients, which is rows()*cols(). * \sa rows(), cols(), SizeAtCompileTime. */ + EIGEN_DEVICE_FUNC inline Index size() const { return rows() * cols(); } /** \internal Don't use it, but do the equivalent: \code dst = *this; \endcode */ - template inline void evalTo(Dest& dst) const + template + EIGEN_DEVICE_FUNC + inline void evalTo(Dest& dst) const { derived().evalTo(dst); } /** \internal Don't use it, but do the equivalent: \code dst += *this; \endcode */ - template inline void addTo(Dest& dst) const + template + EIGEN_DEVICE_FUNC + inline void addTo(Dest& dst) const { // This is the default implementation, // derived class can reimplement it in a more optimized way. @@ -63,7 +83,9 @@ template struct EigenBase } /** \internal Don't use it, but do the equivalent: \code dst -= *this; \endcode */ - template inline void subTo(Dest& dst) const + template + EIGEN_DEVICE_FUNC + inline void subTo(Dest& dst) const { // This is the default implementation, // derived class can reimplement it in a more optimized way. @@ -73,7 +95,8 @@ template struct EigenBase } /** \internal Don't use it, but do the equivalent: \code dst.applyOnTheRight(*this); \endcode */ - template inline void applyThisOnTheRight(Dest& dst) const + template + EIGEN_DEVICE_FUNC inline void applyThisOnTheRight(Dest& dst) const { // This is the default implementation, // derived class can reimplement it in a more optimized way. @@ -81,7 +104,8 @@ template struct EigenBase } /** \internal Don't use it, but do the equivalent: \code dst.applyOnTheLeft(*this); \endcode */ - template inline void applyThisOnTheLeft(Dest& dst) const + template + EIGEN_DEVICE_FUNC inline void applyThisOnTheLeft(Dest& dst) const { // This is the default implementation, // derived class can reimplement it in a more optimized way. @@ -106,7 +130,7 @@ template template Derived& DenseBase::operator=(const EigenBase &other) { - other.derived().evalTo(derived()); + call_assignment(derived(), other.derived()); return derived(); } @@ -114,7 +138,7 @@ template template Derived& DenseBase::operator+=(const EigenBase &other) { - other.derived().addTo(derived()); + call_assignment(derived(), other.derived(), internal::add_assign_op()); return derived(); } @@ -122,7 +146,7 @@ template template Derived& DenseBase::operator-=(const EigenBase &other) { - other.derived().subTo(derived()); + call_assignment(derived(), other.derived(), internal::sub_assign_op()); return derived(); } diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Flagged.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Flagged.h deleted file mode 100644 index 1f2955fc1d..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Flagged.h +++ /dev/null @@ -1,140 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_FLAGGED_H -#define EIGEN_FLAGGED_H - -namespace Eigen { - -/** \class Flagged - * \ingroup Core_Module - * - * \brief Expression with modified flags - * - * \param ExpressionType the type of the object of which we are modifying the flags - * \param Added the flags added to the expression - * \param Removed the flags removed from the expression (has priority over Added). - * - * This class represents an expression whose flags have been modified. - * It is the return type of MatrixBase::flagged() - * and most of the time this is the only way it is used. - * - * \sa MatrixBase::flagged() - */ - -namespace internal { -template -struct traits > : traits -{ - enum { Flags = (ExpressionType::Flags | Added) & ~Removed }; -}; -} - -template class Flagged - : public MatrixBase > -{ - public: - - typedef MatrixBase Base; - - EIGEN_DENSE_PUBLIC_INTERFACE(Flagged) - typedef typename internal::conditional::ret, - ExpressionType, const ExpressionType&>::type ExpressionTypeNested; - typedef typename ExpressionType::InnerIterator InnerIterator; - - inline Flagged(const ExpressionType& matrix) : m_matrix(matrix) {} - - inline Index rows() const { return m_matrix.rows(); } - inline Index cols() const { return m_matrix.cols(); } - inline Index outerStride() const { return m_matrix.outerStride(); } - inline Index innerStride() const { return m_matrix.innerStride(); } - - inline CoeffReturnType coeff(Index row, Index col) const - { - return m_matrix.coeff(row, col); - } - - inline CoeffReturnType coeff(Index index) const - { - return m_matrix.coeff(index); - } - - inline const Scalar& coeffRef(Index row, Index col) const - { - return m_matrix.const_cast_derived().coeffRef(row, col); - } - - inline const Scalar& coeffRef(Index index) const - { - return m_matrix.const_cast_derived().coeffRef(index); - } - - inline Scalar& coeffRef(Index row, Index col) - { - return m_matrix.const_cast_derived().coeffRef(row, col); - } - - inline Scalar& coeffRef(Index index) - { - return m_matrix.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index row, Index col) const - { - return m_matrix.template packet(row, col); - } - - template - inline void writePacket(Index row, Index col, const PacketScalar& x) - { - m_matrix.const_cast_derived().template writePacket(row, col, x); - } - - template - inline const PacketScalar packet(Index index) const - { - return m_matrix.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& x) - { - m_matrix.const_cast_derived().template writePacket(index, x); - } - - const ExpressionType& _expression() const { return m_matrix; } - - template - typename ExpressionType::PlainObject solveTriangular(const MatrixBase& other) const; - - template - void solveTriangularInPlace(const MatrixBase& other) const; - - protected: - ExpressionTypeNested m_matrix; -}; - -/** \returns an expression of *this with added and removed flags - * - * This is mostly for internal use. - * - * \sa class Flagged - */ -template -template -inline const Flagged -DenseBase::flagged() const -{ - return derived(); -} - -} // end namespace Eigen - -#endif // EIGEN_FLAGGED_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ForceAlignedAccess.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ForceAlignedAccess.h index 807c7a2934..7b08b45e67 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/ForceAlignedAccess.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/ForceAlignedAccess.h @@ -39,29 +39,29 @@ template class ForceAlignedAccess typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(ForceAlignedAccess) - inline ForceAlignedAccess(const ExpressionType& matrix) : m_expression(matrix) {} + EIGEN_DEVICE_FUNC explicit inline ForceAlignedAccess(const ExpressionType& matrix) : m_expression(matrix) {} - inline Index rows() const { return m_expression.rows(); } - inline Index cols() const { return m_expression.cols(); } - inline Index outerStride() const { return m_expression.outerStride(); } - inline Index innerStride() const { return m_expression.innerStride(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); } - inline const CoeffReturnType coeff(Index row, Index col) const + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const { return m_expression.coeff(row, col); } - inline Scalar& coeffRef(Index row, Index col) + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) { return m_expression.const_cast_derived().coeffRef(row, col); } - inline const CoeffReturnType coeff(Index index) const + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const { return m_expression.coeff(index); } - inline Scalar& coeffRef(Index index) + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) { return m_expression.const_cast_derived().coeffRef(index); } @@ -90,7 +90,7 @@ template class ForceAlignedAccess m_expression.const_cast_derived().template writePacket(index, x); } - operator const ExpressionType&() const { return m_expression; } + EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } protected: const ExpressionType& m_expression; @@ -127,7 +127,7 @@ template inline typename internal::add_const_on_value_type,Derived&>::type>::type MatrixBase::forceAlignedAccessIf() const { - return derived(); + return derived(); // FIXME This should not work but apparently is never used } /** \returns an expression of *this with forced aligned access if \a Enable is true. @@ -138,7 +138,7 @@ template inline typename internal::conditional,Derived&>::type MatrixBase::forceAlignedAccessIf() { - return derived(); + return derived(); // FIXME This should not work but apparently is never used } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Functors.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Functors.h deleted file mode 100644 index 5f14c6587e..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Functors.h +++ /dev/null @@ -1,1026 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008-2010 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_FUNCTORS_H -#define EIGEN_FUNCTORS_H - -namespace Eigen { - -namespace internal { - -// associative functors: - -/** \internal - * \brief Template functor to compute the sum of two scalars - * - * \sa class CwiseBinaryOp, MatrixBase::operator+, class VectorwiseOp, MatrixBase::sum() - */ -template struct scalar_sum_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a + b; } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return internal::padd(a,b); } - template - EIGEN_STRONG_INLINE const Scalar predux(const Packet& a) const - { return internal::predux(a); } -}; -template -struct functor_traits > { - enum { - Cost = NumTraits::AddCost, - PacketAccess = packet_traits::HasAdd - }; -}; - -/** \internal - * \brief Template functor to compute the product of two scalars - * - * \sa class CwiseBinaryOp, Cwise::operator*(), class VectorwiseOp, MatrixBase::redux() - */ -template struct scalar_product_op { - enum { - // TODO vectorize mixed product - Vectorizable = is_same::value && packet_traits::HasMul && packet_traits::HasMul - }; - typedef typename scalar_product_traits::ReturnType result_type; - EIGEN_EMPTY_STRUCT_CTOR(scalar_product_op) - EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a * b; } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return internal::pmul(a,b); } - template - EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const - { return internal::predux_mul(a); } -}; -template -struct functor_traits > { - enum { - Cost = (NumTraits::MulCost + NumTraits::MulCost)/2, // rough estimate! - PacketAccess = scalar_product_op::Vectorizable - }; -}; - -/** \internal - * \brief Template functor to compute the conjugate product of two scalars - * - * This is a short cut for conj(x) * y which is needed for optimization purpose; in Eigen2 support mode, this becomes x * conj(y) - */ -template struct scalar_conj_product_op { - - enum { - Conj = NumTraits::IsComplex - }; - - typedef typename scalar_product_traits::ReturnType result_type; - - EIGEN_EMPTY_STRUCT_CTOR(scalar_conj_product_op) - EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const - { return conj_helper().pmul(a,b); } - - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return conj_helper().pmul(a,b); } -}; -template -struct functor_traits > { - enum { - Cost = NumTraits::MulCost, - PacketAccess = internal::is_same::value && packet_traits::HasMul - }; -}; - -/** \internal - * \brief Template functor to compute the min of two scalars - * - * \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class VectorwiseOp, MatrixBase::minCoeff() - */ -template struct scalar_min_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_min_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { using std::min; return (min)(a, b); } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return internal::pmin(a,b); } - template - EIGEN_STRONG_INLINE const Scalar predux(const Packet& a) const - { return internal::predux_min(a); } -}; -template -struct functor_traits > { - enum { - Cost = NumTraits::AddCost, - PacketAccess = packet_traits::HasMin - }; -}; - -/** \internal - * \brief Template functor to compute the max of two scalars - * - * \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class VectorwiseOp, MatrixBase::maxCoeff() - */ -template struct scalar_max_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { using std::max; return (max)(a, b); } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return internal::pmax(a,b); } - template - EIGEN_STRONG_INLINE const Scalar predux(const Packet& a) const - { return internal::predux_max(a); } -}; -template -struct functor_traits > { - enum { - Cost = NumTraits::AddCost, - PacketAccess = packet_traits::HasMax - }; -}; - -/** \internal - * \brief Template functor to compute the hypot of two scalars - * - * \sa MatrixBase::stableNorm(), class Redux - */ -template struct scalar_hypot_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_hypot_op) -// typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& _x, const Scalar& _y) const - { - using std::max; - using std::min; - using std::sqrt; - Scalar p = (max)(_x, _y); - Scalar q = (min)(_x, _y); - Scalar qp = q/p; - return p * sqrt(Scalar(1) + qp*qp); - } -}; -template -struct functor_traits > { - enum { Cost = 5 * NumTraits::MulCost, PacketAccess=0 }; -}; - -/** \internal - * \brief Template functor to compute the pow of two scalars - */ -template struct scalar_binary_pow_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_binary_pow_op) - inline Scalar operator() (const Scalar& a, const OtherScalar& b) const { return numext::pow(a, b); } -}; -template -struct functor_traits > { - enum { Cost = 5 * NumTraits::MulCost, PacketAccess = false }; -}; - -// other binary functors: - -/** \internal - * \brief Template functor to compute the difference of two scalars - * - * \sa class CwiseBinaryOp, MatrixBase::operator- - */ -template struct scalar_difference_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_difference_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a - b; } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return internal::psub(a,b); } -}; -template -struct functor_traits > { - enum { - Cost = NumTraits::AddCost, - PacketAccess = packet_traits::HasSub - }; -}; - -/** \internal - * \brief Template functor to compute the quotient of two scalars - * - * \sa class CwiseBinaryOp, Cwise::operator/() - */ -template struct scalar_quotient_op { - enum { - // TODO vectorize mixed product - Vectorizable = is_same::value && packet_traits::HasDiv && packet_traits::HasDiv - }; - typedef typename scalar_product_traits::ReturnType result_type; - EIGEN_EMPTY_STRUCT_CTOR(scalar_quotient_op) - EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a / b; } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const - { return internal::pdiv(a,b); } -}; -template -struct functor_traits > { - enum { - Cost = (NumTraits::MulCost + NumTraits::MulCost), // rough estimate! - PacketAccess = scalar_quotient_op::Vectorizable - }; -}; - - - -/** \internal - * \brief Template functor to compute the and of two booleans - * - * \sa class CwiseBinaryOp, ArrayBase::operator&& - */ -struct scalar_boolean_and_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_and_op) - EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a && b; } -}; -template<> struct functor_traits { - enum { - Cost = NumTraits::AddCost, - PacketAccess = false - }; -}; - -/** \internal - * \brief Template functor to compute the or of two booleans - * - * \sa class CwiseBinaryOp, ArrayBase::operator|| - */ -struct scalar_boolean_or_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_or_op) - EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a || b; } -}; -template<> struct functor_traits { - enum { - Cost = NumTraits::AddCost, - PacketAccess = false - }; -}; - -/** \internal - * \brief Template functors for comparison of two scalars - * \todo Implement packet-comparisons - */ -template struct scalar_cmp_op; - -template -struct functor_traits > { - enum { - Cost = NumTraits::AddCost, - PacketAccess = false - }; -}; - -template -struct result_of(Scalar,Scalar)> { - typedef bool type; -}; - - -template struct scalar_cmp_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) - EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a==b;} -}; -template struct scalar_cmp_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) - EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a struct scalar_cmp_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) - EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a<=b;} -}; -template struct scalar_cmp_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) - EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return !(a<=b || b<=a);} -}; -template struct scalar_cmp_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) - EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a!=b;} -}; - -// unary functors: - -/** \internal - * \brief Template functor to compute the opposite of a scalar - * - * \sa class CwiseUnaryOp, MatrixBase::operator- - */ -template struct scalar_opposite_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_opposite_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return -a; } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const - { return internal::pnegate(a); } -}; -template -struct functor_traits > -{ enum { - Cost = NumTraits::AddCost, - PacketAccess = packet_traits::HasNegate }; -}; - -/** \internal - * \brief Template functor to compute the absolute value of a scalar - * - * \sa class CwiseUnaryOp, Cwise::abs - */ -template struct scalar_abs_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_abs_op) - typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { using std::abs; return abs(a); } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const - { return internal::pabs(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = NumTraits::AddCost, - PacketAccess = packet_traits::HasAbs - }; -}; - -/** \internal - * \brief Template functor to compute the squared absolute value of a scalar - * - * \sa class CwiseUnaryOp, Cwise::abs2 - */ -template struct scalar_abs2_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_abs2_op) - typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return numext::abs2(a); } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const - { return internal::pmul(a,a); } -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = packet_traits::HasAbs2 }; }; - -/** \internal - * \brief Template functor to compute the conjugate of a complex value - * - * \sa class CwiseUnaryOp, MatrixBase::conjugate() - */ -template struct scalar_conjugate_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_conjugate_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { using numext::conj; return conj(a); } - template - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pconj(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = NumTraits::IsComplex ? NumTraits::AddCost : 0, - PacketAccess = packet_traits::HasConj - }; -}; - -/** \internal - * \brief Template functor to cast a scalar to another type - * - * \sa class CwiseUnaryOp, MatrixBase::cast() - */ -template -struct scalar_cast_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) - typedef NewType result_type; - EIGEN_STRONG_INLINE const NewType operator() (const Scalar& a) const { return cast(a); } -}; -template -struct functor_traits > -{ enum { Cost = is_same::value ? 0 : NumTraits::AddCost, PacketAccess = false }; }; - -/** \internal - * \brief Template functor to extract the real part of a complex - * - * \sa class CwiseUnaryOp, MatrixBase::real() - */ -template -struct scalar_real_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_real_op) - typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return numext::real(a); } -}; -template -struct functor_traits > -{ enum { Cost = 0, PacketAccess = false }; }; - -/** \internal - * \brief Template functor to extract the imaginary part of a complex - * - * \sa class CwiseUnaryOp, MatrixBase::imag() - */ -template -struct scalar_imag_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_op) - typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return numext::imag(a); } -}; -template -struct functor_traits > -{ enum { Cost = 0, PacketAccess = false }; }; - -/** \internal - * \brief Template functor to extract the real part of a complex as a reference - * - * \sa class CwiseUnaryOp, MatrixBase::real() - */ -template -struct scalar_real_ref_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_real_ref_op) - typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return numext::real_ref(*const_cast(&a)); } -}; -template -struct functor_traits > -{ enum { Cost = 0, PacketAccess = false }; }; - -/** \internal - * \brief Template functor to extract the imaginary part of a complex as a reference - * - * \sa class CwiseUnaryOp, MatrixBase::imag() - */ -template -struct scalar_imag_ref_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_ref_op) - typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return numext::imag_ref(*const_cast(&a)); } -}; -template -struct functor_traits > -{ enum { Cost = 0, PacketAccess = false }; }; - -/** \internal - * - * \brief Template functor to compute the exponential of a scalar - * - * \sa class CwiseUnaryOp, Cwise::exp() - */ -template struct scalar_exp_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_exp_op) - inline const Scalar operator() (const Scalar& a) const { using std::exp; return exp(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::pexp(a); } -}; -template -struct functor_traits > -{ enum { Cost = 5 * NumTraits::MulCost, PacketAccess = packet_traits::HasExp }; }; - -/** \internal - * - * \brief Template functor to compute the logarithm of a scalar - * - * \sa class CwiseUnaryOp, Cwise::log() - */ -template struct scalar_log_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_log_op) - inline const Scalar operator() (const Scalar& a) const { using std::log; return log(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::plog(a); } -}; -template -struct functor_traits > -{ enum { Cost = 5 * NumTraits::MulCost, PacketAccess = packet_traits::HasLog }; }; - -/** \internal - * \brief Template functor to multiply a scalar by a fixed other one - * - * \sa class CwiseUnaryOp, MatrixBase::operator*, MatrixBase::operator/ - */ -/* NOTE why doing the pset1() in packetOp *is* an optimization ? - * indeed it seems better to declare m_other as a Packet and do the pset1() once - * in the constructor. However, in practice: - * - GCC does not like m_other as a Packet and generate a load every time it needs it - * - on the other hand GCC is able to moves the pset1() outside the loop :) - * - simpler code ;) - * (ICC and gcc 4.4 seems to perform well in both cases, the issue is visible with y = a*x + b*y) - */ -template -struct scalar_multiple_op { - typedef typename packet_traits::type Packet; - // FIXME default copy constructors seems bugged with std::complex<> - EIGEN_STRONG_INLINE scalar_multiple_op(const scalar_multiple_op& other) : m_other(other.m_other) { } - EIGEN_STRONG_INLINE scalar_multiple_op(const Scalar& other) : m_other(other) { } - EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; } - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const - { return internal::pmul(a, pset1(m_other)); } - typename add_const_on_value_type::Nested>::type m_other; -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = packet_traits::HasMul }; }; - -template -struct scalar_multiple2_op { - typedef typename scalar_product_traits::ReturnType result_type; - EIGEN_STRONG_INLINE scalar_multiple2_op(const scalar_multiple2_op& other) : m_other(other.m_other) { } - EIGEN_STRONG_INLINE scalar_multiple2_op(const Scalar2& other) : m_other(other) { } - EIGEN_STRONG_INLINE result_type operator() (const Scalar1& a) const { return a * m_other; } - typename add_const_on_value_type::Nested>::type m_other; -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = false }; }; - -/** \internal - * \brief Template functor to divide a scalar by a fixed other one - * - * This functor is used to implement the quotient of a matrix by - * a scalar where the scalar type is not necessarily a floating point type. - * - * \sa class CwiseUnaryOp, MatrixBase::operator/ - */ -template -struct scalar_quotient1_op { - typedef typename packet_traits::type Packet; - // FIXME default copy constructors seems bugged with std::complex<> - EIGEN_STRONG_INLINE scalar_quotient1_op(const scalar_quotient1_op& other) : m_other(other.m_other) { } - EIGEN_STRONG_INLINE scalar_quotient1_op(const Scalar& other) : m_other(other) {} - EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a / m_other; } - EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const - { return internal::pdiv(a, pset1(m_other)); } - typename add_const_on_value_type::Nested>::type m_other; -}; -template -struct functor_traits > -{ enum { Cost = 2 * NumTraits::MulCost, PacketAccess = packet_traits::HasDiv }; }; - -// nullary functors - -template -struct scalar_constant_op { - typedef typename packet_traits::type Packet; - EIGEN_STRONG_INLINE scalar_constant_op(const scalar_constant_op& other) : m_other(other.m_other) { } - EIGEN_STRONG_INLINE scalar_constant_op(const Scalar& other) : m_other(other) { } - template - EIGEN_STRONG_INLINE const Scalar operator() (Index, Index = 0) const { return m_other; } - template - EIGEN_STRONG_INLINE const Packet packetOp(Index, Index = 0) const { return internal::pset1(m_other); } - const Scalar m_other; -}; -template -struct functor_traits > -// FIXME replace this packet test by a safe one -{ enum { Cost = 1, PacketAccess = packet_traits::Vectorizable, IsRepeatable = true }; }; - -template struct scalar_identity_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_identity_op) - template - EIGEN_STRONG_INLINE const Scalar operator() (Index row, Index col) const { return row==col ? Scalar(1) : Scalar(0); } -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::AddCost, PacketAccess = false, IsRepeatable = true }; }; - -template struct linspaced_op_impl; - -// linear access for packet ops: -// 1) initialization -// base = [low, ..., low] + ([step, ..., step] * [-size, ..., 0]) -// 2) each step (where size is 1 for coeff access or PacketSize for packet access) -// base += [size*step, ..., size*step] -// -// TODO: Perhaps it's better to initialize lazily (so not in the constructor but in packetOp) -// in order to avoid the padd() in operator() ? -template -struct linspaced_op_impl -{ - typedef typename packet_traits::type Packet; - - linspaced_op_impl(const Scalar& low, const Scalar& step) : - m_low(low), m_step(step), - m_packetStep(pset1(packet_traits::size*step)), - m_base(padd(pset1(low), pmul(pset1(step),plset(-packet_traits::size)))) {} - - template - EIGEN_STRONG_INLINE const Scalar operator() (Index i) const - { - m_base = padd(m_base, pset1(m_step)); - return m_low+Scalar(i)*m_step; - } - - template - EIGEN_STRONG_INLINE const Packet packetOp(Index) const { return m_base = padd(m_base,m_packetStep); } - - const Scalar m_low; - const Scalar m_step; - const Packet m_packetStep; - mutable Packet m_base; -}; - -// random access for packet ops: -// 1) each step -// [low, ..., low] + ( [step, ..., step] * ( [i, ..., i] + [0, ..., size] ) ) -template -struct linspaced_op_impl -{ - typedef typename packet_traits::type Packet; - - linspaced_op_impl(const Scalar& low, const Scalar& step) : - m_low(low), m_step(step), - m_lowPacket(pset1(m_low)), m_stepPacket(pset1(m_step)), m_interPacket(plset(0)) {} - - template - EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return m_low+i*m_step; } - - template - EIGEN_STRONG_INLINE const Packet packetOp(Index i) const - { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1(Scalar(i)),m_interPacket))); } - - const Scalar m_low; - const Scalar m_step; - const Packet m_lowPacket; - const Packet m_stepPacket; - const Packet m_interPacket; -}; - -// ----- Linspace functor ---------------------------------------------------------------- - -// Forward declaration (we default to random access which does not really give -// us a speed gain when using packet access but it allows to use the functor in -// nested expressions). -template struct linspaced_op; -template struct functor_traits< linspaced_op > -{ enum { Cost = 1, PacketAccess = packet_traits::HasSetLinear, IsRepeatable = true }; }; -template struct linspaced_op -{ - typedef typename packet_traits::type Packet; - linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/Scalar(num_steps-1))) {} - - template - EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); } - - // We need this function when assigning e.g. a RowVectorXd to a MatrixXd since - // there row==0 and col is used for the actual iteration. - template - EIGEN_STRONG_INLINE const Scalar operator() (Index row, Index col) const - { - eigen_assert(col==0 || row==0); - return impl(col + row); - } - - template - EIGEN_STRONG_INLINE const Packet packetOp(Index i) const { return impl.packetOp(i); } - - // We need this function when assigning e.g. a RowVectorXd to a MatrixXd since - // there row==0 and col is used for the actual iteration. - template - EIGEN_STRONG_INLINE const Packet packetOp(Index row, Index col) const - { - eigen_assert(col==0 || row==0); - return impl.packetOp(col + row); - } - - // This proxy object handles the actual required temporaries, the different - // implementations (random vs. sequential access) as well as the - // correct piping to size 2/4 packet operations. - const linspaced_op_impl impl; -}; - -// all functors allow linear access, except scalar_identity_op. So we fix here a quick meta -// to indicate whether a functor allows linear access, just always answering 'yes' except for -// scalar_identity_op. -// FIXME move this to functor_traits adding a functor_default -template struct functor_has_linear_access { enum { ret = 1 }; }; -template struct functor_has_linear_access > { enum { ret = 0 }; }; - -// In Eigen, any binary op (Product, CwiseBinaryOp) require the Lhs and Rhs to have the same scalar type, except for multiplication -// where the mixing of different types is handled by scalar_product_traits -// In particular, real * complex is allowed. -// FIXME move this to functor_traits adding a functor_default -template struct functor_is_product_like { enum { ret = 0 }; }; -template struct functor_is_product_like > { enum { ret = 1 }; }; -template struct functor_is_product_like > { enum { ret = 1 }; }; -template struct functor_is_product_like > { enum { ret = 1 }; }; - - -/** \internal - * \brief Template functor to add a scalar to a fixed other one - * \sa class CwiseUnaryOp, Array::operator+ - */ -/* If you wonder why doing the pset1() in packetOp() is an optimization check scalar_multiple_op */ -template -struct scalar_add_op { - typedef typename packet_traits::type Packet; - // FIXME default copy constructors seems bugged with std::complex<> - inline scalar_add_op(const scalar_add_op& other) : m_other(other.m_other) { } - inline scalar_add_op(const Scalar& other) : m_other(other) { } - inline Scalar operator() (const Scalar& a) const { return a + m_other; } - inline const Packet packetOp(const Packet& a) const - { return internal::padd(a, pset1(m_other)); } - const Scalar m_other; -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::AddCost, PacketAccess = packet_traits::HasAdd }; }; - -/** \internal - * \brief Template functor to compute the square root of a scalar - * \sa class CwiseUnaryOp, Cwise::sqrt() - */ -template struct scalar_sqrt_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_sqrt_op) - inline const Scalar operator() (const Scalar& a) const { using std::sqrt; return sqrt(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::psqrt(a); } -}; -template -struct functor_traits > -{ enum { - Cost = 5 * NumTraits::MulCost, - PacketAccess = packet_traits::HasSqrt - }; -}; - -/** \internal - * \brief Template functor to compute the cosine of a scalar - * \sa class CwiseUnaryOp, ArrayBase::cos() - */ -template struct scalar_cos_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cos_op) - inline Scalar operator() (const Scalar& a) const { using std::cos; return cos(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::pcos(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = 5 * NumTraits::MulCost, - PacketAccess = packet_traits::HasCos - }; -}; - -/** \internal - * \brief Template functor to compute the sine of a scalar - * \sa class CwiseUnaryOp, ArrayBase::sin() - */ -template struct scalar_sin_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_sin_op) - inline const Scalar operator() (const Scalar& a) const { using std::sin; return sin(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::psin(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = 5 * NumTraits::MulCost, - PacketAccess = packet_traits::HasSin - }; -}; - - -/** \internal - * \brief Template functor to compute the tan of a scalar - * \sa class CwiseUnaryOp, ArrayBase::tan() - */ -template struct scalar_tan_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_tan_op) - inline const Scalar operator() (const Scalar& a) const { using std::tan; return tan(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::ptan(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = 5 * NumTraits::MulCost, - PacketAccess = packet_traits::HasTan - }; -}; - -/** \internal - * \brief Template functor to compute the arc cosine of a scalar - * \sa class CwiseUnaryOp, ArrayBase::acos() - */ -template struct scalar_acos_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_acos_op) - inline const Scalar operator() (const Scalar& a) const { using std::acos; return acos(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::pacos(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = 5 * NumTraits::MulCost, - PacketAccess = packet_traits::HasACos - }; -}; - -/** \internal - * \brief Template functor to compute the arc sine of a scalar - * \sa class CwiseUnaryOp, ArrayBase::asin() - */ -template struct scalar_asin_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_asin_op) - inline const Scalar operator() (const Scalar& a) const { using std::asin; return asin(a); } - typedef typename packet_traits::type Packet; - inline Packet packetOp(const Packet& a) const { return internal::pasin(a); } -}; -template -struct functor_traits > -{ - enum { - Cost = 5 * NumTraits::MulCost, - PacketAccess = packet_traits::HasASin - }; -}; - -/** \internal - * \brief Template functor to raise a scalar to a power - * \sa class CwiseUnaryOp, Cwise::pow - */ -template -struct scalar_pow_op { - // FIXME default copy constructors seems bugged with std::complex<> - inline scalar_pow_op(const scalar_pow_op& other) : m_exponent(other.m_exponent) { } - inline scalar_pow_op(const Scalar& exponent) : m_exponent(exponent) {} - inline Scalar operator() (const Scalar& a) const { return numext::pow(a, m_exponent); } - const Scalar m_exponent; -}; -template -struct functor_traits > -{ enum { Cost = 5 * NumTraits::MulCost, PacketAccess = false }; }; - -/** \internal - * \brief Template functor to compute the quotient between a scalar and array entries. - * \sa class CwiseUnaryOp, Cwise::inverse() - */ -template -struct scalar_inverse_mult_op { - scalar_inverse_mult_op(const Scalar& other) : m_other(other) {} - inline Scalar operator() (const Scalar& a) const { return m_other / a; } - template - inline const Packet packetOp(const Packet& a) const - { return internal::pdiv(pset1(m_other),a); } - Scalar m_other; -}; - -/** \internal - * \brief Template functor to compute the inverse of a scalar - * \sa class CwiseUnaryOp, Cwise::inverse() - */ -template -struct scalar_inverse_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_inverse_op) - inline Scalar operator() (const Scalar& a) const { return Scalar(1)/a; } - template - inline const Packet packetOp(const Packet& a) const - { return internal::pdiv(pset1(Scalar(1)),a); } -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = packet_traits::HasDiv }; }; - -/** \internal - * \brief Template functor to compute the square of a scalar - * \sa class CwiseUnaryOp, Cwise::square() - */ -template -struct scalar_square_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_square_op) - inline Scalar operator() (const Scalar& a) const { return a*a; } - template - inline const Packet packetOp(const Packet& a) const - { return internal::pmul(a,a); } -}; -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = packet_traits::HasMul }; }; - -/** \internal - * \brief Template functor to compute the cube of a scalar - * \sa class CwiseUnaryOp, Cwise::cube() - */ -template -struct scalar_cube_op { - EIGEN_EMPTY_STRUCT_CTOR(scalar_cube_op) - inline Scalar operator() (const Scalar& a) const { return a*a*a; } - template - inline const Packet packetOp(const Packet& a) const - { return internal::pmul(a,pmul(a,a)); } -}; -template -struct functor_traits > -{ enum { Cost = 2*NumTraits::MulCost, PacketAccess = packet_traits::HasMul }; }; - -// default functor traits for STL functors: - -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = NumTraits::MulCost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = functor_traits::Cost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = functor_traits::Cost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1 + functor_traits::Cost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 1 + functor_traits::Cost, PacketAccess = false }; }; - -#ifdef EIGEN_STDEXT_SUPPORT - -template -struct functor_traits > -{ enum { Cost = 0, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = 0, PacketAccess = false }; }; - -template -struct functor_traits > > -{ enum { Cost = 0, PacketAccess = false }; }; - -template -struct functor_traits > > -{ enum { Cost = 0, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = functor_traits::Cost + functor_traits::Cost, PacketAccess = false }; }; - -template -struct functor_traits > -{ enum { Cost = functor_traits::Cost + functor_traits::Cost + functor_traits::Cost, PacketAccess = false }; }; - -#endif // EIGEN_STDEXT_SUPPORT - -// allow to add new functors and specializations of functor_traits from outside Eigen. -// this macro is really needed because functor_traits must be specialized after it is declared but before it is used... -#ifdef EIGEN_FUNCTORS_PLUGIN -#include EIGEN_FUNCTORS_PLUGIN -#endif - -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_FUNCTORS_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Fuzzy.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Fuzzy.h index fe63bd2984..3e403a09d9 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Fuzzy.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Fuzzy.h @@ -19,18 +19,19 @@ namespace internal template::IsInteger> struct isApprox_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) { - using std::min; - typename internal::nested::type nested(x); - typename internal::nested::type otherNested(y); - return (nested - otherNested).cwiseAbs2().sum() <= prec * prec * (min)(nested.cwiseAbs2().sum(), otherNested.cwiseAbs2().sum()); + typename internal::nested_eval::type nested(x); + typename internal::nested_eval::type otherNested(y); + return (nested - otherNested).cwiseAbs2().sum() <= prec * prec * numext::mini(nested.cwiseAbs2().sum(), otherNested.cwiseAbs2().sum()); } }; template struct isApprox_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar&) { return x.matrix() == y.matrix(); @@ -40,6 +41,7 @@ struct isApprox_selector template::IsInteger> struct isMuchSmallerThan_object_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) { return x.cwiseAbs2().sum() <= numext::abs2(prec) * y.cwiseAbs2().sum(); @@ -49,6 +51,7 @@ struct isMuchSmallerThan_object_selector template struct isMuchSmallerThan_object_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const OtherDerived&, const typename Derived::RealScalar&) { return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix(); @@ -58,6 +61,7 @@ struct isMuchSmallerThan_object_selector template::IsInteger> struct isMuchSmallerThan_scalar_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const typename Derived::RealScalar& y, const typename Derived::RealScalar& prec) { return x.cwiseAbs2().sum() <= numext::abs2(prec * y); @@ -67,6 +71,7 @@ struct isMuchSmallerThan_scalar_selector template struct isMuchSmallerThan_scalar_selector { + EIGEN_DEVICE_FUNC static bool run(const Derived& x, const typename Derived::RealScalar&, const typename Derived::RealScalar&) { return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix(); diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/GeneralProduct.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/GeneralProduct.h index 29ac522d2e..0f16cd8e39 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/GeneralProduct.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/GeneralProduct.h @@ -11,29 +11,7 @@ #ifndef EIGEN_GENERAL_PRODUCT_H #define EIGEN_GENERAL_PRODUCT_H -namespace Eigen { - -/** \class GeneralProduct - * \ingroup Core_Module - * - * \brief Expression of the product of two general matrices or vectors - * - * \param LhsNested the type used to store the left-hand side - * \param RhsNested the type used to store the right-hand side - * \param ProductMode the type of the product - * - * This class represents an expression of the product of two general matrices. - * We call a general matrix, a dense matrix with full storage. For instance, - * This excludes triangular, selfadjoint, and sparse matrices. - * It is the return type of the operator* between general matrices. Its template - * arguments are determined automatically by ProductReturnType. Therefore, - * GeneralProduct should never be used direclty. To determine the result type of a - * function which involves a matrix product, use ProductReturnType::Type. - * - * \sa ProductReturnType, MatrixBase::operator*(const MatrixBase&) - */ -template::value> -class GeneralProduct; +namespace Eigen { enum { Large = 2, @@ -47,7 +25,8 @@ template struct product_type_selector; template struct product_size_category { enum { is_large = MaxSize == Dynamic || - Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD, + Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD || + (Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD), value = is_large ? Large : Size == 1 ? 1 : Small @@ -59,15 +38,14 @@ template struct product_type typedef typename remove_all::type _Lhs; typedef typename remove_all::type _Rhs; enum { - MaxRows = _Lhs::MaxRowsAtCompileTime, - Rows = _Lhs::RowsAtCompileTime, - MaxCols = _Rhs::MaxColsAtCompileTime, - Cols = _Rhs::ColsAtCompileTime, - MaxDepth = EIGEN_SIZE_MIN_PREFER_FIXED(_Lhs::MaxColsAtCompileTime, - _Rhs::MaxRowsAtCompileTime), - Depth = EIGEN_SIZE_MIN_PREFER_FIXED(_Lhs::ColsAtCompileTime, - _Rhs::RowsAtCompileTime), - LargeThreshold = EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD + MaxRows = traits<_Lhs>::MaxRowsAtCompileTime, + Rows = traits<_Lhs>::RowsAtCompileTime, + MaxCols = traits<_Rhs>::MaxColsAtCompileTime, + Cols = traits<_Rhs>::ColsAtCompileTime, + MaxDepth = EIGEN_SIZE_MIN_PREFER_FIXED(traits<_Lhs>::MaxColsAtCompileTime, + traits<_Rhs>::MaxRowsAtCompileTime), + Depth = EIGEN_SIZE_MIN_PREFER_FIXED(traits<_Lhs>::ColsAtCompileTime, + traits<_Rhs>::RowsAtCompileTime) }; // the splitting into different lines of code here, introducing the _select enums and the typedef below, @@ -82,7 +60,8 @@ template struct product_type public: enum { - value = selector::ret + value = selector::ret, + ret = selector::ret }; #ifdef EIGEN_DEBUG_PRODUCT static void debug() @@ -98,12 +77,13 @@ template struct product_type #endif }; - /* The following allows to select the kind of product at compile time * based on the three dimensions of the product. * This is a compile time mapping from {1,Small,Large}^3 -> {product types} */ // FIXME I'm not sure the current mapping is the ideal one. template struct product_type_selector { enum { ret = OuterProduct }; }; +template struct product_type_selector { enum { ret = LazyCoeffBasedProductMode }; }; +template struct product_type_selector<1, N, 1> { enum { ret = LazyCoeffBasedProductMode }; }; template struct product_type_selector<1, 1, Depth> { enum { ret = InnerProduct }; }; template<> struct product_type_selector<1, 1, 1> { enum { ret = InnerProduct }; }; template<> struct product_type_selector { enum { ret = CoeffBasedProductMode }; }; @@ -122,60 +102,12 @@ template<> struct product_type_selector { enum template<> struct product_type_selector { enum { ret = GemmProduct }; }; template<> struct product_type_selector { enum { ret = GemmProduct }; }; template<> struct product_type_selector { enum { ret = GemmProduct }; }; -template<> struct product_type_selector { enum { ret = GemmProduct }; }; -template<> struct product_type_selector { enum { ret = GemmProduct }; }; +template<> struct product_type_selector { enum { ret = CoeffBasedProductMode }; }; +template<> struct product_type_selector { enum { ret = CoeffBasedProductMode }; }; template<> struct product_type_selector { enum { ret = GemmProduct }; }; } // end namespace internal -/** \class ProductReturnType - * \ingroup Core_Module - * - * \brief Helper class to get the correct and optimized returned type of operator* - * - * \param Lhs the type of the left-hand side - * \param Rhs the type of the right-hand side - * \param ProductMode the type of the product (determined automatically by internal::product_mode) - * - * This class defines the typename Type representing the optimized product expression - * between two matrix expressions. In practice, using ProductReturnType::Type - * is the recommended way to define the result type of a function returning an expression - * which involve a matrix product. The class Product should never be - * used directly. - * - * \sa class Product, MatrixBase::operator*(const MatrixBase&) - */ -template -struct ProductReturnType -{ - // TODO use the nested type to reduce instanciations ???? -// typedef typename internal::nested::type LhsNested; -// typedef typename internal::nested::type RhsNested; - - typedef GeneralProduct Type; -}; - -template -struct ProductReturnType -{ - typedef typename internal::nested::type >::type LhsNested; - typedef typename internal::nested::type >::type RhsNested; - typedef CoeffBasedProduct Type; -}; - -template -struct ProductReturnType -{ - typedef typename internal::nested::type >::type LhsNested; - typedef typename internal::nested::type >::type RhsNested; - typedef CoeffBasedProduct Type; -}; - -// this is a workaround for sun CC -template -struct LazyProductReturnType : public ProductReturnType -{}; - /*********************************************************************** * Implementation of Inner Vector Vector Product ***********************************************************************/ @@ -187,119 +119,10 @@ struct LazyProductReturnType : public ProductReturnType with: operator=(Scalar x); -namespace internal { - -template -struct traits > - : traits::ReturnType,1,1> > -{}; - -} - -template -class GeneralProduct - : internal::no_assignment_operator, - public Matrix::ReturnType,1,1> -{ - typedef Matrix::ReturnType,1,1> Base; - public: - GeneralProduct(const Lhs& lhs, const Rhs& rhs) - { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - - Base::coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum(); - } - - /** Convertion to scalar */ - operator const typename Base::Scalar() const { - return Base::coeff(0,0); - } -}; - /*********************************************************************** * Implementation of Outer Vector Vector Product ***********************************************************************/ -namespace internal { - -// Column major -template -EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& dest, const Func& func, const false_type&) -{ - typedef typename Dest::Index Index; - // FIXME make sure lhs is sequentially stored - // FIXME not very good if rhs is real and lhs complex while alpha is real too - const Index cols = dest.cols(); - for (Index j=0; j -EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& dest, const Func& func, const true_type&) { - typedef typename Dest::Index Index; - // FIXME make sure rhs is sequentially stored - // FIXME not very good if lhs is real and rhs complex while alpha is real too - const Index rows = dest.rows(); - for (Index i=0; i -struct traits > - : traits, Lhs, Rhs> > -{}; - -} - -template -class GeneralProduct - : public ProductBase, Lhs, Rhs> -{ - template struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; - - public: - EIGEN_PRODUCT_PUBLIC_INTERFACE(GeneralProduct) - - GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) - { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - } - - struct set { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } }; - struct add { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } }; - struct sub { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() -= src; } }; - struct adds { - Scalar m_scale; - adds(const Scalar& s) : m_scale(s) {} - template void operator()(const Dst& dst, const Src& src) const { - dst.const_cast_derived() += m_scale * src; - } - }; - - template - inline void evalTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, set(), is_row_major()); - } - - template - inline void addTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, add(), is_row_major()); - } - - template - inline void subTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, sub(), is_row_major()); - } - - template void scaleAndAddTo(Dest& dest, const Scalar& alpha) const - { - internal::outer_product_selector_run(*this, dest, adds(alpha), is_row_major()); - } -}; - /*********************************************************************** * Implementation of General Matrix Vector Product ***********************************************************************/ @@ -313,60 +136,13 @@ class GeneralProduct */ namespace internal { -template -struct traits > - : traits, Lhs, Rhs> > -{}; - template -struct gemv_selector; +struct gemv_dense_selector; } // end namespace internal -template -class GeneralProduct - : public ProductBase, Lhs, Rhs> -{ - public: - EIGEN_PRODUCT_PUBLIC_INTERFACE(GeneralProduct) - - typedef typename Lhs::Scalar LhsScalar; - typedef typename Rhs::Scalar RhsScalar; - - GeneralProduct(const Lhs& a_lhs, const Rhs& a_rhs) : Base(a_lhs,a_rhs) - { -// EIGEN_STATIC_ASSERT((internal::is_same::value), -// YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - } - - enum { Side = Lhs::IsVectorAtCompileTime ? OnTheLeft : OnTheRight }; - typedef typename internal::conditional::type MatrixType; - - template void scaleAndAddTo(Dest& dst, const Scalar& alpha) const - { - eigen_assert(m_lhs.rows() == dst.rows() && m_rhs.cols() == dst.cols()); - internal::gemv_selector::HasUsableDirectAccess)>::run(*this, dst, alpha); - } -}; - namespace internal { -// The vector is on the left => transposition -template -struct gemv_selector -{ - template - static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) - { - Transpose destT(dest); - enum { OtherStorageOrder = StorageOrder == RowMajor ? ColMajor : RowMajor }; - gemv_selector - ::run(GeneralProduct,Transpose, GemvProduct> - (prod.rhs().transpose(), prod.lhs().transpose()), destT, alpha); - } -}; - template struct gemv_static_vector_if; template @@ -384,46 +160,61 @@ struct gemv_static_vector_if template struct gemv_static_vector_if { - #if EIGEN_ALIGN_STATICALLY - internal::plain_array m_data; - EIGEN_STRONG_INLINE Scalar* data() { return m_data.array; } - #else - // Some architectures cannot align on the stack, - // => let's manually enforce alignment by allocating more data and return the address of the first aligned element. enum { ForceAlignment = internal::packet_traits::Vectorizable, PacketSize = internal::packet_traits::size }; - internal::plain_array m_data; + #if EIGEN_MAX_STATIC_ALIGN_BYTES!=0 + internal::plain_array m_data; + EIGEN_STRONG_INLINE Scalar* data() { return m_data.array; } + #else + // Some architectures cannot align on the stack, + // => let's manually enforce alignment by allocating more data and return the address of the first aligned element. + internal::plain_array m_data; EIGEN_STRONG_INLINE Scalar* data() { return ForceAlignment - ? reinterpret_cast((reinterpret_cast(m_data.array) & ~(size_t(15))) + 16) + ? reinterpret_cast((internal::UIntPtr(m_data.array) & ~(std::size_t(EIGEN_MAX_ALIGN_BYTES-1))) + EIGEN_MAX_ALIGN_BYTES) : m_data.array; } #endif }; -template<> struct gemv_selector +// The vector is on the left => transposition +template +struct gemv_dense_selector +{ + template + static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha) + { + Transpose destT(dest); + enum { OtherStorageOrder = StorageOrder == RowMajor ? ColMajor : RowMajor }; + gemv_dense_selector + ::run(rhs.transpose(), lhs.transpose(), destT, alpha); + } +}; + +template<> struct gemv_dense_selector { - template - static inline void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) + template + static inline void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha) { - typedef typename ProductType::Index Index; - typedef typename ProductType::LhsScalar LhsScalar; - typedef typename ProductType::RhsScalar RhsScalar; - typedef typename ProductType::Scalar ResScalar; - typedef typename ProductType::RealScalar RealScalar; - typedef typename ProductType::ActualLhsType ActualLhsType; - typedef typename ProductType::ActualRhsType ActualRhsType; - typedef typename ProductType::LhsBlasTraits LhsBlasTraits; - typedef typename ProductType::RhsBlasTraits RhsBlasTraits; - typedef Map, Aligned> MappedDest; - - ActualLhsType actualLhs = LhsBlasTraits::extract(prod.lhs()); - ActualRhsType actualRhs = RhsBlasTraits::extract(prod.rhs()); - - ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs()) - * RhsBlasTraits::extractScalarFactor(prod.rhs()); + typedef typename Lhs::Scalar LhsScalar; + typedef typename Rhs::Scalar RhsScalar; + typedef typename Dest::Scalar ResScalar; + typedef typename Dest::RealScalar RealScalar; + + typedef internal::blas_traits LhsBlasTraits; + typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType; + typedef internal::blas_traits RhsBlasTraits; + typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType; + + typedef Map, EIGEN_PLAIN_ENUM_MIN(AlignedMax,internal::packet_traits::size)> MappedDest; + + ActualLhsType actualLhs = LhsBlasTraits::extract(lhs); + ActualRhsType actualRhs = RhsBlasTraits::extract(rhs); + + ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs) + * RhsBlasTraits::extractScalarFactor(rhs); // make sure Dest is a compile-time vector type (bug 1166) typedef typename conditional::type ActualDest; @@ -433,80 +224,97 @@ template<> struct gemv_selector // on, the other hand it is good for the cache to pack the vector anyways... EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1), ComplexByReal = (NumTraits::IsComplex) && (!NumTraits::IsComplex), - MightCannotUseDest = (ActualDest::InnerStrideAtCompileTime!=1) || ComplexByReal + MightCannotUseDest = (!EvalToDestAtCompileTime) || ComplexByReal }; - gemv_static_vector_if static_dest; - - bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0)); - bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible; - + typedef const_blas_data_mapper LhsMapper; + typedef const_blas_data_mapper RhsMapper; RhsScalar compatibleAlpha = get_factor::run(actualAlpha); - ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(), - evalToDest ? dest.data() : static_dest.data()); - - if(!evalToDest) + if(!MightCannotUseDest) { - #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN - int size = dest.size(); - EIGEN_DENSE_STORAGE_CTOR_PLUGIN - #endif - if(!alphaIsCompatible) + // shortcut if we are sure to be able to use dest directly, + // this ease the compiler to generate cleaner and more optimzized code for most common cases + general_matrix_vector_product + ::run( + actualLhs.rows(), actualLhs.cols(), + LhsMapper(actualLhs.data(), actualLhs.outerStride()), + RhsMapper(actualRhs.data(), actualRhs.innerStride()), + dest.data(), 1, + compatibleAlpha); + } + else + { + gemv_static_vector_if static_dest; + + const bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0)); + const bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible; + + ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(), + evalToDest ? dest.data() : static_dest.data()); + + if(!evalToDest) { - MappedDest(actualDestPtr, dest.size()).setZero(); - compatibleAlpha = RhsScalar(1); + #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN + Index size = dest.size(); + EIGEN_DENSE_STORAGE_CTOR_PLUGIN + #endif + if(!alphaIsCompatible) + { + MappedDest(actualDestPtr, dest.size()).setZero(); + compatibleAlpha = RhsScalar(1); + } + else + MappedDest(actualDestPtr, dest.size()) = dest; } - else - MappedDest(actualDestPtr, dest.size()) = dest; - } - general_matrix_vector_product - ::run( - actualLhs.rows(), actualLhs.cols(), - actualLhs.data(), actualLhs.outerStride(), - actualRhs.data(), actualRhs.innerStride(), - actualDestPtr, 1, - compatibleAlpha); + general_matrix_vector_product + ::run( + actualLhs.rows(), actualLhs.cols(), + LhsMapper(actualLhs.data(), actualLhs.outerStride()), + RhsMapper(actualRhs.data(), actualRhs.innerStride()), + actualDestPtr, 1, + compatibleAlpha); - if (!evalToDest) - { - if(!alphaIsCompatible) - dest += actualAlpha * MappedDest(actualDestPtr, dest.size()); - else - dest = MappedDest(actualDestPtr, dest.size()); + if (!evalToDest) + { + if(!alphaIsCompatible) + dest.matrix() += actualAlpha * MappedDest(actualDestPtr, dest.size()); + else + dest = MappedDest(actualDestPtr, dest.size()); + } } } }; -template<> struct gemv_selector +template<> struct gemv_dense_selector { - template - static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) + template + static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha) { - typedef typename ProductType::LhsScalar LhsScalar; - typedef typename ProductType::RhsScalar RhsScalar; - typedef typename ProductType::Scalar ResScalar; - typedef typename ProductType::Index Index; - typedef typename ProductType::ActualLhsType ActualLhsType; - typedef typename ProductType::ActualRhsType ActualRhsType; - typedef typename ProductType::_ActualRhsType _ActualRhsType; - typedef typename ProductType::LhsBlasTraits LhsBlasTraits; - typedef typename ProductType::RhsBlasTraits RhsBlasTraits; - - typename add_const::type actualLhs = LhsBlasTraits::extract(prod.lhs()); - typename add_const::type actualRhs = RhsBlasTraits::extract(prod.rhs()); - - ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs()) - * RhsBlasTraits::extractScalarFactor(prod.rhs()); + typedef typename Lhs::Scalar LhsScalar; + typedef typename Rhs::Scalar RhsScalar; + typedef typename Dest::Scalar ResScalar; + + typedef internal::blas_traits LhsBlasTraits; + typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType; + typedef internal::blas_traits RhsBlasTraits; + typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType; + typedef typename internal::remove_all::type ActualRhsTypeCleaned; + + typename add_const::type actualLhs = LhsBlasTraits::extract(lhs); + typename add_const::type actualRhs = RhsBlasTraits::extract(rhs); + + ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs) + * RhsBlasTraits::extractScalarFactor(rhs); enum { // FIXME find a way to allow an inner stride on the result if packet_traits::size==1 // on, the other hand it is good for the cache to pack the vector anyways... - DirectlyUseRhs = _ActualRhsType::InnerStrideAtCompileTime==1 + DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime==1 }; - gemv_static_vector_if static_rhs; + gemv_static_vector_if static_rhs; ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,actualRhs.size(), DirectlyUseRhs ? const_cast(actualRhs.data()) : static_rhs.data()); @@ -514,45 +322,48 @@ template<> struct gemv_selector if(!DirectlyUseRhs) { #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN - int size = actualRhs.size(); + Index size = actualRhs.size(); EIGEN_DENSE_STORAGE_CTOR_PLUGIN #endif - Map(actualRhsPtr, actualRhs.size()) = actualRhs; + Map(actualRhsPtr, actualRhs.size()) = actualRhs; } + typedef const_blas_data_mapper LhsMapper; + typedef const_blas_data_mapper RhsMapper; general_matrix_vector_product - ::run( + ::run( actualLhs.rows(), actualLhs.cols(), - actualLhs.data(), actualLhs.outerStride(), - actualRhsPtr, 1, + LhsMapper(actualLhs.data(), actualLhs.outerStride()), + RhsMapper(actualRhsPtr, 1), dest.data(), dest.col(0).innerStride(), //NOTE if dest is not a vector at compile-time, then dest.innerStride() might be wrong. (bug 1166) actualAlpha); } }; -template<> struct gemv_selector +template<> struct gemv_dense_selector { - template - static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) + template + static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha) { - typedef typename Dest::Index Index; - // TODO makes sure dest is sequentially stored in memory, otherwise use a temp - const Index size = prod.rhs().rows(); + EIGEN_STATIC_ASSERT((!nested_eval::Evaluate),EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE); + // TODO if rhs is large enough it might be beneficial to make sure that dest is sequentially stored in memory, otherwise use a temp + typename nested_eval::type actual_rhs(rhs); + const Index size = rhs.rows(); for(Index k=0; k struct gemv_selector +template<> struct gemv_dense_selector { - template - static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) + template + static void run(const Lhs &lhs, const Rhs &rhs, Dest& dest, const typename Dest::Scalar& alpha) { - typedef typename Dest::Index Index; - // TODO makes sure rhs is sequentially stored in memory, otherwise use a temp - const Index rows = prod.rows(); + EIGEN_STATIC_ASSERT((!nested_eval::Evaluate),EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE); + typename nested_eval::type actual_rhs(rhs); + const Index rows = dest.rows(); for(Index i=0; i struct gemv_selector * * \sa lazyProduct(), operator*=(const MatrixBase&), Cwise::operator*() */ +#ifndef __CUDACC__ + template template -inline const typename ProductReturnType::Type +inline const Product MatrixBase::operator*(const MatrixBase &other) const { // A note regarding the function declaration: In MSVC, this function will sometimes @@ -595,9 +408,12 @@ MatrixBase::operator*(const MatrixBase &other) const #ifdef EIGEN_DEBUG_PRODUCT internal::product_type::debug(); #endif - return typename ProductReturnType::Type(derived(), other.derived()); + + return Product(derived(), other.derived()); } +#endif // __CUDACC__ + /** \returns an expression of the matrix product of \c *this and \a other without implicit evaluation. * * The returned product will behave like any other expressions: the coefficients of the product will be @@ -611,7 +427,7 @@ MatrixBase::operator*(const MatrixBase &other) const */ template template -const typename LazyProductReturnType::Type +const Product MatrixBase::lazyProduct(const MatrixBase &other) const { enum { @@ -630,7 +446,7 @@ MatrixBase::lazyProduct(const MatrixBase &other) const INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION) EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT) - return typename LazyProductReturnType::Type(derived(), other.derived()); + return Product(derived(), other.derived()); } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/GenericPacketMath.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/GenericPacketMath.h index 5f783ebeee..27033a2dd0 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/GenericPacketMath.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/GenericPacketMath.h @@ -42,21 +42,28 @@ namespace internal { struct default_packet_traits { enum { + HasHalfPacket = 0, + HasAdd = 1, HasSub = 1, HasMul = 1, HasNegate = 1, HasAbs = 1, + HasArg = 0, HasAbs2 = 1, HasMin = 1, HasMax = 1, HasConj = 1, HasSetLinear = 1, + HasBlend = 0, HasDiv = 0, HasSqrt = 0, + HasRsqrt = 0, HasExp = 0, HasLog = 0, + HasLog1p = 0, + HasLog10 = 0, HasPow = 0, HasSin = 0, @@ -64,17 +71,37 @@ struct default_packet_traits HasTan = 0, HasASin = 0, HasACos = 0, - HasATan = 0 + HasATan = 0, + HasSinh = 0, + HasCosh = 0, + HasTanh = 0, + HasLGamma = 0, + HasDiGamma = 0, + HasZeta = 0, + HasPolygamma = 0, + HasErf = 0, + HasErfc = 0, + HasIGamma = 0, + HasIGammac = 0, + HasBetaInc = 0, + + HasRound = 0, + HasFloor = 0, + HasCeil = 0, + + HasSign = 0 }; }; template struct packet_traits : default_packet_traits { typedef T type; + typedef T half; enum { Vectorizable = 0, size = 1, - AlignedOnScalar = 0 + AlignedOnScalar = 0, + HasHalfPacket = 0 }; enum { HasAdd = 0, @@ -90,135 +117,239 @@ template struct packet_traits : default_packet_traits }; }; +template struct packet_traits : packet_traits { }; + +template struct type_casting_traits { + enum { + VectorizedCast = 0, + SrcCoeffRatio = 1, + TgtCoeffRatio = 1 + }; +}; + + +/** \internal \returns static_cast(a) (coeff-wise) */ +template +EIGEN_DEVICE_FUNC inline TgtPacket +pcast(const SrcPacket& a) { + return static_cast(a); +} +template +EIGEN_DEVICE_FUNC inline TgtPacket +pcast(const SrcPacket& a, const SrcPacket& /*b*/) { + return static_cast(a); +} + +template +EIGEN_DEVICE_FUNC inline TgtPacket +pcast(const SrcPacket& a, const SrcPacket& /*b*/, const SrcPacket& /*c*/, const SrcPacket& /*d*/) { + return static_cast(a); +} + /** \internal \returns a + b (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet padd(const Packet& a, const Packet& b) { return a+b; } /** \internal \returns a - b (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet psub(const Packet& a, const Packet& b) { return a-b; } /** \internal \returns -a (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pnegate(const Packet& a) { return -a; } /** \internal \returns conj(a) (coeff-wise) */ -template inline Packet + +template EIGEN_DEVICE_FUNC inline Packet pconj(const Packet& a) { return numext::conj(a); } /** \internal \returns a * b (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pmul(const Packet& a, const Packet& b) { return a*b; } /** \internal \returns a / b (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pdiv(const Packet& a, const Packet& b) { return a/b; } /** \internal \returns the min of \a a and \a b (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, - const Packet& b) { using std::min; return (min)(a, b); } + const Packet& b) { return numext::mini(a, b); } /** \internal \returns the max of \a a and \a b (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, - const Packet& b) { using std::max; return (max)(a, b); } + const Packet& b) { return numext::maxi(a, b); } /** \internal \returns the absolute value of \a a */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pabs(const Packet& a) { using std::abs; return abs(a); } +/** \internal \returns the phase angle of \a a */ +template EIGEN_DEVICE_FUNC inline Packet +parg(const Packet& a) { using numext::arg; return arg(a); } + /** \internal \returns the bitwise and of \a a and \a b */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pand(const Packet& a, const Packet& b) { return a & b; } /** \internal \returns the bitwise or of \a a and \a b */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet por(const Packet& a, const Packet& b) { return a | b; } /** \internal \returns the bitwise xor of \a a and \a b */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pxor(const Packet& a, const Packet& b) { return a ^ b; } /** \internal \returns the bitwise andnot of \a a and \a b */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pandnot(const Packet& a, const Packet& b) { return a & (!b); } /** \internal \returns a packet version of \a *from, from must be 16 bytes aligned */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pload(const typename unpacket_traits::type* from) { return *from; } /** \internal \returns a packet version of \a *from, (un-aligned load) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet ploadu(const typename unpacket_traits::type* from) { return *from; } +/** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */ +template EIGEN_DEVICE_FUNC inline Packet +pset1(const typename unpacket_traits::type& a) { return a; } + +/** \internal \returns a packet with constant coefficients \a a[0], e.g.: (a[0],a[0],a[0],a[0]) */ +template EIGEN_DEVICE_FUNC inline Packet +pload1(const typename unpacket_traits::type *a) { return pset1(*a); } + /** \internal \returns a packet with elements of \a *from duplicated. - * For instance, for a packet of 8 elements, 4 scalar will be read from \a *from and - * duplicated to form: {from[0],from[0],from[1],from[1],,from[2],from[2],,from[3],from[3]} + * For instance, for a packet of 8 elements, 4 scalars will be read from \a *from and + * duplicated to form: {from[0],from[0],from[1],from[1],from[2],from[2],from[3],from[3]} * Currently, this function is only used for scalar * complex products. - */ -template inline Packet + */ +template EIGEN_DEVICE_FUNC inline Packet ploaddup(const typename unpacket_traits::type* from) { return *from; } -/** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */ -template inline Packet -pset1(const typename unpacket_traits::type& a) { return a; } +/** \internal \returns a packet with elements of \a *from quadrupled. + * For instance, for a packet of 8 elements, 2 scalars will be read from \a *from and + * replicated to form: {from[0],from[0],from[0],from[0],from[1],from[1],from[1],from[1]} + * Currently, this function is only used in matrix products. + * For packet-size smaller or equal to 4, this function is equivalent to pload1 + */ +template EIGEN_DEVICE_FUNC inline Packet +ploadquad(const typename unpacket_traits::type* from) +{ return pload1(from); } + +/** \internal equivalent to + * \code + * a0 = pload1(a+0); + * a1 = pload1(a+1); + * a2 = pload1(a+2); + * a3 = pload1(a+3); + * \endcode + * \sa pset1, pload1, ploaddup, pbroadcast2 + */ +template EIGEN_DEVICE_FUNC +inline void pbroadcast4(const typename unpacket_traits::type *a, + Packet& a0, Packet& a1, Packet& a2, Packet& a3) +{ + a0 = pload1(a+0); + a1 = pload1(a+1); + a2 = pload1(a+2); + a3 = pload1(a+3); +} + +/** \internal equivalent to + * \code + * a0 = pload1(a+0); + * a1 = pload1(a+1); + * \endcode + * \sa pset1, pload1, ploaddup, pbroadcast4 + */ +template EIGEN_DEVICE_FUNC +inline void pbroadcast2(const typename unpacket_traits::type *a, + Packet& a0, Packet& a1) +{ + a0 = pload1(a+0); + a1 = pload1(a+1); +} /** \internal \brief Returns a packet with coefficients (a,a+1,...,a+packet_size-1). */ -template inline typename packet_traits::type -plset(const Scalar& a) { return a; } +template inline Packet +plset(const typename unpacket_traits::type& a) { return a; } /** \internal copy the packet \a from to \a *to, \a to must be 16 bytes aligned */ -template inline void pstore(Scalar* to, const Packet& from) +template EIGEN_DEVICE_FUNC inline void pstore(Scalar* to, const Packet& from) { (*to) = from; } /** \internal copy the packet \a from to \a *to, (un-aligned store) */ -template inline void pstoreu(Scalar* to, const Packet& from) -{ (*to) = from; } +template EIGEN_DEVICE_FUNC inline void pstoreu(Scalar* to, const Packet& from) +{ (*to) = from; } + + template EIGEN_DEVICE_FUNC inline Packet pgather(const Scalar* from, Index /*stride*/) + { return ploadu(from); } + + template EIGEN_DEVICE_FUNC inline void pscatter(Scalar* to, const Packet& from, Index /*stride*/) + { pstore(to, from); } /** \internal tries to do cache prefetching of \a addr */ -template inline void prefetch(const Scalar* addr) +template EIGEN_DEVICE_FUNC inline void prefetch(const Scalar* addr) { -#if !defined(_MSC_VER) -__builtin_prefetch(addr); +#ifdef __CUDA_ARCH__ +#if defined(__LP64__) + // 64-bit pointer operand constraint for inlined asm + asm(" prefetch.L1 [ %1 ];" : "=l"(addr) : "l"(addr)); +#else + // 32-bit pointer operand constraint for inlined asm + asm(" prefetch.L1 [ %1 ];" : "=r"(addr) : "r"(addr)); +#endif +#elif (!EIGEN_COMP_MSVC) && (EIGEN_COMP_GNUC || EIGEN_COMP_CLANG || EIGEN_COMP_ICC) + __builtin_prefetch(addr); #endif } /** \internal \returns the first element of a packet */ -template inline typename unpacket_traits::type pfirst(const Packet& a) +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type pfirst(const Packet& a) { return a; } /** \internal \returns a packet where the element i contains the sum of the packet of \a vec[i] */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet preduxp(const Packet* vecs) { return vecs[0]; } /** \internal \returns the sum of the elements of \a a*/ -template inline typename unpacket_traits::type predux(const Packet& a) +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux(const Packet& a) +{ return a; } + +/** \internal \returns the sum of the elements of \a a by block of 4 elements. + * For a packet {a0, a1, a2, a3, a4, a5, a6, a7}, it returns a half packet {a0+a4, a1+a5, a2+a6, a3+a7} + * For packet-size smaller or equal to 4, this boils down to a noop. + */ +template EIGEN_DEVICE_FUNC inline +typename conditional<(unpacket_traits::size%8)==0,typename unpacket_traits::half,Packet>::type +predux_downto4(const Packet& a) { return a; } /** \internal \returns the product of the elements of \a a*/ -template inline typename unpacket_traits::type predux_mul(const Packet& a) +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_mul(const Packet& a) { return a; } /** \internal \returns the min of the elements of \a a*/ -template inline typename unpacket_traits::type predux_min(const Packet& a) +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_min(const Packet& a) { return a; } /** \internal \returns the max of the elements of \a a*/ -template inline typename unpacket_traits::type predux_max(const Packet& a) +template EIGEN_DEVICE_FUNC inline typename unpacket_traits::type predux_max(const Packet& a) { return a; } /** \internal \returns the reversed elements of \a a*/ -template inline Packet preverse(const Packet& a) +template EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& a) { return a; } - /** \internal \returns \a a with real and imaginary part flipped (for complex type only) */ -template inline Packet pcplxflip(const Packet& a) +template EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& a) { // FIXME: uncomment the following in case we drop the internal imag and real functions. // using std::imag; @@ -250,6 +381,22 @@ Packet pasin(const Packet& a) { using std::asin; return asin(a); } template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pacos(const Packet& a) { using std::acos; return acos(a); } +/** \internal \returns the arc tangent of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet patan(const Packet& a) { using std::atan; return atan(a); } + +/** \internal \returns the hyperbolic sine of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet psinh(const Packet& a) { using std::sinh; return sinh(a); } + +/** \internal \returns the hyperbolic cosine of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet pcosh(const Packet& a) { using std::cosh; return cosh(a); } + +/** \internal \returns the hyperbolic tan of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet ptanh(const Packet& a) { using std::tanh; return tanh(a); } + /** \internal \returns the exp of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp(const Packet& a) { using std::exp; return exp(a); } @@ -258,10 +405,36 @@ Packet pexp(const Packet& a) { using std::exp; return exp(a); } template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog(const Packet& a) { using std::log; return log(a); } +/** \internal \returns the log1p of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet plog1p(const Packet& a) { return numext::log1p(a); } + +/** \internal \returns the log10 of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet plog10(const Packet& a) { using std::log10; return log10(a); } + /** \internal \returns the square-root of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet psqrt(const Packet& a) { using std::sqrt; return sqrt(a); } +/** \internal \returns the reciprocal square-root of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet prsqrt(const Packet& a) { + return pdiv(pset1(1), psqrt(a)); +} + +/** \internal \returns the rounded value of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet pround(const Packet& a) { using numext::round; return round(a); } + +/** \internal \returns the floor of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet pfloor(const Packet& a) { using numext::floor; return floor(a); } + +/** \internal \returns the ceil of \a a (coeff-wise) */ +template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS +Packet pceil(const Packet& a) { using numext::ceil; return ceil(a); } + /*************************************************************************** * The following functions might not have to be overwritten for vectorized types ***************************************************************************/ @@ -275,34 +448,45 @@ inline void pstore1(typename unpacket_traits::type* to, const typename u } /** \internal \returns a * b + c (coeff-wise) */ -template inline Packet +template EIGEN_DEVICE_FUNC inline Packet pmadd(const Packet& a, const Packet& b, const Packet& c) { return padd(pmul(a, b),c); } /** \internal \returns a packet version of \a *from. - * If LoadMode equals #Aligned, \a from must be 16 bytes aligned */ -template -inline Packet ploadt(const typename unpacket_traits::type* from) + * The pointer \a from must be aligned on a \a Alignment bytes boundary. */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet ploadt(const typename unpacket_traits::type* from) { - if(LoadMode == Aligned) + if(Alignment >= unpacket_traits::alignment) return pload(from); else return ploadu(from); } /** \internal copy the packet \a from to \a *to. - * If StoreMode equals #Aligned, \a to must be 16 bytes aligned */ -template -inline void pstoret(Scalar* to, const Packet& from) + * The pointer \a from must be aligned on a \a Alignment bytes boundary. */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void pstoret(Scalar* to, const Packet& from) { - if(LoadMode == Aligned) + if(Alignment >= unpacket_traits::alignment) pstore(to, from); else pstoreu(to, from); } +/** \internal \returns a packet version of \a *from. + * Unlike ploadt, ploadt_ro takes advantage of the read-only memory path on the + * hardware if available to speedup the loading of data that won't be modified + * by the current computation. + */ +template +inline Packet ploadt_ro(const typename unpacket_traits::type* from) +{ + return ploadt(from); +} + /** \internal default implementation of palign() allowing partial specialization */ template struct palign_impl @@ -336,15 +520,74 @@ inline void palign(PacketType& first, const PacketType& second) * Fast complex products (GCC generates a function call which is very slow) ***************************************************************************/ +// Eigen+CUDA does not support complexes. +#ifndef __CUDACC__ + template<> inline std::complex pmul(const std::complex& a, const std::complex& b) { return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } template<> inline std::complex pmul(const std::complex& a, const std::complex& b) { return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } +#endif + + +/*************************************************************************** + * PacketBlock, that is a collection of N packets where the number of words + * in the packet is a multiple of N. +***************************************************************************/ +template ::size> struct PacketBlock { + Packet packet[N]; +}; + +template EIGEN_DEVICE_FUNC inline void +ptranspose(PacketBlock& /*kernel*/) { + // Nothing to do in the scalar case, i.e. a 1x1 matrix. +} + +/*************************************************************************** + * Selector, i.e. vector of N boolean values used to select (i.e. blend) + * words from 2 packets. +***************************************************************************/ +template struct Selector { + bool select[N]; +}; + +template EIGEN_DEVICE_FUNC inline Packet +pblend(const Selector::size>& ifPacket, const Packet& thenPacket, const Packet& elsePacket) { + return ifPacket.select[0] ? thenPacket : elsePacket; +} + +/** \internal \returns \a a with the first coefficient replaced by the scalar b */ +template EIGEN_DEVICE_FUNC inline Packet +pinsertfirst(const Packet& a, typename unpacket_traits::type b) +{ + // Default implementation based on pblend. + // It must be specialized for higher performance. + Selector::size> mask; + mask.select[0] = true; + // This for loop should be optimized away by the compiler. + for(Index i=1; i::size; ++i) + mask.select[i] = false; + return pblend(mask, pset1(b), a); +} + +/** \internal \returns \a a with the last coefficient replaced by the scalar b */ +template EIGEN_DEVICE_FUNC inline Packet +pinsertlast(const Packet& a, typename unpacket_traits::type b) +{ + // Default implementation based on pblend. + // It must be specialized for higher performance. + Selector::size> mask; + // This for loop should be optimized away by the compiler. + for(Index i=0; i::size-1; ++i) + mask.select[i] = false; + mask.select[unpacket_traits::size-1] = true; + return pblend(mask, pset1(b), a); +} + } // end namespace internal } // end namespace Eigen #endif // EIGEN_GENERIC_PACKET_MATH_H - diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/GlobalFunctions.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/GlobalFunctions.h index 2acf977233..769dc255c2 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/GlobalFunctions.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/GlobalFunctions.h @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2010-2012 Gael Guennebaud +// Copyright (C) 2010-2016 Gael Guennebaud // Copyright (C) 2010 Benoit Jacob // // This Source Code Form is subject to the terms of the Mozilla @@ -11,13 +11,30 @@ #ifndef EIGEN_GLOBAL_FUNCTIONS_H #define EIGEN_GLOBAL_FUNCTIONS_H -#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME,FUNCTOR) \ +#ifdef EIGEN_PARSED_BY_DOXYGEN + +#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME,FUNCTOR,DOC_OP,DOC_DETAILS) \ + /** \returns an expression of the coefficient-wise DOC_OP of \a x + + DOC_DETAILS + + \sa Math functions, class CwiseUnaryOp + */ \ + template \ + inline const Eigen::CwiseUnaryOp, const Derived> \ + NAME(const Eigen::ArrayBase& x); + +#else + +#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME,FUNCTOR,DOC_OP,DOC_DETAILS) \ template \ inline const Eigen::CwiseUnaryOp, const Derived> \ - NAME(const Eigen::ArrayBase& x) { \ - return x.derived(); \ + (NAME)(const Eigen::ArrayBase& x) { \ + return Eigen::CwiseUnaryOp, const Derived>(x.derived()); \ } +#endif // EIGEN_PARSED_BY_DOXYGEN + #define EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(NAME,FUNCTOR) \ \ template \ @@ -30,55 +47,133 @@ { \ static inline typename NAME##_retval >::type run(const Eigen::ArrayBase& x) \ { \ - return x.derived(); \ + return typename NAME##_retval >::type(x.derived()); \ } \ }; - namespace Eigen { - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(real,scalar_real_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(imag,scalar_imag_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(conj,scalar_conjugate_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sin,scalar_sin_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cos,scalar_cos_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asin,scalar_asin_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acos,scalar_acos_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tan,scalar_tan_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp,scalar_exp_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log,scalar_log_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs,scalar_abs_op) - EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt,scalar_sqrt_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(real,scalar_real_op,real part,\sa ArrayBase::real) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(imag,scalar_imag_op,imaginary part,\sa ArrayBase::imag) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(conj,scalar_conjugate_op,complex conjugate,\sa ArrayBase::conjugate) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(inverse,scalar_inverse_op,inverse,\sa ArrayBase::inverse) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sin,scalar_sin_op,sine,\sa ArrayBase::sin) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cos,scalar_cos_op,cosine,\sa ArrayBase::cos) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tan,scalar_tan_op,tangent,\sa ArrayBase::tan) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atan,scalar_atan_op,arc-tangent,\sa ArrayBase::atan) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asin,scalar_asin_op,arc-sine,\sa ArrayBase::asin) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acos,scalar_acos_op,arc-consine,\sa ArrayBase::acos) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sinh,scalar_sinh_op,hyperbolic sine,\sa ArrayBase::sinh) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cosh,scalar_cosh_op,hyperbolic cosine,\sa ArrayBase::cosh) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tanh,scalar_tanh_op,hyperbolic tangent,\sa ArrayBase::tanh) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(lgamma,scalar_lgamma_op,natural logarithm of the gamma function,\sa ArrayBase::lgamma) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(digamma,scalar_digamma_op,derivative of lgamma,\sa ArrayBase::digamma) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erf,scalar_erf_op,error function,\sa ArrayBase::erf) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erfc,scalar_erfc_op,complement error function,\sa ArrayBase::erfc) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp,scalar_exp_op,exponential,\sa ArrayBase::exp) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log,scalar_log_op,natural logarithm,\sa Eigen::log10 DOXCOMMA ArrayBase::log) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log1p,scalar_log1p_op,natural logarithm of 1 plus the value,\sa ArrayBase::log1p) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log10,scalar_log10_op,base 10 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs,scalar_abs_op,absolute value,\sa ArrayBase::abs DOXCOMMA MatrixBase::cwiseAbs) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs2,scalar_abs2_op,squared absolute value,\sa ArrayBase::abs2 DOXCOMMA MatrixBase::cwiseAbs2) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(arg,scalar_arg_op,complex argument,\sa ArrayBase::arg) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt,scalar_sqrt_op,square root,\sa ArrayBase::sqrt DOXCOMMA MatrixBase::cwiseSqrt) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rsqrt,scalar_rsqrt_op,reciprocal square root,\sa ArrayBase::rsqrt) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(square,scalar_square_op,square (power 2),\sa Eigen::abs2 DOXCOMMA Eigen::pow DOXCOMMA ArrayBase::square) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cube,scalar_cube_op,cube (power 3),\sa Eigen::pow DOXCOMMA ArrayBase::cube) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(round,scalar_round_op,nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(floor,scalar_floor_op,nearest integer not greater than the giben value,\sa Eigen::ceil DOXCOMMA ArrayBase::floor) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ceil,scalar_ceil_op,nearest integer not less than the giben value,\sa Eigen::floor DOXCOMMA ArrayBase::ceil) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isnan,scalar_isnan_op,not-a-number test,\sa Eigen::isinf DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isnan) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isinf,scalar_isinf_op,infinite value test,\sa Eigen::isnan DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isinf) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isfinite,scalar_isfinite_op,finite value test,\sa Eigen::isinf DOXCOMMA Eigen::isnan DOXCOMMA ArrayBase::isfinite) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sign,scalar_sign_op,sign (or 0),\sa ArrayBase::sign) + /** \returns an expression of the coefficient-wise power of \a x to the given constant \a exponent. + * + * \tparam ScalarExponent is the scalar type of \a exponent. It must be compatible with the scalar type of the given expression (\c Derived::Scalar). + * + * \sa ArrayBase::pow() + * + * \relates ArrayBase + */ +#ifdef EIGEN_PARSED_BY_DOXYGEN + template + inline const CwiseBinaryOp,Derived,Constant > + pow(const Eigen::ArrayBase& x, const ScalarExponent& exponent); +#else + template + inline typename internal::enable_if< !(internal::is_same::value) && EIGEN_SCALAR_BINARY_SUPPORTED(pow,typename Derived::Scalar,ScalarExponent), + const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,ScalarExponent,pow) >::type + pow(const Eigen::ArrayBase& x, const ScalarExponent& exponent) { + return x.derived().pow(exponent); + } + template - inline const Eigen::CwiseUnaryOp, const Derived> + inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename Derived::Scalar,pow) pow(const Eigen::ArrayBase& x, const typename Derived::Scalar& exponent) { return x.derived().pow(exponent); } +#endif - template - inline const Eigen::CwiseBinaryOp, const Derived, const Derived> - pow(const Eigen::ArrayBase& x, const Eigen::ArrayBase& exponents) + /** \returns an expression of the coefficient-wise power of \a x to the given array of \a exponents. + * + * This function computes the coefficient-wise power. + * + * Example: \include Cwise_array_power_array.cpp + * Output: \verbinclude Cwise_array_power_array.out + * + * \sa ArrayBase::pow() + * + * \relates ArrayBase + */ + template + inline const Eigen::CwiseBinaryOp, const Derived, const ExponentDerived> + pow(const Eigen::ArrayBase& x, const Eigen::ArrayBase& exponents) { - return Eigen::CwiseBinaryOp, const Derived, const Derived>( + return Eigen::CwiseBinaryOp, const Derived, const ExponentDerived>( x.derived(), exponents.derived() ); } - /** - * \brief Component-wise division of a scalar by array elements. - **/ - template - inline const Eigen::CwiseUnaryOp, const Derived> - operator/(const typename Derived::Scalar& s, const Eigen::ArrayBase& a) + /** \returns an expression of the coefficient-wise power of the scalar \a x to the given array of \a exponents. + * + * This function computes the coefficient-wise power between a scalar and an array of exponents. + * + * \tparam Scalar is the scalar type of \a x. It must be compatible with the scalar type of the given array expression (\c Derived::Scalar). + * + * Example: \include Cwise_scalar_power_array.cpp + * Output: \verbinclude Cwise_scalar_power_array.out + * + * \sa ArrayBase::pow() + * + * \relates ArrayBase + */ +#ifdef EIGEN_PARSED_BY_DOXYGEN + template + inline const CwiseBinaryOp,Constant,Derived> + pow(const Scalar& x,const Eigen::ArrayBase& x); +#else + template + inline typename internal::enable_if< !(internal::is_same::value) && EIGEN_SCALAR_BINARY_SUPPORTED(pow,Scalar,typename Derived::Scalar), + const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,Derived,pow) >::type + pow(const Scalar& x, const Eigen::ArrayBase& exponents) { - return Eigen::CwiseUnaryOp, const Derived>( - a.derived(), - Eigen::internal::scalar_inverse_mult_op(s) - ); + return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,Derived,pow)( + typename internal::plain_constant_type::type(exponents.rows(), exponents.cols(), x), exponents.derived() ); } + template + inline const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename Derived::Scalar,Derived,pow) + pow(const typename Derived::Scalar& x, const Eigen::ArrayBase& exponents) + { + return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename Derived::Scalar,Derived,pow)( + typename internal::plain_constant_type::type(exponents.rows(), exponents.cols(), x), exponents.derived() ); + } +#endif + + namespace internal { EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(real,scalar_real_op) diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/IO.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/IO.h index 8d4bc59e9d..da7fd6cce2 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/IO.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/IO.h @@ -49,7 +49,7 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& */ struct IOFormat { - /** Default contructor, see class IOFormat for the meaning of the parameters */ + /** Default constructor, see class IOFormat for the meaning of the parameters */ IOFormat(int _precision = StreamPrecision, int _flags = 0, const std::string& _coeffSeparator = " ", const std::string& _rowSeparator = "\n", const std::string& _rowPrefix="", const std::string& _rowSuffix="", @@ -57,6 +57,10 @@ struct IOFormat : matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator), rowSpacer(""), coeffSeparator(_coeffSeparator), precision(_precision), flags(_flags) { + // TODO check if rowPrefix, rowSuffix or rowSeparator contains a newline + // don't add rowSpacer if columns are not to be aligned + if((flags & DontAlignCols)) + return; int i = int(matSuffix.length())-1; while (i>=0 && matSuffix[i]!='\n') { @@ -76,7 +80,7 @@ struct IOFormat * * \brief Pseudo expression providing matrix output with given format * - * \param ExpressionType the type of the object on which IO stream operations are performed + * \tparam ExpressionType the type of the object on which IO stream operations are performed * * This class represents an expression with stream operators controlled by a given IOFormat. * It is the return type of DenseBase::format() @@ -101,52 +105,24 @@ class WithFormat } protected: - const typename ExpressionType::Nested m_matrix; + typename ExpressionType::Nested m_matrix; IOFormat m_format; }; -/** \returns a WithFormat proxy object allowing to print a matrix the with given - * format \a fmt. - * - * See class IOFormat for some examples. - * - * \sa class IOFormat, class WithFormat - */ -template -inline const WithFormat -DenseBase::format(const IOFormat& fmt) const -{ - return WithFormat(derived(), fmt); -} - namespace internal { -template -struct significant_decimals_default_impl -{ - typedef typename NumTraits::Real RealScalar; - static inline int run() - { - using std::ceil; - using std::log; - return cast(ceil(-log(NumTraits::epsilon())/log(RealScalar(10)))); - } -}; - +// NOTE: This helper is kept for backward compatibility with previous code specializing +// this internal::significant_decimals_impl structure. In the future we should directly +// call digits10() which has been introduced in July 2016 in 3.3. template -struct significant_decimals_default_impl +struct significant_decimals_impl { static inline int run() { - return 0; + return NumTraits::digits10(); } }; -template -struct significant_decimals_impl - : significant_decimals_default_impl::IsInteger> -{}; - /** \internal * print the matrix \a _m to the output stream \a s using the output format \a fmt */ template @@ -160,7 +136,6 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& typename Derived::Nested m = _m; typedef typename Derived::Scalar Scalar; - typedef typename Derived::Index Index; Index width = 0; diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Inverse.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Inverse.h new file mode 100644 index 0000000000..b76f0439d8 --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Inverse.h @@ -0,0 +1,118 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_INVERSE_H +#define EIGEN_INVERSE_H + +namespace Eigen { + +template class InverseImpl; + +namespace internal { + +template +struct traits > + : traits +{ + typedef typename XprType::PlainObject PlainObject; + typedef traits BaseTraits; + enum { + Flags = BaseTraits::Flags & RowMajorBit + }; +}; + +} // end namespace internal + +/** \class Inverse + * + * \brief Expression of the inverse of another expression + * + * \tparam XprType the type of the expression we are taking the inverse + * + * This class represents an abstract expression of A.inverse() + * and most of the time this is the only way it is used. + * + */ +template +class Inverse : public InverseImpl::StorageKind> +{ +public: + typedef typename XprType::StorageIndex StorageIndex; + typedef typename XprType::PlainObject PlainObject; + typedef typename XprType::Scalar Scalar; + typedef typename internal::ref_selector::type XprTypeNested; + typedef typename internal::remove_all::type XprTypeNestedCleaned; + typedef typename internal::ref_selector::type Nested; + typedef typename internal::remove_all::type NestedExpression; + + explicit EIGEN_DEVICE_FUNC Inverse(const XprType &xpr) + : m_xpr(xpr) + {} + + EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); } + + EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; } + +protected: + XprTypeNested m_xpr; +}; + +// Generic API dispatcher +template +class InverseImpl + : public internal::generic_xpr_base >::type +{ +public: + typedef typename internal::generic_xpr_base >::type Base; + typedef typename XprType::Scalar Scalar; +private: + + Scalar coeff(Index row, Index col) const; + Scalar coeff(Index i) const; +}; + +namespace internal { + +/** \internal + * \brief Default evaluator for Inverse expression. + * + * This default evaluator for Inverse expression simply evaluate the inverse into a temporary + * by a call to internal::call_assignment_no_alias. + * Therefore, inverse implementers only have to specialize Assignment, ...> for + * there own nested expression. + * + * \sa class Inverse + */ +template +struct unary_evaluator > + : public evaluator::PlainObject> +{ + typedef Inverse InverseType; + typedef typename InverseType::PlainObject PlainObject; + typedef evaluator Base; + + enum { Flags = Base::Flags | EvalBeforeNestingBit }; + + unary_evaluator(const InverseType& inv_xpr) + : m_result(inv_xpr.rows(), inv_xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + internal::call_assignment_no_alias(m_result, inv_xpr); + } + +protected: + PlainObject m_result; +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_INVERSE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Map.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Map.h index f804c89d63..06d1967024 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Map.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Map.h @@ -13,13 +13,35 @@ namespace Eigen { +namespace internal { +template +struct traits > + : public traits +{ + typedef traits TraitsBase; + enum { + InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0 + ? int(PlainObjectType::InnerStrideAtCompileTime) + : int(StrideType::InnerStrideAtCompileTime), + OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0 + ? int(PlainObjectType::OuterStrideAtCompileTime) + : int(StrideType::OuterStrideAtCompileTime), + Alignment = int(MapOptions)&int(AlignedMask), + Flags0 = TraitsBase::Flags & (~NestByRefBit), + Flags = is_lvalue::value ? int(Flags0) : (int(Flags0) & ~LvalueBit) + }; +private: + enum { Options }; // Expressions don't have Options +}; +} + /** \class Map * \ingroup Core_Module * * \brief A matrix or vector expression mapping an existing array of data. * * \tparam PlainObjectType the equivalent matrix type of the mapped data - * \tparam MapOptions specifies whether the pointer is \c #Aligned, or \c #Unaligned. + * \tparam MapOptions specifies the pointer alignment in bytes. It can be: \c #Aligned128, , \c #Aligned64, \c #Aligned32, \c #Aligned16, \c #Aligned8 or \c #Unaligned. * The default is \c #Unaligned. * \tparam StrideType optionally specifies strides. By default, Map assumes the memory layout * of an ordinary, contiguous array. This can be overridden by specifying strides. @@ -63,44 +85,6 @@ namespace Eigen { * * \sa PlainObjectBase::Map(), \ref TopicStorageOrders */ - -namespace internal { -template -struct traits > - : public traits -{ - typedef traits TraitsBase; - typedef typename PlainObjectType::Index Index; - typedef typename PlainObjectType::Scalar Scalar; - enum { - InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0 - ? int(PlainObjectType::InnerStrideAtCompileTime) - : int(StrideType::InnerStrideAtCompileTime), - OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0 - ? int(PlainObjectType::OuterStrideAtCompileTime) - : int(StrideType::OuterStrideAtCompileTime), - HasNoInnerStride = InnerStrideAtCompileTime == 1, - HasNoOuterStride = StrideType::OuterStrideAtCompileTime == 0, - HasNoStride = HasNoInnerStride && HasNoOuterStride, - IsAligned = bool(EIGEN_ALIGN) && ((int(MapOptions)&Aligned)==Aligned), - IsDynamicSize = PlainObjectType::SizeAtCompileTime==Dynamic, - KeepsPacketAccess = bool(HasNoInnerStride) - && ( bool(IsDynamicSize) - || HasNoOuterStride - || ( OuterStrideAtCompileTime!=Dynamic - && ((static_cast(sizeof(Scalar))*OuterStrideAtCompileTime)%16)==0 ) ), - Flags0 = TraitsBase::Flags & (~NestByRefBit), - Flags1 = IsAligned ? (int(Flags0) | AlignedBit) : (int(Flags0) & ~AlignedBit), - Flags2 = (bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime)) - ? int(Flags1) : int(Flags1 & ~LinearAccessBit), - Flags3 = is_lvalue::value ? int(Flags2) : (int(Flags2) & ~LvalueBit), - Flags = KeepsPacketAccess ? int(Flags3) : (int(Flags3) & ~PacketAccessBit) - }; -private: - enum { Options }; // Expressions don't have Options -}; -} - template class Map : public MapBase > { @@ -110,19 +94,17 @@ template class Ma EIGEN_DENSE_PUBLIC_INTERFACE(Map) typedef typename Base::PointerType PointerType; -#if EIGEN2_SUPPORT_STAGE <= STAGE30_FULL_EIGEN3_API - typedef const Scalar* PointerArgType; - inline PointerType cast_to_pointer_type(PointerArgType ptr) { return const_cast(ptr); } -#else typedef PointerType PointerArgType; + EIGEN_DEVICE_FUNC inline PointerType cast_to_pointer_type(PointerArgType ptr) { return ptr; } -#endif + EIGEN_DEVICE_FUNC inline Index innerStride() const { return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() @@ -134,10 +116,11 @@ template class Ma /** Constructor in the fixed-size case. * * \param dataPtr pointer to the array to map - * \param a_stride optional Stride object, passing the strides. + * \param stride optional Stride object, passing the strides. */ - inline Map(PointerArgType dataPtr, const StrideType& a_stride = StrideType()) - : Base(cast_to_pointer_type(dataPtr)), m_stride(a_stride) + EIGEN_DEVICE_FUNC + explicit inline Map(PointerArgType dataPtr, const StrideType& stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr)), m_stride(stride) { PlainObjectType::Base::_check_template_params(); } @@ -145,11 +128,12 @@ template class Ma /** Constructor in the dynamic-size vector case. * * \param dataPtr pointer to the array to map - * \param a_size the size of the vector expression - * \param a_stride optional Stride object, passing the strides. + * \param size the size of the vector expression + * \param stride optional Stride object, passing the strides. */ - inline Map(PointerArgType dataPtr, Index a_size, const StrideType& a_stride = StrideType()) - : Base(cast_to_pointer_type(dataPtr), a_size), m_stride(a_stride) + EIGEN_DEVICE_FUNC + inline Map(PointerArgType dataPtr, Index size, const StrideType& stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr), size), m_stride(stride) { PlainObjectType::Base::_check_template_params(); } @@ -157,12 +141,13 @@ template class Ma /** Constructor in the dynamic-size matrix case. * * \param dataPtr pointer to the array to map - * \param nbRows the number of rows of the matrix expression - * \param nbCols the number of columns of the matrix expression - * \param a_stride optional Stride object, passing the strides. + * \param rows the number of rows of the matrix expression + * \param cols the number of columns of the matrix expression + * \param stride optional Stride object, passing the strides. */ - inline Map(PointerArgType dataPtr, Index nbRows, Index nbCols, const StrideType& a_stride = StrideType()) - : Base(cast_to_pointer_type(dataPtr), nbRows, nbCols), m_stride(a_stride) + EIGEN_DEVICE_FUNC + inline Map(PointerArgType dataPtr, Index rows, Index cols, const StrideType& stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr), rows, cols), m_stride(stride) { PlainObjectType::Base::_check_template_params(); } @@ -173,19 +158,6 @@ template class Ma StrideType m_stride; }; -template -inline Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> - ::Array(const Scalar *data) -{ - this->_set_noalias(Eigen::Map(data)); -} - -template -inline Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> - ::Matrix(const Scalar *data) -{ - this->_set_noalias(Eigen::Map(data)); -} } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/MapBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/MapBase.h index 81efc4a6d5..020f939ad6 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/MapBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/MapBase.h @@ -12,15 +12,25 @@ #define EIGEN_MAPBASE_H #define EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) \ - EIGEN_STATIC_ASSERT((int(internal::traits::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \ + EIGEN_STATIC_ASSERT((int(internal::evaluator::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \ YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT) namespace Eigen { -/** \class MapBase - * \ingroup Core_Module +/** \ingroup Core_Module * - * \brief Base class for Map and Block expression with direct access + * \brief Base class for dense Map and Block expression with direct access + * + * This base class provides the const low-level accessors (e.g. coeff, coeffRef) of dense + * Map and Block objects with direct access. + * Typical users do not have to directly deal with this class. + * + * This class can be extended by through the macro plugin \c EIGEN_MAPBASE_PLUGIN. + * See \link TopicCustomizing_Plugins customizing Eigen \endlink for details. + * + * The \c Derived class has to provide the following two methods describing the memory layout: + * \code Index innerStride() const; \endcode + * \code Index outerStride() const; \endcode * * \sa class Map, class Block */ @@ -37,7 +47,6 @@ template class MapBase }; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; @@ -76,8 +85,10 @@ template class MapBase typedef typename Base::CoeffReturnType CoeffReturnType; - inline Index rows() const { return m_rows.value(); } - inline Index cols() const { return m_cols.value(); } + /** \copydoc DenseBase::rows() */ + EIGEN_DEVICE_FUNC inline Index rows() const { return m_rows.value(); } + /** \copydoc DenseBase::cols() */ + EIGEN_DEVICE_FUNC inline Index cols() const { return m_cols.value(); } /** Returns a pointer to the first coefficient of the matrix or vector. * @@ -85,30 +96,39 @@ template class MapBase * * \sa innerStride(), outerStride() */ - inline const Scalar* data() const { return m_data; } + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_data; } + /** \copydoc PlainObjectBase::coeff(Index,Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index rowId, Index colId) const { return m_data[colId * colStride() + rowId * rowStride()]; } + /** \copydoc PlainObjectBase::coeff(Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index index) const { EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) return m_data[index * innerStride()]; } + /** \copydoc PlainObjectBase::coeffRef(Index,Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index rowId, Index colId) const { return this->m_data[colId * colStride() + rowId * rowStride()]; } + /** \copydoc PlainObjectBase::coeffRef(Index) const */ + EIGEN_DEVICE_FUNC inline const Scalar& coeffRef(Index index) const { EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) return this->m_data[index * innerStride()]; } + /** \internal */ template inline PacketScalar packet(Index rowId, Index colId) const { @@ -116,6 +136,7 @@ template class MapBase (m_data + (colId * colStride() + rowId * rowStride())); } + /** \internal */ template inline PacketScalar packet(Index index) const { @@ -123,12 +144,16 @@ template class MapBase return internal::ploadt(m_data + index * innerStride()); } + /** \internal Constructor for fixed size matrices or vectors */ + EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) { EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) - checkSanity(); + checkSanity(); } + /** \internal Constructor for dynamically sized vectors */ + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : m_data(dataPtr), m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)), @@ -137,16 +162,18 @@ template class MapBase EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) eigen_assert(vecSize >= 0); eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize); - checkSanity(); + checkSanity(); } - inline MapBase(PointerType dataPtr, Index nbRows, Index nbCols) - : m_data(dataPtr), m_rows(nbRows), m_cols(nbCols) + /** \internal Constructor for dynamically sized matrices */ + EIGEN_DEVICE_FUNC + inline MapBase(PointerType dataPtr, Index rows, Index cols) + : m_data(dataPtr), m_rows(rows), m_cols(cols) { eigen_assert( (dataPtr == 0) - || ( nbRows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == nbRows) - && nbCols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == nbCols))); - checkSanity(); + || ( rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) + && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols))); + checkSanity(); } #ifdef EIGEN_MAPBASE_PLUGIN @@ -155,20 +182,36 @@ template class MapBase protected: - void checkSanity() const + template + EIGEN_DEVICE_FUNC + void checkSanity(typename internal::enable_if<(internal::traits::Alignment>0),void*>::type = 0) const { - EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(internal::traits::Flags&PacketAccessBit, - internal::inner_stride_at_compile_time::ret==1), - PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); - eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % 16) == 0) - && "input pointer is not aligned on a 16 byte boundary"); +#if EIGEN_MAX_ALIGN_BYTES>0 + eigen_assert(( ((internal::UIntPtr(m_data) % internal::traits::Alignment) == 0) + || (cols() * rows() * innerStride() * sizeof(Scalar)) < internal::traits::Alignment ) && "data is not aligned"); +#endif } + template + EIGEN_DEVICE_FUNC + void checkSanity(typename internal::enable_if::Alignment==0,void*>::type = 0) const + {} + PointerType m_data; const internal::variable_if_dynamic m_rows; const internal::variable_if_dynamic m_cols; }; +/** \ingroup Core_Module + * + * \brief Base class for non-const dense Map and Block expression with direct access + * + * This base class provides the non-const low-level accessors (e.g. coeff and coeffRef) of + * dense Map and Block objects with direct access. + * It inherits MapBase which defines the const variant for reading specific entries. + * + * \sa class Map, class Block + */ template class MapBase : public MapBase { @@ -179,7 +222,7 @@ template class MapBase typedef typename Base::Scalar Scalar; typedef typename Base::PacketScalar PacketScalar; - typedef typename Base::Index Index; + typedef typename Base::StorageIndex StorageIndex; typedef typename Base::PointerType PointerType; using Base::derived; @@ -200,14 +243,18 @@ template class MapBase const Scalar >::type ScalarWithConstIfNotLvalue; + EIGEN_DEVICE_FUNC inline const Scalar* data() const { return this->m_data; } + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return this->m_data; } // no const-cast here so non-const-correct code will give a compile error + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue& coeffRef(Index row, Index col) { return this->m_data[col * colStride() + row * rowStride()]; } + EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue& coeffRef(Index index) { EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) @@ -229,10 +276,11 @@ template class MapBase (this->m_data + index * innerStride(), val); } - explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {} - inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {} - inline MapBase(PointerType dataPtr, Index nbRows, Index nbCols) : Base(dataPtr, nbRows, nbCols) {} + EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {} + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {} + EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) : Base(dataPtr, rows, cols) {} + EIGEN_DEVICE_FUNC Derived& operator=(const MapBase& other) { ReadOnlyMapBase::Base::operator=(other); diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctions.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctions.h index 4e17ecd4b4..8d47fb8a46 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctions.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctions.h @@ -10,11 +10,25 @@ #ifndef EIGEN_MATHFUNCTIONS_H #define EIGEN_MATHFUNCTIONS_H +// source: http://www.geom.uiuc.edu/~huberty/math5337/groupe/digits.html +// TODO this should better be moved to NumTraits +#define EIGEN_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406L + + namespace Eigen { +// On WINCE, std::abs is defined for int only, so let's defined our own overloads: +// This issue has been confirmed with MSVC 2008 only, but the issue might exist for more recent versions too. +#if EIGEN_OS_WINCE && EIGEN_COMP_MSVC && EIGEN_COMP_MSVC<=1500 +long abs(long x) { return (labs(x)); } +double abs(double x) { return (fabs(x)); } +float abs(float x) { return (fabsf(x)); } +long double abs(long double x) { return (fabsl(x)); } +#endif + namespace internal { -/** \internal \struct global_math_functions_filtering_base +/** \internal \class global_math_functions_filtering_base * * What it does: * Defines a typedef 'type' as follows: @@ -62,6 +76,7 @@ template::IsComplex> struct real_default_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x; @@ -72,6 +87,7 @@ template struct real_default_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { using std::real; @@ -81,13 +97,25 @@ struct real_default_impl template struct real_impl : real_default_impl {}; +#ifdef __CUDA_ARCH__ +template +struct real_impl > +{ + typedef T RealScalar; + EIGEN_DEVICE_FUNC + static inline T run(const std::complex& x) + { + return x.real(); + } +}; +#endif + template struct real_retval { typedef typename NumTraits::Real type; }; - /**************************************************************************** * Implementation of imag * ****************************************************************************/ @@ -96,6 +124,7 @@ template::IsComplex> struct imag_default_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar&) { return RealScalar(0); @@ -106,6 +135,7 @@ template struct imag_default_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { using std::imag; @@ -115,6 +145,19 @@ struct imag_default_impl template struct imag_impl : imag_default_impl {}; +#ifdef __CUDA_ARCH__ +template +struct imag_impl > +{ + typedef T RealScalar; + EIGEN_DEVICE_FUNC + static inline T run(const std::complex& x) + { + return x.imag(); + } +}; +#endif + template struct imag_retval { @@ -129,10 +172,12 @@ template struct real_ref_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar& run(Scalar& x) { return reinterpret_cast(&x)[0]; } + EIGEN_DEVICE_FUNC static inline const RealScalar& run(const Scalar& x) { return reinterpret_cast(&x)[0]; @@ -153,10 +198,12 @@ template struct imag_ref_default_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar& run(Scalar& x) { return reinterpret_cast(&x)[1]; } + EIGEN_DEVICE_FUNC static inline const RealScalar& run(const Scalar& x) { return reinterpret_cast(&x)[1]; @@ -166,10 +213,12 @@ struct imag_ref_default_impl template struct imag_ref_default_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(Scalar&) { return Scalar(0); } + EIGEN_DEVICE_FUNC static inline const Scalar run(const Scalar&) { return Scalar(0); @@ -192,6 +241,7 @@ struct imag_ref_retval template::IsComplex> struct conj_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { return x; @@ -201,6 +251,7 @@ struct conj_impl template struct conj_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { using std::conj; @@ -218,25 +269,39 @@ struct conj_retval * Implementation of abs2 * ****************************************************************************/ -template -struct abs2_impl +template +struct abs2_impl_default { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x*x; } }; -template -struct abs2_impl > +template +struct abs2_impl_default // IsComplex { - static inline RealScalar run(const std::complex& x) + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) { return real(x)*real(x) + imag(x)*imag(x); } }; +template +struct abs2_impl +{ + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) + { + return abs2_impl_default::IsComplex>::run(x); + } +}; + template struct abs2_retval { @@ -251,9 +316,10 @@ template struct norm1_default_impl { typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { - using std::abs; + EIGEN_USING_STD_MATH(abs); return abs(real(x)) + abs(imag(x)); } }; @@ -261,9 +327,10 @@ struct norm1_default_impl template struct norm1_default_impl { + EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x) { - using std::abs; + EIGEN_USING_STD_MATH(abs); return abs(x); } }; @@ -287,16 +354,22 @@ struct hypot_impl typedef typename NumTraits::Real RealScalar; static inline RealScalar run(const Scalar& x, const Scalar& y) { - using std::max; - using std::min; - using std::abs; - using std::sqrt; + EIGEN_USING_STD_MATH(abs); + EIGEN_USING_STD_MATH(sqrt); RealScalar _x = abs(x); RealScalar _y = abs(y); - RealScalar p = (max)(_x, _y); + Scalar p, qp; + if(_x>_y) + { + p = _x; + qp = _y / p; + } + else + { + p = _y; + qp = _x / p; + } if(p==RealScalar(0)) return RealScalar(0); - RealScalar q = (min)(_x, _y); - RealScalar qp = q/p; return p * sqrt(RealScalar(1) + qp*qp); } }; @@ -314,6 +387,7 @@ struct hypot_retval template struct cast_impl { + EIGEN_DEVICE_FUNC static inline NewType run(const OldType& x) { return static_cast(x); @@ -323,48 +397,124 @@ struct cast_impl // here, for once, we're plainly returning NewType: we don't want cast to do weird things. template +EIGEN_DEVICE_FUNC inline NewType cast(const OldType& x) { return cast_impl::run(x); } /**************************************************************************** -* Implementation of atanh2 * +* Implementation of round * ****************************************************************************/ -template -struct atanh2_default_impl -{ - typedef Scalar retval; - typedef typename NumTraits::Real RealScalar; - static inline Scalar run(const Scalar& x, const Scalar& y) +#if EIGEN_HAS_CXX11_MATH + template + struct round_impl { + static inline Scalar run(const Scalar& x) + { + EIGEN_STATIC_ASSERT((!NumTraits::IsComplex), NUMERIC_TYPE_MUST_BE_REAL) + using std::round; + return round(x); + } + }; +#else + template + struct round_impl { - using std::abs; - using std::log; - using std::sqrt; - Scalar z = x / y; - if (y == Scalar(0) || abs(z) > sqrt(NumTraits::epsilon())) - return RealScalar(0.5) * log((y + x) / (y - x)); - else - return z + z*z*z / RealScalar(3); - } + static inline Scalar run(const Scalar& x) + { + EIGEN_STATIC_ASSERT((!NumTraits::IsComplex), NUMERIC_TYPE_MUST_BE_REAL) + EIGEN_USING_STD_MATH(floor); + EIGEN_USING_STD_MATH(ceil); + return (x > Scalar(0)) ? floor(x + Scalar(0.5)) : ceil(x - Scalar(0.5)); + } + }; +#endif + +template +struct round_retval +{ + typedef Scalar type; }; +/**************************************************************************** +* Implementation of arg * +****************************************************************************/ + +#if EIGEN_HAS_CXX11_MATH + template + struct arg_impl { + static inline Scalar run(const Scalar& x) + { + EIGEN_USING_STD_MATH(arg); + return arg(x); + } + }; +#else + template::IsComplex> + struct arg_default_impl + { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) + { + return (x < Scalar(0)) ? Scalar(EIGEN_PI) : Scalar(0); } + }; + + template + struct arg_default_impl + { + typedef typename NumTraits::Real RealScalar; + EIGEN_DEVICE_FUNC + static inline RealScalar run(const Scalar& x) + { + EIGEN_USING_STD_MATH(arg); + return arg(x); + } + }; + + template struct arg_impl : arg_default_impl {}; +#endif + template -struct atanh2_default_impl +struct arg_retval { - static inline Scalar run(const Scalar&, const Scalar&) + typedef typename NumTraits::Real type; +}; + +/**************************************************************************** +* Implementation of log1p * +****************************************************************************/ + +namespace std_fallback { + // fallback log1p implementation in case there is no log1p(Scalar) function in namespace of Scalar, + // or that there is no suitable std::log1p function available + template + EIGEN_DEVICE_FUNC inline Scalar log1p(const Scalar& x) { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) + typedef typename NumTraits::Real RealScalar; + EIGEN_USING_STD_MATH(log); + Scalar x1p = RealScalar(1) + x; + return ( x1p == Scalar(1) ) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) ); + } +} + +template +struct log1p_impl { + static inline Scalar run(const Scalar& x) { EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) - return Scalar(0); + #if EIGEN_HAS_CXX11_MATH + using std::log1p; + #endif + using std_fallback::log1p; + return log1p(x); } }; -template -struct atanh2_impl : atanh2_default_impl::IsInteger> {}; template -struct atanh2_retval +struct log1p_retval { typedef Scalar type; }; @@ -373,24 +523,26 @@ struct atanh2_retval * Implementation of pow * ****************************************************************************/ -template -struct pow_default_impl +template::IsInteger&&NumTraits::IsInteger> +struct pow_impl { - typedef Scalar retval; - static inline Scalar run(const Scalar& x, const Scalar& y) + //typedef Scalar retval; + typedef typename ScalarBinaryOpTraits >::ReturnType result_type; + static EIGEN_DEVICE_FUNC inline result_type run(const ScalarX& x, const ScalarY& y) { - using std::pow; + EIGEN_USING_STD_MATH(pow); return pow(x, y); } }; -template -struct pow_default_impl +template +struct pow_impl { - static inline Scalar run(Scalar x, Scalar y) + typedef ScalarX result_type; + static EIGEN_DEVICE_FUNC inline ScalarX run(ScalarX x, ScalarY y) { - Scalar res(1); - eigen_assert(!NumTraits::IsSigned || y >= 0); + ScalarX res(1); + eigen_assert(!NumTraits::IsSigned || y >= 0); if(y & 1) res *= x; y >>= 1; while(y) @@ -403,15 +555,6 @@ struct pow_default_impl } }; -template -struct pow_impl : pow_default_impl::IsInteger> {}; - -template -struct pow_retval -{ - typedef Scalar type; -}; - /**************************************************************************** * Implementation of random * ****************************************************************************/ @@ -447,48 +590,48 @@ struct random_default_impl }; enum { - floor_log2_terminate, - floor_log2_move_up, - floor_log2_move_down, - floor_log2_bogus + meta_floor_log2_terminate, + meta_floor_log2_move_up, + meta_floor_log2_move_down, + meta_floor_log2_bogus }; -template struct floor_log2_selector +template struct meta_floor_log2_selector { enum { middle = (lower + upper) / 2, - value = (upper <= lower + 1) ? int(floor_log2_terminate) - : (n < (1 << middle)) ? int(floor_log2_move_down) - : (n==0) ? int(floor_log2_bogus) - : int(floor_log2_move_up) + value = (upper <= lower + 1) ? int(meta_floor_log2_terminate) + : (n < (1 << middle)) ? int(meta_floor_log2_move_down) + : (n==0) ? int(meta_floor_log2_bogus) + : int(meta_floor_log2_move_up) }; }; template::value> -struct floor_log2 {}; + int selector = meta_floor_log2_selector::value> +struct meta_floor_log2 {}; template -struct floor_log2 +struct meta_floor_log2 { - enum { value = floor_log2::middle>::value }; + enum { value = meta_floor_log2::middle>::value }; }; template -struct floor_log2 +struct meta_floor_log2 { - enum { value = floor_log2::middle, upper>::value }; + enum { value = meta_floor_log2::middle, upper>::value }; }; template -struct floor_log2 +struct meta_floor_log2 { enum { value = (n >= ((unsigned int)(1) << (lower+1))) ? lower+1 : lower }; }; template -struct floor_log2 +struct meta_floor_log2 { // no value, error at compile time }; @@ -496,11 +639,24 @@ struct floor_log2 template struct random_default_impl { - typedef typename NumTraits::NonInteger NonInteger; - static inline Scalar run(const Scalar& x, const Scalar& y) - { - return x + Scalar((NonInteger(y)-x+1) * std::rand() / (RAND_MAX + NonInteger(1))); + { + typedef typename conditional::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX; + if(y=x the result converted to an unsigned long is still correct. + std::size_t range = ScalarX(y)-ScalarX(x); + std::size_t offset = 0; + // rejection sampling + std::size_t divisor = 1; + std::size_t multiplier = 1; + if(range range); + return Scalar(ScalarX(x) + offset); } static inline Scalar run() @@ -508,7 +664,7 @@ struct random_default_impl #ifdef EIGEN_MAKING_DOCS return run(Scalar(NumTraits::IsSigned ? -10 : 0), Scalar(10)); #else - enum { rand_bits = floor_log2<(unsigned int)(RAND_MAX)+1>::value, + enum { rand_bits = meta_floor_log2<(unsigned int)(RAND_MAX)+1>::value, scalar_bits = sizeof(Scalar) * CHAR_BIT, shift = EIGEN_PLAIN_ENUM_MAX(0, int(rand_bits) - int(scalar_bits)), offset = NumTraits::IsSigned ? (1 << (EIGEN_PLAIN_ENUM_MIN(rand_bits,scalar_bits)-1)) : 0 @@ -545,97 +701,588 @@ inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random() return EIGEN_MATHFUNC_IMPL(random, Scalar)::run(); } +// Implementatin of is* functions + +// std::is* do not work with fast-math and gcc, std::is* are available on MSVC 2013 and newer, as well as in clang. +#if (EIGEN_HAS_CXX11_MATH && !(EIGEN_COMP_GNUC_STRICT && __FINITE_MATH_ONLY__)) || (EIGEN_COMP_MSVC>=1800) || (EIGEN_COMP_CLANG) +#define EIGEN_USE_STD_FPCLASSIFY 1 +#else +#define EIGEN_USE_STD_FPCLASSIFY 0 +#endif + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if::value,bool>::type +isnan_impl(const T&) { return false; } + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if::value,bool>::type +isinf_impl(const T&) { return false; } + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if::value,bool>::type +isfinite_impl(const T&) { return true; } + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if<(!internal::is_integral::value)&&(!NumTraits::IsComplex),bool>::type +isfinite_impl(const T& x) +{ + #ifdef __CUDA_ARCH__ + return (::isfinite)(x); + #elif EIGEN_USE_STD_FPCLASSIFY + using std::isfinite; + return isfinite EIGEN_NOT_A_MACRO (x); + #else + return x<=NumTraits::highest() && x>=NumTraits::lowest(); + #endif +} + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if<(!internal::is_integral::value)&&(!NumTraits::IsComplex),bool>::type +isinf_impl(const T& x) +{ + #ifdef __CUDA_ARCH__ + return (::isinf)(x); + #elif EIGEN_USE_STD_FPCLASSIFY + using std::isinf; + return isinf EIGEN_NOT_A_MACRO (x); + #else + return x>NumTraits::highest() || x::lowest(); + #endif +} + +template +EIGEN_DEVICE_FUNC +typename internal::enable_if<(!internal::is_integral::value)&&(!NumTraits::IsComplex),bool>::type +isnan_impl(const T& x) +{ + #ifdef __CUDA_ARCH__ + return (::isnan)(x); + #elif EIGEN_USE_STD_FPCLASSIFY + using std::isnan; + return isnan EIGEN_NOT_A_MACRO (x); + #else + return x != x; + #endif +} + +#if (!EIGEN_USE_STD_FPCLASSIFY) + +#if EIGEN_COMP_MSVC + +template EIGEN_DEVICE_FUNC bool isinf_msvc_helper(T x) +{ + return _fpclass(x)==_FPCLASS_NINF || _fpclass(x)==_FPCLASS_PINF; +} + +//MSVC defines a _isnan builtin function, but for double only +EIGEN_DEVICE_FUNC inline bool isnan_impl(const long double& x) { return _isnan(x)!=0; } +EIGEN_DEVICE_FUNC inline bool isnan_impl(const double& x) { return _isnan(x)!=0; } +EIGEN_DEVICE_FUNC inline bool isnan_impl(const float& x) { return _isnan(x)!=0; } + +EIGEN_DEVICE_FUNC inline bool isinf_impl(const long double& x) { return isinf_msvc_helper(x); } +EIGEN_DEVICE_FUNC inline bool isinf_impl(const double& x) { return isinf_msvc_helper(x); } +EIGEN_DEVICE_FUNC inline bool isinf_impl(const float& x) { return isinf_msvc_helper(x); } + +#elif (defined __FINITE_MATH_ONLY__ && __FINITE_MATH_ONLY__ && EIGEN_COMP_GNUC) + +#if EIGEN_GNUC_AT_LEAST(5,0) + #define EIGEN_TMP_NOOPT_ATTRIB EIGEN_DEVICE_FUNC inline __attribute__((optimize("no-finite-math-only"))) +#else + // NOTE the inline qualifier and noinline attribute are both needed: the former is to avoid linking issue (duplicate symbol), + // while the second prevent too aggressive optimizations in fast-math mode: + #define EIGEN_TMP_NOOPT_ATTRIB EIGEN_DEVICE_FUNC inline __attribute__((noinline,optimize("no-finite-math-only"))) +#endif + +template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const long double& x) { return __builtin_isnan(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const double& x) { return __builtin_isnan(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isnan_impl(const float& x) { return __builtin_isnan(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const double& x) { return __builtin_isinf(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const float& x) { return __builtin_isinf(x); } +template<> EIGEN_TMP_NOOPT_ATTRIB bool isinf_impl(const long double& x) { return __builtin_isinf(x); } + +#undef EIGEN_TMP_NOOPT_ATTRIB + +#endif + +#endif + +// The following overload are defined at the end of this file +template EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex& x); +template EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex& x); +template EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex& x); + +template T generic_fast_tanh_float(const T& a_x); + } // end namespace internal /**************************************************************************** -* Generic math function * +* Generic math functions * ****************************************************************************/ namespace numext { +#ifndef __CUDA_ARCH__ +template +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) +{ + EIGEN_USING_STD_MATH(min); + return min EIGEN_NOT_A_MACRO (x,y); +} + +template +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) +{ + EIGEN_USING_STD_MATH(max); + return max EIGEN_NOT_A_MACRO (x,y); +} +#else +template +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y) +{ + return y < x ? y : x; +} +template<> +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE float mini(const float& x, const float& y) +{ + return fminf(x, y); +} +template +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y) +{ + return x < y ? y : x; +} +template<> +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE float maxi(const float& x, const float& y) +{ + return fmaxf(x, y); +} +#endif + + template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) { return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x); -} +} template +EIGEN_DEVICE_FUNC inline typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) >::type real_ref(const Scalar& x) { return internal::real_ref_impl::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) { return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) { return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x); } template +EIGEN_DEVICE_FUNC +inline EIGEN_MATHFUNC_RETVAL(arg, Scalar) arg(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(arg, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) >::type imag_ref(const Scalar& x) { return internal::imag_ref_impl::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) imag_ref(Scalar& x) { return EIGEN_MATHFUNC_IMPL(imag_ref, Scalar)::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(conj, Scalar) conj(const Scalar& x) { return EIGEN_MATHFUNC_IMPL(conj, Scalar)::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) { return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) { return EIGEN_MATHFUNC_IMPL(norm1, Scalar)::run(x); } template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(hypot, Scalar) hypot(const Scalar& x, const Scalar& y) { return EIGEN_MATHFUNC_IMPL(hypot, Scalar)::run(x, y); } template -inline EIGEN_MATHFUNC_RETVAL(atanh2, Scalar) atanh2(const Scalar& x, const Scalar& y) +EIGEN_DEVICE_FUNC +inline EIGEN_MATHFUNC_RETVAL(log1p, Scalar) log1p(const Scalar& x) { - return EIGEN_MATHFUNC_IMPL(atanh2, Scalar)::run(x, y); + return EIGEN_MATHFUNC_IMPL(log1p, Scalar)::run(x); } +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float log1p(const float &x) { return ::log1pf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double log1p(const double &x) { return ::log1p(x); } +#endif + +template +EIGEN_DEVICE_FUNC +inline typename internal::pow_impl::result_type pow(const ScalarX& x, const ScalarY& y) +{ + return internal::pow_impl::run(x, y); +} + +template EIGEN_DEVICE_FUNC bool (isnan) (const T &x) { return internal::isnan_impl(x); } +template EIGEN_DEVICE_FUNC bool (isinf) (const T &x) { return internal::isinf_impl(x); } +template EIGEN_DEVICE_FUNC bool (isfinite)(const T &x) { return internal::isfinite_impl(x); } + template -inline EIGEN_MATHFUNC_RETVAL(pow, Scalar) pow(const Scalar& x, const Scalar& y) +EIGEN_DEVICE_FUNC +inline EIGEN_MATHFUNC_RETVAL(round, Scalar) round(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(round, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC +T (floor)(const T& x) +{ + EIGEN_USING_STD_MATH(floor); + return floor(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float floor(const float &x) { return ::floorf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double floor(const double &x) { return ::floor(x); } +#endif + +template +EIGEN_DEVICE_FUNC +T (ceil)(const T& x) +{ + EIGEN_USING_STD_MATH(ceil); + return ceil(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float ceil(const float &x) { return ::ceilf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double ceil(const double &x) { return ::ceil(x); } +#endif + + +/** Log base 2 for 32 bits positive integers. + * Conveniently returns 0 for x==0. */ +inline int log2(int x) { - return EIGEN_MATHFUNC_IMPL(pow, Scalar)::run(x, y); + eigen_assert(x>=0); + unsigned int v(x); + static const int table[32] = { 0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30, 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31 }; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + return table[(v * 0x07C4ACDDU) >> 27]; } -// std::isfinite is non standard, so let's define our own version, -// even though it is not very efficient. -template bool (isfinite)(const T& x) +/** \returns the square root of \a x. + * + * It is essentially equivalent to \code using std::sqrt; return sqrt(x); \endcode, + * but slightly faster for float/double and some compilers (e.g., gcc), thanks to + * specializations when SSE is enabled. + * + * It's usage is justified in performance critical functions, like norm/normalize. + */ +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T sqrt(const T &x) { - return x::highest() && x>NumTraits::lowest(); + EIGEN_USING_STD_MATH(sqrt); + return sqrt(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T log(const T &x) { + EIGEN_USING_STD_MATH(log); + return log(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float log(const float &x) { return ::logf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double log(const double &x) { return ::log(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +typename NumTraits::Real abs(const T &x) { + EIGEN_USING_STD_MATH(abs); + return abs(x); } +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float abs(const float &x) { return ::fabsf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double abs(const double &x) { return ::fabs(x); } + +template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float abs(const std::complex& x) { + return ::hypotf(x.real(), x.imag()); +} + +template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double abs(const std::complex& x) { + return ::hypot(x.real(), x.imag()); +} +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T exp(const T &x) { + EIGEN_USING_STD_MATH(exp); + return exp(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float exp(const float &x) { return ::expf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double exp(const double &x) { return ::exp(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T cos(const T &x) { + EIGEN_USING_STD_MATH(cos); + return cos(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float cos(const float &x) { return ::cosf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double cos(const double &x) { return ::cos(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T sin(const T &x) { + EIGEN_USING_STD_MATH(sin); + return sin(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float sin(const float &x) { return ::sinf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double sin(const double &x) { return ::sin(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T tan(const T &x) { + EIGEN_USING_STD_MATH(tan); + return tan(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float tan(const float &x) { return ::tanf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double tan(const double &x) { return ::tan(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T acos(const T &x) { + EIGEN_USING_STD_MATH(acos); + return acos(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float acos(const float &x) { return ::acosf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double acos(const double &x) { return ::acos(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T asin(const T &x) { + EIGEN_USING_STD_MATH(asin); + return asin(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float asin(const float &x) { return ::asinf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double asin(const double &x) { return ::asin(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T atan(const T &x) { + EIGEN_USING_STD_MATH(atan); + return atan(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float atan(const float &x) { return ::atanf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double atan(const double &x) { return ::atan(x); } +#endif + + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T cosh(const T &x) { + EIGEN_USING_STD_MATH(cosh); + return cosh(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float cosh(const float &x) { return ::coshf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double cosh(const double &x) { return ::cosh(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T sinh(const T &x) { + EIGEN_USING_STD_MATH(sinh); + return sinh(x); +} + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float sinh(const float &x) { return ::sinhf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double sinh(const double &x) { return ::sinh(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T tanh(const T &x) { + EIGEN_USING_STD_MATH(tanh); + return tanh(x); +} + +#if (!defined(__CUDACC__)) && EIGEN_FAST_MATH +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float tanh(float x) { return internal::generic_fast_tanh_float(x); } +#endif + +#ifdef __CUDACC__ +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float tanh(const float &x) { return ::tanhf(x); } + +template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double tanh(const double &x) { return ::tanh(x); } +#endif + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T fmod(const T& a, const T& b) { + EIGEN_USING_STD_MATH(fmod); + return fmod(a, b); +} + +#ifdef __CUDACC__ +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float fmod(const float& a, const float& b) { + return ::fmodf(a, b); +} + +template <> +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double fmod(const double& a, const double& b) { + return ::fmod(a, b); +} +#endif + } // end namespace numext namespace internal { +template +EIGEN_DEVICE_FUNC bool isfinite_impl(const std::complex& x) +{ + return (numext::isfinite)(numext::real(x)) && (numext::isfinite)(numext::imag(x)); +} + +template +EIGEN_DEVICE_FUNC bool isnan_impl(const std::complex& x) +{ + return (numext::isnan)(numext::real(x)) || (numext::isnan)(numext::imag(x)); +} + +template +EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex& x) +{ + return ((numext::isinf)(numext::real(x)) || (numext::isinf)(numext::imag(x))) && (!(numext::isnan)(x)); +} + /**************************************************************************** * Implementation of fuzzy comparisons * ****************************************************************************/ @@ -649,18 +1296,17 @@ template struct scalar_fuzzy_default_impl { typedef typename NumTraits::Real RealScalar; - template + template EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const RealScalar& prec) { - using std::abs; - return abs(x) <= abs(y) * prec; + return numext::abs(x) <= numext::abs(y) * prec; } + EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) { - using std::min; - using std::abs; - return abs(x - y) <= (min)(abs(x), abs(y)) * prec; + return numext::abs(x - y) <= numext::mini(numext::abs(x), numext::abs(y)) * prec; } + EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar& prec) { return x <= y || isApprox(x, y, prec); @@ -671,15 +1317,17 @@ template struct scalar_fuzzy_default_impl { typedef typename NumTraits::Real RealScalar; - template + template EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const Scalar&, const RealScalar&) { return x == Scalar(0); } + EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar&) { return x == y; } + EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar&) { return x <= y; @@ -690,36 +1338,36 @@ template struct scalar_fuzzy_default_impl { typedef typename NumTraits::Real RealScalar; - template + template EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const RealScalar& prec) { return numext::abs2(x) <= numext::abs2(y) * prec * prec; } + EIGEN_DEVICE_FUNC static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) { - using std::min; - return numext::abs2(x - y) <= (min)(numext::abs2(x), numext::abs2(y)) * prec * prec; + return numext::abs2(x - y) <= numext::mini(numext::abs2(x), numext::abs2(y)) * prec * prec; } }; template struct scalar_fuzzy_impl : scalar_fuzzy_default_impl::IsComplex, NumTraits::IsInteger> {}; -template +template EIGEN_DEVICE_FUNC inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const typename NumTraits::Real &precision = NumTraits::dummy_precision()) { return scalar_fuzzy_impl::template isMuchSmallerThan(x, y, precision); } -template +template EIGEN_DEVICE_FUNC inline bool isApprox(const Scalar& x, const Scalar& y, const typename NumTraits::Real &precision = NumTraits::dummy_precision()) { return scalar_fuzzy_impl::isApprox(x, y, precision); } -template +template EIGEN_DEVICE_FUNC inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const typename NumTraits::Real &precision = NumTraits::dummy_precision()) { @@ -742,17 +1390,19 @@ template<> struct scalar_fuzzy_impl { typedef bool RealScalar; - template + template EIGEN_DEVICE_FUNC static inline bool isMuchSmallerThan(const bool& x, const bool&, const bool&) { return !x; } + EIGEN_DEVICE_FUNC static inline bool isApprox(bool x, bool y, bool) { return x == y; } + EIGEN_DEVICE_FUNC static inline bool isApproxOrLessThan(const bool& x, const bool& y, const bool&) { return (!x) || y; diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctionsImpl.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctionsImpl.h new file mode 100644 index 0000000000..3c9ef22fa6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/MathFunctionsImpl.h @@ -0,0 +1,78 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Pedro Gonnet (pedro.gonnet@gmail.com) +// Copyright (C) 2016 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_MATHFUNCTIONSIMPL_H +#define EIGEN_MATHFUNCTIONSIMPL_H + +namespace Eigen { + +namespace internal { + +/** \internal \returns the hyperbolic tan of \a a (coeff-wise) + Doesn't do anything fancy, just a 13/6-degree rational interpolant which + is accurate up to a couple of ulp in the range [-9, 9], outside of which + the tanh(x) = +/-1. + + This implementation works on both scalars and packets. +*/ +template +T generic_fast_tanh_float(const T& a_x) +{ + // Clamp the inputs to the range [-9, 9] since anything outside + // this range is +/-1.0f in single-precision. + const T plus_9 = pset1(9.f); + const T minus_9 = pset1(-9.f); + // NOTE GCC prior to 6.3 might improperly optimize this max/min + // step such that if a_x is nan, x will be either 9 or -9, + // and tanh will return 1 or -1 instead of nan. + // This is supposed to be fixed in gcc6.3, + // see: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867 + const T x = pmax(minus_9,pmin(plus_9,a_x)); + // The monomial coefficients of the numerator polynomial (odd). + const T alpha_1 = pset1(4.89352455891786e-03f); + const T alpha_3 = pset1(6.37261928875436e-04f); + const T alpha_5 = pset1(1.48572235717979e-05f); + const T alpha_7 = pset1(5.12229709037114e-08f); + const T alpha_9 = pset1(-8.60467152213735e-11f); + const T alpha_11 = pset1(2.00018790482477e-13f); + const T alpha_13 = pset1(-2.76076847742355e-16f); + + // The monomial coefficients of the denominator polynomial (even). + const T beta_0 = pset1(4.89352518554385e-03f); + const T beta_2 = pset1(2.26843463243900e-03f); + const T beta_4 = pset1(1.18534705686654e-04f); + const T beta_6 = pset1(1.19825839466702e-06f); + + // Since the polynomials are odd/even, we need x^2. + const T x2 = pmul(x, x); + + // Evaluate the numerator polynomial p. + T p = pmadd(x2, alpha_13, alpha_11); + p = pmadd(x2, p, alpha_9); + p = pmadd(x2, p, alpha_7); + p = pmadd(x2, p, alpha_5); + p = pmadd(x2, p, alpha_3); + p = pmadd(x2, p, alpha_1); + p = pmul(x, p); + + // Evaluate the denominator polynomial p. + T q = pmadd(x2, beta_6, beta_4); + q = pmadd(x2, q, beta_2); + q = pmadd(x2, q, beta_0); + + // Divide the numerator by the denominator. + return pdiv(p, q); +} + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_MATHFUNCTIONSIMPL_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Matrix.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Matrix.h index 02be142d8c..90c336d8ca 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Matrix.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Matrix.h @@ -13,6 +13,45 @@ namespace Eigen { +namespace internal { +template +struct traits > +{ +private: + enum { size = internal::size_at_compile_time<_Rows,_Cols>::ret }; + typedef typename find_best_packet<_Scalar,size>::type PacketScalar; + enum { + row_major_bit = _Options&RowMajor ? RowMajorBit : 0, + is_dynamic_size_storage = _MaxRows==Dynamic || _MaxCols==Dynamic, + max_size = is_dynamic_size_storage ? Dynamic : _MaxRows*_MaxCols, + default_alignment = compute_default_alignment<_Scalar,max_size>::value, + actual_alignment = ((_Options&DontAlign)==0) ? default_alignment : 0, + required_alignment = unpacket_traits::alignment, + packet_access_bit = (packet_traits<_Scalar>::Vectorizable && (EIGEN_UNALIGNED_VECTORIZE || (actual_alignment>=required_alignment))) ? PacketAccessBit : 0 + }; + +public: + typedef _Scalar Scalar; + typedef Dense StorageKind; + typedef Eigen::Index StorageIndex; + typedef MatrixXpr XprKind; + enum { + RowsAtCompileTime = _Rows, + ColsAtCompileTime = _Cols, + MaxRowsAtCompileTime = _MaxRows, + MaxColsAtCompileTime = _MaxCols, + Flags = compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, + Options = _Options, + InnerStrideAtCompileTime = 1, + OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime, + + // FIXME, the following flag in only used to define NeedsToAlign in PlainObjectBase + EvaluatorFlags = LinearAccessBit | DirectAccessBit | packet_access_bit | row_major_bit, + Alignment = actual_alignment + }; +}; +} + /** \class Matrix * \ingroup Core_Module * @@ -24,13 +63,13 @@ namespace Eigen { * The %Matrix class encompasses \em both fixed-size and dynamic-size objects (\ref fixedsize "note"). * * The first three template parameters are required: - * \tparam _Scalar \anchor matrix_tparam_scalar Numeric type, e.g. float, double, int or std::complex. - * User defined sclar types are supported as well (see \ref user_defined_scalars "here"). + * \tparam _Scalar Numeric type, e.g. float, double, int or std::complex. + * User defined scalar types are supported as well (see \ref user_defined_scalars "here"). * \tparam _Rows Number of rows, or \b Dynamic * \tparam _Cols Number of columns, or \b Dynamic * * The remaining template parameters are optional -- in most cases you don't have to worry about them. - * \tparam _Options \anchor matrix_tparam_options A combination of either \b #RowMajor or \b #ColMajor, and of either + * \tparam _Options A combination of either \b #RowMajor or \b #ColMajor, and of either * \b #AutoAlign or \b #DontAlign. * The former controls \ref TopicStorageOrders "storage order", and defaults to column-major. The latter controls alignment, which is required * for vectorization. It defaults to aligning matrices except for fixed sizes that aren't a multiple of the packet size. @@ -67,7 +106,7 @@ namespace Eigen { * \endcode * * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_MATRIX_PLUGIN. + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_MATRIX_PLUGIN. * * Some notes: * @@ -97,32 +136,44 @@ namespace Eigen { * are the dimensions of the original matrix, while _Rows and _Cols are Dynamic. * * - * \see MatrixBase for the majority of the API methods for matrices, \ref TopicClassHierarchy, - * \ref TopicStorageOrders + * ABI and storage layout + * + * The table below summarizes the ABI of some possible Matrix instances which is fixed thorough the lifetime of Eigen 3. + * + * + * + * + * + * + *
Matrix typeEquivalent C structure
\code Matrix \endcode\code + * struct { + * T *data; // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0 + * Eigen::Index rows, cols; + * }; + * \endcode
\code + * Matrix + * Matrix \endcode\code + * struct { + * T *data; // with (size_t(data)%EIGEN_MAX_ALIGN_BYTES)==0 + * Eigen::Index size; + * }; + * \endcode
\code Matrix \endcode\code + * struct { + * T data[Rows*Cols]; // with (size_t(data)%A(Rows*Cols*sizeof(T)))==0 + * }; + * \endcode
\code Matrix \endcode\code + * struct { + * T data[MaxRows*MaxCols]; // with (size_t(data)%A(MaxRows*MaxCols*sizeof(T)))==0 + * Eigen::Index rows, cols; + * }; + * \endcode
+ * Note that in this table Rows, Cols, MaxRows and MaxCols are all positive integers. A(S) is defined to the largest possible power-of-two + * smaller to EIGEN_MAX_STATIC_ALIGN_BYTES. + * + * \see MatrixBase for the majority of the API methods for matrices, \ref TopicClassHierarchy, + * \ref TopicStorageOrders */ -namespace internal { -template -struct traits > -{ - typedef _Scalar Scalar; - typedef Dense StorageKind; - typedef DenseIndex Index; - typedef MatrixXpr XprKind; - enum { - RowsAtCompileTime = _Rows, - ColsAtCompileTime = _Cols, - MaxRowsAtCompileTime = _MaxRows, - MaxColsAtCompileTime = _MaxCols, - Flags = compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, - CoeffReadCost = NumTraits::ReadCost, - Options = _Options, - InnerStrideAtCompileTime = 1, - OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime - }; -}; -} - template class Matrix : public PlainObjectBase > @@ -151,6 +202,7 @@ class Matrix * * \callgraph */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const Matrix& other) { return Base::_set(other); @@ -167,7 +219,8 @@ class Matrix * remain row-vectors and vectors remain vectors. */ template - EIGEN_STRONG_INLINE Matrix& operator=(const MatrixBase& other) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Matrix& operator=(const DenseBase& other) { return Base::_set(other); } @@ -179,12 +232,14 @@ class Matrix * \copydetails DenseBase::operator=(const EigenBase &other) */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const EigenBase &other) { return Base::operator=(other); } template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix& operator=(const ReturnByValue& func) { return Base::operator=(func); @@ -200,6 +255,7 @@ class Matrix * * \sa resize(Index,Index) */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix() : Base() { Base::_check_template_params(); @@ -207,60 +263,87 @@ class Matrix } // FIXME is it still needed - Matrix(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC + explicit Matrix(internal::constructor_without_unaligned_array_assert) : Base(internal::constructor_without_unaligned_array_assert()) { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } -#ifdef EIGEN_HAVE_RVALUE_REFERENCES - Matrix(Matrix&& other) +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + Matrix(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) : Base(std::move(other)) { Base::_check_template_params(); if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) Base::_set_noalias(other); } - Matrix& operator=(Matrix&& other) + EIGEN_DEVICE_FUNC + Matrix& operator=(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) { other.swap(*this); return *this; } #endif - /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors - * - * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, - * it is redundant to pass the dimension here, so it makes more sense to use the default - * constructor Matrix() instead. - */ - EIGEN_STRONG_INLINE explicit Matrix(Index dim) - : Base(dim, RowsAtCompileTime == 1 ? 1 : dim, ColsAtCompileTime == 1 ? 1 : dim) + #ifndef EIGEN_PARSED_BY_DOXYGEN + + // This constructor is for both 1x1 matrices and dynamic vectors + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE explicit Matrix(const T& x) { Base::_check_template_params(); - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Matrix) - eigen_assert(dim >= 0); - eigen_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim); - EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + Base::template _init1(x); } - #ifndef EIGEN_PARSED_BY_DOXYGEN template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const T0& x, const T1& y) { Base::_check_template_params(); Base::template _init2(x, y); } #else + /** \brief Constructs a fixed-sized matrix initialized with coefficients starting at \a data */ + EIGEN_DEVICE_FUNC + explicit Matrix(const Scalar *data); + + /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors + * + * This is useful for dynamic-size vectors. For fixed-size vectors, + * it is redundant to pass these parameters, so one should use the default constructor + * Matrix() instead. + * + * \warning This constructor is disabled for fixed-size \c 1x1 matrices. For instance, + * calling Matrix(1) will call the initialization constructor: Matrix(const Scalar&). + * For fixed-size \c 1x1 matrices it is therefore recommended to use the default + * constructor Matrix() instead, especially when using one of the non standard + * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives). + */ + EIGEN_STRONG_INLINE explicit Matrix(Index dim); + /** \brief Constructs an initialized 1x1 matrix with the given coefficient */ + Matrix(const Scalar& x); /** \brief Constructs an uninitialized matrix with \a rows rows and \a cols columns. * * This is useful for dynamic-size matrices. For fixed-size matrices, * it is redundant to pass these parameters, so one should use the default constructor - * Matrix() instead. */ + * Matrix() instead. + * + * \warning This constructor is disabled for fixed-size \c 1x2 and \c 2x1 vectors. For instance, + * calling Matrix2f(2,1) will call the initialization constructor: Matrix(const Scalar& x, const Scalar& y). + * For fixed-size \c 1x2 or \c 2x1 vectors it is therefore recommended to use the default + * constructor Matrix() instead, especially when using one of the non standard + * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives). + */ + EIGEN_DEVICE_FUNC Matrix(Index rows, Index cols); + /** \brief Constructs an initialized 2D vector with given coefficients */ Matrix(const Scalar& x, const Scalar& y); #endif /** \brief Constructs an initialized 3D vector with given coefficients */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z) { Base::_check_template_params(); @@ -270,6 +353,7 @@ class Matrix m_storage.data()[2] = z; } /** \brief Constructs an initialized 4D vector with given coefficients */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w) { Base::_check_template_params(); @@ -280,76 +364,33 @@ class Matrix m_storage.data()[3] = w; } - explicit Matrix(const Scalar *data); - /** \brief Constructor copying the value of the expression \a other */ - template - EIGEN_STRONG_INLINE Matrix(const MatrixBase& other) - : Base(other.rows() * other.cols(), other.rows(), other.cols()) - { - // This test resides here, to bring the error messages closer to the user. Normally, these checks - // are performed deeply within the library, thus causing long and scary error traces. - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - - Base::_check_template_params(); - Base::_set_noalias(other); - } /** \brief Copy constructor */ - EIGEN_STRONG_INLINE Matrix(const Matrix& other) - : Base(other.rows() * other.cols(), other.rows(), other.cols()) - { - Base::_check_template_params(); - Base::_set_noalias(other); - } - /** \brief Copy constructor with in-place evaluation */ - template - EIGEN_STRONG_INLINE Matrix(const ReturnByValue& other) - { - Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); - other.evalTo(*this); - } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Matrix(const Matrix& other) : Base(other) + { } /** \brief Copy constructor for generic expressions. * \sa MatrixBase::operator=(const EigenBase&) */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const EigenBase &other) - : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) - { - Base::_check_template_params(); - Base::_resize_to_match(other); - // FIXME/CHECK: isn't *this = other.derived() more efficient. it allows to - // go for pure _set() implementations, right? - *this = other; - } - - /** \internal - * \brief Override MatrixBase::swap() since for dynamic-sized matrices - * of same type it is enough to swap the data pointers. - */ - template - void swap(MatrixBase const & other) - { this->_swap(other.derived()); } + : Base(other.derived()) + { } - inline Index innerStride() const { return 1; } - inline Index outerStride() const { return this->innerSize(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return 1; } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return this->innerSize(); } /////////// Geometry module /////////// template + EIGEN_DEVICE_FUNC explicit Matrix(const RotationBase& r); template + EIGEN_DEVICE_FUNC Matrix& operator=(const RotationBase& r); - #ifdef EIGEN2_SUPPORT - template - explicit Matrix(const eigen2_RotationBase& r); - template - Matrix& operator=(const eigen2_RotationBase& r); - #endif - // allow to extend Matrix outside Eigen #ifdef EIGEN_MATRIX_PLUGIN #include EIGEN_MATRIX_PLUGIN diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/MatrixBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/MatrixBase.h index e83ef4dc05..f7cf04cde3 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/MatrixBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/MatrixBase.h @@ -41,9 +41,9 @@ namespace Eigen { * \endcode * * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_MATRIXBASE_PLUGIN. + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_MATRIXBASE_PLUGIN. * - * \sa \ref TopicClassHierarchy + * \sa \blank \ref TopicClassHierarchy */ template class MatrixBase : public DenseBase @@ -52,7 +52,7 @@ template class MatrixBase #ifndef EIGEN_PARSED_BY_DOXYGEN typedef MatrixBase StorageBaseType; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; + typedef typename internal::traits::StorageIndex StorageIndex; typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; @@ -66,7 +66,6 @@ template class MatrixBase using Base::MaxSizeAtCompileTime; using Base::IsVectorAtCompileTime; using Base::Flags; - using Base::CoeffReadCost; using Base::derived; using Base::const_cast_derived; @@ -98,25 +97,14 @@ template class MatrixBase /** \returns the size of the main diagonal, which is min(rows(),cols()). * \sa rows(), cols(), SizeAtCompileTime. */ - inline Index diagonalSize() const { return (std::min)(rows(),cols()); } + EIGEN_DEVICE_FUNC + inline Index diagonalSize() const { return (numext::mini)(rows(),cols()); } - /** \brief The plain matrix type corresponding to this expression. - * - * This is not necessarily exactly the return type of eval(). In the case of plain matrices, - * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed - * that the return type of eval() is either PlainObject or const PlainObject&. - */ - typedef Matrix::Scalar, - internal::traits::RowsAtCompileTime, - internal::traits::ColsAtCompileTime, - AutoAlign | (internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor), - internal::traits::MaxRowsAtCompileTime, - internal::traits::MaxColsAtCompileTime - > PlainObject; + typedef typename Base::PlainObject PlainObject; #ifndef EIGEN_PARSED_BY_DOXYGEN /** \internal Represents a matrix with all coefficients equal to one another*/ - typedef CwiseNullaryOp,Derived> ConstantReturnType; + typedef CwiseNullaryOp,PlainObject> ConstantReturnType; /** \internal the return type of MatrixBase::adjoint() */ typedef typename internal::conditional::IsComplex, CwiseUnaryOp, ConstTransposeReturnType>, @@ -125,7 +113,7 @@ template class MatrixBase /** \internal Return type of eigenvalues() */ typedef Matrix, internal::traits::ColsAtCompileTime, 1, ColMajor> EigenvaluesReturnType; /** \internal the return type of identity */ - typedef CwiseNullaryOp,Derived> IdentityReturnType; + typedef CwiseNullaryOp,PlainObject> IdentityReturnType; /** \internal the return type of unit vectors */ typedef Block, SquareMatrixType>, internal::traits::RowsAtCompileTime, @@ -133,6 +121,7 @@ template class MatrixBase #endif // not EIGEN_PARSED_BY_DOXYGEN #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::MatrixBase +#define EIGEN_DOC_UNARY_ADDONS(X,Y) # include "../plugins/CommonCwiseUnaryOps.h" # include "../plugins/CommonCwiseBinaryOps.h" # include "../plugins/MatrixCwiseUnaryOps.h" @@ -141,41 +130,53 @@ template class MatrixBase # include EIGEN_MATRIXBASE_PLUGIN # endif #undef EIGEN_CURRENT_STORAGE_BASE_CLASS +#undef EIGEN_DOC_UNARY_ADDONS /** Special case of the template operator=, in order to prevent the compiler * from generating a default operator= (issue hit with g++ 4.1) */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const MatrixBase& other); // We cannot inherit here via Base::operator= since it is causing // trouble with MSVC. template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& other); template + EIGEN_DEVICE_FUNC Derived& operator=(const EigenBase& other); template + EIGEN_DEVICE_FUNC Derived& operator=(const ReturnByValue& other); - template - Derived& lazyAssign(const ProductBase& other); - - template - Derived& lazyAssign(const MatrixPowerProduct& other); - template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const MatrixBase& other); template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const MatrixBase& other); +#ifdef __CUDACC__ + template + EIGEN_DEVICE_FUNC + const Product + operator*(const MatrixBase &other) const + { return this->lazyProduct(other); } +#else + template - const typename ProductReturnType::Type + const Product operator*(const MatrixBase &other) const; +#endif + template - const typename LazyProductReturnType::Type + EIGEN_DEVICE_FUNC + const Product lazyProduct(const MatrixBase &other) const; template @@ -188,84 +189,93 @@ template class MatrixBase void applyOnTheRight(const EigenBase& other); template - const DiagonalProduct + EIGEN_DEVICE_FUNC + const Product operator*(const DiagonalBase &diagonal) const; template - typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType + EIGEN_DEVICE_FUNC + typename ScalarBinaryOpTraits::Scalar,typename internal::traits::Scalar>::ReturnType dot(const MatrixBase& other) const; - #ifdef EIGEN2_SUPPORT - template - Scalar eigen2_dot(const MatrixBase& other) const; - #endif - - RealScalar squaredNorm() const; - RealScalar norm() const; + EIGEN_DEVICE_FUNC RealScalar squaredNorm() const; + EIGEN_DEVICE_FUNC RealScalar norm() const; RealScalar stableNorm() const; RealScalar blueNorm() const; RealScalar hypotNorm() const; - const PlainObject normalized() const; - void normalize(); + EIGEN_DEVICE_FUNC const PlainObject normalized() const; + EIGEN_DEVICE_FUNC const PlainObject stableNormalized() const; + EIGEN_DEVICE_FUNC void normalize(); + EIGEN_DEVICE_FUNC void stableNormalize(); - const AdjointReturnType adjoint() const; - void adjointInPlace(); + EIGEN_DEVICE_FUNC const AdjointReturnType adjoint() const; + EIGEN_DEVICE_FUNC void adjointInPlace(); typedef Diagonal DiagonalReturnType; + EIGEN_DEVICE_FUNC DiagonalReturnType diagonal(); + typedef typename internal::add_const >::type ConstDiagonalReturnType; + EIGEN_DEVICE_FUNC ConstDiagonalReturnType diagonal() const; template struct DiagonalIndexReturnType { typedef Diagonal Type; }; template struct ConstDiagonalIndexReturnType { typedef const Diagonal Type; }; - template typename DiagonalIndexReturnType::Type diagonal(); - template typename ConstDiagonalIndexReturnType::Type diagonal() const; - + template + EIGEN_DEVICE_FUNC + typename DiagonalIndexReturnType::Type diagonal(); + + template + EIGEN_DEVICE_FUNC + typename ConstDiagonalIndexReturnType::Type diagonal() const; + typedef Diagonal DiagonalDynamicIndexReturnType; typedef typename internal::add_const >::type ConstDiagonalDynamicIndexReturnType; + EIGEN_DEVICE_FUNC DiagonalDynamicIndexReturnType diagonal(Index index); + EIGEN_DEVICE_FUNC ConstDiagonalDynamicIndexReturnType diagonal(Index index) const; - #ifdef EIGEN2_SUPPORT - template typename internal::eigen2_part_return_type::type part(); - template const typename internal::eigen2_part_return_type::type part() const; - - // huuuge hack. make Eigen2's matrix.part() work in eigen3. Problem: Diagonal is now a class template instead - // of an integer constant. Solution: overload the part() method template wrt template parameters list. - template class U> - const DiagonalWrapper part() const - { return diagonal().asDiagonal(); } - #endif // EIGEN2_SUPPORT - template struct TriangularViewReturnType { typedef TriangularView Type; }; template struct ConstTriangularViewReturnType { typedef const TriangularView Type; }; - template typename TriangularViewReturnType::Type triangularView(); - template typename ConstTriangularViewReturnType::Type triangularView() const; + template + EIGEN_DEVICE_FUNC + typename TriangularViewReturnType::Type triangularView(); + template + EIGEN_DEVICE_FUNC + typename ConstTriangularViewReturnType::Type triangularView() const; template struct SelfAdjointViewReturnType { typedef SelfAdjointView Type; }; template struct ConstSelfAdjointViewReturnType { typedef const SelfAdjointView Type; }; - template typename SelfAdjointViewReturnType::Type selfadjointView(); - template typename ConstSelfAdjointViewReturnType::Type selfadjointView() const; + template + EIGEN_DEVICE_FUNC + typename SelfAdjointViewReturnType::Type selfadjointView(); + template + EIGEN_DEVICE_FUNC + typename ConstSelfAdjointViewReturnType::Type selfadjointView() const; const SparseView sparseView(const Scalar& m_reference = Scalar(0), const typename NumTraits::Real& m_epsilon = NumTraits::dummy_precision()) const; - static const IdentityReturnType Identity(); - static const IdentityReturnType Identity(Index rows, Index cols); - static const BasisReturnType Unit(Index size, Index i); - static const BasisReturnType Unit(Index i); - static const BasisReturnType UnitX(); - static const BasisReturnType UnitY(); - static const BasisReturnType UnitZ(); - static const BasisReturnType UnitW(); - + EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(); + EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index size, Index i); + EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index i); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitX(); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitY(); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitZ(); + EIGEN_DEVICE_FUNC static const BasisReturnType UnitW(); + + EIGEN_DEVICE_FUNC const DiagonalWrapper asDiagonal() const; const PermutationWrapper asPermutation() const; + EIGEN_DEVICE_FUNC Derived& setIdentity(); + EIGEN_DEVICE_FUNC Derived& setIdentity(Index rows, Index cols); bool isIdentity(const RealScalar& prec = NumTraits::dummy_precision()) const; @@ -297,59 +307,45 @@ template class MatrixBase NoAlias noalias(); - inline const ForceAlignedAccess forceAlignedAccess() const; - inline ForceAlignedAccess forceAlignedAccess(); - template inline typename internal::add_const_on_value_type,Derived&>::type>::type forceAlignedAccessIf() const; - template inline typename internal::conditional,Derived&>::type forceAlignedAccessIf(); + // TODO forceAlignedAccess is temporarily disabled + // Need to find a nicer workaround. + inline const Derived& forceAlignedAccess() const { return derived(); } + inline Derived& forceAlignedAccess() { return derived(); } + template inline const Derived& forceAlignedAccessIf() const { return derived(); } + template inline Derived& forceAlignedAccessIf() { return derived(); } - Scalar trace() const; + EIGEN_DEVICE_FUNC Scalar trace() const; -/////////// Array module /////////// + template EIGEN_DEVICE_FUNC RealScalar lpNorm() const; - template RealScalar lpNorm() const; - - MatrixBase& matrix() { return *this; } - const MatrixBase& matrix() const { return *this; } + EIGEN_DEVICE_FUNC MatrixBase& matrix() { return *this; } + EIGEN_DEVICE_FUNC const MatrixBase& matrix() const { return *this; } /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix * \sa ArrayBase::matrix() */ - ArrayWrapper array() { return derived(); } - const ArrayWrapper array() const { return derived(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ArrayWrapper array() { return ArrayWrapper(derived()); } + /** \returns a const \link Eigen::ArrayBase Array \endlink expression of this matrix + * \sa ArrayBase::matrix() */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArrayWrapper array() const { return ArrayWrapper(derived()); } /////////// LU module /////////// - const FullPivLU fullPivLu() const; - const PartialPivLU partialPivLu() const; + inline const FullPivLU fullPivLu() const; + inline const PartialPivLU partialPivLu() const; - #if EIGEN2_SUPPORT_STAGE < STAGE20_RESOLVE_API_CONFLICTS - const LU lu() const; - #endif + inline const PartialPivLU lu() const; - #ifdef EIGEN2_SUPPORT - const LU eigen2_lu() const; - #endif + inline const Inverse inverse() const; - #if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS - const PartialPivLU lu() const; - #endif - - #ifdef EIGEN2_SUPPORT template - void computeInverse(MatrixBase *result) const { - *result = this->inverse(); - } - #endif - - const internal::inverse_impl inverse() const; - template - void computeInverseAndDetWithCheck( + inline void computeInverseAndDetWithCheck( ResultType& inverse, typename ResultType::Scalar& determinant, bool& invertible, const RealScalar& absDeterminantThreshold = NumTraits::dummy_precision() ) const; template - void computeInverseWithCheck( + inline void computeInverseWithCheck( ResultType& inverse, bool& invertible, const RealScalar& absDeterminantThreshold = NumTraits::dummy_precision() @@ -358,65 +354,70 @@ template class MatrixBase /////////// Cholesky module /////////// - const LLT llt() const; - const LDLT ldlt() const; + inline const LLT llt() const; + inline const LDLT ldlt() const; /////////// QR module /////////// - const HouseholderQR householderQr() const; - const ColPivHouseholderQR colPivHouseholderQr() const; - const FullPivHouseholderQR fullPivHouseholderQr() const; - - #ifdef EIGEN2_SUPPORT - const QR qr() const; - #endif + inline const HouseholderQR householderQr() const; + inline const ColPivHouseholderQR colPivHouseholderQr() const; + inline const FullPivHouseholderQR fullPivHouseholderQr() const; + inline const CompleteOrthogonalDecomposition completeOrthogonalDecomposition() const; - EigenvaluesReturnType eigenvalues() const; - RealScalar operatorNorm() const; +/////////// Eigenvalues module /////////// -/////////// SVD module /////////// + inline EigenvaluesReturnType eigenvalues() const; + inline RealScalar operatorNorm() const; - JacobiSVD jacobiSvd(unsigned int computationOptions = 0) const; +/////////// SVD module /////////// - #ifdef EIGEN2_SUPPORT - SVD svd() const; - #endif + inline JacobiSVD jacobiSvd(unsigned int computationOptions = 0) const; + inline BDCSVD bdcSvd(unsigned int computationOptions = 0) const; /////////// Geometry module /////////// #ifndef EIGEN_PARSED_BY_DOXYGEN /// \internal helper struct to form the return type of the cross product template struct cross_product_return_type { - typedef typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType Scalar; + typedef typename ScalarBinaryOpTraits::Scalar,typename internal::traits::Scalar>::ReturnType Scalar; typedef Matrix type; }; #endif // EIGEN_PARSED_BY_DOXYGEN template - typename cross_product_return_type::type + EIGEN_DEVICE_FUNC +#ifndef EIGEN_PARSED_BY_DOXYGEN + inline typename cross_product_return_type::type +#else + inline PlainObject +#endif cross(const MatrixBase& other) const; + template - PlainObject cross3(const MatrixBase& other) const; - PlainObject unitOrthogonal(void) const; - Matrix eulerAngles(Index a0, Index a1, Index a2) const; - - #if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS - ScalarMultipleReturnType operator*(const UniformScaling& s) const; + EIGEN_DEVICE_FUNC + inline PlainObject cross3(const MatrixBase& other) const; + + EIGEN_DEVICE_FUNC + inline PlainObject unitOrthogonal(void) const; + + EIGEN_DEVICE_FUNC + inline Matrix eulerAngles(Index a0, Index a1, Index a2) const; + // put this as separate enum value to work around possible GCC 4.3 bug (?) - enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1?Vertical:Horizontal }; + enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical) + : ColsAtCompileTime==1 ? Vertical : Horizontal }; typedef Homogeneous HomogeneousReturnType; - HomogeneousReturnType homogeneous() const; - #endif - + EIGEN_DEVICE_FUNC + inline HomogeneousReturnType homogeneous() const; + enum { SizeMinusOne = SizeAtCompileTime==Dynamic ? Dynamic : SizeAtCompileTime-1 }; typedef Block::ColsAtCompileTime==1 ? SizeMinusOne : 1, internal::traits::ColsAtCompileTime==1 ? 1 : SizeMinusOne> ConstStartMinusOne; - typedef CwiseUnaryOp::Scalar>, - const ConstStartMinusOne > HNormalizedReturnType; - - const HNormalizedReturnType hnormalized() const; + typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(ConstStartMinusOne,Scalar,quotient) HNormalizedReturnType; + EIGEN_DEVICE_FUNC + inline const HNormalizedReturnType hnormalized() const; ////////// Householder module /////////// @@ -461,49 +462,15 @@ template class MatrixBase const MatrixSquareRootReturnValue sqrt() const; const MatrixLogarithmReturnValue log() const; const MatrixPowerReturnValue pow(const RealScalar& p) const; - -#ifdef EIGEN2_SUPPORT - template - Derived& operator+=(const Flagged, 0, - EvalBeforeAssigningBit>& other); - - template - Derived& operator-=(const Flagged, 0, - EvalBeforeAssigningBit>& other); - - /** \deprecated because .lazy() is deprecated - * Overloaded for cache friendly product evaluation */ - template - Derived& lazyAssign(const Flagged& other) - { return lazyAssign(other._expression()); } - - template - const Flagged marked() const; - const Flagged lazy() const; - - inline const Cwise cwise() const; - inline Cwise cwise(); - - VectorBlock start(Index size); - const VectorBlock start(Index size) const; - VectorBlock end(Index size); - const VectorBlock end(Index size) const; - template VectorBlock start(); - template const VectorBlock start() const; - template VectorBlock end(); - template const VectorBlock end() const; - - Minor minor(Index row, Index col); - const Minor minor(Index row, Index col) const; -#endif + const MatrixComplexPowerReturnValue pow(const std::complex& p) const; protected: - MatrixBase() : Base() {} + EIGEN_DEVICE_FUNC MatrixBase() : Base() {} private: - explicit MatrixBase(int); - MatrixBase(int,int); - template explicit MatrixBase(const MatrixBase&); + EIGEN_DEVICE_FUNC explicit MatrixBase(int); + EIGEN_DEVICE_FUNC MatrixBase(int,int); + template EIGEN_DEVICE_FUNC explicit MatrixBase(const MatrixBase&); protected: // mixing arrays and matrices is not legal template Derived& operator+=(const ArrayBase& ) diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/NestByValue.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/NestByValue.h index a893b1761b..13adf070e8 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/NestByValue.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/NestByValue.h @@ -13,25 +13,24 @@ namespace Eigen { +namespace internal { +template +struct traits > : public traits +{}; +} + /** \class NestByValue * \ingroup Core_Module * * \brief Expression which must be nested by value * - * \param ExpressionType the type of the object of which we are requiring nesting-by-value + * \tparam ExpressionType the type of the object of which we are requiring nesting-by-value * * This class is the return type of MatrixBase::nestByValue() * and most of the time this is the only way it is used. * * \sa MatrixBase::nestByValue() */ - -namespace internal { -template -struct traits > : public traits -{}; -} - template class NestByValue : public internal::dense_xpr_base< NestByValue >::type { @@ -40,29 +39,29 @@ template class NestByValue typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue) - inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} + EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} - inline Index rows() const { return m_expression.rows(); } - inline Index cols() const { return m_expression.cols(); } - inline Index outerStride() const { return m_expression.outerStride(); } - inline Index innerStride() const { return m_expression.innerStride(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } + EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); } - inline const CoeffReturnType coeff(Index row, Index col) const + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const { return m_expression.coeff(row, col); } - inline Scalar& coeffRef(Index row, Index col) + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) { return m_expression.const_cast_derived().coeffRef(row, col); } - inline const CoeffReturnType coeff(Index index) const + EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const { return m_expression.coeff(index); } - inline Scalar& coeffRef(Index index) + EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) { return m_expression.const_cast_derived().coeffRef(index); } @@ -91,7 +90,7 @@ template class NestByValue m_expression.const_cast_derived().template writePacket(index, x); } - operator const ExpressionType&() const { return m_expression; } + EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } protected: const ExpressionType m_expression; diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/NoAlias.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/NoAlias.h index 768bfb18ca..33908010b4 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/NoAlias.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/NoAlias.h @@ -17,7 +17,7 @@ namespace Eigen { * * \brief Pseudo expression providing an operator = assuming no aliasing * - * \param ExpressionType the type of the object on which to do the lazy assignment + * \tparam ExpressionType the type of the object on which to do the lazy assignment * * This class represents an expression with special assignment operators * assuming no aliasing between the target expression and the source expression. @@ -30,62 +30,36 @@ namespace Eigen { template class StorageBase> class NoAlias { - typedef typename ExpressionType::Scalar Scalar; public: - NoAlias(ExpressionType& expression) : m_expression(expression) {} - - /** Behaves like MatrixBase::lazyAssign(other) - * \sa MatrixBase::lazyAssign() */ + typedef typename ExpressionType::Scalar Scalar; + + explicit NoAlias(ExpressionType& expression) : m_expression(expression) {} + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator=(const StorageBase& other) - { return internal::assign_selector::run(m_expression,other.derived()); } - - /** \sa MatrixBase::operator+= */ + { + call_assignment_no_alias(m_expression, other.derived(), internal::assign_op()); + return m_expression; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator+=(const StorageBase& other) { - typedef SelfCwiseBinaryOp, ExpressionType, OtherDerived> SelfAdder; - SelfAdder tmp(m_expression); - typedef typename internal::nested::type OtherDerivedNested; - typedef typename internal::remove_all::type _OtherDerivedNested; - internal::assign_selector::run(tmp,OtherDerivedNested(other.derived())); + call_assignment_no_alias(m_expression, other.derived(), internal::add_assign_op()); return m_expression; } - - /** \sa MatrixBase::operator-= */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ExpressionType& operator-=(const StorageBase& other) { - typedef SelfCwiseBinaryOp, ExpressionType, OtherDerived> SelfAdder; - SelfAdder tmp(m_expression); - typedef typename internal::nested::type OtherDerivedNested; - typedef typename internal::remove_all::type _OtherDerivedNested; - internal::assign_selector::run(tmp,OtherDerivedNested(other.derived())); + call_assignment_no_alias(m_expression, other.derived(), internal::sub_assign_op()); return m_expression; } -#ifndef EIGEN_PARSED_BY_DOXYGEN - template - EIGEN_STRONG_INLINE ExpressionType& operator+=(const ProductBase& other) - { other.derived().addTo(m_expression); return m_expression; } - - template - EIGEN_STRONG_INLINE ExpressionType& operator-=(const ProductBase& other) - { other.derived().subTo(m_expression); return m_expression; } - - template - EIGEN_STRONG_INLINE ExpressionType& operator+=(const CoeffBasedProduct& other) - { return m_expression.derived() += CoeffBasedProduct(other.lhs(), other.rhs()); } - - template - EIGEN_STRONG_INLINE ExpressionType& operator-=(const CoeffBasedProduct& other) - { return m_expression.derived() -= CoeffBasedProduct(other.lhs(), other.rhs()); } - - template - ExpressionType& operator=(const ReturnByValue& func) - { return m_expression = func; } -#endif - + EIGEN_DEVICE_FUNC ExpressionType& expression() const { return m_expression; @@ -126,7 +100,7 @@ class NoAlias template NoAlias MatrixBase::noalias() { - return derived(); + return NoAlias(derived()); } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/NumTraits.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/NumTraits.h index bac9e50b85..dd61195bc2 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/NumTraits.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/NumTraits.h @@ -12,24 +12,57 @@ namespace Eigen { +namespace internal { + +// default implementation of digits10(), based on numeric_limits if specialized, +// 0 for integer types, and log10(epsilon()) otherwise. +template< typename T, + bool use_numeric_limits = std::numeric_limits::is_specialized, + bool is_integer = NumTraits::IsInteger> +struct default_digits10_impl +{ + static int run() { return std::numeric_limits::digits10; } +}; + +template +struct default_digits10_impl // Floating point +{ + static int run() { + using std::log10; + using std::ceil; + typedef typename NumTraits::Real Real; + return int(ceil(-log10(NumTraits::epsilon()))); + } +}; + +template +struct default_digits10_impl // Integer +{ + static int run() { return 0; } +}; + +} // end namespace internal + /** \class NumTraits * \ingroup Core_Module * * \brief Holds information about the various numeric (i.e. scalar) types allowed by Eigen. * - * \param T the numeric type at hand + * \tparam T the numeric type at hand * * This class stores enums, typedefs and static methods giving information about a numeric type. * * The provided data consists of: - * \li A typedef \a Real, giving the "real part" type of \a T. If \a T is already real, - * then \a Real is just a typedef to \a T. If \a T is \c std::complex then \a Real + * \li A typedef \c Real, giving the "real part" type of \a T. If \a T is already real, + * then \c Real is just a typedef to \a T. If \a T is \c std::complex then \c Real * is a typedef to \a U. - * \li A typedef \a NonInteger, giving the type that should be used for operations producing non-integral values, + * \li A typedef \c NonInteger, giving the type that should be used for operations producing non-integral values, * such as quotients, square roots, etc. If \a T is a floating-point type, then this typedef just gives * \a T again. Note however that many Eigen functions such as internal::sqrt simply refuse to * take integers. Outside of a few cases, Eigen doesn't do automatic type promotion. Thus, this typedef is * only intended as a helper for code that needs to explicitly promote types. + * \li A typedef \c Literal giving the type to use for numeric literals such as "2" or "0.5". For instance, for \c std::complex, Literal is defined as \c U. + * Of course, this type must be fully compatible with \a T. In doubt, just use \a T here. * \li A typedef \a Nested giving the type to use to nest a value inside of the expression tree. If you don't know what * this means, just use \a T here. * \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c std::complex @@ -42,10 +75,14 @@ namespace Eigen { * \li An enum value \a IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T is unsigned. * \li An enum value \a RequireInitialization. It is equal to \c 1 if the constructor of the numeric type \a T must * be called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1 otherwise. - * \li An epsilon() function which, unlike std::numeric_limits::epsilon(), returns a \a Real instead of a \a T. + * \li An epsilon() function which, unlike std::numeric_limits::epsilon(), + * it returns a \a Real instead of a \a T. * \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a default * value by the fuzzy comparison operators. * \li highest() and lowest() functions returning the highest and lowest possible values respectively. + * \li digits10() function returning the number of decimal digits that can be represented without change. This is + * the analogue of std::numeric_limits::digits10 + * which is used as the default implementation if specialized. */ template struct GenericNumTraits @@ -67,22 +104,47 @@ template struct GenericNumTraits T >::type NonInteger; typedef T Nested; + typedef T Literal; + + EIGEN_DEVICE_FUNC + static inline Real epsilon() + { + return numext::numeric_limits::epsilon(); + } - static inline Real epsilon() { return std::numeric_limits::epsilon(); } + EIGEN_DEVICE_FUNC + static inline int digits10() + { + return internal::default_digits10_impl::run(); + } + + EIGEN_DEVICE_FUNC static inline Real dummy_precision() { // make sure to override this for floating-point types return Real(0); } - static inline T highest() { return (std::numeric_limits::max)(); } - static inline T lowest() { return IsInteger ? (std::numeric_limits::min)() : (-(std::numeric_limits::max)()); } - -#ifdef EIGEN2_SUPPORT - enum { - HasFloatingPoint = !IsInteger - }; - typedef NonInteger FloatingPoint; -#endif + + + EIGEN_DEVICE_FUNC + static inline T highest() { + return (numext::numeric_limits::max)(); + } + + EIGEN_DEVICE_FUNC + static inline T lowest() { + return IsInteger ? (numext::numeric_limits::min)() : (-(numext::numeric_limits::max)()); + } + + EIGEN_DEVICE_FUNC + static inline T infinity() { + return numext::numeric_limits::infinity(); + } + + EIGEN_DEVICE_FUNC + static inline T quiet_NaN() { + return numext::numeric_limits::quiet_NaN(); + } }; template struct NumTraits : GenericNumTraits @@ -91,11 +153,13 @@ template struct NumTraits : GenericNumTraits template<> struct NumTraits : GenericNumTraits { + EIGEN_DEVICE_FUNC static inline float dummy_precision() { return 1e-5f; } }; template<> struct NumTraits : GenericNumTraits { + EIGEN_DEVICE_FUNC static inline double dummy_precision() { return 1e-12; } }; @@ -109,6 +173,7 @@ template struct NumTraits > : GenericNumTraits > { typedef _Real Real; + typedef typename NumTraits<_Real>::Literal Literal; enum { IsComplex = 1, RequireInitialization = NumTraits<_Real>::RequireInitialization, @@ -117,8 +182,12 @@ template struct NumTraits > MulCost = 4 * NumTraits::MulCost + 2 * NumTraits::AddCost }; + EIGEN_DEVICE_FUNC static inline Real epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC static inline Real dummy_precision() { return NumTraits::dummy_precision(); } + EIGEN_DEVICE_FUNC + static inline int digits10() { return NumTraits::digits10(); } }; template @@ -130,21 +199,48 @@ struct NumTraits > typedef typename NumTraits::NonInteger NonIntegerScalar; typedef Array NonInteger; typedef ArrayType & Nested; - + typedef typename NumTraits::Literal Literal; + enum { IsComplex = NumTraits::IsComplex, IsInteger = NumTraits::IsInteger, IsSigned = NumTraits::IsSigned, RequireInitialization = 1, - ReadCost = ArrayType::SizeAtCompileTime==Dynamic ? Dynamic : ArrayType::SizeAtCompileTime * NumTraits::ReadCost, - AddCost = ArrayType::SizeAtCompileTime==Dynamic ? Dynamic : ArrayType::SizeAtCompileTime * NumTraits::AddCost, - MulCost = ArrayType::SizeAtCompileTime==Dynamic ? Dynamic : ArrayType::SizeAtCompileTime * NumTraits::MulCost + ReadCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits::ReadCost, + AddCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits::AddCost, + MulCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits::MulCost }; - + + EIGEN_DEVICE_FUNC static inline RealScalar epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC static inline RealScalar dummy_precision() { return NumTraits::dummy_precision(); } }; +template<> struct NumTraits + : GenericNumTraits +{ + enum { + RequireInitialization = 1, + ReadCost = HugeCost, + AddCost = HugeCost, + MulCost = HugeCost + }; + + static inline int digits10() { return 0; } + +private: + static inline std::string epsilon(); + static inline std::string dummy_precision(); + static inline std::string lowest(); + static inline std::string highest(); + static inline std::string infinity(); + static inline std::string quiet_NaN(); +}; + +// Empty specialization for void to allow template specialization based on NumTraits::Real with T==void and SFINAE. +template<> struct NumTraits {}; + } // end namespace Eigen #endif // EIGEN_NUMTRAITS_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/PermutationMatrix.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/PermutationMatrix.h index 85ffae2653..b1fb455b98 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/PermutationMatrix.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/PermutationMatrix.h @@ -2,7 +2,7 @@ // for linear algebra. // // Copyright (C) 2009 Benoit Jacob -// Copyright (C) 2009-2011 Gael Guennebaud +// Copyright (C) 2009-2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -13,14 +13,18 @@ namespace Eigen { -template class PermutedImpl; +namespace internal { + +enum PermPermProduct_t {PermPermProduct}; + +} // end namespace internal /** \class PermutationBase * \ingroup Core_Module * * \brief Base class for permutations * - * \param Derived the derived class + * \tparam Derived the derived class * * This class is the base class for all expressions representing a permutation matrix, * internally stored as a vector of integers. @@ -38,17 +42,6 @@ template -struct permut_matrix_product_retval; -template -struct permut_sparsematrix_product_retval; -enum PermPermProduct_t {PermPermProduct}; - -} // end namespace internal - template class PermutationBase : public EigenBase { @@ -60,19 +53,20 @@ class PermutationBase : public EigenBase typedef typename Traits::IndicesType IndicesType; enum { Flags = Traits::Flags, - CoeffReadCost = Traits::CoeffReadCost, RowsAtCompileTime = Traits::RowsAtCompileTime, ColsAtCompileTime = Traits::ColsAtCompileTime, MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime, MaxColsAtCompileTime = Traits::MaxColsAtCompileTime }; - typedef typename Traits::Scalar Scalar; - typedef typename Traits::Index Index; - typedef Matrix + typedef typename Traits::StorageIndex StorageIndex; + typedef Matrix DenseMatrixType; - typedef PermutationMatrix + typedef PermutationMatrix PlainPermutationType; + typedef PlainPermutationType PlainObject; using Base::derived; + typedef Inverse InverseReturnType; + typedef void Scalar; #endif /** Copies the other permutation into *this */ @@ -118,7 +112,7 @@ class PermutationBase : public EigenBase void evalTo(MatrixBase& other) const { other.setZero(); - for (int i=0; i /** Sets *this to be the identity permutation matrix */ void setIdentity() { - for(Index i = 0; i < size(); ++i) + StorageIndex n = StorageIndex(size()); + for(StorageIndex i = 0; i < n; ++i) indices().coeffRef(i) = i; } @@ -163,18 +158,18 @@ class PermutationBase : public EigenBase * * \returns a reference to *this. * - * \warning This is much slower than applyTranspositionOnTheRight(int,int): + * \warning This is much slower than applyTranspositionOnTheRight(Index,Index): * this has linear complexity and requires a lot of branching. * - * \sa applyTranspositionOnTheRight(int,int) + * \sa applyTranspositionOnTheRight(Index,Index) */ Derived& applyTranspositionOnTheLeft(Index i, Index j) { eigen_assert(i>=0 && j>=0 && i * * This is a fast operation, it only consists in swapping two indices. * - * \sa applyTranspositionOnTheLeft(int,int) + * \sa applyTranspositionOnTheLeft(Index,Index) */ Derived& applyTranspositionOnTheRight(Index i, Index j) { @@ -196,16 +191,16 @@ class PermutationBase : public EigenBase /** \returns the inverse permutation matrix. * - * \note \note_try_to_help_rvo + * \note \blank \note_try_to_help_rvo */ - inline Transpose inverse() const - { return derived(); } + inline InverseReturnType inverse() const + { return InverseReturnType(derived()); } /** \returns the tranpose permutation matrix. * - * \note \note_try_to_help_rvo + * \note \blank \note_try_to_help_rvo */ - inline Transpose transpose() const - { return derived(); } + inline InverseReturnType transpose() const + { return InverseReturnType(derived()); } /**** multiplication helpers to hopefully get RVO ****/ @@ -215,13 +210,13 @@ class PermutationBase : public EigenBase template void assignTranspose(const PermutationBase& other) { - for (int i=0; i void assignProduct(const Lhs& lhs, const Rhs& rhs) { eigen_assert(lhs.cols() == rhs.rows()); - for (int i=0; i /** \returns the product permutation matrix. * - * \note \note_try_to_help_rvo + * \note \blank \note_try_to_help_rvo */ template inline PlainPermutationType operator*(const PermutationBase& other) const @@ -237,18 +232,18 @@ class PermutationBase : public EigenBase /** \returns the product of a permutation with another inverse permutation. * - * \note \note_try_to_help_rvo + * \note \blank \note_try_to_help_rvo */ template - inline PlainPermutationType operator*(const Transpose >& other) const + inline PlainPermutationType operator*(const InverseImpl& other) const { return PlainPermutationType(internal::PermPermProduct, *this, other.eval()); } /** \returns the product of an inverse permutation with another permutation. * - * \note \note_try_to_help_rvo + * \note \blank \note_try_to_help_rvo */ template friend - inline PlainPermutationType operator*(const Transpose >& other, const PermutationBase& perm) + inline PlainPermutationType operator*(const InverseImpl& other, const PermutationBase& perm) { return PlainPermutationType(internal::PermPermProduct, other.eval(), perm); } /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the permutation. @@ -284,39 +279,43 @@ class PermutationBase : public EigenBase }; +namespace internal { +template +struct traits > + : traits > +{ + typedef PermutationStorage StorageKind; + typedef Matrix<_StorageIndex, SizeAtCompileTime, 1, 0, MaxSizeAtCompileTime, 1> IndicesType; + typedef _StorageIndex StorageIndex; + typedef void Scalar; +}; +} + /** \class PermutationMatrix * \ingroup Core_Module * * \brief Permutation matrix * - * \param SizeAtCompileTime the number of rows/cols, or Dynamic - * \param MaxSizeAtCompileTime the maximum number of rows/cols, or Dynamic. This optional parameter defaults to SizeAtCompileTime. Most of the time, you should not have to specify it. - * \param IndexType the interger type of the indices + * \tparam SizeAtCompileTime the number of rows/cols, or Dynamic + * \tparam MaxSizeAtCompileTime the maximum number of rows/cols, or Dynamic. This optional parameter defaults to SizeAtCompileTime. Most of the time, you should not have to specify it. + * \tparam _StorageIndex the integer type of the indices * * This class represents a permutation matrix, internally stored as a vector of integers. * * \sa class PermutationBase, class PermutationWrapper, class DiagonalMatrix */ - -namespace internal { -template -struct traits > - : traits > -{ - typedef IndexType Index; - typedef Matrix IndicesType; -}; -} - -template -class PermutationMatrix : public PermutationBase > +template +class PermutationMatrix : public PermutationBase > { typedef PermutationBase Base; typedef internal::traits Traits; public: + typedef const PermutationMatrix& Nested; + #ifndef EIGEN_PARSED_BY_DOXYGEN typedef typename Traits::IndicesType IndicesType; + typedef typename Traits::StorageIndex StorageIndex; #endif inline PermutationMatrix() @@ -324,8 +323,10 @@ class PermutationMatrix : public PermutationBase::highest()); + } /** Copy constructor. */ template @@ -346,7 +347,7 @@ class PermutationMatrix : public PermutationBase - explicit inline PermutationMatrix(const MatrixBase& a_indices) : m_indices(a_indices) + explicit inline PermutationMatrix(const MatrixBase& indices) : m_indices(indices) {} /** Convert the Transpositions \a tr to a permutation matrix */ @@ -393,10 +394,13 @@ class PermutationMatrix : public PermutationBase - PermutationMatrix(const Transpose >& other) - : m_indices(other.nestedPermutation().size()) + PermutationMatrix(const InverseImpl& other) + : m_indices(other.derived().nestedExpression().size()) { - for (int i=0; i::highest()); + StorageIndex end = StorageIndex(m_indices.size()); + for (StorageIndex i=0; i PermutationMatrix(internal::PermPermProduct_t, const Lhs& lhs, const Rhs& rhs) @@ -413,18 +417,20 @@ class PermutationMatrix : public PermutationBase -struct traits,_PacketAccess> > - : traits > +template +struct traits,_PacketAccess> > + : traits > { - typedef IndexType Index; - typedef Map, _PacketAccess> IndicesType; + typedef PermutationStorage StorageKind; + typedef Map, _PacketAccess> IndicesType; + typedef _StorageIndex StorageIndex; + typedef void Scalar; }; } -template -class Map,_PacketAccess> - : public PermutationBase,_PacketAccess> > +template +class Map,_PacketAccess> + : public PermutationBase,_PacketAccess> > { typedef PermutationBase Base; typedef internal::traits Traits; @@ -432,14 +438,14 @@ class Map, #ifndef EIGEN_PARSED_BY_DOXYGEN typedef typename Traits::IndicesType IndicesType; - typedef typename IndicesType::Scalar Index; + typedef typename IndicesType::Scalar StorageIndex; #endif - inline Map(const Index* indicesPtr) + inline Map(const StorageIndex* indicesPtr) : m_indices(indicesPtr) {} - inline Map(const Index* indicesPtr, Index size) + inline Map(const StorageIndex* indicesPtr, Index size) : m_indices(indicesPtr,size) {} @@ -474,40 +480,36 @@ class Map, IndicesType m_indices; }; -/** \class PermutationWrapper - * \ingroup Core_Module - * - * \brief Class to view a vector of integers as a permutation matrix - * - * \param _IndicesType the type of the vector of integer (can be any compatible expression) - * - * This class allows to view any vector expression of integers as a permutation matrix. - * - * \sa class PermutationBase, class PermutationMatrix - */ - -struct PermutationStorage {}; - template class TranspositionsWrapper; namespace internal { template struct traits > { typedef PermutationStorage StorageKind; - typedef typename _IndicesType::Scalar Scalar; - typedef typename _IndicesType::Scalar Index; + typedef void Scalar; + typedef typename _IndicesType::Scalar StorageIndex; typedef _IndicesType IndicesType; enum { RowsAtCompileTime = _IndicesType::SizeAtCompileTime, ColsAtCompileTime = _IndicesType::SizeAtCompileTime, - MaxRowsAtCompileTime = IndicesType::MaxRowsAtCompileTime, - MaxColsAtCompileTime = IndicesType::MaxColsAtCompileTime, - Flags = 0, - CoeffReadCost = _IndicesType::CoeffReadCost + MaxRowsAtCompileTime = IndicesType::MaxSizeAtCompileTime, + MaxColsAtCompileTime = IndicesType::MaxSizeAtCompileTime, + Flags = 0 }; }; } +/** \class PermutationWrapper + * \ingroup Core_Module + * + * \brief Class to view a vector of integers as a permutation matrix + * + * \tparam _IndicesType the type of the vector of integer (can be any compatible expression) + * + * This class allows to view any vector expression of integers as a permutation matrix. + * + * \sa class PermutationBase, class PermutationMatrix + */ template class PermutationWrapper : public PermutationBase > { @@ -519,8 +521,8 @@ class PermutationWrapper : public PermutationBase -inline const internal::permut_matrix_product_retval -operator*(const MatrixBase& matrix, - const PermutationBase &permutation) +template +EIGEN_DEVICE_FUNC +const Product +operator*(const MatrixBase &matrix, + const PermutationBase& permutation) { - return internal::permut_matrix_product_retval - - (permutation.derived(), matrix.derived()); + return Product + (matrix.derived(), permutation.derived()); } /** \returns the matrix with the permutation applied to the rows. */ -template -inline const internal::permut_matrix_product_retval - +template +EIGEN_DEVICE_FUNC +const Product operator*(const PermutationBase &permutation, - const MatrixBase& matrix) + const MatrixBase& matrix) { - return internal::permut_matrix_product_retval - - (permutation.derived(), matrix.derived()); + return Product + (permutation.derived(), matrix.derived()); } -namespace internal { -template -struct traits > +template +class InverseImpl + : public EigenBase > { - typedef typename MatrixType::PlainObject ReturnType; -}; - -template -struct permut_matrix_product_retval - : public ReturnByValue > -{ - typedef typename remove_all::type MatrixTypeNestedCleaned; - typedef typename MatrixType::Index Index; - - permut_matrix_product_retval(const PermutationType& perm, const MatrixType& matrix) - : m_permutation(perm), m_matrix(matrix) - {} - - inline Index rows() const { return m_matrix.rows(); } - inline Index cols() const { return m_matrix.cols(); } - - template inline void evalTo(Dest& dst) const - { - const Index n = Side==OnTheLeft ? rows() : cols(); - // FIXME we need an is_same for expression that is not sensitive to constness. For instance - // is_same_xpr, Block >::value should be true. - if( is_same::value - && blas_traits::HasUsableDirectAccess - && blas_traits::HasUsableDirectAccess - && extract_data(dst) == extract_data(m_matrix)) - { - // apply the permutation inplace - Matrix mask(m_permutation.size()); - mask.fill(false); - Index r = 0; - while(r < m_permutation.size()) - { - // search for the next seed - while(r=m_permutation.size()) - break; - // we got one, let's follow it until we are back to the seed - Index k0 = r++; - Index kPrev = k0; - mask.coeffRef(k0) = true; - for(Index k=m_permutation.indices().coeff(k0); k!=k0; k=m_permutation.indices().coeff(k)) - { - Block(dst, k) - .swap(Block - (dst,((Side==OnTheLeft) ^ Transposed) ? k0 : kPrev)); - - mask.coeffRef(k) = true; - kPrev = k; - } - } - } - else - { - for(int i = 0; i < n; ++i) - { - Block - (dst, ((Side==OnTheLeft) ^ Transposed) ? m_permutation.indices().coeff(i) : i) - - = - - Block - (m_matrix, ((Side==OnTheRight) ^ Transposed) ? m_permutation.indices().coeff(i) : i); - } - } - } - - protected: - const PermutationType& m_permutation; - typename MatrixType::Nested m_matrix; -}; - -/* Template partial specialization for transposed/inverse permutations */ - -template -struct traits > > - : traits -{}; - -} // end namespace internal - -template -class Transpose > - : public EigenBase > > -{ - typedef Derived PermutationType; - typedef typename PermutationType::IndicesType IndicesType; typedef typename PermutationType::PlainPermutationType PlainPermutationType; + typedef internal::traits PermTraits; + protected: + InverseImpl() {} public: + typedef Inverse InverseType; + using EigenBase >::derived; #ifndef EIGEN_PARSED_BY_DOXYGEN - typedef internal::traits Traits; - typedef typename Derived::DenseMatrixType DenseMatrixType; + typedef typename PermutationType::DenseMatrixType DenseMatrixType; enum { - Flags = Traits::Flags, - CoeffReadCost = Traits::CoeffReadCost, - RowsAtCompileTime = Traits::RowsAtCompileTime, - ColsAtCompileTime = Traits::ColsAtCompileTime, - MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime, - MaxColsAtCompileTime = Traits::MaxColsAtCompileTime + RowsAtCompileTime = PermTraits::RowsAtCompileTime, + ColsAtCompileTime = PermTraits::ColsAtCompileTime, + MaxRowsAtCompileTime = PermTraits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = PermTraits::MaxColsAtCompileTime }; - typedef typename Traits::Scalar Scalar; #endif - Transpose(const PermutationType& p) : m_permutation(p) {} - - inline int rows() const { return m_permutation.rows(); } - inline int cols() const { return m_permutation.cols(); } - #ifndef EIGEN_PARSED_BY_DOXYGEN template void evalTo(MatrixBase& other) const { other.setZero(); - for (int i=0; i friend - inline const internal::permut_matrix_product_retval - operator*(const MatrixBase& matrix, const Transpose& trPerm) + const Product + operator*(const MatrixBase& matrix, const InverseType& trPerm) { - return internal::permut_matrix_product_retval(trPerm.m_permutation, matrix.derived()); + return Product(matrix.derived(), trPerm.derived()); } /** \returns the matrix with the inverse permutation applied to the rows. */ template - inline const internal::permut_matrix_product_retval + const Product operator*(const MatrixBase& matrix) const { - return internal::permut_matrix_product_retval(m_permutation, matrix.derived()); + return Product(derived(), matrix.derived()); } - - const PermutationType& nestedPermutation() const { return m_permutation; } - - protected: - const PermutationType& m_permutation; }; template @@ -716,6 +622,12 @@ const PermutationWrapper MatrixBase::asPermutation() con return derived(); } +namespace internal { + +template<> struct AssignmentKind { typedef EigenBase2EigenBase Kind; }; + +} // end namespace internal + } // end namespace Eigen #endif // EIGEN_PERMUTATIONMATRIX_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/PlainObjectBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/PlainObjectBase.h index a4e4af4a7b..77f4f60667 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/PlainObjectBase.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/PlainObjectBase.h @@ -28,6 +28,7 @@ namespace internal { template struct check_rows_cols_for_overflow { template + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE void run(Index, Index) { } @@ -35,11 +36,12 @@ template struct check_rows_cols_for_overflow { template<> struct check_rows_cols_for_overflow { template + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE void run(Index rows, Index cols) { // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242 // we assume Index is signed - Index max_index = (size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed + Index max_index = (std::size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed bool error = (rows == 0 || cols == 0) ? false : (rows > max_index / cols); if (error) @@ -56,33 +58,41 @@ template struct m } // end namespace internal -/** \class PlainObjectBase - * \brief %Dense storage base class for matrices and arrays. - * - * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_PLAINOBJECTBASE_PLUGIN. - * - * \sa \ref TopicClassHierarchy - */ #ifdef EIGEN_PARSED_BY_DOXYGEN -namespace internal { +namespace doxygen { -// this is a warkaround to doxygen not being able to understand the inheritence logic +// This is a workaround to doxygen not being able to understand the inheritance logic // when it is hidden by the dense_xpr_base helper struct. -template struct dense_xpr_base_dispatcher_for_doxygen;// : public MatrixBase {}; +// Moreover, doxygen fails to include members that are not documented in the declaration body of +// MatrixBase if we inherits MatrixBase >, +// this is why we simply inherits MatrixBase, though this does not make sense. + +/** This class is just a workaround for Doxygen and it does not not actually exist. */ +template struct dense_xpr_base_dispatcher; /** This class is just a workaround for Doxygen and it does not not actually exist. */ template -struct dense_xpr_base_dispatcher_for_doxygen > - : public MatrixBase > {}; +struct dense_xpr_base_dispatcher > + : public MatrixBase {}; /** This class is just a workaround for Doxygen and it does not not actually exist. */ template -struct dense_xpr_base_dispatcher_for_doxygen > - : public ArrayBase > {}; +struct dense_xpr_base_dispatcher > + : public ArrayBase {}; -} // namespace internal +} // namespace doxygen +/** \class PlainObjectBase + * \ingroup Core_Module + * \brief %Dense storage base class for matrices and arrays. + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_PLAINOBJECTBASE_PLUGIN. + * + * \tparam Derived is the derived type, e.g., a Matrix or Array + * + * \sa \ref TopicClassHierarchy + */ template -class PlainObjectBase : public internal::dense_xpr_base_dispatcher_for_doxygen +class PlainObjectBase : public doxygen::dense_xpr_base_dispatcher #else template class PlainObjectBase : public internal::dense_xpr_base::type @@ -93,8 +103,8 @@ class PlainObjectBase : public internal::dense_xpr_base::type typedef typename internal::dense_xpr_base::type Base; typedef typename internal::traits::StorageKind StorageKind; - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; + typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; typedef Derived DenseType; @@ -113,28 +123,40 @@ class PlainObjectBase : public internal::dense_xpr_base::type typedef Eigen::Map MapType; friend class Eigen::Map; typedef const Eigen::Map ConstMapType; - friend class Eigen::Map; - typedef Eigen::Map AlignedMapType; - friend class Eigen::Map; - typedef const Eigen::Map ConstAlignedMapType; +#if EIGEN_MAX_ALIGN_BYTES>0 + // for EIGEN_MAX_ALIGN_BYTES==0, AlignedMax==Unaligned, and many compilers generate warnings for friend-ing a class twice. + friend class Eigen::Map; + friend class Eigen::Map; +#endif + typedef Eigen::Map AlignedMapType; + typedef const Eigen::Map ConstAlignedMapType; template struct StridedMapType { typedef Eigen::Map type; }; template struct StridedConstMapType { typedef Eigen::Map type; }; - template struct StridedAlignedMapType { typedef Eigen::Map type; }; - template struct StridedConstAlignedMapType { typedef Eigen::Map type; }; + template struct StridedAlignedMapType { typedef Eigen::Map type; }; + template struct StridedConstAlignedMapType { typedef Eigen::Map type; }; protected: DenseStorage m_storage; public: - enum { NeedsToAlign = SizeAtCompileTime != Dynamic && (internal::traits::Flags & AlignedBit) != 0 }; + enum { NeedsToAlign = (SizeAtCompileTime != Dynamic) && (internal::traits::Alignment>0) }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + EIGEN_DEVICE_FUNC Base& base() { return *static_cast(this); } + EIGEN_DEVICE_FUNC const Base& base() const { return *static_cast(this); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_storage.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_storage.cols(); } + /** This is an overloaded version of DenseCoeffsBase::coeff(Index,Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeff(Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index rowId, Index colId) const { if(Flags & RowMajorBit) @@ -143,11 +165,21 @@ class PlainObjectBase : public internal::dense_xpr_base::type return m_storage.data()[rowId + colId * m_storage.rows()]; } + /** This is an overloaded version of DenseCoeffsBase::coeff(Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeff(Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const { return m_storage.data()[index]; } + /** This is an overloaded version of DenseCoeffsBase::coeffRef(Index,Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeffRef(Index,Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index rowId, Index colId) { if(Flags & RowMajorBit) @@ -156,11 +188,19 @@ class PlainObjectBase : public internal::dense_xpr_base::type return m_storage.data()[rowId + colId * m_storage.rows()]; } + /** This is an overloaded version of DenseCoeffsBase::coeffRef(Index) const + * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. + * + * See DenseCoeffsBase::coeffRef(Index) const for details. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { return m_storage.data()[index]; } + /** This is the const version of coeffRef(Index,Index) which is thus synonym of coeff(Index,Index). + * It is provided for convenience. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeffRef(Index rowId, Index colId) const { if(Flags & RowMajorBit) @@ -169,6 +209,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type return m_storage.data()[rowId + colId * m_storage.rows()]; } + /** This is the const version of coeffRef(Index) which is thus synonym of coeff(Index). + * It is provided for convenience. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeffRef(Index index) const { return m_storage.data()[index]; @@ -209,11 +252,11 @@ class PlainObjectBase : public internal::dense_xpr_base::type } /** \returns a const pointer to the data array of this matrix */ - EIGEN_STRONG_INLINE const Scalar *data() const + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar *data() const { return m_storage.data(); } /** \returns a pointer to the data array of this matrix */ - EIGEN_STRONG_INLINE Scalar *data() + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar *data() { return m_storage.data(); } /** Resizes \c *this to a \a rows x \a cols matrix. @@ -232,22 +275,22 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index) for vectors, resize(NoChange_t, Index), resize(Index, NoChange_t) */ - EIGEN_STRONG_INLINE void resize(Index nbRows, Index nbCols) - { - eigen_assert( EIGEN_IMPLIES(RowsAtCompileTime!=Dynamic,nbRows==RowsAtCompileTime) - && EIGEN_IMPLIES(ColsAtCompileTime!=Dynamic,nbCols==ColsAtCompileTime) - && EIGEN_IMPLIES(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic,nbRows<=MaxRowsAtCompileTime) - && EIGEN_IMPLIES(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic,nbCols<=MaxColsAtCompileTime) - && nbRows>=0 && nbCols>=0 && "Invalid sizes when resizing a matrix or array."); - internal::check_rows_cols_for_overflow::run(nbRows, nbCols); + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void resize(Index rows, Index cols) + { + eigen_assert( EIGEN_IMPLIES(RowsAtCompileTime!=Dynamic,rows==RowsAtCompileTime) + && EIGEN_IMPLIES(ColsAtCompileTime!=Dynamic,cols==ColsAtCompileTime) + && EIGEN_IMPLIES(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic,rows<=MaxRowsAtCompileTime) + && EIGEN_IMPLIES(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic,cols<=MaxColsAtCompileTime) + && rows>=0 && cols>=0 && "Invalid sizes when resizing a matrix or array."); + internal::check_rows_cols_for_overflow::run(rows, cols); #ifdef EIGEN_INITIALIZE_COEFFS - Index size = nbRows*nbCols; + Index size = rows*cols; bool size_changed = size != this->size(); - m_storage.resize(size, nbRows, nbCols); + m_storage.resize(size, rows, cols); if(size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED #else - internal::check_rows_cols_for_overflow::run(nbRows, nbCols); - m_storage.resize(nbRows*nbCols, nbRows, nbCols); + m_storage.resize(rows*cols, rows, cols); #endif } @@ -262,6 +305,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index), resize(NoChange_t, Index), resize(Index, NoChange_t) */ + EIGEN_DEVICE_FUNC inline void resize(Index size) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(PlainObjectBase) @@ -286,9 +330,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index) */ - inline void resize(NoChange_t, Index nbCols) + EIGEN_DEVICE_FUNC + inline void resize(NoChange_t, Index cols) { - resize(rows(), nbCols); + resize(rows(), cols); } /** Resizes the matrix, changing only the number of rows. For the parameter of type NoChange_t, just pass the special value \c NoChange @@ -299,9 +344,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index) */ - inline void resize(Index nbRows, NoChange_t) + EIGEN_DEVICE_FUNC + inline void resize(Index rows, NoChange_t) { - resize(nbRows, cols()); + resize(rows, cols()); } /** Resizes \c *this to have the same dimensions as \a other. @@ -312,6 +358,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * remain row-vectors and vectors remain vectors. */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { const OtherDerived& other = _other.derived(); @@ -339,9 +386,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type * Matrices are resized relative to the top-left element. In case values need to be * appended to the matrix they will be uninitialized. */ - EIGEN_STRONG_INLINE void conservativeResize(Index nbRows, Index nbCols) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void conservativeResize(Index rows, Index cols) { - internal::conservative_resize_like_impl::run(*this, nbRows, nbCols); + internal::conservative_resize_like_impl::run(*this, rows, cols); } /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. @@ -351,10 +399,11 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * In case the matrix is growing, new rows will be uninitialized. */ - EIGEN_STRONG_INLINE void conservativeResize(Index nbRows, NoChange_t) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void conservativeResize(Index rows, NoChange_t) { // Note: see the comment in conservativeResize(Index,Index) - conservativeResize(nbRows, cols()); + conservativeResize(rows, cols()); } /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. @@ -364,10 +413,11 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * In case the matrix is growing, new columns will be uninitialized. */ - EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index nbCols) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index cols) { // Note: see the comment in conservativeResize(Index,Index) - conservativeResize(rows(), nbCols); + conservativeResize(rows(), cols); } /** Resizes the vector to \a size while retaining old values. @@ -378,6 +428,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * When values are appended, they will be uninitialized. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResize(Index size) { internal::conservative_resize_like_impl::run(*this, size); @@ -393,6 +444,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * appended to the matrix they will copied from \c other. */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void conservativeResizeLike(const DenseBase& other) { internal::conservative_resize_like_impl::run(*this, other); @@ -401,6 +453,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type /** This is a special case of the templated operator=. Its purpose is to * prevent a default operator= from hiding the templated operator=. */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const PlainObjectBase& other) { return _set(other); @@ -408,6 +461,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type /** \sa MatrixBase::lazyAssign() */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& lazyAssign(const DenseBase& other) { _resize_to_match(other); @@ -415,12 +469,18 @@ class PlainObjectBase : public internal::dense_xpr_base::type } template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const ReturnByValue& func) { resize(func.rows(), func.cols()); return Base::operator=(func); } + // Prevent user from trying to instantiate PlainObjectBase objects + // by making all its constructor protected. See bug 1074. + protected: + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase() : m_storage() { // _check_template_params(); @@ -430,20 +490,23 @@ class PlainObjectBase : public internal::dense_xpr_base::type #ifndef EIGEN_PARSED_BY_DOXYGEN // FIXME is it still needed ? /** \internal */ - PlainObjectBase(internal::constructor_without_unaligned_array_assert) + EIGEN_DEVICE_FUNC + explicit PlainObjectBase(internal::constructor_without_unaligned_array_assert) : m_storage(internal::constructor_without_unaligned_array_assert()) { // _check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #endif -#ifdef EIGEN_HAVE_RVALUE_REFERENCES - PlainObjectBase(PlainObjectBase&& other) +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC + PlainObjectBase(PlainObjectBase&& other) EIGEN_NOEXCEPT : m_storage( std::move(other.m_storage) ) { } - PlainObjectBase& operator=(PlainObjectBase&& other) + EIGEN_DEVICE_FUNC + PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT { using std::swap; swap(m_storage, other.m_storage); @@ -452,31 +515,56 @@ class PlainObjectBase : public internal::dense_xpr_base::type #endif /** Copy constructor */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other) - : m_storage() + : Base(), m_storage(other.m_storage) { } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PlainObjectBase(Index size, Index rows, Index cols) + : m_storage(size, rows, cols) { - _check_template_params(); - lazyAssign(other); +// _check_template_params(); +// EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } + /** \sa PlainObjectBase::operator=(const EigenBase&) */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase &other) : m_storage() { _check_template_params(); - lazyAssign(other); + resizeLike(other); + _set_noalias(other); } - EIGEN_STRONG_INLINE PlainObjectBase(Index a_size, Index nbRows, Index nbCols) - : m_storage(a_size, nbRows, nbCols) + /** \sa PlainObjectBase::operator=(const EigenBase&) */ + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase &other) + : m_storage() { -// _check_template_params(); -// EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + _check_template_params(); + resizeLike(other); + *this = other.derived(); + } + /** \brief Copy constructor with in-place evaluation */ + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PlainObjectBase(const ReturnByValue& other) + { + _check_template_params(); + // FIXME this does not automatically transpose vectors if necessary + resize(other.rows(), other.cols()); + other.evalTo(this->derived()); } - /** \copydoc MatrixBase::operator=(const EigenBase&) + public: + + /** \brief Copies the generic expression \a other into *this. + * \copydetails DenseBase::operator=(const EigenBase &other) */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const EigenBase &other) { _resize_to_match(other); @@ -484,16 +572,6 @@ class PlainObjectBase : public internal::dense_xpr_base::type return this->derived(); } - /** \sa MatrixBase::operator=(const EigenBase&) */ - template - EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase &other) - : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) - { - _check_template_params(); - internal::check_rows_cols_for_overflow::run(other.derived().rows(), other.derived().cols()); - Base::operator=(other.derived()); - } - /** \name Map * These are convenience functions returning Map objects. The Map() static functions return unaligned Map objects, * while the AlignedMap() functions return aligned Map objects and thus should be called only with 16-byte-aligned @@ -568,16 +646,16 @@ class PlainObjectBase : public internal::dense_xpr_base::type //@} using Base::setConstant; - Derived& setConstant(Index size, const Scalar& value); - Derived& setConstant(Index rows, Index cols, const Scalar& value); + EIGEN_DEVICE_FUNC Derived& setConstant(Index size, const Scalar& val); + EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, Index cols, const Scalar& val); using Base::setZero; - Derived& setZero(Index size); - Derived& setZero(Index rows, Index cols); + EIGEN_DEVICE_FUNC Derived& setZero(Index size); + EIGEN_DEVICE_FUNC Derived& setZero(Index rows, Index cols); using Base::setOnes; - Derived& setOnes(Index size); - Derived& setOnes(Index rows, Index cols); + EIGEN_DEVICE_FUNC Derived& setOnes(Index size); + EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, Index cols); using Base::setRandom; Derived& setRandom(Index size); @@ -596,6 +674,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * remain row-vectors and vectors remain vectors. */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _resize_to_match(const EigenBase& other) { #ifdef EIGEN_NO_AUTOMATIC_RESIZING @@ -603,8 +682,6 @@ class PlainObjectBase : public internal::dense_xpr_base::type : (rows() == other.rows() && cols() == other.cols()))) && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined"); EIGEN_ONLY_USED_FOR_DEBUG(other); - if(this->size()==0) - resizeLike(other); #else resizeLike(other); #endif @@ -624,25 +701,23 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \internal */ + // aliasing is dealt once in internall::call_assignment + // so at this stage we have to assume aliasing... and resising has to be done later. template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& _set(const DenseBase& other) { - _set_selector(other.derived(), typename internal::conditional(int(OtherDerived::Flags) & EvalBeforeAssigningBit), internal::true_type, internal::false_type>::type()); + internal::call_assignment(this->derived(), other.derived()); return this->derived(); } - template - EIGEN_STRONG_INLINE void _set_selector(const OtherDerived& other, const internal::true_type&) { _set_noalias(other.eval()); } - - template - EIGEN_STRONG_INLINE void _set_selector(const OtherDerived& other, const internal::false_type&) { _set_noalias(other); } - /** \internal Like _set() but additionally makes the assumption that no aliasing effect can happen (which * is the case when creating a new matrix) so one can enforce lazy evaluation. * * \sa operator=(const MatrixBase&), _set() */ template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& _set_noalias(const DenseBase& other) { // I don't think we need this resize call since the lazyAssign will anyways resize @@ -650,40 +725,175 @@ class PlainObjectBase : public internal::dense_xpr_base::type //_resize_to_match(other); // the 'false' below means to enforce lazy evaluation. We don't use lazyAssign() because // it wouldn't allow to copy a row-vector into a column-vector. - return internal::assign_selector::run(this->derived(), other.derived()); + internal::call_assignment_no_alias(this->derived(), other.derived(), internal::assign_op()); + return this->derived(); } template - EIGEN_STRONG_INLINE void _init2(Index nbRows, Index nbCols, typename internal::enable_if::type* = 0) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, typename internal::enable_if::type* = 0) { EIGEN_STATIC_ASSERT(bool(NumTraits::IsInteger) && bool(NumTraits::IsInteger), FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) - resize(nbRows,nbCols); + resize(rows,cols); + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init2(const T0& val0, const T1& val1, typename internal::enable_if::type* = 0) + { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2) + m_storage.data()[0] = Scalar(val0); + m_storage.data()[1] = Scalar(val1); } + template - EIGEN_STRONG_INLINE void _init2(const Scalar& val0, const Scalar& val1, typename internal::enable_if::type* = 0) + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init2(const Index& val0, const Index& val1, + typename internal::enable_if< (!internal::is_same::value) + && (internal::is_same::value) + && (internal::is_same::value) + && Base::SizeAtCompileTime==2,T1>::type* = 0) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2) + m_storage.data()[0] = Scalar(val0); + m_storage.data()[1] = Scalar(val1); + } + + // The argument is convertible to the Index type and we either have a non 1x1 Matrix, or a dynamic-sized Array, + // then the argument is meant to be the size of the object. + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(Index size, typename internal::enable_if< (Base::SizeAtCompileTime!=1 || !internal::is_convertible::value) + && ((!internal::is_same::XprKind,ArrayXpr>::value || Base::SizeAtCompileTime==Dynamic)),T>::type* = 0) + { + // NOTE MSVC 2008 complains if we directly put bool(NumTraits::IsInteger) as the EIGEN_STATIC_ASSERT argument. + const bool is_integer = NumTraits::IsInteger; + EIGEN_UNUSED_VARIABLE(is_integer); + EIGEN_STATIC_ASSERT(is_integer, + FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) + resize(size); + } + + // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type can be implicitely converted) + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Scalar& val0, typename internal::enable_if::value,T>::type* = 0) + { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1) m_storage.data()[0] = val0; - m_storage.data()[1] = val1; + } + + // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type match the index type) + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Index& val0, + typename internal::enable_if< (!internal::is_same::value) + && (internal::is_same::value) + && Base::SizeAtCompileTime==1 + && internal::is_convertible::value,T*>::type* = 0) + { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1) + m_storage.data()[0] = Scalar(val0); } + // Initialize a fixed size matrix from a pointer to raw data + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Scalar* data){ + this->_set_noalias(ConstMapType(data)); + } + + // Initialize an arbitrary matrix from a dense expression + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const DenseBase& other){ + this->_set_noalias(other); + } + + // Initialize an arbitrary matrix from an object convertible to the Derived type. + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Derived& other){ + this->_set_noalias(other); + } + + // Initialize an arbitrary matrix from a generic Eigen expression + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const EigenBase& other){ + this->derived() = other; + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const ReturnByValue& other) + { + resize(other.rows(), other.cols()); + other.evalTo(this->derived()); + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const RotationBase& r) + { + this->derived() = r; + } + + // For fixed-size Array + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Scalar& val0, + typename internal::enable_if< Base::SizeAtCompileTime!=Dynamic + && Base::SizeAtCompileTime!=1 + && internal::is_convertible::value + && internal::is_same::XprKind,ArrayXpr>::value,T>::type* = 0) + { + Base::setConstant(val0); + } + + // For fixed-size Array + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void _init1(const Index& val0, + typename internal::enable_if< (!internal::is_same::value) + && (internal::is_same::value) + && Base::SizeAtCompileTime!=Dynamic + && Base::SizeAtCompileTime!=1 + && internal::is_convertible::value + && internal::is_same::XprKind,ArrayXpr>::value,T*>::type* = 0) + { + Base::setConstant(val0); + } + template friend struct internal::matrix_swap_impl; - /** \internal generic implementation of swap for dense storage since for dynamic-sized matrices of same type it is enough to swap the - * data pointers. + public: + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** \internal + * \brief Override DenseBase::swap() since for dynamic-sized matrices + * of same type it is enough to swap the data pointers. */ template - void _swap(DenseBase const & other) + EIGEN_DEVICE_FUNC + void swap(DenseBase & other) { enum { SwapPointers = internal::is_same::value && Base::SizeAtCompileTime==Dynamic }; - internal::matrix_swap_impl::run(this->derived(), other.const_cast_derived()); + internal::matrix_swap_impl::run(this->derived(), other.derived()); } - - public: -#ifndef EIGEN_PARSED_BY_DOXYGEN + + /** \internal + * \brief const version forwarded to DenseBase::swap + */ + template + EIGEN_DEVICE_FUNC + void swap(DenseBase const & other) + { Base::swap(other.derived()); } + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void _check_template_params() { EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, (Options&RowMajor)==RowMajor) @@ -697,10 +907,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type && (Options & (DontAlign|RowMajor)) == Options), INVALID_MATRIX_TEMPLATE_PARAMETERS) } -#endif -private: - enum { ThisConstantIsPrivateInPlainObjectBase }; + enum { IsPlainObjectBase = 1 }; +#endif }; namespace internal { @@ -708,7 +917,6 @@ namespace internal { template struct conservative_resize_like_impl { - typedef typename Derived::Index Index; static void run(DenseBase& _this, Index rows, Index cols) { if (_this.rows() == rows && _this.cols() == cols) return; @@ -724,8 +932,8 @@ struct conservative_resize_like_impl { // The storage order does not allow us to use reallocation. typename Derived::PlainObject tmp(rows,cols); - const Index common_rows = (std::min)(rows, _this.rows()); - const Index common_cols = (std::min)(cols, _this.cols()); + const Index common_rows = numext::mini(rows, _this.rows()); + const Index common_cols = numext::mini(cols, _this.cols()); tmp.block(0,0,common_rows,common_cols) = _this.block(0,0,common_rows,common_cols); _this.derived().swap(tmp); } @@ -758,8 +966,8 @@ struct conservative_resize_like_impl { // The storage order does not allow us to use reallocation. typename Derived::PlainObject tmp(other); - const Index common_rows = (std::min)(tmp.rows(), _this.rows()); - const Index common_cols = (std::min)(tmp.cols(), _this.cols()); + const Index common_rows = numext::mini(tmp.rows(), _this.rows()); + const Index common_cols = numext::mini(tmp.cols(), _this.cols()); tmp.block(0,0,common_rows,common_cols) = _this.block(0,0,common_rows,common_cols); _this.derived().swap(tmp); } @@ -774,7 +982,6 @@ struct conservative_resize_like_impl { using conservative_resize_like_impl::run; - typedef typename Derived::Index Index; static void run(DenseBase& _this, Index size) { const Index new_rows = Derived::RowsAtCompileTime==1 ? 1 : size; @@ -800,6 +1007,7 @@ struct conservative_resize_like_impl template struct matrix_swap_impl { + EIGEN_DEVICE_FUNC static inline void run(MatrixTypeA& a, MatrixTypeB& b) { a.base().swap(b); @@ -809,6 +1017,7 @@ struct matrix_swap_impl template struct matrix_swap_impl { + EIGEN_DEVICE_FUNC static inline void run(MatrixTypeA& a, MatrixTypeB& b) { static_cast(a).m_storage.swap(static_cast(b).m_storage); diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Product.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Product.h new file mode 100644 index 0000000000..ae0c94b38e --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Product.h @@ -0,0 +1,186 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2011 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_PRODUCT_H +#define EIGEN_PRODUCT_H + +namespace Eigen { + +template class ProductImpl; + +namespace internal { + +template +struct traits > +{ + typedef typename remove_all::type LhsCleaned; + typedef typename remove_all::type RhsCleaned; + typedef traits LhsTraits; + typedef traits RhsTraits; + + typedef MatrixXpr XprKind; + + typedef typename ScalarBinaryOpTraits::Scalar, typename traits::Scalar>::ReturnType Scalar; + typedef typename product_promote_storage_type::ret>::ret StorageKind; + typedef typename promote_index_type::type StorageIndex; + + enum { + RowsAtCompileTime = LhsTraits::RowsAtCompileTime, + ColsAtCompileTime = RhsTraits::ColsAtCompileTime, + MaxRowsAtCompileTime = LhsTraits::MaxRowsAtCompileTime, + MaxColsAtCompileTime = RhsTraits::MaxColsAtCompileTime, + + // FIXME: only needed by GeneralMatrixMatrixTriangular + InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(LhsTraits::ColsAtCompileTime, RhsTraits::RowsAtCompileTime), + + // The storage order is somewhat arbitrary here. The correct one will be determined through the evaluator. + Flags = (MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1) ? RowMajorBit + : (MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1) ? 0 + : ( ((LhsTraits::Flags&NoPreferredStorageOrderBit) && (RhsTraits::Flags&RowMajorBit)) + || ((RhsTraits::Flags&NoPreferredStorageOrderBit) && (LhsTraits::Flags&RowMajorBit)) ) ? RowMajorBit + : NoPreferredStorageOrderBit + }; +}; + +} // end namespace internal + +/** \class Product + * \ingroup Core_Module + * + * \brief Expression of the product of two arbitrary matrices or vectors + * + * \tparam _Lhs the type of the left-hand side expression + * \tparam _Rhs the type of the right-hand side expression + * + * This class represents an expression of the product of two arbitrary matrices. + * + * The other template parameters are: + * \tparam Option can be DefaultProduct, AliasFreeProduct, or LazyProduct + * + */ +template +class Product : public ProductImpl<_Lhs,_Rhs,Option, + typename internal::product_promote_storage_type::StorageKind, + typename internal::traits<_Rhs>::StorageKind, + internal::product_type<_Lhs,_Rhs>::ret>::ret> +{ + public: + + typedef _Lhs Lhs; + typedef _Rhs Rhs; + + typedef typename ProductImpl< + Lhs, Rhs, Option, + typename internal::product_promote_storage_type::StorageKind, + typename internal::traits::StorageKind, + internal::product_type::ret>::ret>::Base Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(Product) + + typedef typename internal::ref_selector::type LhsNested; + typedef typename internal::ref_selector::type RhsNested; + typedef typename internal::remove_all::type LhsNestedCleaned; + typedef typename internal::remove_all::type RhsNestedCleaned; + + EIGEN_DEVICE_FUNC Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) + { + eigen_assert(lhs.cols() == rhs.rows() + && "invalid matrix product" + && "if you wanted a coeff-wise or a dot product use the respective explicit functions"); + } + + EIGEN_DEVICE_FUNC inline Index rows() const { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_rhs.cols(); } + + EIGEN_DEVICE_FUNC const LhsNestedCleaned& lhs() const { return m_lhs; } + EIGEN_DEVICE_FUNC const RhsNestedCleaned& rhs() const { return m_rhs; } + + protected: + + LhsNested m_lhs; + RhsNested m_rhs; +}; + +namespace internal { + +template::ret> +class dense_product_base + : public internal::dense_xpr_base >::type +{}; + +/** Convertion to scalar for inner-products */ +template +class dense_product_base + : public internal::dense_xpr_base >::type +{ + typedef Product ProductXpr; + typedef typename internal::dense_xpr_base::type Base; +public: + using Base::derived; + typedef typename Base::Scalar Scalar; + + operator const Scalar() const + { + return internal::evaluator(derived()).coeff(0,0); + } +}; + +} // namespace internal + +// Generic API dispatcher +template +class ProductImpl : public internal::generic_xpr_base, MatrixXpr, StorageKind>::type +{ + public: + typedef typename internal::generic_xpr_base, MatrixXpr, StorageKind>::type Base; +}; + +template +class ProductImpl + : public internal::dense_product_base +{ + typedef Product Derived; + + public: + + typedef typename internal::dense_product_base Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Derived) + protected: + enum { + IsOneByOne = (RowsAtCompileTime == 1 || RowsAtCompileTime == Dynamic) && + (ColsAtCompileTime == 1 || ColsAtCompileTime == Dynamic), + EnableCoeff = IsOneByOne || Option==LazyProduct + }; + + public: + + EIGEN_DEVICE_FUNC Scalar coeff(Index row, Index col) const + { + EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS); + eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) ); + + return internal::evaluator(derived()).coeff(row,col); + } + + EIGEN_DEVICE_FUNC Scalar coeff(Index i) const + { + EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS); + eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) ); + + return internal::evaluator(derived()).coeff(i); + } + + +}; + +} // end namespace Eigen + +#endif // EIGEN_PRODUCT_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ProductBase.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ProductBase.h deleted file mode 100644 index cf74470a9a..0000000000 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/ProductBase.h +++ /dev/null @@ -1,290 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009-2010 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_PRODUCTBASE_H -#define EIGEN_PRODUCTBASE_H - -namespace Eigen { - -/** \class ProductBase - * \ingroup Core_Module - * - */ - -namespace internal { -template -struct traits > -{ - typedef MatrixXpr XprKind; - typedef typename remove_all<_Lhs>::type Lhs; - typedef typename remove_all<_Rhs>::type Rhs; - typedef typename scalar_product_traits::ReturnType Scalar; - typedef typename promote_storage_type::StorageKind, - typename traits::StorageKind>::ret StorageKind; - typedef typename promote_index_type::Index, - typename traits::Index>::type Index; - enum { - RowsAtCompileTime = traits::RowsAtCompileTime, - ColsAtCompileTime = traits::ColsAtCompileTime, - MaxRowsAtCompileTime = traits::MaxRowsAtCompileTime, - MaxColsAtCompileTime = traits::MaxColsAtCompileTime, - Flags = (MaxRowsAtCompileTime==1 ? RowMajorBit : 0) - | EvalBeforeNestingBit | EvalBeforeAssigningBit | NestByRefBit, - // Note that EvalBeforeNestingBit and NestByRefBit - // are not used in practice because nested is overloaded for products - CoeffReadCost = 0 // FIXME why is it needed ? - }; -}; -} - -#define EIGEN_PRODUCT_PUBLIC_INTERFACE(Derived) \ - typedef ProductBase Base; \ - EIGEN_DENSE_PUBLIC_INTERFACE(Derived) \ - typedef typename Base::LhsNested LhsNested; \ - typedef typename Base::_LhsNested _LhsNested; \ - typedef typename Base::LhsBlasTraits LhsBlasTraits; \ - typedef typename Base::ActualLhsType ActualLhsType; \ - typedef typename Base::_ActualLhsType _ActualLhsType; \ - typedef typename Base::RhsNested RhsNested; \ - typedef typename Base::_RhsNested _RhsNested; \ - typedef typename Base::RhsBlasTraits RhsBlasTraits; \ - typedef typename Base::ActualRhsType ActualRhsType; \ - typedef typename Base::_ActualRhsType _ActualRhsType; \ - using Base::m_lhs; \ - using Base::m_rhs; - -template -class ProductBase : public MatrixBase -{ - public: - typedef MatrixBase Base; - EIGEN_DENSE_PUBLIC_INTERFACE(ProductBase) - - typedef typename Lhs::Nested LhsNested; - typedef typename internal::remove_all::type _LhsNested; - typedef internal::blas_traits<_LhsNested> LhsBlasTraits; - typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType; - typedef typename internal::remove_all::type _ActualLhsType; - typedef typename internal::traits::Scalar LhsScalar; - - typedef typename Rhs::Nested RhsNested; - typedef typename internal::remove_all::type _RhsNested; - typedef internal::blas_traits<_RhsNested> RhsBlasTraits; - typedef typename RhsBlasTraits::DirectLinearAccessType ActualRhsType; - typedef typename internal::remove_all::type _ActualRhsType; - typedef typename internal::traits::Scalar RhsScalar; - - // Diagonal of a product: no need to evaluate the arguments because they are going to be evaluated only once - typedef CoeffBasedProduct FullyLazyCoeffBaseProductType; - - public: - -#ifndef EIGEN_NO_MALLOC - typedef typename Base::PlainObject BasePlainObject; - typedef Matrix DynPlainObject; - typedef typename internal::conditional<(BasePlainObject::SizeAtCompileTime==Dynamic) || (BasePlainObject::SizeAtCompileTime*int(sizeof(Scalar)) < int(EIGEN_STACK_ALLOCATION_LIMIT)), - BasePlainObject, DynPlainObject>::type PlainObject; -#else - typedef typename Base::PlainObject PlainObject; -#endif - - ProductBase(const Lhs& a_lhs, const Rhs& a_rhs) - : m_lhs(a_lhs), m_rhs(a_rhs) - { - eigen_assert(a_lhs.cols() == a_rhs.rows() - && "invalid matrix product" - && "if you wanted a coeff-wise or a dot product use the respective explicit functions"); - } - - inline Index rows() const { return m_lhs.rows(); } - inline Index cols() const { return m_rhs.cols(); } - - template - inline void evalTo(Dest& dst) const { dst.setZero(); scaleAndAddTo(dst,Scalar(1)); } - - template - inline void addTo(Dest& dst) const { scaleAndAddTo(dst,Scalar(1)); } - - template - inline void subTo(Dest& dst) const { scaleAndAddTo(dst,Scalar(-1)); } - - template - inline void scaleAndAddTo(Dest& dst, const Scalar& alpha) const { derived().scaleAndAddTo(dst,alpha); } - - const _LhsNested& lhs() const { return m_lhs; } - const _RhsNested& rhs() const { return m_rhs; } - - // Implicit conversion to the nested type (trigger the evaluation of the product) - operator const PlainObject& () const - { - m_result.resize(m_lhs.rows(), m_rhs.cols()); - derived().evalTo(m_result); - return m_result; - } - - const Diagonal diagonal() const - { return FullyLazyCoeffBaseProductType(m_lhs, m_rhs); } - - template - const Diagonal diagonal() const - { return FullyLazyCoeffBaseProductType(m_lhs, m_rhs); } - - const Diagonal diagonal(Index index) const - { return FullyLazyCoeffBaseProductType(m_lhs, m_rhs).diagonal(index); } - - // restrict coeff accessors to 1x1 expressions. No need to care about mutators here since this isnt a Lvalue expression - typename Base::CoeffReturnType coeff(Index row, Index col) const - { -#ifdef EIGEN2_SUPPORT - return lhs().row(row).cwiseProduct(rhs().col(col).transpose()).sum(); -#else - EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) - eigen_assert(this->rows() == 1 && this->cols() == 1); - Matrix result = *this; - return result.coeff(row,col); -#endif - } - - typename Base::CoeffReturnType coeff(Index i) const - { - EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) - eigen_assert(this->rows() == 1 && this->cols() == 1); - Matrix result = *this; - return result.coeff(i); - } - - const Scalar& coeffRef(Index row, Index col) const - { - EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) - eigen_assert(this->rows() == 1 && this->cols() == 1); - return derived().coeffRef(row,col); - } - - const Scalar& coeffRef(Index i) const - { - EIGEN_STATIC_ASSERT_SIZE_1x1(Derived) - eigen_assert(this->rows() == 1 && this->cols() == 1); - return derived().coeffRef(i); - } - - protected: - - LhsNested m_lhs; - RhsNested m_rhs; - - mutable PlainObject m_result; -}; - -// here we need to overload the nested rule for products -// such that the nested type is a const reference to a plain matrix -namespace internal { -template -struct nested, N, PlainObject> -{ - typedef typename GeneralProduct::PlainObject const& type; -}; -template -struct nested, N, PlainObject> -{ - typedef typename GeneralProduct::PlainObject const& type; -}; -} - -template -class ScaledProduct; - -// Note that these two operator* functions are not defined as member -// functions of ProductBase, because, otherwise we would have to -// define all overloads defined in MatrixBase. Furthermore, Using -// "using Base::operator*" would not work with MSVC. -// -// Also note that here we accept any compatible scalar types -template -const ScaledProduct -operator*(const ProductBase& prod, const typename Derived::Scalar& x) -{ return ScaledProduct(prod.derived(), x); } - -template -typename internal::enable_if::value, - const ScaledProduct >::type -operator*(const ProductBase& prod, const typename Derived::RealScalar& x) -{ return ScaledProduct(prod.derived(), x); } - - -template -const ScaledProduct -operator*(const typename Derived::Scalar& x,const ProductBase& prod) -{ return ScaledProduct(prod.derived(), x); } - -template -typename internal::enable_if::value, - const ScaledProduct >::type -operator*(const typename Derived::RealScalar& x,const ProductBase& prod) -{ return ScaledProduct(prod.derived(), x); } - -namespace internal { -template -struct traits > - : traits, - typename NestedProduct::_LhsNested, - typename NestedProduct::_RhsNested> > -{ - typedef typename traits::StorageKind StorageKind; -}; -} - -template -class ScaledProduct - : public ProductBase, - typename NestedProduct::_LhsNested, - typename NestedProduct::_RhsNested> -{ - public: - typedef ProductBase, - typename NestedProduct::_LhsNested, - typename NestedProduct::_RhsNested> Base; - typedef typename Base::Scalar Scalar; - typedef typename Base::PlainObject PlainObject; -// EIGEN_PRODUCT_PUBLIC_INTERFACE(ScaledProduct) - - ScaledProduct(const NestedProduct& prod, const Scalar& x) - : Base(prod.lhs(),prod.rhs()), m_prod(prod), m_alpha(x) {} - - template - inline void evalTo(Dest& dst) const { dst.setZero(); scaleAndAddTo(dst, Scalar(1)); } - - template - inline void addTo(Dest& dst) const { scaleAndAddTo(dst, Scalar(1)); } - - template - inline void subTo(Dest& dst) const { scaleAndAddTo(dst, Scalar(-1)); } - - template - inline void scaleAndAddTo(Dest& dst, const Scalar& a_alpha) const { m_prod.derived().scaleAndAddTo(dst,a_alpha * m_alpha); } - - const Scalar& alpha() const { return m_alpha; } - - protected: - const NestedProduct& m_prod; - Scalar m_alpha; -}; - -/** \internal - * Overloaded to perform an efficient C = (A*B).lazy() */ -template -template -Derived& MatrixBase::lazyAssign(const ProductBase& other) -{ - other.derived().evalTo(derived()); - return derived(); -} - -} // end namespace Eigen - -#endif // EIGEN_PRODUCTBASE_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ProductEvaluators.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ProductEvaluators.h new file mode 100644 index 0000000000..583b7f59e3 --- /dev/null +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/ProductEvaluators.h @@ -0,0 +1,1099 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2008-2010 Gael Guennebaud +// Copyright (C) 2011 Jitse Niesen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + + +#ifndef EIGEN_PRODUCTEVALUATORS_H +#define EIGEN_PRODUCTEVALUATORS_H + +namespace Eigen { + +namespace internal { + +/** \internal + * Evaluator of a product expression. + * Since products require special treatments to handle all possible cases, + * we simply deffer the evaluation logic to a product_evaluator class + * which offers more partial specialization possibilities. + * + * \sa class product_evaluator + */ +template +struct evaluator > + : public product_evaluator > +{ + typedef Product XprType; + typedef product_evaluator Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +// Catch "scalar * ( A * B )" and transform it to "(A*scalar) * B" +// TODO we should apply that rule only if that's really helpful +template +struct evaluator_assume_aliasing, + const CwiseNullaryOp, Plain1>, + const Product > > +{ + static const bool value = true; +}; +template +struct evaluator, + const CwiseNullaryOp, Plain1>, + const Product > > + : public evaluator > +{ + typedef CwiseBinaryOp, + const CwiseNullaryOp, Plain1>, + const Product > XprType; + typedef evaluator > Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) + : Base(xpr.lhs().functor().m_other * xpr.rhs().lhs() * xpr.rhs().rhs()) + {} +}; + + +template +struct evaluator, DiagIndex> > + : public evaluator, DiagIndex> > +{ + typedef Diagonal, DiagIndex> XprType; + typedef evaluator, DiagIndex> > Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) + : Base(Diagonal, DiagIndex>( + Product(xpr.nestedExpression().lhs(), xpr.nestedExpression().rhs()), + xpr.index() )) + {} +}; + + +// Helper class to perform a matrix product with the destination at hand. +// Depending on the sizes of the factors, there are different evaluation strategies +// as controlled by internal::product_type. +template< typename Lhs, typename Rhs, + typename LhsShape = typename evaluator_traits::Shape, + typename RhsShape = typename evaluator_traits::Shape, + int ProductType = internal::product_type::value> +struct generic_product_impl; + +template +struct evaluator_assume_aliasing > { + static const bool value = true; +}; + +// This is the default evaluator implementation for products: +// It creates a temporary and call generic_product_impl +template +struct product_evaluator, ProductTag, LhsShape, RhsShape> + : public evaluator::PlainObject> +{ + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator Base; + enum { + Flags = Base::Flags | EvalBeforeNestingBit + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + explicit product_evaluator(const XprType& xpr) + : m_result(xpr.rows(), xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + +// FIXME shall we handle nested_eval here?, +// if so, then we must take care at removing the call to nested_eval in the specializations (e.g., in permutation_matrix_product, transposition_matrix_product, etc.) +// typedef typename internal::nested_eval::type LhsNested; +// typedef typename internal::nested_eval::type RhsNested; +// typedef typename internal::remove_all::type LhsNestedCleaned; +// typedef typename internal::remove_all::type RhsNestedCleaned; +// +// const LhsNested lhs(xpr.lhs()); +// const RhsNested rhs(xpr.rhs()); +// +// generic_product_impl::evalTo(m_result, lhs, rhs); + + generic_product_impl::evalTo(m_result, xpr.lhs(), xpr.rhs()); + } + +protected: + PlainObject m_result; +}; + +// The following three shortcuts are enabled only if the scalar types match excatly. +// TODO: we could enable them for different scalar types when the product is not vectorized. + +// Dense = Product +template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar> +struct Assignment, internal::assign_op, Dense2Dense, + typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type> +{ + typedef Product SrcXprType; + static EIGEN_STRONG_INLINE + void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op &) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + // FIXME shall we handle nested_eval here? + generic_product_impl::evalTo(dst, src.lhs(), src.rhs()); + } +}; + +// Dense += Product +template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar> +struct Assignment, internal::add_assign_op, Dense2Dense, + typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type> +{ + typedef Product SrcXprType; + static EIGEN_STRONG_INLINE + void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op &) + { + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + // FIXME shall we handle nested_eval here? + generic_product_impl::addTo(dst, src.lhs(), src.rhs()); + } +}; + +// Dense -= Product +template< typename DstXprType, typename Lhs, typename Rhs, int Options, typename Scalar> +struct Assignment, internal::sub_assign_op, Dense2Dense, + typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type> +{ + typedef Product SrcXprType; + static EIGEN_STRONG_INLINE + void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op &) + { + eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); + // FIXME shall we handle nested_eval here? + generic_product_impl::subTo(dst, src.lhs(), src.rhs()); + } +}; + + +// Dense ?= scalar * Product +// TODO we should apply that rule if that's really helpful +// for instance, this is not good for inner products +template< typename DstXprType, typename Lhs, typename Rhs, typename AssignFunc, typename Scalar, typename ScalarBis, typename Plain> +struct Assignment, const CwiseNullaryOp,Plain>, + const Product >, AssignFunc, Dense2Dense> +{ + typedef CwiseBinaryOp, + const CwiseNullaryOp,Plain>, + const Product > SrcXprType; + static EIGEN_STRONG_INLINE + void run(DstXprType &dst, const SrcXprType &src, const AssignFunc& func) + { + call_assignment_no_alias(dst, (src.lhs().functor().m_other * src.rhs().lhs())*src.rhs().rhs(), func); + } +}; + +//---------------------------------------- +// Catch "Dense ?= xpr + Product<>" expression to save one temporary +// FIXME we could probably enable these rules for any product, i.e., not only Dense and DefaultProduct + +template +struct evaluator_assume_aliasing::Scalar>, const OtherXpr, + const Product >, DenseShape > { + static const bool value = true; +}; + +template +struct assignment_from_xpr_op_product +{ + template + static EIGEN_STRONG_INLINE + void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/) + { + call_assignment_no_alias(dst, src.lhs(), Func1()); + call_assignment_no_alias(dst, src.rhs(), Func2()); + } +}; + +#define EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(ASSIGN_OP,BINOP,ASSIGN_OP2) \ + template< typename DstXprType, typename OtherXpr, typename Lhs, typename Rhs, typename DstScalar, typename SrcScalar, typename OtherScalar,typename ProdScalar> \ + struct Assignment, const OtherXpr, \ + const Product >, internal::ASSIGN_OP, Dense2Dense> \ + : assignment_from_xpr_op_product, internal::ASSIGN_OP, internal::ASSIGN_OP2 > \ + {} + +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op, scalar_sum_op,add_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op,scalar_sum_op,add_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op,scalar_sum_op,sub_assign_op); + +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(assign_op, scalar_difference_op,sub_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(add_assign_op,scalar_difference_op,sub_assign_op); +EIGEN_CATCH_ASSIGN_XPR_OP_PRODUCT(sub_assign_op,scalar_difference_op,add_assign_op); + +//---------------------------------------- + +template +struct generic_product_impl +{ + template + static inline void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + dst.coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum(); + } + + template + static inline void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + dst.coeffRef(0,0) += (lhs.transpose().cwiseProduct(rhs)).sum(); + } + + template + static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { dst.coeffRef(0,0) -= (lhs.transpose().cwiseProduct(rhs)).sum(); } +}; + + +/*********************************************************************** +* Implementation of outer dense * dense vector product +***********************************************************************/ + +// Column major result +template +void outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const false_type&) +{ + evaluator rhsEval(rhs); + typename nested_eval::type actual_lhs(lhs); + // FIXME if cols is large enough, then it might be useful to make sure that lhs is sequentially stored + // FIXME not very good if rhs is real and lhs complex while alpha is real too + const Index cols = dst.cols(); + for (Index j=0; j +void outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const true_type&) +{ + evaluator lhsEval(lhs); + typename nested_eval::type actual_rhs(rhs); + // FIXME if rows is large enough, then it might be useful to make sure that rhs is sequentially stored + // FIXME not very good if lhs is real and rhs complex while alpha is real too + const Index rows = dst.rows(); + for (Index i=0; i +struct generic_product_impl +{ + template struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; + typedef typename Product::Scalar Scalar; + + // TODO it would be nice to be able to exploit our *_assign_op functors for that purpose + struct set { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } }; + struct add { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } }; + struct sub { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() -= src; } }; + struct adds { + Scalar m_scale; + explicit adds(const Scalar& s) : m_scale(s) {} + template void operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() += m_scale * src; + } + }; + + template + static inline void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + internal::outer_product_selector_run(dst, lhs, rhs, set(), is_row_major()); + } + + template + static inline void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + internal::outer_product_selector_run(dst, lhs, rhs, add(), is_row_major()); + } + + template + static inline void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + internal::outer_product_selector_run(dst, lhs, rhs, sub(), is_row_major()); + } + + template + static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + internal::outer_product_selector_run(dst, lhs, rhs, adds(alpha), is_row_major()); + } + +}; + + +// This base class provides default implementations for evalTo, addTo, subTo, in terms of scaleAndAddTo +template +struct generic_product_impl_base +{ + typedef typename Product::Scalar Scalar; + + template + static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { dst.setZero(); scaleAndAddTo(dst, lhs, rhs, Scalar(1)); } + + template + static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { scaleAndAddTo(dst,lhs, rhs, Scalar(1)); } + + template + static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); } + + template + static EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { Derived::scaleAndAddTo(dst,lhs,rhs,alpha); } + +}; + +template +struct generic_product_impl + : generic_product_impl_base > +{ + typedef typename nested_eval::type LhsNested; + typedef typename nested_eval::type RhsNested; + typedef typename Product::Scalar Scalar; + enum { Side = Lhs::IsVectorAtCompileTime ? OnTheLeft : OnTheRight }; + typedef typename internal::remove_all::type>::type MatrixType; + + template + static EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + LhsNested actual_lhs(lhs); + RhsNested actual_rhs(rhs); + internal::gemv_dense_selector::HasUsableDirectAccess) + >::run(actual_lhs, actual_rhs, dst, alpha); + } +}; + +template +struct generic_product_impl +{ + typedef typename Product::Scalar Scalar; + + template + static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + // Same as: dst.noalias() = lhs.lazyProduct(rhs); + // but easier on the compiler side + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op()); + } + + template + static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + // dst.noalias() += lhs.lazyProduct(rhs); + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::add_assign_op()); + } + + template + static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) + { + // dst.noalias() -= lhs.lazyProduct(rhs); + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op()); + } + +// template +// static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) +// { dst.noalias() += alpha * lhs.lazyProduct(rhs); } +}; + +// This specialization enforces the use of a coefficient-based evaluation strategy +template +struct generic_product_impl + : generic_product_impl {}; + +// Case 2: Evaluate coeff by coeff +// +// This is mostly taken from CoeffBasedProduct.h +// The main difference is that we add an extra argument to the etor_product_*_impl::run() function +// for the inner dimension of the product, because evaluator object do not know their size. + +template +struct etor_product_coeff_impl; + +template +struct etor_product_packet_impl; + +template +struct product_evaluator, ProductTag, DenseShape, DenseShape> + : evaluator_base > +{ + typedef Product XprType; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + explicit product_evaluator(const XprType& xpr) + : m_lhs(xpr.lhs()), + m_rhs(xpr.rhs()), + m_lhsImpl(m_lhs), // FIXME the creation of the evaluator objects should result in a no-op, but check that! + m_rhsImpl(m_rhs), // Moreover, they are only useful for the packet path, so we could completely disable them when not needed, + // or perhaps declare them on the fly on the packet method... We have experiment to check what's best. + m_innerDim(xpr.lhs().cols()) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits::MulCost); + EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits::AddCost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); +#if 0 + std::cerr << "LhsOuterStrideBytes= " << LhsOuterStrideBytes << "\n"; + std::cerr << "RhsOuterStrideBytes= " << RhsOuterStrideBytes << "\n"; + std::cerr << "LhsAlignment= " << LhsAlignment << "\n"; + std::cerr << "RhsAlignment= " << RhsAlignment << "\n"; + std::cerr << "CanVectorizeLhs= " << CanVectorizeLhs << "\n"; + std::cerr << "CanVectorizeRhs= " << CanVectorizeRhs << "\n"; + std::cerr << "CanVectorizeInner= " << CanVectorizeInner << "\n"; + std::cerr << "EvalToRowMajor= " << EvalToRowMajor << "\n"; + std::cerr << "Alignment= " << Alignment << "\n"; + std::cerr << "Flags= " << Flags << "\n"; +#endif + } + + // Everything below here is taken from CoeffBasedProduct.h + + typedef typename internal::nested_eval::type LhsNested; + typedef typename internal::nested_eval::type RhsNested; + + typedef typename internal::remove_all::type LhsNestedCleaned; + typedef typename internal::remove_all::type RhsNestedCleaned; + + typedef evaluator LhsEtorType; + typedef evaluator RhsEtorType; + + enum { + RowsAtCompileTime = LhsNestedCleaned::RowsAtCompileTime, + ColsAtCompileTime = RhsNestedCleaned::ColsAtCompileTime, + InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(LhsNestedCleaned::ColsAtCompileTime, RhsNestedCleaned::RowsAtCompileTime), + MaxRowsAtCompileTime = LhsNestedCleaned::MaxRowsAtCompileTime, + MaxColsAtCompileTime = RhsNestedCleaned::MaxColsAtCompileTime + }; + + typedef typename find_best_packet::type LhsVecPacketType; + typedef typename find_best_packet::type RhsVecPacketType; + + enum { + + LhsCoeffReadCost = LhsEtorType::CoeffReadCost, + RhsCoeffReadCost = RhsEtorType::CoeffReadCost, + CoeffReadCost = InnerSize==0 ? NumTraits::ReadCost + : InnerSize == Dynamic ? HugeCost + : InnerSize * (NumTraits::MulCost + LhsCoeffReadCost + RhsCoeffReadCost) + + (InnerSize - 1) * NumTraits::AddCost, + + Unroll = CoeffReadCost <= EIGEN_UNROLLING_LIMIT, + + LhsFlags = LhsEtorType::Flags, + RhsFlags = RhsEtorType::Flags, + + LhsRowMajor = LhsFlags & RowMajorBit, + RhsRowMajor = RhsFlags & RowMajorBit, + + LhsVecPacketSize = unpacket_traits::size, + RhsVecPacketSize = unpacket_traits::size, + + // Here, we don't care about alignment larger than the usable packet size. + LhsAlignment = EIGEN_PLAIN_ENUM_MIN(LhsEtorType::Alignment,LhsVecPacketSize*int(sizeof(typename LhsNestedCleaned::Scalar))), + RhsAlignment = EIGEN_PLAIN_ENUM_MIN(RhsEtorType::Alignment,RhsVecPacketSize*int(sizeof(typename RhsNestedCleaned::Scalar))), + + SameType = is_same::value, + + CanVectorizeRhs = bool(RhsRowMajor) && (RhsFlags & PacketAccessBit) && (ColsAtCompileTime!=1), + CanVectorizeLhs = (!LhsRowMajor) && (LhsFlags & PacketAccessBit) && (RowsAtCompileTime!=1), + + EvalToRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 + : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 + : (bool(RhsRowMajor) && !CanVectorizeLhs), + + Flags = ((unsigned int)(LhsFlags | RhsFlags) & HereditaryBits & ~RowMajorBit) + | (EvalToRowMajor ? RowMajorBit : 0) + // TODO enable vectorization for mixed types + | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0) + | (XprType::IsVectorAtCompileTime ? LinearAccessBit : 0), + + LhsOuterStrideBytes = int(LhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename LhsNestedCleaned::Scalar)), + RhsOuterStrideBytes = int(RhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename RhsNestedCleaned::Scalar)), + + Alignment = bool(CanVectorizeLhs) ? (LhsOuterStrideBytes<=0 || (int(LhsOuterStrideBytes) % EIGEN_PLAIN_ENUM_MAX(1,LhsAlignment))!=0 ? 0 : LhsAlignment) + : bool(CanVectorizeRhs) ? (RhsOuterStrideBytes<=0 || (int(RhsOuterStrideBytes) % EIGEN_PLAIN_ENUM_MAX(1,RhsAlignment))!=0 ? 0 : RhsAlignment) + : 0, + + /* CanVectorizeInner deserves special explanation. It does not affect the product flags. It is not used outside + * of Product. If the Product itself is not a packet-access expression, there is still a chance that the inner + * loop of the product might be vectorized. This is the meaning of CanVectorizeInner. Since it doesn't affect + * the Flags, it is safe to make this value depend on ActualPacketAccessBit, that doesn't affect the ABI. + */ + CanVectorizeInner = SameType + && LhsRowMajor + && (!RhsRowMajor) + && (LhsFlags & RhsFlags & ActualPacketAccessBit) + && (InnerSize % packet_traits::size == 0) + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index row, Index col) const + { + return (m_lhs.row(row).transpose().cwiseProduct( m_rhs.col(col) )).sum(); + } + + /* Allow index-based non-packet access. It is impossible though to allow index-based packed access, + * which is why we don't set the LinearAccessBit. + * TODO: this seems possible when the result is a vector + */ + EIGEN_DEVICE_FUNC const CoeffReturnType coeff(Index index) const + { + const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? 0 : index; + const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? index : 0; + return (m_lhs.row(row).transpose().cwiseProduct( m_rhs.col(col) )).sum(); + } + + template + const PacketType packet(Index row, Index col) const + { + PacketType res; + typedef etor_product_packet_impl PacketImpl; + PacketImpl::run(row, col, m_lhsImpl, m_rhsImpl, m_innerDim, res); + return res; + } + + template + const PacketType packet(Index index) const + { + const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? 0 : index; + const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? index : 0; + return packet(row,col); + } + +protected: + typename internal::add_const_on_value_type::type m_lhs; + typename internal::add_const_on_value_type::type m_rhs; + + LhsEtorType m_lhsImpl; + RhsEtorType m_rhsImpl; + + // TODO: Get rid of m_innerDim if known at compile time + Index m_innerDim; +}; + +template +struct product_evaluator, LazyCoeffBasedProductMode, DenseShape, DenseShape> + : product_evaluator, CoeffBasedProductMode, DenseShape, DenseShape> +{ + typedef Product XprType; + typedef Product BaseProduct; + typedef product_evaluator Base; + enum { + Flags = Base::Flags | EvalBeforeNestingBit + }; + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base(BaseProduct(xpr.lhs(),xpr.rhs())) + {} +}; + +/**************************************** +*** Coeff based product, Packet path *** +****************************************/ + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res) + { + etor_product_packet_impl::run(row, col, lhs, rhs, innerDim, res); + res = pmadd(pset1(lhs.coeff(row, Index(UnrollingIndex-1))), rhs.template packet(Index(UnrollingIndex-1), col), res); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res) + { + etor_product_packet_impl::run(row, col, lhs, rhs, innerDim, res); + res = pmadd(lhs.template packet(row, Index(UnrollingIndex-1)), pset1(rhs.coeff(Index(UnrollingIndex-1), col)), res); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res) + { + res = pmul(pset1(lhs.coeff(row, Index(0))),rhs.template packet(Index(0), col)); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res) + { + res = pmul(lhs.template packet(row, Index(0)), pset1(rhs.coeff(Index(0), col))); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res) + { + res = pset1(typename unpacket_traits::type(0)); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res) + { + res = pset1(typename unpacket_traits::type(0)); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res) + { + res = pset1(typename unpacket_traits::type(0)); + for(Index i = 0; i < innerDim; ++i) + res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); + } +}; + +template +struct etor_product_packet_impl +{ + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res) + { + res = pset1(typename unpacket_traits::type(0)); + for(Index i = 0; i < innerDim; ++i) + res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); + } +}; + + +/*************************************************************************** +* Triangular products +***************************************************************************/ +template +struct triangular_product_impl; + +template +struct generic_product_impl + : generic_product_impl_base > +{ + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + triangular_product_impl + ::run(dst, lhs.nestedExpression(), rhs, alpha); + } +}; + +template +struct generic_product_impl +: generic_product_impl_base > +{ + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + triangular_product_impl::run(dst, lhs, rhs.nestedExpression(), alpha); + } +}; + + +/*************************************************************************** +* SelfAdjoint products +***************************************************************************/ +template +struct selfadjoint_product_impl; + +template +struct generic_product_impl + : generic_product_impl_base > +{ + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + selfadjoint_product_impl::run(dst, lhs.nestedExpression(), rhs, alpha); + } +}; + +template +struct generic_product_impl +: generic_product_impl_base > +{ + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) + { + selfadjoint_product_impl::run(dst, lhs, rhs.nestedExpression(), alpha); + } +}; + + +/*************************************************************************** +* Diagonal products +***************************************************************************/ + +template +struct diagonal_product_evaluator_base + : evaluator_base +{ + typedef typename ScalarBinaryOpTraits::ReturnType Scalar; +public: + enum { + CoeffReadCost = NumTraits::MulCost + evaluator::CoeffReadCost + evaluator::CoeffReadCost, + + MatrixFlags = evaluator::Flags, + DiagFlags = evaluator::Flags, + _StorageOrder = MatrixFlags & RowMajorBit ? RowMajor : ColMajor, + _ScalarAccessOnDiag = !((int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheLeft) + ||(int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheRight)), + _SameTypes = is_same::value, + // FIXME currently we need same types, but in the future the next rule should be the one + //_Vectorizable = bool(int(MatrixFlags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagFlags)&PacketAccessBit))), + _Vectorizable = bool(int(MatrixFlags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagFlags)&PacketAccessBit))), + _LinearAccessMask = (MatrixType::RowsAtCompileTime==1 || MatrixType::ColsAtCompileTime==1) ? LinearAccessBit : 0, + Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixFlags)) | (_Vectorizable ? PacketAccessBit : 0), + Alignment = evaluator::Alignment + }; + + diagonal_product_evaluator_base(const MatrixType &mat, const DiagonalType &diag) + : m_diagImpl(diag), m_matImpl(mat) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits::MulCost); + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const + { + return m_diagImpl.coeff(idx) * m_matImpl.coeff(idx); + } + +protected: + template + EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::true_type) const + { + return internal::pmul(m_matImpl.template packet(row, col), + internal::pset1(m_diagImpl.coeff(id))); + } + + template + EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::false_type) const + { + enum { + InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime, + DiagonalPacketLoadMode = EIGEN_PLAIN_ENUM_MIN(LoadMode,((InnerSize%16) == 0) ? int(Aligned16) : int(evaluator::Alignment)) // FIXME hardcoded 16!! + }; + return internal::pmul(m_matImpl.template packet(row, col), + m_diagImpl.template packet(id)); + } + + evaluator m_diagImpl; + evaluator m_matImpl; +}; + +// diagonal * dense +template +struct product_evaluator, ProductTag, DiagonalShape, DenseShape> + : diagonal_product_evaluator_base, OnTheLeft> +{ + typedef diagonal_product_evaluator_base, OnTheLeft> Base; + using Base::m_diagImpl; + using Base::m_matImpl; + using Base::coeff; + typedef typename Base::Scalar Scalar; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + enum { + StorageOrder = int(Rhs::Flags) & RowMajorBit ? RowMajor : ColMajor + }; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base(xpr.rhs(), xpr.lhs().diagonal()) + { + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const + { + return m_diagImpl.coeff(row) * m_matImpl.coeff(row, col); + } + +#ifndef __CUDACC__ + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const + { + // FIXME: NVCC used to complain about the template keyword, but we have to check whether this is still the case. + // See also similar calls below. + return this->template packet_impl(row,col, row, + typename internal::conditional::type()); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index idx) const + { + return packet(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx); + } +#endif +}; + +// dense * diagonal +template +struct product_evaluator, ProductTag, DenseShape, DiagonalShape> + : diagonal_product_evaluator_base, OnTheRight> +{ + typedef diagonal_product_evaluator_base, OnTheRight> Base; + using Base::m_diagImpl; + using Base::m_matImpl; + using Base::coeff; + typedef typename Base::Scalar Scalar; + + typedef Product XprType; + typedef typename XprType::PlainObject PlainObject; + + enum { StorageOrder = int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor }; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base(xpr.lhs(), xpr.rhs().diagonal()) + { + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const + { + return m_matImpl.coeff(row, col) * m_diagImpl.coeff(col); + } + +#ifndef __CUDACC__ + template + EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const + { + return this->template packet_impl(row,col, col, + typename internal::conditional::type()); + } + + template + EIGEN_STRONG_INLINE PacketType packet(Index idx) const + { + return packet(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx); + } +#endif +}; + +/*************************************************************************** +* Products with permutation matrices +***************************************************************************/ + +/** \internal + * \class permutation_matrix_product + * Internal helper class implementing the product between a permutation matrix and a matrix. + * This class is specialized for DenseShape below and for SparseShape in SparseCore/SparsePermutation.h + */ +template +struct permutation_matrix_product; + +template +struct permutation_matrix_product +{ + typedef typename nested_eval::type MatrixType; + typedef typename remove_all::type MatrixTypeCleaned; + + template + static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr) + { + MatrixType mat(xpr); + const Index n = Side==OnTheLeft ? mat.rows() : mat.cols(); + // FIXME we need an is_same for expression that is not sensitive to constness. For instance + // is_same_xpr, Block >::value should be true. + //if(is_same::value && extract_data(dst) == extract_data(mat)) + if(is_same_dense(dst, mat)) + { + // apply the permutation inplace + Matrix mask(perm.size()); + mask.fill(false); + Index r = 0; + while(r < perm.size()) + { + // search for the next seed + while(r=perm.size()) + break; + // we got one, let's follow it until we are back to the seed + Index k0 = r++; + Index kPrev = k0; + mask.coeffRef(k0) = true; + for(Index k=perm.indices().coeff(k0); k!=k0; k=perm.indices().coeff(k)) + { + Block(dst, k) + .swap(Block + (dst,((Side==OnTheLeft) ^ Transposed) ? k0 : kPrev)); + + mask.coeffRef(k) = true; + kPrev = k; + } + } + } + else + { + for(Index i = 0; i < n; ++i) + { + Block + (dst, ((Side==OnTheLeft) ^ Transposed) ? perm.indices().coeff(i) : i) + + = + + Block + (mat, ((Side==OnTheRight) ^ Transposed) ? perm.indices().coeff(i) : i); + } + } + } +}; + +template +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + permutation_matrix_product::run(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + permutation_matrix_product::run(dst, rhs, lhs); + } +}; + +template +struct generic_product_impl, Rhs, PermutationShape, MatrixShape, ProductTag> +{ + template + static void evalTo(Dest& dst, const Inverse& lhs, const Rhs& rhs) + { + permutation_matrix_product::run(dst, lhs.nestedExpression(), rhs); + } +}; + +template +struct generic_product_impl, MatrixShape, PermutationShape, ProductTag> +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Inverse& rhs) + { + permutation_matrix_product::run(dst, rhs.nestedExpression(), lhs); + } +}; + + +/*************************************************************************** +* Products with transpositions matrices +***************************************************************************/ + +// FIXME could we unify Transpositions and Permutation into a single "shape"?? + +/** \internal + * \class transposition_matrix_product + * Internal helper class implementing the product between a permutation matrix and a matrix. + */ +template +struct transposition_matrix_product +{ + typedef typename nested_eval::type MatrixType; + typedef typename remove_all::type MatrixTypeCleaned; + + template + static inline void run(Dest& dst, const TranspositionType& tr, const ExpressionType& xpr) + { + MatrixType mat(xpr); + typedef typename TranspositionType::StorageIndex StorageIndex; + const Index size = tr.size(); + StorageIndex j = 0; + + if(!is_same_dense(dst,mat)) + dst = mat; + + for(Index k=(Transposed?size-1:0) ; Transposed?k>=0:k +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + transposition_matrix_product::run(dst, lhs, rhs); + } +}; + +template +struct generic_product_impl +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs) + { + transposition_matrix_product::run(dst, rhs, lhs); + } +}; + + +template +struct generic_product_impl, Rhs, TranspositionsShape, MatrixShape, ProductTag> +{ + template + static void evalTo(Dest& dst, const Transpose& lhs, const Rhs& rhs) + { + transposition_matrix_product::run(dst, lhs.nestedExpression(), rhs); + } +}; + +template +struct generic_product_impl, MatrixShape, TranspositionsShape, ProductTag> +{ + template + static void evalTo(Dest& dst, const Lhs& lhs, const Transpose& rhs) + { + transposition_matrix_product::run(dst, rhs.nestedExpression(), lhs); + } +}; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_PRODUCT_EVALUATORS_H diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Random.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Random.h index 480fea408d..6faf789c76 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Random.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Random.h @@ -16,8 +16,7 @@ namespace internal { template struct scalar_random_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_random_op) - template - inline const Scalar operator() (Index, Index = 0) const { return random(); } + inline const Scalar operator() () const { return random(); } }; template @@ -28,12 +27,18 @@ struct functor_traits > /** \returns a random matrix expression * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * * The parameters \a rows and \a cols are the number of rows and of columns of * the returned matrix. Must be compatible with this MatrixBase type. * + * \not_reentrant + * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, * it is redundant to pass \a rows and \a cols as arguments, so Random() should be used * instead. + * * * Example: \include MatrixBase_random_int_int.cpp * Output: \verbinclude MatrixBase_random_int_int.out @@ -41,22 +46,28 @@ struct functor_traits > * This expression has the "evaluate before nesting" flag so that it will be evaluated into * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected * behavior with expressions involving random matrices. + * + * See DenseBase::NullaryExpr(Index, const CustomNullaryOp&) for an example using C++11 random generators. * - * \sa MatrixBase::setRandom(), MatrixBase::Random(Index), MatrixBase::Random() + * \sa DenseBase::setRandom(), DenseBase::Random(Index), DenseBase::Random() */ template -inline const CwiseNullaryOp::Scalar>, Derived> +inline const typename DenseBase::RandomReturnType DenseBase::Random(Index rows, Index cols) { return NullaryExpr(rows, cols, internal::scalar_random_op()); } /** \returns a random vector expression + * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. * * The parameter \a size is the size of the returned vector. * Must be compatible with this MatrixBase type. * * \only_for_vectors + * \not_reentrant * * This variant is meant to be used for dynamic-size vector types. For fixed-size types, * it is redundant to pass \a size as argument, so Random() should be used @@ -69,10 +80,10 @@ DenseBase::Random(Index rows, Index cols) * a temporary vector whenever it is nested in a larger expression. This prevents unexpected * behavior with expressions involving random matrices. * - * \sa MatrixBase::setRandom(), MatrixBase::Random(Index,Index), MatrixBase::Random() + * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random() */ template -inline const CwiseNullaryOp::Scalar>, Derived> +inline const typename DenseBase::RandomReturnType DenseBase::Random(Index size) { return NullaryExpr(size, internal::scalar_random_op()); @@ -80,6 +91,9 @@ DenseBase::Random(Index size) /** \returns a fixed-size random matrix or vector expression * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you * need to use the variants taking size arguments. * @@ -89,11 +103,13 @@ DenseBase::Random(Index size) * This expression has the "evaluate before nesting" flag so that it will be evaluated into * a temporary matrix whenever it is nested in a larger expression. This prevents unexpected * behavior with expressions involving random matrices. + * + * \not_reentrant * - * \sa MatrixBase::setRandom(), MatrixBase::Random(Index,Index), MatrixBase::Random(Index) + * \sa DenseBase::setRandom(), DenseBase::Random(Index,Index), DenseBase::Random(Index) */ template -inline const CwiseNullaryOp::Scalar>, Derived> +inline const typename DenseBase::RandomReturnType DenseBase::Random() { return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_random_op()); @@ -101,6 +117,11 @@ DenseBase::Random() /** Sets all coefficients in this expression to random values. * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \not_reentrant + * * Example: \include MatrixBase_setRandom.cpp * Output: \verbinclude MatrixBase_setRandom.out * @@ -114,12 +135,16 @@ inline Derived& DenseBase::setRandom() /** Resizes to the given \a newSize, and sets all coefficients in this expression to random values. * + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * * \only_for_vectors + * \not_reentrant * * Example: \include Matrix_setRandom_int.cpp * Output: \verbinclude Matrix_setRandom_int.out * - * \sa MatrixBase::setRandom(), setRandom(Index,Index), class CwiseNullaryOp, MatrixBase::Random() + * \sa DenseBase::setRandom(), setRandom(Index,Index), class CwiseNullaryOp, DenseBase::Random() */ template EIGEN_STRONG_INLINE Derived& @@ -131,19 +156,24 @@ PlainObjectBase::setRandom(Index newSize) /** Resizes to the given size, and sets all coefficients in this expression to random values. * - * \param nbRows the new number of rows - * \param nbCols the new number of columns + * Numbers are uniformly spread through their whole definition range for integer types, + * and in the [-1:1] range for floating point scalar types. + * + * \not_reentrant + * + * \param rows the new number of rows + * \param cols the new number of columns * * Example: \include Matrix_setRandom_int_int.cpp * Output: \verbinclude Matrix_setRandom_int_int.out * - * \sa MatrixBase::setRandom(), setRandom(Index), class CwiseNullaryOp, MatrixBase::Random() + * \sa DenseBase::setRandom(), setRandom(Index), class CwiseNullaryOp, DenseBase::Random() */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setRandom(Index nbRows, Index nbCols) +PlainObjectBase::setRandom(Index rows, Index cols) { - resize(nbRows, nbCols); + resize(rows, cols); return setRandom(); } diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Redux.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Redux.h index 9b8662a6f9..b6e8f88870 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Redux.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Redux.h @@ -27,8 +27,9 @@ template struct redux_traits { public: + typedef typename find_best_packet::type PacketType; enum { - PacketSize = packet_traits::size, + PacketSize = unpacket_traits::size, InnerMaxSize = int(Derived::IsRowMajor) ? Derived::MaxColsAtCompileTime : Derived::MaxRowsAtCompileTime @@ -37,8 +38,8 @@ struct redux_traits enum { MightVectorize = (int(Derived::Flags)&ActualPacketAccessBit) && (functor_traits::PacketAccess), - MayLinearVectorize = MightVectorize && (int(Derived::Flags)&LinearAccessBit), - MaySliceVectorize = MightVectorize && int(InnerMaxSize)>=3*PacketSize + MayLinearVectorize = bool(MightVectorize) && (int(Derived::Flags)&LinearAccessBit), + MaySliceVectorize = bool(MightVectorize) && int(InnerMaxSize)>=3*PacketSize }; public: @@ -50,21 +51,34 @@ struct redux_traits public: enum { - Cost = ( Derived::SizeAtCompileTime == Dynamic - || Derived::CoeffReadCost == Dynamic - || (Derived::SizeAtCompileTime!=1 && functor_traits::Cost == Dynamic) - ) ? Dynamic - : Derived::SizeAtCompileTime * Derived::CoeffReadCost - + (Derived::SizeAtCompileTime-1) * functor_traits::Cost, + Cost = Derived::SizeAtCompileTime == Dynamic ? HugeCost + : Derived::SizeAtCompileTime * Derived::CoeffReadCost + (Derived::SizeAtCompileTime-1) * functor_traits::Cost, UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Traversal) == int(DefaultTraversal) ? 1 : int(PacketSize)) }; public: enum { - Unrolling = Cost != Dynamic && Cost <= UnrollingLimit - ? CompleteUnrolling - : NoUnrolling + Unrolling = Cost <= UnrollingLimit ? CompleteUnrolling : NoUnrolling }; + +#ifdef EIGEN_DEBUG_ASSIGN + static void debug() + { + std::cerr << "Xpr: " << typeid(typename Derived::XprType).name() << std::endl; + std::cerr.setf(std::ios::hex, std::ios::basefield); + EIGEN_DEBUG_VAR(Derived::Flags) + std::cerr.unsetf(std::ios::hex); + EIGEN_DEBUG_VAR(InnerMaxSize) + EIGEN_DEBUG_VAR(PacketSize) + EIGEN_DEBUG_VAR(MightVectorize) + EIGEN_DEBUG_VAR(MayLinearVectorize) + EIGEN_DEBUG_VAR(MaySliceVectorize) + EIGEN_DEBUG_VAR(Traversal) + EIGEN_DEBUG_VAR(UnrollingLimit) + EIGEN_DEBUG_VAR(Unrolling) + std::cerr << std::endl; + } +#endif }; /*************************************************************************** @@ -82,6 +96,7 @@ struct redux_novec_unroller typedef typename Derived::Scalar Scalar; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func) { return func(redux_novec_unroller::run(mat,func), @@ -99,6 +114,7 @@ struct redux_novec_unroller typedef typename Derived::Scalar Scalar; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func&) { return mat.coeffByOuterInner(outer, inner); @@ -112,6 +128,7 @@ template struct redux_novec_unroller { typedef typename Derived::Scalar Scalar; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Derived&, const Func&) { return Scalar(); } }; @@ -121,12 +138,12 @@ template struct redux_vec_unroller { enum { - PacketSize = packet_traits::size, + PacketSize = redux_traits::PacketSize, HalfLength = Length/2 }; typedef typename Derived::Scalar Scalar; - typedef typename packet_traits::type PacketScalar; + typedef typename redux_traits::PacketType PacketScalar; static EIGEN_STRONG_INLINE PacketScalar run(const Derived &mat, const Func& func) { @@ -140,18 +157,18 @@ template struct redux_vec_unroller { enum { - index = Start * packet_traits::size, + index = Start * redux_traits::PacketSize, outer = index / int(Derived::InnerSizeAtCompileTime), inner = index % int(Derived::InnerSizeAtCompileTime), - alignment = (Derived::Flags & AlignedBit) ? Aligned : Unaligned + alignment = Derived::Alignment }; typedef typename Derived::Scalar Scalar; - typedef typename packet_traits::type PacketScalar; + typedef typename redux_traits::PacketType PacketScalar; static EIGEN_STRONG_INLINE PacketScalar run(const Derived &mat, const Func&) { - return mat.template packetByOuterInner(outer, inner); + return mat.template packetByOuterInner(outer, inner); } }; @@ -169,8 +186,8 @@ template struct redux_impl { typedef typename Derived::Scalar Scalar; - typedef typename Derived::Index Index; - static EIGEN_STRONG_INLINE Scalar run(const Derived& mat, const Func& func) + EIGEN_DEVICE_FUNC + static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func) { eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix"); Scalar res; @@ -193,19 +210,19 @@ template struct redux_impl { typedef typename Derived::Scalar Scalar; - typedef typename packet_traits::type PacketScalar; - typedef typename Derived::Index Index; + typedef typename redux_traits::PacketType PacketScalar; - static Scalar run(const Derived& mat, const Func& func) + static Scalar run(const Derived &mat, const Func& func) { const Index size = mat.size(); - eigen_assert(size && "you are using an empty matrix"); - const Index packetSize = packet_traits::size; - const Index alignedStart = internal::first_aligned(mat); + + const Index packetSize = redux_traits::PacketSize; + const int packetAlignment = unpacket_traits::alignment; enum { - alignment = bool(Derived::Flags & DirectAccessBit) || bool(Derived::Flags & AlignedBit) - ? Aligned : Unaligned + alignment0 = (bool(Derived::Flags & DirectAccessBit) && bool(packet_traits::AlignedOnScalar)) ? int(packetAlignment) : int(Unaligned), + alignment = EIGEN_PLAIN_ENUM_MAX(alignment0, Derived::Alignment) }; + const Index alignedStart = internal::first_default_aligned(mat.nestedExpression()); const Index alignedSize2 = ((size-alignedStart)/(2*packetSize))*(2*packetSize); const Index alignedSize = ((size-alignedStart)/(packetSize))*(packetSize); const Index alignedEnd2 = alignedStart + alignedSize2; @@ -213,19 +230,19 @@ struct redux_impl Scalar res; if(alignedSize) { - PacketScalar packet_res0 = mat.template packet(alignedStart); + PacketScalar packet_res0 = mat.template packet(alignedStart); if(alignedSize>packetSize) // we have at least two packets to partly unroll the loop { - PacketScalar packet_res1 = mat.template packet(alignedStart+packetSize); + PacketScalar packet_res1 = mat.template packet(alignedStart+packetSize); for(Index index = alignedStart + 2*packetSize; index < alignedEnd2; index += 2*packetSize) { - packet_res0 = func.packetOp(packet_res0, mat.template packet(index)); - packet_res1 = func.packetOp(packet_res1, mat.template packet(index+packetSize)); + packet_res0 = func.packetOp(packet_res0, mat.template packet(index)); + packet_res1 = func.packetOp(packet_res1, mat.template packet(index+packetSize)); } packet_res0 = func.packetOp(packet_res0,packet_res1); if(alignedEnd>alignedEnd2) - packet_res0 = func.packetOp(packet_res0, mat.template packet(alignedEnd2)); + packet_res0 = func.packetOp(packet_res0, mat.template packet(alignedEnd2)); } res = func.predux(packet_res0); @@ -252,25 +269,24 @@ template struct redux_impl { typedef typename Derived::Scalar Scalar; - typedef typename packet_traits::type PacketScalar; - typedef typename Derived::Index Index; + typedef typename redux_traits::PacketType PacketType; - static Scalar run(const Derived& mat, const Func& func) + EIGEN_DEVICE_FUNC static Scalar run(const Derived &mat, const Func& func) { eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix"); const Index innerSize = mat.innerSize(); const Index outerSize = mat.outerSize(); enum { - packetSize = packet_traits::size + packetSize = redux_traits::PacketSize }; const Index packetedInnerSize = ((innerSize)/packetSize)*packetSize; Scalar res; if(packetedInnerSize) { - PacketScalar packet_res = mat.template packet(0,0); + PacketType packet_res = mat.template packet(0,0); for(Index j=0; j(j,i)); + packet_res = func.packetOp(packet_res, mat.template packetByOuterInner(j,i)); res = func.predux(packet_res); for(Index j=0; j struct redux_impl { typedef typename Derived::Scalar Scalar; - typedef typename packet_traits::type PacketScalar; + + typedef typename redux_traits::PacketType PacketScalar; enum { - PacketSize = packet_traits::size, + PacketSize = redux_traits::PacketSize, Size = Derived::SizeAtCompileTime, VectorizedSize = (Size / PacketSize) * PacketSize }; - static EIGEN_STRONG_INLINE Scalar run(const Derived& mat, const Func& func) + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func) { eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix"); - Scalar res = func.predux(redux_vec_unroller::run(mat,func)); - if (VectorizedSize != Size) - res = func(res,redux_novec_unroller::run(mat,func)); - return res; + if (VectorizedSize > 0) { + Scalar res = func.predux(redux_vec_unroller::run(mat,func)); + if (VectorizedSize != Size) + res = func(res,redux_novec_unroller::run(mat,func)); + return res; + } + else { + return redux_novec_unroller::run(mat,func); + } } }; +// evaluator adaptor +template +class redux_evaluator +{ +public: + typedef _XprType XprType; + EIGEN_DEVICE_FUNC explicit redux_evaluator(const XprType &xpr) : m_evaluator(xpr), m_xpr(xpr) {} + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename XprType::PacketScalar PacketScalar; + typedef typename XprType::PacketReturnType PacketReturnType; + + enum { + MaxRowsAtCompileTime = XprType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = XprType::MaxColsAtCompileTime, + // TODO we should not remove DirectAccessBit and rather find an elegant way to query the alignment offset at runtime from the evaluator + Flags = evaluator::Flags & ~DirectAccessBit, + IsRowMajor = XprType::IsRowMajor, + SizeAtCompileTime = XprType::SizeAtCompileTime, + InnerSizeAtCompileTime = XprType::InnerSizeAtCompileTime, + CoeffReadCost = evaluator::CoeffReadCost, + Alignment = evaluator::Alignment + }; + + EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); } + EIGEN_DEVICE_FUNC Index size() const { return m_xpr.size(); } + EIGEN_DEVICE_FUNC Index innerSize() const { return m_xpr.innerSize(); } + EIGEN_DEVICE_FUNC Index outerSize() const { return m_xpr.outerSize(); } + + EIGEN_DEVICE_FUNC + CoeffReturnType coeff(Index row, Index col) const + { return m_evaluator.coeff(row, col); } + + EIGEN_DEVICE_FUNC + CoeffReturnType coeff(Index index) const + { return m_evaluator.coeff(index); } + + template + PacketType packet(Index row, Index col) const + { return m_evaluator.template packet(row, col); } + + template + PacketType packet(Index index) const + { return m_evaluator.template packet(index); } + + EIGEN_DEVICE_FUNC + CoeffReturnType coeffByOuterInner(Index outer, Index inner) const + { return m_evaluator.coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); } + + template + PacketType packetByOuterInner(Index outer, Index inner) const + { return m_evaluator.template packet(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); } + + const XprType & nestedExpression() const { return m_xpr; } + +protected: + internal::evaluator m_evaluator; + const XprType &m_xpr; +}; + } // end namespace internal /*************************************************************************** @@ -317,18 +401,21 @@ struct redux_impl /** \returns the result of a full redux operation on the whole matrix or vector using \a func * * The template parameter \a BinaryOp is the type of the functor \a func which must be - * an associative operator. Both current STL and TR1 functor styles are handled. + * an associative operator. Both current C++98 and C++11 functor styles are handled. * * \sa DenseBase::sum(), DenseBase::minCoeff(), DenseBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise() */ template template -EIGEN_STRONG_INLINE typename internal::result_of::Scalar)>::type +typename internal::traits::Scalar DenseBase::redux(const Func& func) const { - typedef typename internal::remove_all::type ThisNested; - return internal::redux_impl - ::run(derived(), func); + eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix"); + + typedef typename internal::redux_evaluator ThisEvaluator; + ThisEvaluator thisEval(derived()); + + return internal::redux_impl::run(thisEval, func); } /** \returns the minimum of all coefficients of \c *this. @@ -338,7 +425,7 @@ template EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::minCoeff() const { - return this->redux(Eigen::internal::scalar_min_op()); + return derived().redux(Eigen::internal::scalar_min_op()); } /** \returns the maximum of all coefficients of \c *this. @@ -348,10 +435,12 @@ template EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::maxCoeff() const { - return this->redux(Eigen::internal::scalar_max_op()); + return derived().redux(Eigen::internal::scalar_max_op()); } -/** \returns the sum of all coefficients of *this +/** \returns the sum of all coefficients of \c *this + * + * If \c *this is empty, then the value 0 is returned. * * \sa trace(), prod(), mean() */ @@ -361,7 +450,7 @@ DenseBase::sum() const { if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) return Scalar(0); - return this->redux(Eigen::internal::scalar_sum_op()); + return derived().redux(Eigen::internal::scalar_sum_op()); } /** \returns the mean of all coefficients of *this @@ -372,7 +461,14 @@ template EIGEN_STRONG_INLINE typename internal::traits::Scalar DenseBase::mean() const { - return Scalar(this->redux(Eigen::internal::scalar_sum_op())) / Scalar(this->size()); +#ifdef __INTEL_COMPILER + #pragma warning push + #pragma warning ( disable : 2259 ) +#endif + return Scalar(derived().redux(Eigen::internal::scalar_sum_op())) / Scalar(this->size()); +#ifdef __INTEL_COMPILER + #pragma warning pop +#endif } /** \returns the product of all coefficients of *this @@ -388,7 +484,7 @@ DenseBase::prod() const { if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) return Scalar(1); - return this->redux(Eigen::internal::scalar_product_op()); + return derived().redux(Eigen::internal::scalar_product_op()); } /** \returns the trace of \c *this, i.e. the sum of the coefficients on the main diagonal. diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Ref.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Ref.h index 7a3becaf88..bdf24f52ad 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Ref.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Ref.h @@ -12,79 +12,6 @@ namespace Eigen { -template class RefBase; -template,OuterStride<> >::type > class Ref; - -/** \class Ref - * \ingroup Core_Module - * - * \brief A matrix or vector expression mapping an existing expressions - * - * \tparam PlainObjectType the equivalent matrix type of the mapped data - * \tparam Options specifies whether the pointer is \c #Aligned, or \c #Unaligned. - * The default is \c #Unaligned. - * \tparam StrideType optionally specifies strides. By default, Ref implies a contiguous storage along the inner dimension (inner stride==1), - * but accept a variable outer stride (leading dimension). - * This can be overridden by specifying strides. - * The type passed here must be a specialization of the Stride template, see examples below. - * - * This class permits to write non template functions taking Eigen's object as parameters while limiting the number of copies. - * A Ref<> object can represent either a const expression or a l-value: - * \code - * // in-out argument: - * void foo1(Ref x); - * - * // read-only const argument: - * void foo2(const Ref& x); - * \endcode - * - * In the in-out case, the input argument must satisfies the constraints of the actual Ref<> type, otherwise a compilation issue will be triggered. - * By default, a Ref can reference any dense vector expression of float having a contiguous memory layout. - * Likewise, a Ref can reference any column major dense matrix expression of float whose column's elements are contiguously stored with - * the possibility to have a constant space inbetween each column, i.e.: the inner stride mmust be equal to 1, but the outer-stride (or leading dimension), - * can be greater than the number of rows. - * - * In the const case, if the input expression does not match the above requirement, then it is evaluated into a temporary before being passed to the function. - * Here are some examples: - * \code - * MatrixXf A; - * VectorXf a; - * foo1(a.head()); // OK - * foo1(A.col()); // OK - * foo1(A.row()); // compilation error because here innerstride!=1 - * foo2(A.row()); // The row is copied into a contiguous temporary - * foo2(2*a); // The expression is evaluated into a temporary - * foo2(A.col().segment(2,4)); // No temporary - * \endcode - * - * The range of inputs that can be referenced without temporary can be enlarged using the last two template parameter. - * Here is an example accepting an innerstride!=1: - * \code - * // in-out argument: - * void foo3(Ref > x); - * foo3(A.row()); // OK - * \endcode - * The downside here is that the function foo3 might be significantly slower than foo1 because it won't be able to exploit vectorization, and will involved more - * expensive address computations even if the input is contiguously stored in memory. To overcome this issue, one might propose to overloads internally calling a - * template function, e.g.: - * \code - * // in the .h: - * void foo(const Ref& A); - * void foo(const Ref >& A); - * - * // in the .cpp: - * template void foo_impl(const TypeOfA& A) { - * ... // crazy code goes here - * } - * void foo(const Ref& A) { foo_impl(A); } - * void foo(const Ref >& A) { foo_impl(A); } - * \endcode - * - * - * \sa PlainObjectBase::Map(), \ref TopicStorageOrders - */ - namespace internal { template @@ -95,7 +22,8 @@ struct traits > typedef _StrideType StrideType; enum { Options = _Options, - Flags = traits >::Flags | NestByRefBit + Flags = traits >::Flags | NestByRefBit, + Alignment = traits >::Alignment }; template struct match { @@ -107,7 +35,13 @@ struct traits > || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1), OuterStrideMatch = Derived::IsVectorAtCompileTime || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime), - AlignmentMatch = (_Options!=Aligned) || ((PlainObjectType::Flags&AlignedBit)==0) || ((traits::Flags&AlignedBit)==AlignedBit), + // NOTE, this indirection of evaluator::Alignment is needed + // to workaround a very strange bug in MSVC related to the instantiation + // of has_*ary_operator in evaluator. + // This line is surprisingly very sensitive. For instance, simply adding parenthesis + // as "DerivedAlignment = (int(evaluator::Alignment))," will make MSVC fail... + DerivedAlignment = int(evaluator::Alignment), + AlignmentMatch = (int(traits::Alignment)==int(Unaligned)) || (DerivedAlignment >= int(Alignment)), // FIXME the first condition is not very clear, it should be replaced by the required alignment ScalarTypeMatch = internal::is_same::value, MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch && ScalarTypeMatch }; @@ -132,12 +66,12 @@ template class RefBase typedef MapBase Base; EIGEN_DENSE_PUBLIC_INTERFACE(RefBase) - inline Index innerStride() const + EIGEN_DEVICE_FUNC inline Index innerStride() const { return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; } - inline Index outerStride() const + EIGEN_DEVICE_FUNC inline Index outerStride() const { return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() : IsVectorAtCompileTime ? this->size() @@ -145,7 +79,7 @@ template class RefBase : this->rows(); } - RefBase() + EIGEN_DEVICE_FUNC RefBase() : Base(0,RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime), // Stride<> does not allow default ctor for Dynamic strides, so let' initialize it with dummy values: m_stride(StrideType::OuterStrideAtCompileTime==Dynamic?0:StrideType::OuterStrideAtCompileTime, @@ -159,7 +93,7 @@ template class RefBase typedef Stride StrideBase; template - void construct(Expression& expr) + EIGEN_DEVICE_FUNC void construct(Expression& expr) { if(PlainObjectType::RowsAtCompileTime==1) { @@ -184,15 +118,83 @@ template class RefBase StrideBase m_stride; }; - +/** \class Ref + * \ingroup Core_Module + * + * \brief A matrix or vector expression mapping an existing expression + * + * \tparam PlainObjectType the equivalent matrix type of the mapped data + * \tparam Options specifies the pointer alignment in bytes. It can be: \c #Aligned128, , \c #Aligned64, \c #Aligned32, \c #Aligned16, \c #Aligned8 or \c #Unaligned. + * The default is \c #Unaligned. + * \tparam StrideType optionally specifies strides. By default, Ref implies a contiguous storage along the inner dimension (inner stride==1), + * but accepts a variable outer stride (leading dimension). + * This can be overridden by specifying strides. + * The type passed here must be a specialization of the Stride template, see examples below. + * + * This class provides a way to write non-template functions taking Eigen objects as parameters while limiting the number of copies. + * A Ref<> object can represent either a const expression or a l-value: + * \code + * // in-out argument: + * void foo1(Ref x); + * + * // read-only const argument: + * void foo2(const Ref& x); + * \endcode + * + * In the in-out case, the input argument must satisfy the constraints of the actual Ref<> type, otherwise a compilation issue will be triggered. + * By default, a Ref can reference any dense vector expression of float having a contiguous memory layout. + * Likewise, a Ref can reference any column-major dense matrix expression of float whose column's elements are contiguously stored with + * the possibility to have a constant space in-between each column, i.e. the inner stride must be equal to 1, but the outer stride (or leading dimension) + * can be greater than the number of rows. + * + * In the const case, if the input expression does not match the above requirement, then it is evaluated into a temporary before being passed to the function. + * Here are some examples: + * \code + * MatrixXf A; + * VectorXf a; + * foo1(a.head()); // OK + * foo1(A.col()); // OK + * foo1(A.row()); // Compilation error because here innerstride!=1 + * foo2(A.row()); // Compilation error because A.row() is a 1xN object while foo2 is expecting a Nx1 object + * foo2(A.row().transpose()); // The row is copied into a contiguous temporary + * foo2(2*a); // The expression is evaluated into a temporary + * foo2(A.col().segment(2,4)); // No temporary + * \endcode + * + * The range of inputs that can be referenced without temporary can be enlarged using the last two template parameters. + * Here is an example accepting an innerstride!=1: + * \code + * // in-out argument: + * void foo3(Ref > x); + * foo3(A.row()); // OK + * \endcode + * The downside here is that the function foo3 might be significantly slower than foo1 because it won't be able to exploit vectorization, and will involve more + * expensive address computations even if the input is contiguously stored in memory. To overcome this issue, one might propose to overload internally calling a + * template function, e.g.: + * \code + * // in the .h: + * void foo(const Ref& A); + * void foo(const Ref >& A); + * + * // in the .cpp: + * template void foo_impl(const TypeOfA& A) { + * ... // crazy code goes here + * } + * void foo(const Ref& A) { foo_impl(A); } + * void foo(const Ref >& A) { foo_impl(A); } + * \endcode + * + * + * \sa PlainObjectBase::Map(), \ref TopicStorageOrders + */ template class Ref : public RefBase > { private: typedef internal::traits Traits; template - inline Ref(const PlainObjectBase& expr, - typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0); + EIGEN_DEVICE_FUNC inline Ref(const PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0); public: typedef RefBase Base; @@ -201,23 +203,24 @@ template class Ref #ifndef EIGEN_PARSED_BY_DOXYGEN template - inline Ref(PlainObjectBase& expr, - typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + EIGEN_DEVICE_FUNC inline Ref(PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) { - EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + EIGEN_STATIC_ASSERT(bool(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); Base::construct(expr.derived()); } template - inline Ref(const DenseBase& expr, - typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + EIGEN_DEVICE_FUNC inline Ref(const DenseBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) #else + /** Implicit constructor from any dense expression */ template inline Ref(DenseBase& expr) #endif { - EIGEN_STATIC_ASSERT(static_cast(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); - EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); - enum { THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY = Derived::ThisConstantIsPrivateInPlainObjectBase}; + EIGEN_STATIC_ASSERT(bool(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + EIGEN_STATIC_ASSERT(bool(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + EIGEN_STATIC_ASSERT(!Derived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); Base::construct(expr.const_cast_derived()); } @@ -236,36 +239,36 @@ template class Ref< EIGEN_DENSE_PUBLIC_INTERFACE(Ref) template - inline Ref(const DenseBase& expr, - typename internal::enable_if::ScalarTypeMatch),Derived>::type* = 0) + EIGEN_DEVICE_FUNC inline Ref(const DenseBase& expr, + typename internal::enable_if::ScalarTypeMatch),Derived>::type* = 0) { // std::cout << match_helper::HasDirectAccess << "," << match_helper::OuterStrideMatch << "," << match_helper::InnerStrideMatch << "\n"; // std::cout << int(StrideType::OuterStrideAtCompileTime) << " - " << int(Derived::OuterStrideAtCompileTime) << "\n"; // std::cout << int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n"; construct(expr.derived(), typename Traits::template match::type()); } - - inline Ref(const Ref& other) : Base(other) { + + EIGEN_DEVICE_FUNC inline Ref(const Ref& other) : Base(other) { // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy } template - inline Ref(const RefBase& other) { + EIGEN_DEVICE_FUNC inline Ref(const RefBase& other) { construct(other.derived(), typename Traits::template match::type()); } protected: template - void construct(const Expression& expr,internal::true_type) + EIGEN_DEVICE_FUNC void construct(const Expression& expr,internal::true_type) { Base::construct(expr); } template - void construct(const Expression& expr, internal::false_type) + EIGEN_DEVICE_FUNC void construct(const Expression& expr, internal::false_type) { - m_object.lazyAssign(expr); + internal::call_assignment_no_alias(m_object,expr,internal::assign_op()); Base::construct(m_object); } diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Replicate.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Replicate.h index ac4537c142..9960ef884e 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Replicate.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Replicate.h @@ -12,21 +12,6 @@ namespace Eigen { -/** - * \class Replicate - * \ingroup Core_Module - * - * \brief Expression of the multiple replication of a matrix or vector - * - * \param MatrixType the type of the object we are replicating - * - * This class represents an expression of the multiple replication of a matrix or vector. - * It is the return type of DenseBase::replicate() and most of the time - * this is the only way it is used. - * - * \sa DenseBase::replicate() - */ - namespace internal { template struct traits > @@ -35,10 +20,7 @@ struct traits > typedef typename MatrixType::Scalar Scalar; typedef typename traits::StorageKind StorageKind; typedef typename traits::XprKind XprKind; - enum { - Factor = (RowFactor==Dynamic || ColFactor==Dynamic) ? Dynamic : RowFactor*ColFactor - }; - typedef typename nested::type MatrixTypeNested; + typedef typename ref_selector::type MatrixTypeNested; typedef typename remove_reference::type _MatrixTypeNested; enum { RowsAtCompileTime = RowFactor==Dynamic || int(MatrixType::RowsAtCompileTime)==Dynamic @@ -53,12 +35,29 @@ struct traits > IsRowMajor = MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1 ? 1 : MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1 ? 0 : (MatrixType::Flags & RowMajorBit) ? 1 : 0, - Flags = (_MatrixTypeNested::Flags & HereditaryBits & ~RowMajorBit) | (IsRowMajor ? RowMajorBit : 0), - CoeffReadCost = _MatrixTypeNested::CoeffReadCost + + // FIXME enable DirectAccess with negative strides? + Flags = IsRowMajor ? RowMajorBit : 0 }; }; } +/** + * \class Replicate + * \ingroup Core_Module + * + * \brief Expression of the multiple replication of a matrix or vector + * + * \tparam MatrixType the type of the object we are replicating + * \tparam RowFactor number of repetitions at compile time along the vertical direction, can be Dynamic. + * \tparam ColFactor number of repetitions at compile time along the horizontal direction, can be Dynamic. + * + * This class represents an expression of the multiple replication of a matrix or vector. + * It is the return type of DenseBase::replicate() and most of the time + * this is the only way it is used. + * + * \sa DenseBase::replicate() + */ template class Replicate : public internal::dense_xpr_base< Replicate >::type { @@ -68,10 +67,12 @@ template class Replicate typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(Replicate) + typedef typename internal::remove_all::type NestedExpression; template - inline explicit Replicate(const OriginalMatrixType& a_matrix) - : m_matrix(a_matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor) + EIGEN_DEVICE_FUNC + inline explicit Replicate(const OriginalMatrixType& matrix) + : m_matrix(matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor) { EIGEN_STATIC_ASSERT((internal::is_same::type,OriginalMatrixType>::value), THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE) @@ -79,41 +80,20 @@ template class Replicate } template - inline Replicate(const OriginalMatrixType& a_matrix, Index rowFactor, Index colFactor) - : m_matrix(a_matrix), m_rowFactor(rowFactor), m_colFactor(colFactor) + EIGEN_DEVICE_FUNC + inline Replicate(const OriginalMatrixType& matrix, Index rowFactor, Index colFactor) + : m_matrix(matrix), m_rowFactor(rowFactor), m_colFactor(colFactor) { EIGEN_STATIC_ASSERT((internal::is_same::type,OriginalMatrixType>::value), THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE) } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.rows() * m_rowFactor.value(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.cols() * m_colFactor.value(); } - inline Scalar coeff(Index rowId, Index colId) const - { - // try to avoid using modulo; this is a pure optimization strategy - const Index actual_row = internal::traits::RowsAtCompileTime==1 ? 0 - : RowFactor==1 ? rowId - : rowId%m_matrix.rows(); - const Index actual_col = internal::traits::ColsAtCompileTime==1 ? 0 - : ColFactor==1 ? colId - : colId%m_matrix.cols(); - - return m_matrix.coeff(actual_row, actual_col); - } - template - inline PacketScalar packet(Index rowId, Index colId) const - { - const Index actual_row = internal::traits::RowsAtCompileTime==1 ? 0 - : RowFactor==1 ? rowId - : rowId%m_matrix.rows(); - const Index actual_col = internal::traits::ColsAtCompileTime==1 ? 0 - : ColFactor==1 ? colId - : colId%m_matrix.cols(); - - return m_matrix.template packet(actual_row, actual_col); - } - + EIGEN_DEVICE_FUNC const _MatrixTypeNested& nestedExpression() const { return m_matrix; @@ -141,21 +121,6 @@ DenseBase::replicate() const return Replicate(derived()); } -/** - * \return an expression of the replication of \c *this - * - * Example: \include MatrixBase_replicate_int_int.cpp - * Output: \verbinclude MatrixBase_replicate_int_int.out - * - * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate - */ -template -const typename DenseBase::ReplicateReturnType -DenseBase::replicate(Index rowFactor,Index colFactor) const -{ - return Replicate(derived(),rowFactor,colFactor); -} - /** * \return an expression of the replication of each column (or row) of \c *this * diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/ReturnByValue.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/ReturnByValue.h index f635598dcc..c44b7673bb 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/ReturnByValue.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/ReturnByValue.h @@ -13,11 +13,6 @@ namespace Eigen { -/** \class ReturnByValue - * \ingroup Core_Module - * - */ - namespace internal { template @@ -38,17 +33,22 @@ struct traits > * So internal::nested always gives the plain return matrix type. * * FIXME: I don't understand why we need this specialization: isn't this taken care of by the EvalBeforeNestingBit ?? + * Answer: EvalBeforeNestingBit should be deprecated since we have the evaluators */ template -struct nested, n, PlainObject> +struct nested_eval, n, PlainObject> { typedef typename traits::ReturnType type; }; } // end namespace internal +/** \class ReturnByValue + * \ingroup Core_Module + * + */ template class ReturnByValue - : internal::no_assignment_operator, public internal::dense_xpr_base< ReturnByValue >::type + : public internal::dense_xpr_base< ReturnByValue >::type, internal::no_assignment_operator { public: typedef typename internal::traits::ReturnType ReturnType; @@ -57,10 +57,11 @@ template class ReturnByValue EIGEN_DENSE_PUBLIC_INTERFACE(ReturnByValue) template + EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const { static_cast(this)->evalTo(dst); } - inline Index rows() const { return static_cast(this)->rows(); } - inline Index cols() const { return static_cast(this)->cols(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return static_cast(this)->rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return static_cast(this)->cols(); } #ifndef EIGEN_PARSED_BY_DOXYGEN #define Unusable YOU_ARE_TRYING_TO_ACCESS_A_SINGLE_COEFFICIENT_IN_A_SPECIAL_EXPRESSION_WHERE_THAT_IS_NOT_ALLOWED_BECAUSE_THAT_WOULD_BE_INEFFICIENT @@ -72,8 +73,7 @@ template class ReturnByValue const Unusable& coeff(Index,Index) const { return *reinterpret_cast(this); } Unusable& coeffRef(Index) { return *reinterpret_cast(this); } Unusable& coeffRef(Index,Index) { return *reinterpret_cast(this); } - template Unusable& packet(Index) const; - template Unusable& packet(Index, Index) const; +#undef Unusable #endif }; @@ -85,14 +85,32 @@ Derived& DenseBase::operator=(const ReturnByValue& other) return derived(); } +namespace internal { + +// Expression is evaluated in a temporary; default implementation of Assignment is bypassed so that +// when a ReturnByValue expression is assigned, the evaluator is not constructed. +// TODO: Finalize port to new regime; ReturnByValue should not exist in the expression world + template -template -Derived& DenseBase::lazyAssign(const ReturnByValue& other) +struct evaluator > + : public evaluator::ReturnType> { - other.evalTo(derived()); - return derived(); -} + typedef ReturnByValue XprType; + typedef typename internal::traits::ReturnType PlainObject; + typedef evaluator Base; + + EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) + : m_result(xpr.rows(), xpr.cols()) + { + ::new (static_cast(this)) Base(m_result); + xpr.evalTo(m_result); + } + +protected: + PlainObject m_result; +}; +} // end namespace internal } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Reverse.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Reverse.h index e30ae3d281..0640cda2a1 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Reverse.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Reverse.h @@ -14,20 +14,6 @@ namespace Eigen { -/** \class Reverse - * \ingroup Core_Module - * - * \brief Expression of the reverse of a vector or matrix - * - * \param MatrixType the type of the object of which we are taking the reverse - * - * This class represents an expression of the reverse of a vector. - * It is the return type of MatrixBase::reverse() and VectorwiseOp::reverse() - * and most of the time this is the only way it is used. - * - * \sa MatrixBase::reverse(), VectorwiseOp::reverse() - */ - namespace internal { template @@ -37,36 +23,43 @@ struct traits > typedef typename MatrixType::Scalar Scalar; typedef typename traits::StorageKind StorageKind; typedef typename traits::XprKind XprKind; - typedef typename nested::type MatrixTypeNested; + typedef typename ref_selector::type MatrixTypeNested; typedef typename remove_reference::type _MatrixTypeNested; enum { RowsAtCompileTime = MatrixType::RowsAtCompileTime, ColsAtCompileTime = MatrixType::ColsAtCompileTime, MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, - - // let's enable LinearAccess only with vectorization because of the product overhead - LinearAccess = ( (Direction==BothDirections) && (int(_MatrixTypeNested::Flags)&PacketAccessBit) ) - ? LinearAccessBit : 0, - - Flags = int(_MatrixTypeNested::Flags) & (HereditaryBits | LvalueBit | PacketAccessBit | LinearAccess), - - CoeffReadCost = _MatrixTypeNested::CoeffReadCost + Flags = _MatrixTypeNested::Flags & (RowMajorBit | LvalueBit) }; }; -template struct reverse_packet_cond +template struct reverse_packet_cond { - static inline PacketScalar run(const PacketScalar& x) { return preverse(x); } + static inline PacketType run(const PacketType& x) { return preverse(x); } }; -template struct reverse_packet_cond +template struct reverse_packet_cond { - static inline PacketScalar run(const PacketScalar& x) { return x; } + static inline PacketType run(const PacketType& x) { return x; } }; } // end namespace internal +/** \class Reverse + * \ingroup Core_Module + * + * \brief Expression of the reverse of a vector or matrix + * + * \tparam MatrixType the type of the object of which we are taking the reverse + * \tparam Direction defines the direction of the reverse operation, can be Vertical, Horizontal, or BothDirections + * + * This class represents an expression of the reverse of a vector. + * It is the return type of MatrixBase::reverse() and VectorwiseOp::reverse() + * and most of the time this is the only way it is used. + * + * \sa MatrixBase::reverse(), VectorwiseOp::reverse() + */ template class Reverse : public internal::dense_xpr_base< Reverse >::type { @@ -74,12 +67,9 @@ template class Reverse typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(Reverse) + typedef typename internal::remove_all::type NestedExpression; using Base::IsRowMajor; - // next line is necessary because otherwise const version of operator() - // is hidden by non-const version defined in this file - using Base::operator(); - protected: enum { PacketSize = internal::packet_traits::size, @@ -95,82 +85,19 @@ template class Reverse typedef internal::reverse_packet_cond reverse_packet; public: - inline Reverse(const MatrixType& matrix) : m_matrix(matrix) { } + EIGEN_DEVICE_FUNC explicit inline Reverse(const MatrixType& matrix) : m_matrix(matrix) { } EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reverse) - inline Index rows() const { return m_matrix.rows(); } - inline Index cols() const { return m_matrix.cols(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.cols(); } - inline Index innerStride() const + EIGEN_DEVICE_FUNC inline Index innerStride() const { return -m_matrix.innerStride(); } - inline Scalar& operator()(Index row, Index col) - { - eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); - return coeffRef(row, col); - } - - inline Scalar& coeffRef(Index row, Index col) - { - return m_matrix.const_cast_derived().coeffRef(ReverseRow ? m_matrix.rows() - row - 1 : row, - ReverseCol ? m_matrix.cols() - col - 1 : col); - } - - inline CoeffReturnType coeff(Index row, Index col) const - { - return m_matrix.coeff(ReverseRow ? m_matrix.rows() - row - 1 : row, - ReverseCol ? m_matrix.cols() - col - 1 : col); - } - - inline CoeffReturnType coeff(Index index) const - { - return m_matrix.coeff(m_matrix.size() - index - 1); - } - - inline Scalar& coeffRef(Index index) - { - return m_matrix.const_cast_derived().coeffRef(m_matrix.size() - index - 1); - } - - inline Scalar& operator()(Index index) - { - eigen_assert(index >= 0 && index < m_matrix.size()); - return coeffRef(index); - } - - template - inline const PacketScalar packet(Index row, Index col) const - { - return reverse_packet::run(m_matrix.template packet( - ReverseRow ? m_matrix.rows() - row - OffsetRow : row, - ReverseCol ? m_matrix.cols() - col - OffsetCol : col)); - } - - template - inline void writePacket(Index row, Index col, const PacketScalar& x) - { - m_matrix.const_cast_derived().template writePacket( - ReverseRow ? m_matrix.rows() - row - OffsetRow : row, - ReverseCol ? m_matrix.cols() - col - OffsetCol : col, - reverse_packet::run(x)); - } - - template - inline const PacketScalar packet(Index index) const - { - return internal::preverse(m_matrix.template packet( m_matrix.size() - index - PacketSize )); - } - - template - inline void writePacket(Index index, const PacketScalar& x) - { - m_matrix.const_cast_derived().template writePacket(m_matrix.size() - index - PacketSize, internal::preverse(x)); - } - - const typename internal::remove_all::type& + EIGEN_DEVICE_FUNC const typename internal::remove_all::type& nestedExpression() const { return m_matrix; @@ -190,33 +117,93 @@ template inline typename DenseBase::ReverseReturnType DenseBase::reverse() { - return derived(); + return ReverseReturnType(derived()); } -/** This is the const version of reverse(). */ -template -inline const typename DenseBase::ConstReverseReturnType -DenseBase::reverse() const -{ - return derived(); -} + +//reverse const overload moved DenseBase.h due to a CUDA compiler bug /** This is the "in place" version of reverse: it reverses \c *this. * * In most cases it is probably better to simply use the reversed expression * of a matrix. However, when reversing the matrix data itself is really needed, * then this "in-place" version is probably the right choice because it provides - * the following additional features: + * the following additional benefits: * - less error prone: doing the same operation with .reverse() requires special care: * \code m = m.reverse().eval(); \endcode - * - this API allows to avoid creating a temporary (the current implementation creates a temporary, but that could be avoided using swap) + * - this API enables reverse operations without the need for a temporary * - it allows future optimizations (cache friendliness, etc.) * - * \sa reverse() */ + * \sa VectorwiseOp::reverseInPlace(), reverse() */ template inline void DenseBase::reverseInPlace() { - derived() = derived().reverse().eval(); + if(cols()>rows()) + { + Index half = cols()/2; + leftCols(half).swap(rightCols(half).reverse()); + if((cols()%2)==1) + { + Index half2 = rows()/2; + col(half).head(half2).swap(col(half).tail(half2).reverse()); + } + } + else + { + Index half = rows()/2; + topRows(half).swap(bottomRows(half).reverse()); + if((rows()%2)==1) + { + Index half2 = cols()/2; + row(half).head(half2).swap(row(half).tail(half2).reverse()); + } + } +} + +namespace internal { + +template +struct vectorwise_reverse_inplace_impl; + +template<> +struct vectorwise_reverse_inplace_impl +{ + template + static void run(ExpressionType &xpr) + { + Index half = xpr.rows()/2; + xpr.topRows(half).swap(xpr.bottomRows(half).colwise().reverse()); + } +}; + +template<> +struct vectorwise_reverse_inplace_impl +{ + template + static void run(ExpressionType &xpr) + { + Index half = xpr.cols()/2; + xpr.leftCols(half).swap(xpr.rightCols(half).rowwise().reverse()); + } +}; + +} // end namespace internal + +/** This is the "in place" version of VectorwiseOp::reverse: it reverses each column or row of \c *this. + * + * In most cases it is probably better to simply use the reversed expression + * of a matrix. However, when reversing the matrix data itself is really needed, + * then this "in-place" version is probably the right choice because it provides + * the following additional benefits: + * - less error prone: doing the same operation with .reverse() requires special care: + * \code m = m.reverse().eval(); \endcode + * - this API enables reverse operations without the need for a temporary + * + * \sa DenseBase::reverseInPlace(), reverse() */ +template +void VectorwiseOp::reverseInPlace() +{ + internal::vectorwise_reverse_inplace_impl::run(_expression().const_cast_derived()); } } // end namespace Eigen diff --git a/ground/gcs/src/libs/eigen/Eigen/src/Core/Select.h b/ground/gcs/src/libs/eigen/Eigen/src/Core/Select.h index 87993bbb55..79eec1b5b0 100644 --- a/ground/gcs/src/libs/eigen/Eigen/src/Core/Select.h +++ b/ground/gcs/src/libs/eigen/Eigen/src/Core/Select.h @@ -43,23 +43,21 @@ struct traits > ColsAtCompileTime = ConditionMatrixType::ColsAtCompileTime, MaxRowsAtCompileTime = ConditionMatrixType::MaxRowsAtCompileTime, MaxColsAtCompileTime = ConditionMatrixType::MaxColsAtCompileTime, - Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & HereditaryBits, - CoeffReadCost = traits::type>::CoeffReadCost - + EIGEN_SIZE_MAX(traits::type>::CoeffReadCost, - traits::type>::CoeffReadCost) + Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & RowMajorBit }; }; } template -class Select : internal::no_assignment_operator, - public internal::dense_xpr_base< Select >::type +class Select : public internal::dense_xpr_base< Select >::type, + internal::no_assignment_operator { public: typedef typename internal::dense_xpr_base" << endl; + cerr << "available actions:" << endl; + for (auto it = available_actions.begin(); it != available_actions.end(); ++it) { + cerr << " " << (*it)->invokation_name() << endl; + } + cerr << "the input files should each contain an output of benchmark-blocking-sizes" << endl; + exit(1); +} + +int main(int argc, char* argv[]) +{ + cout.precision(default_precision); + cerr.precision(default_precision); + + vector> available_actions; + available_actions.emplace_back(new partition_action_t); + available_actions.emplace_back(new evaluate_defaults_action_t); + + vector input_filenames; + + action_t* action = nullptr; + + if (argc < 2) { + show_usage_and_exit(argc, argv, available_actions); + } + for (int i = 1; i < argc; i++) { + bool arg_handled = false; + // Step 1. Try to match action invokation names. + for (auto it = available_actions.begin(); it != available_actions.end(); ++it) { + if (!strcmp(argv[i], (*it)->invokation_name())) { + if (!action) { + action = it->get(); + arg_handled = true; + break; + } else { + cerr << "can't specify more than one action!" << endl; + show_usage_and_exit(argc, argv, available_actions); + } + } + } + if (arg_handled) { + continue; + } + // Step 2. Try to match option names. + if (argv[i][0] == '-') { + if (!strcmp(argv[i], "--only-cubic-sizes")) { + only_cubic_sizes = true; + arg_handled = true; + } + if (!strcmp(argv[i], "--dump-tables")) { + dump_tables = true; + arg_handled = true; + } + if (!arg_handled) { + cerr << "Unrecognized option: " << argv[i] << endl; + show_usage_and_exit(argc, argv, available_actions); + } + } + if (arg_handled) { + continue; + } + // Step 3. Default to interpreting args as input filenames. + input_filenames.emplace_back(argv[i]); + } + + if (dump_tables && only_cubic_sizes) { + cerr << "Incompatible options: --only-cubic-sizes and --dump-tables." << endl; + show_usage_and_exit(argc, argv, available_actions); + } + + if (!action) { + show_usage_and_exit(argc, argv, available_actions); + } + + action->run(input_filenames); +} diff --git a/ground/gcs/src/libs/eigen/bench/benchCholesky.cpp b/ground/gcs/src/libs/eigen/bench/benchCholesky.cpp index 42b3e1285f..9a8e7cf638 100644 --- a/ground/gcs/src/libs/eigen/bench/benchCholesky.cpp +++ b/ground/gcs/src/libs/eigen/bench/benchCholesky.cpp @@ -31,7 +31,7 @@ __attribute__ ((noinline)) void benchLLT(const MatrixType& m) int rows = m.rows(); int cols = m.cols(); - int cost = 0; + double cost = 0; for (int j=0; j0; ++i) + for (int i=0; dynsizes[i]>0; ++i) benchLLT(Matrix(dynsizes[i],dynsizes[i])); benchLLT(Matrix()); diff --git a/ground/gcs/src/libs/eigen/bench/bench_gemm.cpp b/ground/gcs/src/libs/eigen/bench/bench_gemm.cpp index 41ca8b3b6e..8528c55874 100644 --- a/ground/gcs/src/libs/eigen/bench/bench_gemm.cpp +++ b/ground/gcs/src/libs/eigen/bench/bench_gemm.cpp @@ -2,6 +2,14 @@ // g++-4.4 bench_gemm.cpp -I .. -O2 -DNDEBUG -lrt -fopenmp && OMP_NUM_THREADS=2 ./a.out // icpc bench_gemm.cpp -I .. -O3 -DNDEBUG -lrt -openmp && OMP_NUM_THREADS=2 ./a.out +// Compilation options: +// +// -DSCALAR=std::complex +// -DSCALARA=double or -DSCALARB=double +// -DHAVE_BLAS +// -DDECOUPLED +// + #include #include #include @@ -14,10 +22,18 @@ using namespace Eigen; #define SCALAR float #endif +#ifndef SCALARA +#define SCALARA SCALAR +#endif + +#ifndef SCALARB +#define SCALARB SCALAR +#endif + typedef SCALAR Scalar; typedef NumTraits::Real RealScalar; -typedef Matrix A; -typedef Matrix B; +typedef Matrix A; +typedef Matrix B; typedef Matrix C; typedef Matrix M; @@ -129,35 +145,69 @@ int main(int argc, char ** argv) int tries = 2; // number of tries, we keep the best int s = 2048; - int cache_size = -1; + int m = s; + int n = s; + int p = s; + int cache_size1=-1, cache_size2=l2, cache_size3 = 0; bool need_help = false; - for (int i=1; i c t p\n"; + std::cout << argv[0] << " -s -c -t -p \n"; + std::cout << " : size\n"; + std::cout << " : rows columns depth\n"; return 1; } - if(cache_size>0) - setCpuCacheSizes(cache_size,96*cache_size); - - int m = s; - int n = s; - int p = s; +#if EIGEN_VERSION_AT_LEAST(3,2,90) + if(cache_size1>0) + setCpuCacheSizes(cache_size1,cache_size2,cache_size3); +#endif + A a(m,p); a.setRandom(); B b(p,n); b.setRandom(); C c(m,n); c.setOnes(); @@ -172,6 +222,7 @@ int main(int argc, char ** argv) // check the parallel product is correct #if defined EIGEN_HAS_OPENMP + Eigen::initParallel(); int procs = omp_get_max_threads(); if(procs>1) { @@ -188,11 +239,20 @@ int main(int argc, char ** argv) #elif defined HAVE_BLAS blas_gemm(a,b,r); c.noalias() += a * b; - if(!r.isApprox(c)) std::cerr << "Warning, your product is crap!\n\n"; + if(!r.isApprox(c)) { + std::cout << r - c << "\n"; + std::cerr << "Warning, your product is crap!\n\n"; + } #else - gemm(a,b,c); - r.noalias() += a.cast() * b.cast(); - if(!r.isApprox(c)) std::cerr << "Warning, your product is crap!\n\n"; + if(1.*m*n*p<2000.*2000*2000) + { + gemm(a,b,c); + r.noalias() += a.cast() .lazyProduct( b.cast() ); + if(!r.isApprox(c)) { + std::cout << r - c << "\n"; + std::cerr << "Warning, your product is crap!\n\n"; + } + } #endif #ifdef HAVE_BLAS @@ -214,7 +274,7 @@ int main(int argc, char ** argv) { BenchTimer tmono; omp_set_num_threads(1); - Eigen::internal::setNbThreads(1); + Eigen::setNbThreads(1); c = rc; BENCH(tmono, tries, rep, gemm(a,b,c)); std::cout << "eigen mono cpu " << tmono.best(CPU_TIMER)/rep << "s \t" << (double(m)*n*p*rep*2/tmono.best(CPU_TIMER))*1e-9 << " GFLOPS \t(" << tmono.total(CPU_TIMER) << "s)\n"; @@ -223,6 +283,15 @@ int main(int argc, char ** argv) } #endif + if(1.*m*n*p<30*30*30) + { + BenchTimer tmt; + c = rc; + BENCH(tmt, tries, rep, c.noalias()+=a.lazyProduct(b)); + std::cout << "lazy cpu " << tmt.best(CPU_TIMER)/rep << "s \t" << (double(m)*n*p*rep*2/tmt.best(CPU_TIMER))*1e-9 << " GFLOPS \t(" << tmt.total(CPU_TIMER) << "s)\n"; + std::cout << "lazy real " << tmt.best(REAL_TIMER)/rep << "s \t" << (double(m)*n*p*rep*2/tmt.best(REAL_TIMER))*1e-9 << " GFLOPS \t(" << tmt.total(REAL_TIMER) << "s)\n"; + } + #ifdef DECOUPLED if((NumTraits::IsComplex) && (NumTraits::IsComplex)) { diff --git a/ground/gcs/src/libs/eigen/bench/bench_norm.cpp b/ground/gcs/src/libs/eigen/bench/bench_norm.cpp index 806db292c5..129afcfb25 100644 --- a/ground/gcs/src/libs/eigen/bench/bench_norm.cpp +++ b/ground/gcs/src/libs/eigen/bench/bench_norm.cpp @@ -6,19 +6,25 @@ using namespace Eigen; using namespace std; template -EIGEN_DONT_INLINE typename T::Scalar sqsumNorm(const T& v) +EIGEN_DONT_INLINE typename T::Scalar sqsumNorm(T& v) { return v.norm(); } template -EIGEN_DONT_INLINE typename T::Scalar hypotNorm(const T& v) +EIGEN_DONT_INLINE typename T::Scalar stableNorm(T& v) +{ + return v.stableNorm(); +} + +template +EIGEN_DONT_INLINE typename T::Scalar hypotNorm(T& v) { return v.hypotNorm(); } template -EIGEN_DONT_INLINE typename T::Scalar blueNorm(const T& v) +EIGEN_DONT_INLINE typename T::Scalar blueNorm(T& v) { return v.blueNorm(); } @@ -32,25 +38,25 @@ EIGEN_DONT_INLINE typename T::Scalar lapackNorm(T& v) Scalar ssq = 1; for (int i=0;i= ax) { - ssq += internal::abs2(ax/scale); + ssq += numext::abs2(ax/scale); } else { - ssq = Scalar(1) + ssq * internal::abs2(scale/ax); + ssq = Scalar(1) + ssq * numext::abs2(scale/ax); scale = ax; } } - return scale * internal::sqrt(ssq); + return scale * std::sqrt(ssq); } template EIGEN_DONT_INLINE typename T::Scalar twopassNorm(T& v) { typedef typename T::Scalar Scalar; - Scalar s = v.cwise().abs().maxCoeff(); + Scalar s = v.array().abs().maxCoeff(); return s*(v/s).norm(); } @@ -73,16 +79,20 @@ EIGEN_DONT_INLINE typename T::Scalar divacNorm(T& v) v(i) = v(2*i) + v(2*i+1); n = n/2; } - return internal::sqrt(v(0)); + return std::sqrt(v(0)); } +namespace Eigen { +namespace internal { #ifdef EIGEN_VECTORIZE -Packet4f internal::plt(const Packet4f& a, Packet4f& b) { return _mm_cmplt_ps(a,b); } -Packet2d internal::plt(const Packet2d& a, Packet2d& b) { return _mm_cmplt_pd(a,b); } +Packet4f plt(const Packet4f& a, Packet4f& b) { return _mm_cmplt_ps(a,b); } +Packet2d plt(const Packet2d& a, Packet2d& b) { return _mm_cmplt_pd(a,b); } -Packet4f internal::pandnot(const Packet4f& a, Packet4f& b) { return _mm_andnot_ps(a,b); } -Packet2d internal::pandnot(const Packet2d& a, Packet2d& b) { return _mm_andnot_pd(a,b); } +Packet4f pandnot(const Packet4f& a, Packet4f& b) { return _mm_andnot_ps(a,b); } +Packet2d pandnot(const Packet2d& a, Packet2d& b) { return _mm_andnot_pd(a,b); } #endif +} +} template EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v) @@ -126,7 +136,7 @@ EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v) overfl = rbig*s2m; // overfow boundary for abig eps = std::pow(ibeta, 1-it); - relerr = internal::sqrt(eps); // tolerance for neglecting asml + relerr = std::sqrt(eps); // tolerance for neglecting asml abig = 1.0/eps - 1.0; if (Scalar(nbig)>abig) nmax = abig; // largest safe n else nmax = nbig; @@ -134,13 +144,13 @@ EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v) typedef typename internal::packet_traits::type Packet; const int ps = internal::packet_traits::size; - Packet pasml = internal::pset1(Scalar(0)); - Packet pamed = internal::pset1(Scalar(0)); - Packet pabig = internal::pset1(Scalar(0)); - Packet ps2m = internal::pset1(s2m); - Packet ps1m = internal::pset1(s1m); - Packet pb2 = internal::pset1(b2); - Packet pb1 = internal::pset1(b1); + Packet pasml = internal::pset1(Scalar(0)); + Packet pamed = internal::pset1(Scalar(0)); + Packet pabig = internal::pset1(Scalar(0)); + Packet ps2m = internal::pset1(s2m); + Packet ps1m = internal::pset1(s1m); + Packet pb2 = internal::pset1(b2); + Packet pb1 = internal::pset1(b1); for(int j=0; j(j)); @@ -170,7 +180,7 @@ EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v) Scalar amed = internal::predux(pamed); if(abig > Scalar(0)) { - abig = internal::sqrt(abig); + abig = std::sqrt(abig); if(abig > overfl) { eigen_assert(false && "overflow"); @@ -179,7 +189,7 @@ EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v) if(amed > Scalar(0)) { abig = abig/s2m; - amed = internal::sqrt(amed); + amed = std::sqrt(amed); } else { @@ -191,55 +201,56 @@ EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v) { if (amed > Scalar(0)) { - abig = internal::sqrt(amed); - amed = internal::sqrt(asml) / s1m; + abig = std::sqrt(amed); + amed = std::sqrt(asml) / s1m; } else { - return internal::sqrt(asml)/s1m; + return std::sqrt(asml)/s1m; } } else { - return internal::sqrt(amed); + return std::sqrt(amed); } asml = std::min(abig, amed); abig = std::max(abig, amed); if(asml <= abig*relerr) return abig; else - return abig * internal::sqrt(Scalar(1) + internal::abs2(asml/abig)); + return abig * std::sqrt(Scalar(1) + numext::abs2(asml/abig)); #endif } #define BENCH_PERF(NRM) { \ + float af = 0; double ad = 0; std::complex ac = 0; \ Eigen::BenchTimer tf, td, tcf; tf.reset(); td.reset(); tcf.reset();\ for (int k=0; k()); - double yd = based * internal::abs(internal::random()); + double yf = basef * std::abs(internal::random()); + double yd = based * std::abs(internal::random()); VectorXf vf = VectorXf::Ones(s) * yf; VectorXd vd = VectorXd::Ones(s) * yd; - std::cout << "reference\t" << internal::sqrt(double(s))*yf << "\t" << internal::sqrt(double(s))*yd << "\n"; + std::cout << "reference\t" << std::sqrt(double(s))*yf << "\t" << std::sqrt(double(s))*yd << "\n"; std::cout << "sqsumNorm\t" << sqsumNorm(vf) << "\t" << sqsumNorm(vd) << "\n"; std::cout << "hypotNorm\t" << hypotNorm(vf) << "\t" << hypotNorm(vd) << "\n"; std::cout << "blueNorm\t" << blueNorm(vf) << "\t" << blueNorm(vd) << "\n"; @@ -255,8 +266,8 @@ void check_accuracy_var(int ef0, int ef1, int ed0, int ed1, int s) VectorXd vd(s); for (int i=0; i()) * std::pow(double(10), internal::random(ef0,ef1)); - vd[i] = internal::abs(internal::random()) * std::pow(double(10), internal::random(ed0,ed1)); + vf[i] = std::abs(internal::random()) * std::pow(double(10), internal::random(ef0,ef1)); + vd[i] = std::abs(internal::random()) * std::pow(double(10), internal::random(ed0,ed1)); } //std::cout << "reference\t" << internal::sqrt(double(s))*yf << "\t" << internal::sqrt(double(s))*yd << "\n"; @@ -312,34 +323,38 @@ int main(int argc, char** argv) std::cout << "\n"; } + y = 1; std::cout.precision(4); - std::cerr << "Performance (out of cache):\n"; + int s1 = 1024*1024*32; + std::cerr << "Performance (out of cache, " << s1 << "):\n"; { int iters = 1; - VectorXf vf = VectorXf::Random(1024*1024*32) * y; - VectorXd vd = VectorXd::Random(1024*1024*32) * y; - VectorXcf vcf = VectorXcf::Random(1024*1024*32) * y; + VectorXf vf = VectorXf::Random(s1) * y; + VectorXd vd = VectorXd::Random(s1) * y; + VectorXcf vcf = VectorXcf::Random(s1) * y; BENCH_PERF(sqsumNorm); + BENCH_PERF(stableNorm); BENCH_PERF(blueNorm); -// BENCH_PERF(pblueNorm); -// BENCH_PERF(lapackNorm); -// BENCH_PERF(hypotNorm); -// BENCH_PERF(twopassNorm); + BENCH_PERF(pblueNorm); + BENCH_PERF(lapackNorm); + BENCH_PERF(hypotNorm); + BENCH_PERF(twopassNorm); BENCH_PERF(bl2passNorm); } - std::cerr << "\nPerformance (in cache):\n"; + std::cerr << "\nPerformance (in cache, " << 512 << "):\n"; { int iters = 100000; VectorXf vf = VectorXf::Random(512) * y; VectorXd vd = VectorXd::Random(512) * y; VectorXcf vcf = VectorXcf::Random(512) * y; BENCH_PERF(sqsumNorm); + BENCH_PERF(stableNorm); BENCH_PERF(blueNorm); -// BENCH_PERF(pblueNorm); -// BENCH_PERF(lapackNorm); -// BENCH_PERF(hypotNorm); -// BENCH_PERF(twopassNorm); + BENCH_PERF(pblueNorm); + BENCH_PERF(lapackNorm); + BENCH_PERF(hypotNorm); + BENCH_PERF(twopassNorm); BENCH_PERF(bl2passNorm); } } diff --git a/ground/gcs/src/libs/eigen/bench/benchmark-blocking-sizes.cpp b/ground/gcs/src/libs/eigen/bench/benchmark-blocking-sizes.cpp new file mode 100644 index 0000000000..827be28802 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/benchmark-blocking-sizes.cpp @@ -0,0 +1,677 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include +#include +#include +#include + +bool eigen_use_specific_block_size; +int eigen_block_size_k, eigen_block_size_m, eigen_block_size_n; +#define EIGEN_TEST_SPECIFIC_BLOCKING_SIZES eigen_use_specific_block_size +#define EIGEN_TEST_SPECIFIC_BLOCKING_SIZE_K eigen_block_size_k +#define EIGEN_TEST_SPECIFIC_BLOCKING_SIZE_M eigen_block_size_m +#define EIGEN_TEST_SPECIFIC_BLOCKING_SIZE_N eigen_block_size_n +#include + +#include + +using namespace Eigen; +using namespace std; + +static BenchTimer timer; + +// how many times we repeat each measurement. +// measurements are randomly shuffled - we're not doing +// all N identical measurements in a row. +const int measurement_repetitions = 3; + +// Timings below this value are too short to be accurate, +// we'll repeat measurements with more iterations until +// we get a timing above that threshold. +const float min_accurate_time = 1e-2f; + +// See --min-working-set-size command line parameter. +size_t min_working_set_size = 0; + +float max_clock_speed = 0.0f; + +// range of sizes that we will benchmark (in all 3 K,M,N dimensions) +const size_t maxsize = 2048; +const size_t minsize = 16; + +typedef MatrixXf MatrixType; +typedef MatrixType::Scalar Scalar; +typedef internal::packet_traits::type Packet; + +static_assert((maxsize & (maxsize - 1)) == 0, "maxsize must be a power of two"); +static_assert((minsize & (minsize - 1)) == 0, "minsize must be a power of two"); +static_assert(maxsize > minsize, "maxsize must be larger than minsize"); +static_assert(maxsize < (minsize << 16), "maxsize must be less than (minsize<<16)"); + +// just a helper to store a triple of K,M,N sizes for matrix product +struct size_triple_t +{ + size_t k, m, n; + size_triple_t() : k(0), m(0), n(0) {} + size_triple_t(size_t _k, size_t _m, size_t _n) : k(_k), m(_m), n(_n) {} + size_triple_t(const size_triple_t& o) : k(o.k), m(o.m), n(o.n) {} + size_triple_t(uint16_t compact) + { + k = 1 << ((compact & 0xf00) >> 8); + m = 1 << ((compact & 0x0f0) >> 4); + n = 1 << ((compact & 0x00f) >> 0); + } +}; + +uint8_t log2_pot(size_t x) { + size_t l = 0; + while (x >>= 1) l++; + return l; +} + +// Convert between size tripes and a compact form fitting in 12 bits +// where each size, which must be a POT, is encoded as its log2, on 4 bits +// so the largest representable size is 2^15 == 32k ... big enough. +uint16_t compact_size_triple(size_t k, size_t m, size_t n) +{ + return (log2_pot(k) << 8) | (log2_pot(m) << 4) | log2_pot(n); +} + +uint16_t compact_size_triple(const size_triple_t& t) +{ + return compact_size_triple(t.k, t.m, t.n); +} + +// A single benchmark. Initially only contains benchmark params. +// Then call run(), which stores the result in the gflops field. +struct benchmark_t +{ + uint16_t compact_product_size; + uint16_t compact_block_size; + bool use_default_block_size; + float gflops; + benchmark_t() + : compact_product_size(0) + , compact_block_size(0) + , use_default_block_size(false) + , gflops(0) + { + } + benchmark_t(size_t pk, size_t pm, size_t pn, + size_t bk, size_t bm, size_t bn) + : compact_product_size(compact_size_triple(pk, pm, pn)) + , compact_block_size(compact_size_triple(bk, bm, bn)) + , use_default_block_size(false) + , gflops(0) + {} + benchmark_t(size_t pk, size_t pm, size_t pn) + : compact_product_size(compact_size_triple(pk, pm, pn)) + , compact_block_size(0) + , use_default_block_size(true) + , gflops(0) + {} + + void run(); +}; + +ostream& operator<<(ostream& s, const benchmark_t& b) +{ + s << hex << b.compact_product_size << dec; + if (b.use_default_block_size) { + size_triple_t t(b.compact_product_size); + Index k = t.k, m = t.m, n = t.n; + internal::computeProductBlockingSizes(k, m, n); + s << " default(" << k << ", " << m << ", " << n << ")"; + } else { + s << " " << hex << b.compact_block_size << dec; + } + s << " " << b.gflops; + return s; +} + +// We sort first by increasing benchmark parameters, +// then by decreasing performance. +bool operator<(const benchmark_t& b1, const benchmark_t& b2) +{ + return b1.compact_product_size < b2.compact_product_size || + (b1.compact_product_size == b2.compact_product_size && ( + (b1.compact_block_size < b2.compact_block_size || ( + b1.compact_block_size == b2.compact_block_size && + b1.gflops > b2.gflops)))); +} + +void benchmark_t::run() +{ + size_triple_t productsizes(compact_product_size); + + if (use_default_block_size) { + eigen_use_specific_block_size = false; + } else { + // feed eigen with our custom blocking params + eigen_use_specific_block_size = true; + size_triple_t blocksizes(compact_block_size); + eigen_block_size_k = blocksizes.k; + eigen_block_size_m = blocksizes.m; + eigen_block_size_n = blocksizes.n; + } + + // set up the matrix pool + + const size_t combined_three_matrices_sizes = + sizeof(Scalar) * + (productsizes.k * productsizes.m + + productsizes.k * productsizes.n + + productsizes.m * productsizes.n); + + // 64 M is large enough that nobody has a cache bigger than that, + // while still being small enough that everybody has this much RAM, + // so conveniently we don't need to special-case platforms here. + const size_t unlikely_large_cache_size = 64 << 20; + + const size_t working_set_size = + min_working_set_size ? min_working_set_size : unlikely_large_cache_size; + + const size_t matrix_pool_size = + 1 + working_set_size / combined_three_matrices_sizes; + + MatrixType *lhs = new MatrixType[matrix_pool_size]; + MatrixType *rhs = new MatrixType[matrix_pool_size]; + MatrixType *dst = new MatrixType[matrix_pool_size]; + + for (size_t i = 0; i < matrix_pool_size; i++) { + lhs[i] = MatrixType::Zero(productsizes.m, productsizes.k); + rhs[i] = MatrixType::Zero(productsizes.k, productsizes.n); + dst[i] = MatrixType::Zero(productsizes.m, productsizes.n); + } + + // main benchmark loop + + int iters_at_a_time = 1; + float time_per_iter = 0.0f; + size_t matrix_index = 0; + while (true) { + + double starttime = timer.getCpuTime(); + for (int i = 0; i < iters_at_a_time; i++) { + dst[matrix_index].noalias() = lhs[matrix_index] * rhs[matrix_index]; + matrix_index++; + if (matrix_index == matrix_pool_size) { + matrix_index = 0; + } + } + double endtime = timer.getCpuTime(); + + const float timing = float(endtime - starttime); + + if (timing >= min_accurate_time) { + time_per_iter = timing / iters_at_a_time; + break; + } + + iters_at_a_time *= 2; + } + + delete[] lhs; + delete[] rhs; + delete[] dst; + + gflops = 2e-9 * productsizes.k * productsizes.m * productsizes.n / time_per_iter; +} + +void print_cpuinfo() +{ +#ifdef __linux__ + cout << "contents of /proc/cpuinfo:" << endl; + string line; + ifstream cpuinfo("/proc/cpuinfo"); + if (cpuinfo.is_open()) { + while (getline(cpuinfo, line)) { + cout << line << endl; + } + cpuinfo.close(); + } + cout << endl; +#elif defined __APPLE__ + cout << "output of sysctl hw:" << endl; + system("sysctl hw"); + cout << endl; +#endif +} + +template +string type_name() +{ + return "unknown"; +} + +template<> +string type_name() +{ + return "float"; +} + +template<> +string type_name() +{ + return "double"; +} + +struct action_t +{ + virtual const char* invokation_name() const { abort(); return nullptr; } + virtual void run() const { abort(); } + virtual ~action_t() {} +}; + +void show_usage_and_exit(int /*argc*/, char* argv[], + const vector>& available_actions) +{ + cerr << "usage: " << argv[0] << " [options...]" << endl << endl; + cerr << "available actions:" << endl << endl; + for (auto it = available_actions.begin(); it != available_actions.end(); ++it) { + cerr << " " << (*it)->invokation_name() << endl; + } + cerr << endl; + cerr << "options:" << endl << endl; + cerr << " --min-working-set-size=N:" << endl; + cerr << " Set the minimum working set size to N bytes." << endl; + cerr << " This is rounded up as needed to a multiple of matrix size." << endl; + cerr << " A larger working set lowers the chance of a warm cache." << endl; + cerr << " The default value 0 means use a large enough working" << endl; + cerr << " set to likely outsize caches." << endl; + cerr << " A value of 1 (that is, 1 byte) would mean don't do anything to" << endl; + cerr << " avoid warm caches." << endl; + exit(1); +} + +float measure_clock_speed() +{ + cerr << "Measuring clock speed... \r" << flush; + + vector all_gflops; + for (int i = 0; i < 8; i++) { + benchmark_t b(1024, 1024, 1024); + b.run(); + all_gflops.push_back(b.gflops); + } + + sort(all_gflops.begin(), all_gflops.end()); + float stable_estimate = all_gflops[2] + all_gflops[3] + all_gflops[4] + all_gflops[5]; + + // multiply by an arbitrary constant to discourage trying doing anything with the + // returned values besides just comparing them with each other. + float result = stable_estimate * 123.456f; + + return result; +} + +struct human_duration_t +{ + int seconds; + human_duration_t(int s) : seconds(s) {} +}; + +ostream& operator<<(ostream& s, const human_duration_t& d) +{ + int remainder = d.seconds; + if (remainder > 3600) { + int hours = remainder / 3600; + s << hours << " h "; + remainder -= hours * 3600; + } + if (remainder > 60) { + int minutes = remainder / 60; + s << minutes << " min "; + remainder -= minutes * 60; + } + if (d.seconds < 600) { + s << remainder << " s"; + } + return s; +} + +const char session_filename[] = "/data/local/tmp/benchmark-blocking-sizes-session.data"; + +void serialize_benchmarks(const char* filename, const vector& benchmarks, size_t first_benchmark_to_run) +{ + FILE* file = fopen(filename, "w"); + if (!file) { + cerr << "Could not open file " << filename << " for writing." << endl; + cerr << "Do you have write permissions on the current working directory?" << endl; + exit(1); + } + size_t benchmarks_vector_size = benchmarks.size(); + fwrite(&max_clock_speed, sizeof(max_clock_speed), 1, file); + fwrite(&benchmarks_vector_size, sizeof(benchmarks_vector_size), 1, file); + fwrite(&first_benchmark_to_run, sizeof(first_benchmark_to_run), 1, file); + fwrite(benchmarks.data(), sizeof(benchmark_t), benchmarks.size(), file); + fclose(file); +} + +bool deserialize_benchmarks(const char* filename, vector& benchmarks, size_t& first_benchmark_to_run) +{ + FILE* file = fopen(filename, "r"); + if (!file) { + return false; + } + if (1 != fread(&max_clock_speed, sizeof(max_clock_speed), 1, file)) { + return false; + } + size_t benchmarks_vector_size = 0; + if (1 != fread(&benchmarks_vector_size, sizeof(benchmarks_vector_size), 1, file)) { + return false; + } + if (1 != fread(&first_benchmark_to_run, sizeof(first_benchmark_to_run), 1, file)) { + return false; + } + benchmarks.resize(benchmarks_vector_size); + if (benchmarks.size() != fread(benchmarks.data(), sizeof(benchmark_t), benchmarks.size(), file)) { + return false; + } + unlink(filename); + return true; +} + +void try_run_some_benchmarks( + vector& benchmarks, + double time_start, + size_t& first_benchmark_to_run) +{ + if (first_benchmark_to_run == benchmarks.size()) { + return; + } + + double time_last_progress_update = 0; + double time_last_clock_speed_measurement = 0; + double time_now = 0; + + size_t benchmark_index = first_benchmark_to_run; + + while (true) { + float ratio_done = float(benchmark_index) / benchmarks.size(); + time_now = timer.getRealTime(); + + // We check clock speed every minute and at the end. + if (benchmark_index == benchmarks.size() || + time_now > time_last_clock_speed_measurement + 60.0f) + { + time_last_clock_speed_measurement = time_now; + + // Ensure that clock speed is as expected + float current_clock_speed = measure_clock_speed(); + + // The tolerance needs to be smaller than the relative difference between + // clock speeds that a device could operate under. + // It seems unlikely that a device would be throttling clock speeds by + // amounts smaller than 2%. + // With a value of 1%, I was getting within noise on a Sandy Bridge. + const float clock_speed_tolerance = 0.02f; + + if (current_clock_speed > (1 + clock_speed_tolerance) * max_clock_speed) { + // Clock speed is now higher than we previously measured. + // Either our initial measurement was inaccurate, which won't happen + // too many times as we are keeping the best clock speed value and + // and allowing some tolerance; or something really weird happened, + // which invalidates all benchmark results collected so far. + // Either way, we better restart all over again now. + if (benchmark_index) { + cerr << "Restarting at " << 100.0f * ratio_done + << " % because clock speed increased. " << endl; + } + max_clock_speed = current_clock_speed; + first_benchmark_to_run = 0; + return; + } + + bool rerun_last_tests = false; + + if (current_clock_speed < (1 - clock_speed_tolerance) * max_clock_speed) { + cerr << "Measurements completed so far: " + << 100.0f * ratio_done + << " % " << endl; + cerr << "Clock speed seems to be only " + << current_clock_speed/max_clock_speed + << " times what it used to be." << endl; + + unsigned int seconds_to_sleep_if_lower_clock_speed = 1; + + while (current_clock_speed < (1 - clock_speed_tolerance) * max_clock_speed) { + if (seconds_to_sleep_if_lower_clock_speed > 32) { + cerr << "Sleeping longer probably won't make a difference." << endl; + cerr << "Serializing benchmarks to " << session_filename << endl; + serialize_benchmarks(session_filename, benchmarks, first_benchmark_to_run); + cerr << "Now restart this benchmark, and it should pick up where we left." << endl; + exit(2); + } + rerun_last_tests = true; + cerr << "Sleeping " + << seconds_to_sleep_if_lower_clock_speed + << " s... \r" << endl; + sleep(seconds_to_sleep_if_lower_clock_speed); + current_clock_speed = measure_clock_speed(); + seconds_to_sleep_if_lower_clock_speed *= 2; + } + } + + if (rerun_last_tests) { + cerr << "Redoing the last " + << 100.0f * float(benchmark_index - first_benchmark_to_run) / benchmarks.size() + << " % because clock speed had been low. " << endl; + return; + } + + // nothing wrong with the clock speed so far, so there won't be a need to rerun + // benchmarks run so far in case we later encounter a lower clock speed. + first_benchmark_to_run = benchmark_index; + } + + if (benchmark_index == benchmarks.size()) { + // We're done! + first_benchmark_to_run = benchmarks.size(); + // Erase progress info + cerr << " " << endl; + return; + } + + // Display progress info on stderr + if (time_now > time_last_progress_update + 1.0f) { + time_last_progress_update = time_now; + cerr << "Measurements... " << 100.0f * ratio_done + << " %, ETA " + << human_duration_t(float(time_now - time_start) * (1.0f - ratio_done) / ratio_done) + << " \r" << flush; + } + + // This is where we actually run a benchmark! + benchmarks[benchmark_index].run(); + benchmark_index++; + } +} + +void run_benchmarks(vector& benchmarks) +{ + size_t first_benchmark_to_run; + vector deserialized_benchmarks; + bool use_deserialized_benchmarks = false; + if (deserialize_benchmarks(session_filename, deserialized_benchmarks, first_benchmark_to_run)) { + cerr << "Found serialized session with " + << 100.0f * first_benchmark_to_run / deserialized_benchmarks.size() + << " % already done" << endl; + if (deserialized_benchmarks.size() == benchmarks.size() && + first_benchmark_to_run > 0 && + first_benchmark_to_run < benchmarks.size()) + { + use_deserialized_benchmarks = true; + } + } + + if (use_deserialized_benchmarks) { + benchmarks = deserialized_benchmarks; + } else { + // not using deserialized benchmarks, starting from scratch + first_benchmark_to_run = 0; + + // Randomly shuffling benchmarks allows us to get accurate enough progress info, + // as now the cheap/expensive benchmarks are randomly mixed so they average out. + // It also means that if data is corrupted for some time span, the odds are that + // not all repetitions of a given benchmark will be corrupted. + random_shuffle(benchmarks.begin(), benchmarks.end()); + } + + for (int i = 0; i < 4; i++) { + max_clock_speed = max(max_clock_speed, measure_clock_speed()); + } + + double time_start = 0.0; + while (first_benchmark_to_run < benchmarks.size()) { + if (first_benchmark_to_run == 0) { + time_start = timer.getRealTime(); + } + try_run_some_benchmarks(benchmarks, + time_start, + first_benchmark_to_run); + } + + // Sort timings by increasing benchmark parameters, and decreasing gflops. + // The latter is very important. It means that we can ignore all but the first + // benchmark with given parameters. + sort(benchmarks.begin(), benchmarks.end()); + + // Collect best (i.e. now first) results for each parameter values. + vector best_benchmarks; + for (auto it = benchmarks.begin(); it != benchmarks.end(); ++it) { + if (best_benchmarks.empty() || + best_benchmarks.back().compact_product_size != it->compact_product_size || + best_benchmarks.back().compact_block_size != it->compact_block_size) + { + best_benchmarks.push_back(*it); + } + } + + // keep and return only the best benchmarks + benchmarks = best_benchmarks; +} + +struct measure_all_pot_sizes_action_t : action_t +{ + virtual const char* invokation_name() const { return "all-pot-sizes"; } + virtual void run() const + { + vector benchmarks; + for (int repetition = 0; repetition < measurement_repetitions; repetition++) { + for (size_t ksize = minsize; ksize <= maxsize; ksize *= 2) { + for (size_t msize = minsize; msize <= maxsize; msize *= 2) { + for (size_t nsize = minsize; nsize <= maxsize; nsize *= 2) { + for (size_t kblock = minsize; kblock <= ksize; kblock *= 2) { + for (size_t mblock = minsize; mblock <= msize; mblock *= 2) { + for (size_t nblock = minsize; nblock <= nsize; nblock *= 2) { + benchmarks.emplace_back(ksize, msize, nsize, kblock, mblock, nblock); + } + } + } + } + } + } + } + + run_benchmarks(benchmarks); + + cout << "BEGIN MEASUREMENTS ALL POT SIZES" << endl; + for (auto it = benchmarks.begin(); it != benchmarks.end(); ++it) { + cout << *it << endl; + } + } +}; + +struct measure_default_sizes_action_t : action_t +{ + virtual const char* invokation_name() const { return "default-sizes"; } + virtual void run() const + { + vector benchmarks; + for (int repetition = 0; repetition < measurement_repetitions; repetition++) { + for (size_t ksize = minsize; ksize <= maxsize; ksize *= 2) { + for (size_t msize = minsize; msize <= maxsize; msize *= 2) { + for (size_t nsize = minsize; nsize <= maxsize; nsize *= 2) { + benchmarks.emplace_back(ksize, msize, nsize); + } + } + } + } + + run_benchmarks(benchmarks); + + cout << "BEGIN MEASUREMENTS DEFAULT SIZES" << endl; + for (auto it = benchmarks.begin(); it != benchmarks.end(); ++it) { + cout << *it << endl; + } + } +}; + +int main(int argc, char* argv[]) +{ + double time_start = timer.getRealTime(); + cout.precision(4); + cerr.precision(4); + + vector> available_actions; + available_actions.emplace_back(new measure_all_pot_sizes_action_t); + available_actions.emplace_back(new measure_default_sizes_action_t); + + auto action = available_actions.end(); + + if (argc <= 1) { + show_usage_and_exit(argc, argv, available_actions); + } + for (auto it = available_actions.begin(); it != available_actions.end(); ++it) { + if (!strcmp(argv[1], (*it)->invokation_name())) { + action = it; + break; + } + } + + if (action == available_actions.end()) { + show_usage_and_exit(argc, argv, available_actions); + } + + for (int i = 2; i < argc; i++) { + if (argv[i] == strstr(argv[i], "--min-working-set-size=")) { + const char* equals_sign = strchr(argv[i], '='); + min_working_set_size = strtoul(equals_sign+1, nullptr, 10); + } else { + cerr << "unrecognized option: " << argv[i] << endl << endl; + show_usage_and_exit(argc, argv, available_actions); + } + } + + print_cpuinfo(); + + cout << "benchmark parameters:" << endl; + cout << "pointer size: " << 8*sizeof(void*) << " bits" << endl; + cout << "scalar type: " << type_name() << endl; + cout << "packet size: " << internal::packet_traits::size << endl; + cout << "minsize = " << minsize << endl; + cout << "maxsize = " << maxsize << endl; + cout << "measurement_repetitions = " << measurement_repetitions << endl; + cout << "min_accurate_time = " << min_accurate_time << endl; + cout << "min_working_set_size = " << min_working_set_size; + if (min_working_set_size == 0) { + cout << " (try to outsize caches)"; + } + cout << endl << endl; + + (*action)->run(); + + double time_end = timer.getRealTime(); + cerr << "Finished in " << human_duration_t(time_end - time_start) << endl; +} diff --git a/ground/gcs/src/libs/eigen/bench/btl/CMakeLists.txt b/ground/gcs/src/libs/eigen/bench/btl/CMakeLists.txt index 119b470d93..38ff9f4832 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/bench/btl/CMakeLists.txt @@ -11,29 +11,24 @@ SET(CMAKE_INCLUDE_CURRENT_DIR ON) string(REGEX MATCH icpc IS_ICPC ${CMAKE_CXX_COMPILER}) IF(CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) - SET(CMAKE_CXX_FLAGS "-g0 -O3 -DNDEBUG") - SET(CMAKE_Fortran_FLAGS "-g0 -O3 -DNDEBUG") - IF(NOT BTL_NOVEC) - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse2") - SET(CMAKE_Fortran_FLAGS "${CMAKE_Fortran_FLAGS} -msse2") - ELSE(NOT BTL_NOVEC) + SET(CMAKE_CXX_FLAGS "-g0 -O3 -DNDEBUG ${CMAKE_CXX_FLAGS}") + SET(CMAKE_Fortran_FLAGS "-g0 -O3 -DNDEBUG ${CMAKE_Fortran_FLAGS}") + IF(BTL_NOVEC) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DEIGEN_DONT_VECTORIZE") - ENDIF(NOT BTL_NOVEC) + ENDIF(BTL_NOVEC) ENDIF(CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) IF(MSVC) SET(CMAKE_CXX_FLAGS " /O2 /Ot /GL /fp:fast -DNDEBUG") # SET(CMAKE_Fortran_FLAGS "-g0 -O3 -DNDEBUG") - IF(NOT BTL_NOVEC) - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:SSE2") - ELSE(NOT BTL_NOVEC) + IF(BTL_NOVEC) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DEIGEN_DONT_VECTORIZE") - ENDIF(NOT BTL_NOVEC) + ENDIF(BTL_NOVEC) ENDIF(MSVC) if(IS_ICPC) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fast") - set(CMAKE_Fortran_FLAGS "${CMAKE_Fortran_FLAGS} -fast") + set(CMAKE_CXX_FLAGS "-fast ${CMAKE_CXX_FLAGS}") + set(CMAKE_Fortran_FLAGS "-fast ${CMAKE_Fortran_FLAGS}") endif(IS_ICPC) include_directories( @@ -48,6 +43,12 @@ include_directories( # set(DEFAULT_LIBRARIES ${MKL_LIBRARIES}) # endif (MKL_FOUND) +find_library(EIGEN_BTL_RT_LIBRARY rt) +# if we cannot find it easily, then we don't need it! +if(NOT EIGEN_BTL_RT_LIBRARY) + set(EIGEN_BTL_RT_LIBRARY "") +endif() + MACRO(BTL_ADD_BENCH targetname) foreach(_current_var ${ARGN}) @@ -70,7 +71,7 @@ MACRO(BTL_ADD_BENCH targetname) IF(BUILD_${targetname}) ADD_EXECUTABLE(${targetname} ${_sources}) ADD_TEST(${targetname} "${targetname}") - target_link_libraries(${targetname} ${DEFAULT_LIBRARIES} rt) + target_link_libraries(${targetname} ${DEFAULT_LIBRARIES} ${EIGEN_BTL_RT_LIBRARY}) ENDIF(BUILD_${targetname}) ENDMACRO(BTL_ADD_BENCH) @@ -91,6 +92,7 @@ ENABLE_TESTING() add_subdirectory(libs/eigen3) add_subdirectory(libs/eigen2) +add_subdirectory(libs/tensors) add_subdirectory(libs/BLAS) add_subdirectory(libs/ublas) add_subdirectory(libs/gmm) @@ -98,6 +100,7 @@ add_subdirectory(libs/mtl4) add_subdirectory(libs/blitz) add_subdirectory(libs/tvmet) add_subdirectory(libs/STL) +add_subdirectory(libs/blaze) add_subdirectory(data) diff --git a/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpby.hh b/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpby.hh index 98511ab6a5..dadd0ccf3c 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpby.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpby.hh @@ -33,7 +33,7 @@ class Action_axpby { public : // Ctor - Action_axpby( int size ):_size(size),_alpha(0.5),_beta(0.95) + Action_axpby( int size ):_alpha(0.5),_beta(0.95),_size(size) { MESSAGE("Action_axpby Ctor"); diff --git a/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpy.hh b/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpy.hh index e4cb3a5bd6..261be4cb8e 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpy.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/actions/action_axpy.hh @@ -35,7 +35,7 @@ public : // Ctor - Action_axpy( int size ):_size(size),_coef(1.0) + Action_axpy( int size ):_coef(1.0),_size(size) { MESSAGE("Action_axpy Ctor"); diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindACML.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindACML.cmake index f45ae1b0de..4989fa2f4c 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindACML.cmake +++ b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindACML.cmake @@ -17,6 +17,7 @@ find_file(ACML_LIBRARIES libacml_mp.so PATHS /usr/lib + /usr/lib64 $ENV{ACMLDIR}/lib ${LIB_INSTALL_DIR} ) @@ -35,6 +36,7 @@ if(NOT ACML_LIBRARIES) libacml.so libacml_mv.so PATHS /usr/lib + /usr/lib64 $ENV{ACMLDIR}/lib ${LIB_INSTALL_DIR} ) diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindATLAS.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindATLAS.cmake index 6b90652066..4136a989d6 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindATLAS.cmake +++ b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindATLAS.cmake @@ -3,33 +3,25 @@ if (ATLAS_LIBRARIES) set(ATLAS_FIND_QUIETLY TRUE) endif (ATLAS_LIBRARIES) -find_file(ATLAS_LIB libatlas.so.3 PATHS /usr/lib $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) -find_library(ATLAS_LIB atlas PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) +find_file(ATLAS_LIB libatlas.so.3 PATHS /usr/lib /usr/lib/atlas /usr/lib64 /usr/lib64/atlas $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) +find_library(ATLAS_LIB satlas PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) -find_file(ATLAS_CBLAS libcblas.so.3 PATHS /usr/lib $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) -find_library(ATLAS_CBLAS cblas PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) +find_file(ATLAS_LAPACK NAMES liblapack_atlas.so.3 liblapack.so.3 PATHS /usr/lib /usr/lib/atlas /usr/lib64 /usr/lib64/atlas $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) +find_library(ATLAS_LAPACK NAMES lapack_atlas lapack PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) -find_file(ATLAS_LAPACK liblapack_atlas.so.3 PATHS /usr/lib $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) -find_library(ATLAS_LAPACK lapack_atlas PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) - -if(NOT ATLAS_LAPACK) - find_file(ATLAS_LAPACK liblapack.so.3 PATHS /usr/lib/atlas $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) - find_library(ATLAS_LAPACK lapack PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) -endif(NOT ATLAS_LAPACK) - -find_file(ATLAS_F77BLAS libf77blas.so.3 PATHS /usr/lib $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) +find_file(ATLAS_F77BLAS libf77blas.so.3 PATHS /usr/lib /usr/lib/atlas /usr/lib64 /usr/lib64/atlas $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) find_library(ATLAS_F77BLAS f77blas PATHS $ENV{ATLASDIR} ${LIB_INSTALL_DIR}) if(ATLAS_LIB AND ATLAS_CBLAS AND ATLAS_LAPACK AND ATLAS_F77BLAS) - set(ATLAS_LIBRARIES ${ATLAS_LAPACK} ${ATLAS_CBLAS} ${ATLAS_F77BLAS} ${ATLAS_LIB}) + set(ATLAS_LIBRARIES ${ATLAS_LAPACK} ${ATLAS_LIB}) # search the default lapack lib link to it find_file(ATLAS_REFERENCE_LAPACK liblapack.so.3 PATHS /usr/lib /usr/lib64) find_library(ATLAS_REFERENCE_LAPACK NAMES lapack) - if(ATLAS_REFERENCE_LAPACK) - set(ATLAS_LIBRARIES ${ATLAS_LIBRARIES} ${ATLAS_REFERENCE_LAPACK}) - endif() +# if(ATLAS_REFERENCE_LAPACK) +# set(ATLAS_LIBRARIES ${ATLAS_LIBRARIES} ${ATLAS_REFERENCE_LAPACK}) +# endif() endif(ATLAS_LIB AND ATLAS_CBLAS AND ATLAS_LAPACK AND ATLAS_F77BLAS) diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindBLAZE.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindBLAZE.cmake new file mode 100644 index 0000000000..dba4c89f2d --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindBLAZE.cmake @@ -0,0 +1,31 @@ +# - Try to find eigen2 headers +# Once done this will define +# +# BLAZE_FOUND - system has blaze lib +# BLAZE_INCLUDE_DIR - the blaze include directory +# +# Copyright (C) 2008 Gael Guennebaud +# Adapted from FindEigen.cmake: +# Copyright (c) 2006, 2007 Montel Laurent, +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + +if (BLAZE_INCLUDE_DIR) + + # in cache already + set(BLAZE_FOUND TRUE) + +else (BLAZE_INCLUDE_DIR) + +find_path(BLAZE_INCLUDE_DIR NAMES blaze/Blaze.h + PATHS + ${INCLUDE_INSTALL_DIR} + ) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(BLAZE DEFAULT_MSG BLAZE_INCLUDE_DIR) + +mark_as_advanced(BLAZE_INCLUDE_DIR) + +endif(BLAZE_INCLUDE_DIR) + diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindCBLAS.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindCBLAS.cmake index 554f0291b4..ce0f2f2b2d 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindCBLAS.cmake +++ b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindCBLAS.cmake @@ -23,6 +23,7 @@ find_file(CBLAS_LIBRARIES libcblas.so.3 PATHS /usr/lib + /usr/lib64 $ENV{CBLASDIR}/lib ${LIB_INSTALL_DIR} ) diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindGOTO.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindGOTO.cmake deleted file mode 100644 index 67ea0934a5..0000000000 --- a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindGOTO.cmake +++ /dev/null @@ -1,15 +0,0 @@ - -if (GOTO_LIBRARIES) - set(GOTO_FIND_QUIETLY TRUE) -endif (GOTO_LIBRARIES) - -find_library(GOTO_LIBRARIES goto PATHS $ENV{GOTODIR} ${LIB_INSTALL_DIR}) - -if(GOTO_LIBRARIES AND CMAKE_COMPILER_IS_GNUCXX) - set(GOTO_LIBRARIES ${GOTO_LIBRARIES} "-lpthread -lgfortran") -endif() - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(GOTO DEFAULT_MSG GOTO_LIBRARIES) - -mark_as_advanced(GOTO_LIBRARIES) diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindGOTO2.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindGOTO2.cmake deleted file mode 100644 index baa68d2136..0000000000 --- a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindGOTO2.cmake +++ /dev/null @@ -1,25 +0,0 @@ - -if (GOTO2_LIBRARIES) - set(GOTO2_FIND_QUIETLY TRUE) -endif (GOTO2_LIBRARIES) -# -# find_path(GOTO_INCLUDES -# NAMES -# cblas.h -# PATHS -# $ENV{GOTODIR}/include -# ${INCLUDE_INSTALL_DIR} -# ) - -find_file(GOTO2_LIBRARIES libgoto2.so PATHS /usr/lib $ENV{GOTO2DIR} ${LIB_INSTALL_DIR}) -find_library(GOTO2_LIBRARIES goto2 PATHS $ENV{GOTO2DIR} ${LIB_INSTALL_DIR}) - -if(GOTO2_LIBRARIES AND CMAKE_COMPILER_IS_GNUCXX) - set(GOTO2_LIBRARIES ${GOTO2_LIBRARIES} "-lpthread -lgfortran") -endif() - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(GOTO2 DEFAULT_MSG - GOTO2_LIBRARIES) - -mark_as_advanced(GOTO2_LIBRARIES) diff --git a/ground/gcs/src/libs/eigen/bench/btl/cmake/FindOPENBLAS.cmake b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindOPENBLAS.cmake new file mode 100644 index 0000000000..2a09194364 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/cmake/FindOPENBLAS.cmake @@ -0,0 +1,17 @@ + +if (OPENBLAS_LIBRARIES) + set(OPENBLAS_FIND_QUIETLY TRUE) +endif (OPENBLAS_LIBRARIES) + +find_file(OPENBLAS_LIBRARIES NAMES libopenblas.so libopenblas.so.0 PATHS /usr/lib /usr/lib64 $ENV{OPENBLASDIR} ${LIB_INSTALL_DIR}) +find_library(OPENBLAS_LIBRARIES openblas PATHS $ENV{OPENBLASDIR} ${LIB_INSTALL_DIR}) + +if(OPENBLAS_LIBRARIES AND CMAKE_COMPILER_IS_GNUCXX) + set(OPENBLAS_LIBRARIES ${OPENBLAS_LIBRARIES} "-lpthread -lgfortran") +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OPENBLAS DEFAULT_MSG + OPENBLAS_LIBRARIES) + +mark_as_advanced(OPENBLAS_LIBRARIES) diff --git a/ground/gcs/src/libs/eigen/bench/btl/data/action_settings.txt b/ground/gcs/src/libs/eigen/bench/btl/data/action_settings.txt index e32213e22d..39d2b5dc48 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/data/action_settings.txt +++ b/ground/gcs/src/libs/eigen/bench/btl/data/action_settings.txt @@ -1,19 +1,19 @@ -aat ; "{/*1.5 A x A^T}" ; "matrix size" ; 4:3000 -ata ; "{/*1.5 A^T x A}" ; "matrix size" ; 4:3000 -atv ; "{/*1.5 matrix^T x vector}" ; "matrix size" ; 4:3000 +aat ; "{/*1.5 A x A^T}" ; "matrix size" ; 4:5000 +ata ; "{/*1.5 A^T x A}" ; "matrix size" ; 4:5000 +atv ; "{/*1.5 matrix^T x vector}" ; "matrix size" ; 4:5000 axpby ; "{/*1.5 Y = alpha X + beta Y}" ; "vector size" ; 5:1000000 axpy ; "{/*1.5 Y += alpha X}" ; "vector size" ; 5:1000000 -matrix_matrix ; "{/*1.5 matrix matrix product}" ; "matrix size" ; 4:3000 -matrix_vector ; "{/*1.5 matrix vector product}" ; "matrix size" ; 4:3000 -trmm ; "{/*1.5 triangular matrix matrix product}" ; "matrix size" ; 4:3000 -trisolve_vector ; "{/*1.5 triangular solver - vector (X = inv(L) X)}" ; "size" ; 4:3000 -trisolve_matrix ; "{/*1.5 triangular solver - matrix (M = inv(L) M)}" ; "size" ; 4:3000 -cholesky ; "{/*1.5 Cholesky decomposition}" ; "matrix size" ; 4:3000 -complete_lu_decomp ; "{/*1.5 Complete LU decomposition}" ; "matrix size" ; 4:3000 -partial_lu_decomp ; "{/*1.5 Partial LU decomposition}" ; "matrix size" ; 4:3000 -tridiagonalization ; "{/*1.5 Tridiagonalization}" ; "matrix size" ; 4:3000 -hessenberg ; "{/*1.5 Hessenberg decomposition}" ; "matrix size" ; 4:3000 -symv ; "{/*1.5 symmetric matrix vector product}" ; "matrix size" ; 4:3000 -syr2 ; "{/*1.5 symmretric rank-2 update (A += u^T v + u v^T)}" ; "matrix size" ; 4:3000 -ger ; "{/*1.5 general rank-1 update (A += u v^T)}" ; "matrix size" ; 4:3000 -rot ; "{/*1.5 apply rotation in the plane}" ; "vector size" ; 4:1000000 \ No newline at end of file +matrix_matrix ; "{/*1.5 matrix matrix product}" ; "matrix size" ; 4:5000 +matrix_vector ; "{/*1.5 matrix vector product}" ; "matrix size" ; 4:5000 +trmm ; "{/*1.5 triangular matrix matrix product}" ; "matrix size" ; 4:5000 +trisolve_vector ; "{/*1.5 triangular solver - vector (X = inv(L) X)}" ; "size" ; 4:5000 +trisolve_matrix ; "{/*1.5 triangular solver - matrix (M = inv(L) M)}" ; "size" ; 4:5000 +cholesky ; "{/*1.5 Cholesky decomposition}" ; "matrix size" ; 4:5000 +complete_lu_decomp ; "{/*1.5 Complete LU decomposition}" ; "matrix size" ; 4:5000 +partial_lu_decomp ; "{/*1.5 Partial LU decomposition}" ; "matrix size" ; 4:5000 +tridiagonalization ; "{/*1.5 Tridiagonalization}" ; "matrix size" ; 4:5000 +hessenberg ; "{/*1.5 Hessenberg decomposition}" ; "matrix size" ; 4:5000 +symv ; "{/*1.5 symmetric matrix vector product}" ; "matrix size" ; 4:5000 +syr2 ; "{/*1.5 symmretric rank-2 update (A += u^T v + u v^T)}" ; "matrix size" ; 4:5000 +ger ; "{/*1.5 general rank-1 update (A += u v^T)}" ; "matrix size" ; 4:5000 +rot ; "{/*1.5 apply rotation in the plane}" ; "vector size" ; 4:1000000 diff --git a/ground/gcs/src/libs/eigen/bench/btl/data/perlib_plot_settings.txt b/ground/gcs/src/libs/eigen/bench/btl/data/perlib_plot_settings.txt index 6844bab289..f023cfe02f 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/data/perlib_plot_settings.txt +++ b/ground/gcs/src/libs/eigen/bench/btl/data/perlib_plot_settings.txt @@ -10,7 +10,7 @@ ublas ; with lines lw 3 lt 1 lc rgbcolor "#00b7ff" mtl4 ; with lines lw 3 lt 1 lc rgbcolor "#d18847" blitz ; with lines lw 3 lt 1 lc rgbcolor "#ff00ff" F77 ; with lines lw 3 lt 3 lc rgbcolor "#e6e64c" -GOTO ; with lines lw 3 lt 3 lc rgbcolor "#C05600" -GOTO2 ; with lines lw 3 lt 1 lc rgbcolor "#C05600" +OPENBLAS ; with lines lw 3 lt 1 lc rgbcolor "#C05600" C ; with lines lw 3 lt 3 lc rgbcolor "#e6bd96" ACML ; with lines lw 2 lt 3 lc rgbcolor "#e6e64c" +blaze ; with lines lw 3 lt 1 lc rgbcolor "#ff00ff" diff --git a/ground/gcs/src/libs/eigen/bench/btl/generic_bench/bench.hh b/ground/gcs/src/libs/eigen/bench/btl/generic_bench/bench.hh index 005c363955..7b7b951b50 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/generic_bench/bench.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/generic_bench/bench.hh @@ -102,8 +102,8 @@ BTL_DONT_INLINE void bench( int size_min, int size_max, int nb_point ) // merge the two data std::vector newSizes; std::vector newFlops; - int i=0; - int j=0; + unsigned int i=0; + unsigned int j=0; while (i config = BtlString(_config).split(" \t\n"); - for (int i = 0; i BTL_DONT_INLINE void init_matrix(Vector & A, int size){ A.resize(size); - for (int row=0; row(A[row],size,row); } } @@ -50,11 +50,11 @@ BTL_DONT_INLINE void init_matrix(Vector & A, int size){ template BTL_DONT_INLINE void init_matrix_symm(Matrix& A, int size){ A.resize(size); - for (int row=0; row @@ -87,6 +87,48 @@ }; // Portable_Timer +#elif defined(__APPLE__) +#include +#include + + +class Portable_Timer +{ + public: + + Portable_Timer() + { + } + + void start() + { + m_start_time = double(mach_absolute_time())*1e-9;; + + } + + void stop() + { + m_stop_time = double(mach_absolute_time())*1e-9;; + + } + + double elapsed() + { + return user_time(); + } + + double user_time() + { + return m_stop_time - m_start_time; + } + + +private: + + double m_stop_time, m_start_time; + +}; // Portable_Timer (Apple) + #else #include @@ -138,7 +180,7 @@ private: int m_clkid; double m_stop_time, m_start_time; -}; // Portable_Timer +}; // Portable_Timer (Linux) #endif diff --git a/ground/gcs/src/libs/eigen/bench/btl/generic_bench/utils/size_lin_log.hh b/ground/gcs/src/libs/eigen/bench/btl/generic_bench/utils/size_lin_log.hh index bca3932aea..bbc9f543df 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/generic_bench/utils/size_lin_log.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/generic_bench/utils/size_lin_log.hh @@ -23,7 +23,7 @@ #include "size_log.hh" template -void size_lin_log(const int nb_point, const int size_min, const int size_max, Vector & X) +void size_lin_log(const int nb_point, const int /*size_min*/, const int size_max, Vector & X) { int ten=10; int nine=9; diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/CMakeLists.txt b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/CMakeLists.txt index de42fe047c..0272ccad07 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/CMakeLists.txt @@ -18,27 +18,14 @@ if (MKL_FOUND) endif (MKL_FOUND) -find_package(GOTO2) -if (GOTO2_FOUND) - btl_add_bench(btl_goto2 main.cpp) - if(BUILD_btl_goto2) - target_link_libraries(btl_goto2 ${GOTO_LIBRARIES} ) - set_target_properties(btl_goto2 PROPERTIES COMPILE_FLAGS "-DCBLASNAME=GOTO2") - endif(BUILD_btl_goto2) -endif (GOTO2_FOUND) - -find_package(GOTO) -if (GOTO_FOUND) - if(GOTO2_FOUND) - btl_add_bench(btl_goto main.cpp OFF) - else() - btl_add_bench(btl_goto main.cpp) - endif() - if(BUILD_btl_goto) - target_link_libraries(btl_goto ${GOTO_LIBRARIES} ) - set_target_properties(btl_goto PROPERTIES COMPILE_FLAGS "-DCBLASNAME=GOTO") - endif(BUILD_btl_goto) -endif (GOTO_FOUND) +find_package(OPENBLAS) +if (OPENBLAS_FOUND) + btl_add_bench(btl_openblas main.cpp) + if(BUILD_btl_openblas) + target_link_libraries(btl_openblas ${OPENBLAS_LIBRARIES} ) + set_target_properties(btl_openblas PROPERTIES COMPILE_FLAGS "-DCBLASNAME=OPENBLAS") + endif(BUILD_btl_openblas) +endif (OPENBLAS_FOUND) find_package(ACML) if (ACML_FOUND) diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/blas_interface_impl.hh b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/blas_interface_impl.hh index 0e84df0386..fc4ba2a1f3 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/blas_interface_impl.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/blas_interface_impl.hh @@ -75,7 +75,6 @@ public : static inline void partial_lu_decomp(const gene_matrix & X, gene_matrix & C, int N){ int N2 = N*N; BLAS_FUNC(copy)(&N2, X, &intone, C, &intone); - char uplo = 'L'; int info = 0; int * ipiv = (int*)alloca(sizeof(int)*N); BLAS_FUNC(getrf)(&N, &N, C, &N, ipiv, &info); @@ -92,7 +91,7 @@ public : BLAS_FUNC(trsm)(&right, &lower, ¬rans, &nonunit, &N, &N, &fone, L, &N, X, &N); } - static inline void trmm(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N){ + static inline void trmm(gene_matrix & A, gene_matrix & B, gene_matrix & /*X*/, int N){ BLAS_FUNC(trmm)(&left, &lower, ¬rans,&nonunit, &N,&N,&fone,A,&N,B,&N); } @@ -101,7 +100,6 @@ public : static inline void lu_decomp(const gene_matrix & X, gene_matrix & C, int N){ int N2 = N*N; BLAS_FUNC(copy)(&N2, X, &intone, C, &intone); - char uplo = 'L'; int info = 0; int * ipiv = (int*)alloca(sizeof(int)*N); int * jpiv = (int*)alloca(sizeof(int)*N); @@ -134,8 +132,6 @@ public : } char uplo = 'U'; int info = 0; - int ilo = 1; - int ihi = N; int bsize = 64; int worksize = N*bsize; SCALAR* d = new SCALAR[3*N+worksize]; diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/c_interface_base.h b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/c_interface_base.h index 515d8dcfc3..de613803b7 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/c_interface_base.h +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/c_interface_base.h @@ -17,12 +17,12 @@ template class c_interface_base typedef real* gene_matrix; typedef real* gene_vector; - static void free_matrix(gene_matrix & A, int N){ - delete A; + static void free_matrix(gene_matrix & A, int /*N*/){ + delete[] A; } static void free_vector(gene_vector & B){ - delete B; + delete[] B; } static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){ diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/main.cpp b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/main.cpp index 8347c9f0ba..564d55ef23 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/main.cpp +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/BLAS/main.cpp @@ -56,13 +56,13 @@ int main() bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); #ifdef HAS_LAPACK - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); +// bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); #endif //bench > >(MIN_LU,MAX_LU,NB_POINT); diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/STL/STL_interface.hh b/ground/gcs/src/libs/eigen/bench/btl/libs/STL/STL_interface.hh index 93e76bd553..ef4cc92330 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/STL/STL_interface.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/STL/STL_interface.hh @@ -44,9 +44,9 @@ public : return "STL"; } - static void free_matrix(gene_matrix & A, int N){} + static void free_matrix(gene_matrix & /*A*/, int /*N*/){} - static void free_vector(gene_vector & B){} + static void free_vector(gene_vector & /*B*/){} static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){ A = A_stl; diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/CMakeLists.txt b/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/CMakeLists.txt new file mode 100644 index 0000000000..e99a0855c9 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/CMakeLists.txt @@ -0,0 +1,13 @@ + +find_package(BLAZE) +find_package(Boost COMPONENTS system) +if (BLAZE_FOUND AND Boost_FOUND) + include_directories(${BLAZE_INCLUDE_DIR} ${Boost_INCLUDE_DIRS}) + btl_add_bench(btl_blaze main.cpp) + # Note: The newest blaze version requires C++14. + # Ideally, we should set this depending on the version of Blaze we found + set_property(TARGET btl_blaze PROPERTY CXX_STANDARD 14) + if(BUILD_btl_blaze) + target_link_libraries(btl_blaze ${Boost_LIBRARIES}) + endif() +endif () diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/blaze_interface.hh b/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/blaze_interface.hh new file mode 100644 index 0000000000..ee15239441 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/blaze_interface.hh @@ -0,0 +1,140 @@ +//===================================================== +// Copyright (C) 2008 Gael Guennebaud +//===================================================== +// +// 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 2 +// 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 BLAZE_INTERFACE_HH +#define BLAZE_INTERFACE_HH + +#include +#include +// using namespace blaze; + +#include + +template +class blaze_interface { + +public : + + typedef real real_type ; + + typedef std::vector stl_vector; + typedef std::vector stl_matrix; + + typedef blaze::DynamicMatrix gene_matrix; + typedef blaze::DynamicVector gene_vector; + + static inline std::string name() { return "blaze"; } + + static void free_matrix(gene_matrix & A, int N){ + return ; + } + + static void free_vector(gene_vector & B){ + return ; + } + + static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){ + A.resize(A_stl[0].size(), A_stl.size()); + + for (int j=0; j ipvt(N); +// lu_factor(R, ipvt); +// } + +// static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector & X, int N){ +// X = lower_trisolve(L, B); +// } + + static inline void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){ + cible = source; + } + + static inline void copy_vector(const gene_vector & source, gene_vector & cible, int N){ + cible = source; + } + +}; + +#endif diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/main.cpp b/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/main.cpp new file mode 100644 index 0000000000..80e8f4eaa0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/blaze/main.cpp @@ -0,0 +1,40 @@ +//===================================================== +// Copyright (C) 2008 Gael Guennebaud +//===================================================== +// +// 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 2 +// 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 "utilities.h" +#include "blaze_interface.hh" +#include "bench.hh" +#include "basic_actions.hh" + +BTL_MAIN; + +int main() +{ + + bench > >(MIN_AXPY,MAX_AXPY,NB_POINT); + bench > >(MIN_AXPY,MAX_AXPY,NB_POINT); + + bench > >(MIN_MV,MAX_MV,NB_POINT); + bench > >(MIN_MV,MAX_MV,NB_POINT); +// bench > >(MIN_MM,MAX_MM,NB_POINT); +// bench > >(MIN_MM,MAX_MM,NB_POINT); +// bench > >(MIN_MM,MAX_MM,NB_POINT); + + return 0; +} + + diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/eigen2/eigen2_interface.hh b/ground/gcs/src/libs/eigen/bench/btl/libs/eigen2/eigen2_interface.hh index 47fe581357..1deabdae21 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/eigen2/eigen2_interface.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/eigen2/eigen2_interface.hh @@ -47,7 +47,7 @@ public : { #if defined(EIGEN_VECTORIZE_SSE) if (SIZE==Dynamic) return "eigen2"; else return "tiny_eigen2"; - #elif defined(EIGEN_VECTORIZE_ALTIVEC) + #elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX) if (SIZE==Dynamic) return "eigen2"; else return "tiny_eigen2"; #else if (SIZE==Dynamic) return "eigen2_novec"; else return "tiny_eigen2_novec"; diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/eigen3_interface.hh b/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/eigen3_interface.hh index 31bcc1f933..b821fd7211 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/eigen3_interface.hh +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/eigen3_interface.hh @@ -45,15 +45,15 @@ public : return EIGEN_MAKESTRING(BTL_PREFIX); } - static void free_matrix(gene_matrix & A, int N) {} + static void free_matrix(gene_matrix & /*A*/, int /*N*/) {} - static void free_vector(gene_vector & B) {} + static void free_vector(gene_vector & /*B*/) {} static BTL_DONT_INLINE void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){ A.resize(A_stl[0].size(), A_stl.size()); - for (int j=0; j().setZero(); X.template selfadjointView().rankUpdate(A); } - static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N){ + static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int /*N*/){ X.noalias() = A*B; } - static inline void symv(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N){ + static inline void symv(const gene_matrix & A, const gene_vector & B, gene_vector & X, int /*N*/){ X.noalias() = (A.template selfadjointView() * B); // internal::product_selfadjoint_vector(N,A.data(),N, B.data(), 1, X.data(), 1); } @@ -155,54 +155,54 @@ public : } } - static EIGEN_DONT_INLINE void syr2(gene_matrix & A, gene_vector & X, gene_vector & Y, int N){ + static EIGEN_DONT_INLINE void syr2(gene_matrix & A, gene_vector & X, gene_vector & Y, int N){ // internal::product_selfadjoint_rank2_update(N,A.data(),N, X.data(), 1, Y.data(), 1, -1); for(int j=0; j(c,s)); } - static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){ + static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int /*N*/){ X.noalias() = (A.transpose()*B); } - static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int N){ + static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int /*N*/){ Y += coef * X; } - static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int N){ + static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int /*N*/){ Y = a*X + b*Y; } - static EIGEN_DONT_INLINE void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){ + static EIGEN_DONT_INLINE void copy_matrix(const gene_matrix & source, gene_matrix & cible, int /*N*/){ cible = source; } - static EIGEN_DONT_INLINE void copy_vector(const gene_vector & source, gene_vector & cible, int N){ + static EIGEN_DONT_INLINE void copy_vector(const gene_vector & source, gene_vector & cible, int /*N*/){ cible = source; } - static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector& X, int N){ + static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector& X, int /*N*/){ X = L.template triangularView().solve(B); } - static inline void trisolve_lower_matrix(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int N){ + static inline void trisolve_lower_matrix(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int /*N*/){ X = L.template triangularView().solve(B); } - static inline void trmm(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int N){ + static inline void trmm(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int /*N*/){ X.noalias() = L.template triangularView() * B; } - static inline void cholesky(const gene_matrix & X, gene_matrix & C, int N){ + static inline void cholesky(const gene_matrix & X, gene_matrix & C, int /*N*/){ C = X; internal::llt_inplace::blocked(C); //C = X.llt().matrixL(); @@ -211,11 +211,11 @@ public : // Cholesky::computeInPlaceBlock(C); } - static inline void lu_decomp(const gene_matrix & X, gene_matrix & C, int N){ + static inline void lu_decomp(const gene_matrix & X, gene_matrix & C, int /*N*/){ C = X.fullPivLu().matrixLU(); } - static inline void partial_lu_decomp(const gene_matrix & X, gene_matrix & C, int N){ + static inline void partial_lu_decomp(const gene_matrix & X, gene_matrix & C, int N){ Matrix piv(N); DenseIndex nb; C = X; @@ -223,13 +223,13 @@ public : // C = X.partialPivLu().matrixLU(); } - static inline void tridiagonalization(const gene_matrix & X, gene_matrix & C, int N){ + static inline void tridiagonalization(const gene_matrix & X, gene_matrix & C, int N){ typename Tridiagonalization::CoeffVectorType aux(N-1); C = X; internal::tridiagonalization_inplace(C, aux); } - static inline void hessenberg(const gene_matrix & X, gene_matrix & C, int N){ + static inline void hessenberg(const gene_matrix & X, gene_matrix & C, int /*N*/){ C = HessenbergDecomposition(X).packedMatrix(); } diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/main_adv.cpp b/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/main_adv.cpp index efe5857e48..95865357e0 100644 --- a/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/main_adv.cpp +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/eigen3/main_adv.cpp @@ -29,14 +29,14 @@ BTL_MAIN; int main() { - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); +// bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); - bench > >(MIN_MM,MAX_MM,NB_POINT); +// bench > >(MIN_LU,MAX_LU,NB_POINT); + bench > >(MIN_LU,MAX_LU,NB_POINT); return 0; } diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/CMakeLists.txt b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/CMakeLists.txt new file mode 100644 index 0000000000..09d6d8e43e --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/CMakeLists.txt @@ -0,0 +1,44 @@ + + +if((NOT TENSOR_INCLUDE_DIR) AND Eigen_SOURCE_DIR) + # unless TENSOR_INCLUDE_DIR is defined, let's use current Eigen version + set(TENSOR_INCLUDE_DIR ${Eigen_SOURCE_DIR}) + set(TENSOR_FOUND TRUE) +else() + find_package(Tensor) +endif() + +if (TENSOR_FOUND) + + include_directories(${TENSOR_INCLUDE_DIR}) + btl_add_bench(btl_tensor_linear main_linear.cpp) + btl_add_bench(btl_tensor_vecmat main_vecmat.cpp) + btl_add_bench(btl_tensor_matmat main_matmat.cpp) + + btl_add_target_property(btl_tensor_linear COMPILE_FLAGS "-fno-exceptions -DBTL_PREFIX=tensor") + btl_add_target_property(btl_tensor_vecmat COMPILE_FLAGS "-fno-exceptions -DBTL_PREFIX=tensor") + btl_add_target_property(btl_tensor_matmat COMPILE_FLAGS "-fno-exceptions -DBTL_PREFIX=tensor") + + option(BTL_BENCH_NOGCCVEC "also bench Eigen explicit vec without GCC's auto vec" OFF) + if(CMAKE_COMPILER_IS_GNUCXX AND BTL_BENCH_NOGCCVEC) + btl_add_bench(btl_tensor_nogccvec_linear main_linear.cpp) + btl_add_bench(btl_tensor_nogccvec_vecmat main_vecmat.cpp) + btl_add_bench(btl_tensor_nogccvec_matmat main_matmat.cpp) + + btl_add_target_property(btl_tensor_nogccvec_linear COMPILE_FLAGS "-fno-exceptions -fno-tree-vectorize -DBTL_PREFIX=tensor_nogccvec") + btl_add_target_property(btl_tensor_nogccvec_vecmat COMPILE_FLAGS "-fno-exceptions -fno-tree-vectorize -DBTL_PREFIX=tensor_nogccvec") + btl_add_target_property(btl_tensor_nogccvec_matmat COMPILE_FLAGS "-fno-exceptions -fno-tree-vectorize -DBTL_PREFIX=tensor_nogccvec") + endif() + + + if(NOT BTL_NOVEC) + btl_add_bench(btl_tensor_novec_linear main_linear.cpp OFF) + btl_add_bench(btl_tensor_novec_vecmat main_vecmat.cpp OFF) + btl_add_bench(btl_tensor_novec_matmat main_matmat.cpp OFF) + btl_add_target_property(btl_tensor_novec_linear COMPILE_FLAGS "-fno-exceptions -DEIGEN_DONT_VECTORIZE -DBTL_PREFIX=tensor_novec") + btl_add_target_property(btl_tensor_novec_vecmat COMPILE_FLAGS "-fno-exceptions -DEIGEN_DONT_VECTORIZE -DBTL_PREFIX=tensor_novec") + btl_add_target_property(btl_tensor_novec_matmat COMPILE_FLAGS "-fno-exceptions -DEIGEN_DONT_VECTORIZE -DBTL_PREFIX=tensor_novec") + + endif(NOT BTL_NOVEC) + +endif (TENSOR_FOUND) diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_linear.cpp b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_linear.cpp new file mode 100644 index 0000000000..e257f1e72e --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_linear.cpp @@ -0,0 +1,23 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "utilities.h" +#include "tensor_interface.hh" +#include "bench.hh" +#include "basic_actions.hh" + +BTL_MAIN; + +int main() +{ + bench > >(MIN_AXPY,MAX_AXPY,NB_POINT); + bench > >(MIN_AXPY,MAX_AXPY,NB_POINT); + + return 0; +} diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_matmat.cpp b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_matmat.cpp new file mode 100644 index 0000000000..675fcfc6d5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_matmat.cpp @@ -0,0 +1,21 @@ +//===================================================== +// Copyright (C) 2014 Benoit Steiner +//===================================================== +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +// +#include "utilities.h" +#include "tensor_interface.hh" +#include "bench.hh" +#include "basic_actions.hh" + +BTL_MAIN; + +int main() +{ + bench > >(MIN_MM,MAX_MM,NB_POINT); + + return 0; +} diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_vecmat.cpp b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_vecmat.cpp new file mode 100644 index 0000000000..1af00c81b4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/main_vecmat.cpp @@ -0,0 +1,21 @@ +//===================================================== +// Copyright (C) 2014 Benoit Steiner +//===================================================== +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +// +#include "utilities.h" +#include "tensor_interface.hh" +#include "bench.hh" +#include "basic_actions.hh" + +BTL_MAIN; + +int main() +{ + bench > >(MIN_MV,MAX_MV,NB_POINT); + + return 0; +} diff --git a/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/tensor_interface.hh b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/tensor_interface.hh new file mode 100644 index 0000000000..97b8e0f0b3 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/btl/libs/tensors/tensor_interface.hh @@ -0,0 +1,105 @@ +//===================================================== +// Copyright (C) 2014 Benoit Steiner +//===================================================== +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +// +#ifndef TENSOR_INTERFACE_HH +#define TENSOR_INTERFACE_HH + +#include +#include +#include "btl.hh" + +using namespace Eigen; + +template +class tensor_interface +{ +public : + typedef real real_type; + typedef typename Eigen::Tensor::Index Index; + + typedef std::vector stl_vector; + typedef std::vector stl_matrix; + + typedef Eigen::Tensor gene_matrix; + typedef Eigen::Tensor gene_vector; + + + static inline std::string name( void ) + { + return EIGEN_MAKESTRING(BTL_PREFIX); + } + + static void free_matrix(gene_matrix & /*A*/, int /*N*/) {} + + static void free_vector(gene_vector & /*B*/) {} + + static BTL_DONT_INLINE void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){ + A.resize(Eigen::array(A_stl[0].size(), A_stl.size())); + + for (unsigned int j=0; j(i,j)) = A_stl[j][i]; + } + } + } + + static BTL_DONT_INLINE void vector_from_stl(gene_vector & B, stl_vector & B_stl){ + B.resize(B_stl.size()); + + for (unsigned int i=0; i(i,j)); + } + } + } + + static inline void matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int /*N*/){ + typedef typename Eigen::Tensor::DimensionPair DimPair; + const Eigen::array dims(DimPair(1, 0)); + X/*.noalias()*/ = A.contract(B, dims); + } + + static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int /*N*/){ + typedef typename Eigen::Tensor::DimensionPair DimPair; + const Eigen::array dims(DimPair(1, 0)); + X/*.noalias()*/ = A.contract(B, dims); + } + + static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int /*N*/){ + Y += X.constant(coef) * X; + } + + static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int /*N*/){ + Y = X.constant(a)*X + Y.constant(b)*Y; + } + + static EIGEN_DONT_INLINE void copy_matrix(const gene_matrix & source, gene_matrix & cible, int /*N*/){ + cible = source; + } + + static EIGEN_DONT_INLINE void copy_vector(const gene_vector & source, gene_vector & cible, int /*N*/){ + cible = source; + } +}; + +#endif diff --git a/ground/gcs/src/libs/eigen/bench/dense_solvers.cpp b/ground/gcs/src/libs/eigen/bench/dense_solvers.cpp new file mode 100644 index 0000000000..24343dcd88 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/dense_solvers.cpp @@ -0,0 +1,186 @@ +#include +#include "BenchTimer.h" +#include +#include +#include +#include +#include +using namespace Eigen; + +std::map > results; +std::vector labels; +std::vector sizes; + +template +EIGEN_DONT_INLINE +void compute_norm_equation(Solver &solver, const MatrixType &A) { + if(A.rows()!=A.cols()) + solver.compute(A.transpose()*A); + else + solver.compute(A); +} + +template +EIGEN_DONT_INLINE +void compute(Solver &solver, const MatrixType &A) { + solver.compute(A); +} + +template +void bench(int id, int rows, int size = Size) +{ + typedef Matrix Mat; + typedef Matrix MatDyn; + typedef Matrix MatSquare; + Mat A(rows,size); + A.setRandom(); + if(rows==size) + A = A*A.adjoint(); + BenchTimer t_llt, t_ldlt, t_lu, t_fplu, t_qr, t_cpqr, t_cod, t_fpqr, t_jsvd, t_bdcsvd; + + int svd_opt = ComputeThinU|ComputeThinV; + + int tries = 5; + int rep = 1000/size; + if(rep==0) rep = 1; +// rep = rep*rep; + + LLT llt(size); + LDLT ldlt(size); + PartialPivLU lu(size); + FullPivLU fplu(size,size); + HouseholderQR qr(A.rows(),A.cols()); + ColPivHouseholderQR cpqr(A.rows(),A.cols()); + CompleteOrthogonalDecomposition cod(A.rows(),A.cols()); + FullPivHouseholderQR fpqr(A.rows(),A.cols()); + JacobiSVD jsvd(A.rows(),A.cols()); + BDCSVD bdcsvd(A.rows(),A.cols()); + + BENCH(t_llt, tries, rep, compute_norm_equation(llt,A)); + BENCH(t_ldlt, tries, rep, compute_norm_equation(ldlt,A)); + BENCH(t_lu, tries, rep, compute_norm_equation(lu,A)); + if(size<=1000) + BENCH(t_fplu, tries, rep, compute_norm_equation(fplu,A)); + BENCH(t_qr, tries, rep, compute(qr,A)); + BENCH(t_cpqr, tries, rep, compute(cpqr,A)); + BENCH(t_cod, tries, rep, compute(cod,A)); + if(size*rows<=10000000) + BENCH(t_fpqr, tries, rep, compute(fpqr,A)); + if(size<500) // JacobiSVD is really too slow for too large matrices + BENCH(t_jsvd, tries, rep, jsvd.compute(A,svd_opt)); +// if(size*rows<=20000000) + BENCH(t_bdcsvd, tries, rep, bdcsvd.compute(A,svd_opt)); + + results["LLT"][id] = t_llt.best(); + results["LDLT"][id] = t_ldlt.best(); + results["PartialPivLU"][id] = t_lu.best(); + results["FullPivLU"][id] = t_fplu.best(); + results["HouseholderQR"][id] = t_qr.best(); + results["ColPivHouseholderQR"][id] = t_cpqr.best(); + results["CompleteOrthogonalDecomposition"][id] = t_cod.best(); + results["FullPivHouseholderQR"][id] = t_fpqr.best(); + results["JacobiSVD"][id] = t_jsvd.best(); + results["BDCSVD"][id] = t_bdcsvd.best(); +} + + +int main() +{ + labels.push_back("LLT"); + labels.push_back("LDLT"); + labels.push_back("PartialPivLU"); + labels.push_back("FullPivLU"); + labels.push_back("HouseholderQR"); + labels.push_back("ColPivHouseholderQR"); + labels.push_back("CompleteOrthogonalDecomposition"); + labels.push_back("FullPivHouseholderQR"); + labels.push_back("JacobiSVD"); + labels.push_back("BDCSVD"); + + for(int i=0; i(k,sizes[k](0),sizes[k](1)); + } + + cout.width(32); + cout << "solver/size"; + cout << " "; + for(int k=0; k=1e6) cout << "-"; + else cout << r(k); + cout << " "; + } + cout << endl; + } + + // HTML output + cout << "" << endl; + cout << "" << endl; + for(int k=0; k" << sizes[k](0) << "x" << sizes[k](1) << ""; + cout << "" << endl; + for(int i=0; i"; + ArrayXf r = (results[labels[i]]*100000.f).floor()/100.f; + for(int k=0; k=1e6) cout << ""; + else + { + cout << ""; + } + } + cout << "" << endl; + } + cout << "
solver/size
" << labels[i] << "-" << r(k); + if(i>0) + cout << " (x" << numext::round(10.f*results[labels[i]](k)/results["LLT"](k))/10.f << ")"; + if(i<4 && sizes[k](0)!=sizes[k](1)) + cout << " *"; + cout << "
" << endl; + +// cout << "LLT (ms) " << (results["LLT"]*1000.).format(fmt) << "\n"; +// cout << "LDLT (%) " << (results["LDLT"]/results["LLT"]).format(fmt) << "\n"; +// cout << "PartialPivLU (%) " << (results["PartialPivLU"]/results["LLT"]).format(fmt) << "\n"; +// cout << "FullPivLU (%) " << (results["FullPivLU"]/results["LLT"]).format(fmt) << "\n"; +// cout << "HouseholderQR (%) " << (results["HouseholderQR"]/results["LLT"]).format(fmt) << "\n"; +// cout << "ColPivHouseholderQR (%) " << (results["ColPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n"; +// cout << "CompleteOrthogonalDecomposition (%) " << (results["CompleteOrthogonalDecomposition"]/results["LLT"]).format(fmt) << "\n"; +// cout << "FullPivHouseholderQR (%) " << (results["FullPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n"; +// cout << "JacobiSVD (%) " << (results["JacobiSVD"]/results["LLT"]).format(fmt) << "\n"; +// cout << "BDCSVD (%) " << (results["BDCSVD"]/results["LLT"]).format(fmt) << "\n"; +} diff --git a/ground/gcs/src/libs/eigen/bench/eig33.cpp b/ground/gcs/src/libs/eigen/bench/eig33.cpp index 1608b999d0..47947a9bed 100644 --- a/ground/gcs/src/libs/eigen/bench/eig33.cpp +++ b/ground/gcs/src/libs/eigen/bench/eig33.cpp @@ -50,7 +50,7 @@ inline void computeRoots(const Matrix& m, Roots& roots) { typedef typename Matrix::Scalar Scalar; const Scalar s_inv3 = 1.0/3.0; - const Scalar s_sqrt3 = internal::sqrt(Scalar(3.0)); + const Scalar s_sqrt3 = std::sqrt(Scalar(3.0)); // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The // eigenvalues are the roots to this equation, all guaranteed to be @@ -73,23 +73,13 @@ inline void computeRoots(const Matrix& m, Roots& roots) q = Scalar(0); // Compute the eigenvalues by solving for the roots of the polynomial. - Scalar rho = internal::sqrt(-a_over_3); - Scalar theta = std::atan2(internal::sqrt(-q),half_b)*s_inv3; - Scalar cos_theta = internal::cos(theta); - Scalar sin_theta = internal::sin(theta); - roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta; - roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); - roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); - - // Sort in increasing order. - if (roots(0) >= roots(1)) - std::swap(roots(0),roots(1)); - if (roots(1) >= roots(2)) - { - std::swap(roots(1),roots(2)); - if (roots(0) >= roots(1)) - std::swap(roots(0),roots(1)); - } + Scalar rho = std::sqrt(-a_over_3); + Scalar theta = std::atan2(std::sqrt(-q),half_b)*s_inv3; + Scalar cos_theta = std::cos(theta); + Scalar sin_theta = std::sin(theta); + roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta; + roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); + roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); } template @@ -99,9 +89,12 @@ void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals) // Scale the matrix so its entries are in [-1,1]. The scaling is applied // only when at least one matrix entry has magnitude larger than 1. - Scalar scale = mat.cwiseAbs()/*.template triangularView()*/.maxCoeff(); + Scalar shift = mat.trace()/3; + Matrix scaledMat = mat; + scaledMat.diagonal().array() -= shift; + Scalar scale = scaledMat.cwiseAbs()/*.template triangularView()*/.maxCoeff(); scale = std::max(scale,Scalar(1)); - Matrix scaledMat = mat / scale; + scaledMat/=scale; // Compute the eigenvalues // scaledMat.setZero(); @@ -166,6 +159,7 @@ void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals) // Rescale back to the original size. evals *= scale; + evals.array()+=shift; } int main() @@ -173,24 +167,29 @@ int main() BenchTimer t; int tries = 10; int rep = 400000; - typedef Matrix3f Mat; - typedef Vector3f Vec; + typedef Matrix3d Mat; + typedef Vector3d Vec; Mat A = Mat::Random(3,3); A = A.adjoint() * A; +// Mat Q = A.householderQr().householderQ(); +// A = Q * Vec(2.2424567,2.2424566,7.454353).asDiagonal() * Q.transpose(); SelfAdjointEigenSolver eig(A); BENCH(t, tries, rep, eig.compute(A)); - std::cout << "Eigen: " << t.best() << "s\n"; + std::cout << "Eigen iterative: " << t.best() << "s\n"; + + BENCH(t, tries, rep, eig.computeDirect(A)); + std::cout << "Eigen direct : " << t.best() << "s\n"; Mat evecs; Vec evals; BENCH(t, tries, rep, eigen33(A,evecs,evals)); std::cout << "Direct: " << t.best() << "s\n\n"; - std::cerr << "Eigenvalue/eigenvector diffs:\n"; - std::cerr << (evals - eig.eigenvalues()).transpose() << "\n"; - for(int k=0;k<3;++k) - if(evecs.col(k).dot(eig.eigenvectors().col(k))<0) - evecs.col(k) = -evecs.col(k); - std::cerr << evecs - eig.eigenvectors() << "\n\n"; +// std::cerr << "Eigenvalue/eigenvector diffs:\n"; +// std::cerr << (evals - eig.eigenvalues()).transpose() << "\n"; +// for(int k=0;k<3;++k) +// if(evecs.col(k).dot(eig.eigenvectors().col(k))<0) +// evecs.col(k) = -evecs.col(k); +// std::cerr << evecs - eig.eigenvectors() << "\n\n"; } diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/changesets.txt b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/changesets.txt new file mode 100644 index 0000000000..af8eb9b8f7 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/changesets.txt @@ -0,0 +1,61 @@ +#3.0.1 +#3.1.1 +#3.2.0 +3.2.4 +#5745:37f59e65eb6c +5891:d8652709345d # introduce AVX +#5893:24b4dc92c6d3 # merge +5895:997c2ef9fc8b # introduce FMA +#5904:e1eafd14eaa1 # complex and AVX +5908:f8ee3c721251 # improve packing with ptranspose +#5921:ca808bb456b0 # merge +#5927:8b1001f9e3ac +5937:5a4ca1ad8c53 # New gebp kernel handling up to 3 packets x 4 register-level blocks +#5949:f3488f4e45b2 # merge +#5969:e09031dccfd9 # Disable 3pX4 kernel on Altivec +#5992:4a429f5e0483 # merge +before-evaluators +#6334:f6a45e5b8b7c # Implement evaluator for sparse outer products +#6639:c9121c60b5c7 +#6655:06f163b5221f # Properly detect FMA support on ARM +#6677:700e023044e7 # FMA has been wrongly disabled +#6681:11d31dafb0e3 +#6699:5e6e8e10aad1 # merge default to tensors +#6726:ff2d2388e7b9 # merge default to tensors +#6742:0cbd6195e829 # merge default to tensors +#6747:853d2bafeb8f # Generalized the gebp apis +6765:71584fd55762 # Made the blocking computation aware of the l3 cache; Also optimized the blocking parameters to take into account the number of threads used for a computation +#6781:9cc5a931b2c6 # generalized gemv +#6792:f6e1daab600a # ensured that contractions that can be reduced to a matrix vector product +#6844:039efd86b75c # merge tensor +6845:7333ed40c6ef # change prefetching in gebp +#6856:b5be5e10eb7f # merge index conversion +#6893:c3a64aba7c70 # clean blocking size computation +#6898:6fb31ebe6492 # rotating kernel for ARM +6899:877facace746 # rotating kernel for ARM only +#6904:c250623ae9fa # result_of +6921:915f1b1fc158 # fix prefetching change for ARM +6923:9ff25f6dacc6 # prefetching +6933:52572e60b5d3 # blocking size strategy +6937:c8c042f286b2 # avoid redundant pack_rhs +6981:7e5d6f78da59 # dynamic loop swapping +6984:45f26866c091 # rm dynamic loop swapping, adjust lhs's micro panel height to fully exploit L1 cache +6986:a675d05b6f8f # blocking heuristic: block on the rhs in L1 if the lhs fit in L1. +7013:f875e75f07e5 # organize a little our default cache sizes, and use a saner default L1 outside of x86 (10% faster on Nexus 5) +7015:8aad8f35c955 # Refactor computeProductBlockingSizes to make room for the possibility of using lookup tables +7016:a58d253e8c91 # Polish lookup tables generation +7018:9b27294a8186 # actual_panel_rows computation should always be resilient to parameters not consistent with the known L1 cache size, see comment +7019:c758b1e2c073 # Provide a empirical lookup table for blocking sizes measured on a Nexus 5. Only for float, only for Android on ARM 32bit for now. +7085:627e039fba68 # Bug 986: add support for coefficient-based product with 0 depth. +7098:b6f1db9cf9ec # Bug 992: don't select a 3p GEMM path with non-vectorizable scalar types, this hits unsupported paths in symm/triangular products code +7591:09a8e2186610 # 3.3-alpha1 +7650:b0f3c8f43025 # help clang inlining +#8744:74b789ada92a # Improved the matrix multiplication blocking in the case where mr is not a power of 2 (e.g on Haswell CPUs) +8789:efcb912e4356 # Made the index type a template parameter to evaluateProductBlockingSizes. Use numext::mini and numext::maxi instead of std::min/std::max to compute blocking sizes +8972:81d53c711775 # Don't optimize the processing of the last rows of a matrix matrix product in cases that violate the assumptions made by the optimized code path +8985:d935df21a082 # Remove the rotating kernel. +8988:6c2dc56e73b3 # Bug 256: enable vectorization with unaligned loads/stores. +9148:b8b8c421e36c # Relax mixing-type constraints for binary coefficient-wise operators +9174:d228bc282ac9 # merge +9212:c90098affa7b # Fix performance regression introduced in changeset 8aad8f35c955 +9213:9f1c14e4694b # Fix performance regression in dgemm introduced by changeset 81d53c711775 diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/gemm.cpp b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/gemm.cpp new file mode 100644 index 0000000000..614bd47373 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/gemm.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include +#include "../../BenchTimer.h" +using namespace Eigen; + +#ifndef SCALAR +#error SCALAR must be defined +#endif + +typedef SCALAR Scalar; + +typedef Matrix Mat; + +EIGEN_DONT_INLINE +void gemm(const Mat &A, const Mat &B, Mat &C) +{ + C.noalias() += A * B; +} + +EIGEN_DONT_INLINE +double bench(long m, long n, long k) +{ + Mat A(m,k); + Mat B(k,n); + Mat C(m,n); + A.setRandom(); + B.setRandom(); + C.setZero(); + + BenchTimer t; + + double up = 1e8*4/sizeof(Scalar); + double tm0 = 4, tm1 = 10; + if(NumTraits::IsComplex) + { + up /= 4; + tm0 = 2; + tm1 = 4; + } + + double flops = 2. * m * n * k; + long rep = std::max(1., std::min(100., up/flops) ); + long tries = std::max(tm0, std::min(tm1, up/flops) ); + + BENCH(t, tries, rep, gemm(A,B,C)); + + return 1e-9 * rep * flops / t.best(); +} + +int main(int argc, char **argv) +{ + std::vector results; + + std::ifstream settings("gemm_settings.txt"); + long m, n, k; + while(settings >> m >> n >> k) + { + //std::cerr << " Testing " << m << " " << n << " " << k << std::endl; + results.push_back( bench(m, n, k) ); + } + + std::cout << RowVectorXd::Map(results.data(), results.size()); + + return 0; +} diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/gemm_settings.txt b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/gemm_settings.txt new file mode 100644 index 0000000000..5c43e1c7d1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/gemm_settings.txt @@ -0,0 +1,15 @@ +8 8 8 +9 9 9 +24 24 24 +239 239 239 +240 240 240 +2400 24 24 +24 2400 24 +24 24 2400 +24 2400 2400 +2400 24 2400 +2400 2400 24 +2400 2400 64 +4800 23 160 +23 4800 160 +2400 2400 2400 diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/lazy_gemm.cpp b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/lazy_gemm.cpp new file mode 100644 index 0000000000..6dc3701552 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/lazy_gemm.cpp @@ -0,0 +1,98 @@ +#include +#include +#include +#include +#include "../../BenchTimer.h" +using namespace Eigen; + +#ifndef SCALAR +#error SCALAR must be defined +#endif + +typedef SCALAR Scalar; + +template +EIGEN_DONT_INLINE +void lazy_gemm(const MatA &A, const MatB &B, MatC &C) +{ +// escape((void*)A.data()); +// escape((void*)B.data()); + C.noalias() += A.lazyProduct(B); +// escape((void*)C.data()); +} + +template +EIGEN_DONT_INLINE +double bench() +{ + typedef Matrix MatA; + typedef Matrix MatB; + typedef Matrix MatC; + + MatA A(m,k); + MatB B(k,n); + MatC C(m,n); + A.setRandom(); + B.setRandom(); + C.setZero(); + + BenchTimer t; + + double up = 1e7*4/sizeof(Scalar); + double tm0 = 10, tm1 = 20; + + double flops = 2. * m * n * k; + long rep = std::max(10., std::min(10000., up/flops) ); + long tries = std::max(tm0, std::min(tm1, up/flops) ); + + BENCH(t, tries, rep, lazy_gemm(A,B,C)); + + return 1e-9 * rep * flops / t.best(); +} + +template +double bench_t(int t) +{ + if(t) + return bench(); + else + return bench(); +} + +EIGEN_DONT_INLINE +double bench_mnk(int m, int n, int k, int t) +{ + int id = m*10000 + n*100 + k; + switch(id) { + case 10101 : return bench_t< 1, 1, 1>(t); break; + case 20202 : return bench_t< 2, 2, 2>(t); break; + case 30303 : return bench_t< 3, 3, 3>(t); break; + case 40404 : return bench_t< 4, 4, 4>(t); break; + case 50505 : return bench_t< 5, 5, 5>(t); break; + case 60606 : return bench_t< 6, 6, 6>(t); break; + case 70707 : return bench_t< 7, 7, 7>(t); break; + case 80808 : return bench_t< 8, 8, 8>(t); break; + case 90909 : return bench_t< 9, 9, 9>(t); break; + case 101010 : return bench_t<10,10,10>(t); break; + case 111111 : return bench_t<11,11,11>(t); break; + case 121212 : return bench_t<12,12,12>(t); break; + } + return 0; +} + +int main(int argc, char **argv) +{ + std::vector results; + + std::ifstream settings("lazy_gemm_settings.txt"); + long m, n, k, t; + while(settings >> m >> n >> k >> t) + { + //std::cerr << " Testing " << m << " " << n << " " << k << std::endl; + results.push_back( bench_mnk(m, n, k, t) ); + } + + std::cout << RowVectorXd::Map(results.data(), results.size()); + + return 0; +} diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/lazy_gemm_settings.txt b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/lazy_gemm_settings.txt new file mode 100644 index 0000000000..407d5d4fad --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/lazy_gemm_settings.txt @@ -0,0 +1,15 @@ +1 1 1 0 +2 2 2 0 +3 3 3 0 +4 4 4 0 +4 4 4 1 +5 5 5 0 +6 6 6 0 +7 7 7 0 +7 7 7 1 +8 8 8 0 +9 9 9 0 +10 10 10 0 +11 11 11 0 +12 12 12 0 +12 12 12 1 diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/make_plot.sh b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/make_plot.sh new file mode 100644 index 0000000000..cd3214ac91 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/make_plot.sh @@ -0,0 +1,38 @@ +#!/bin/bash + +# base name of the bench +# it reads $1.out +# and generates $1.pdf +WHAT=$1 +bench=$2 + +header="rev " +while read line +do + if [ ! -z '$line' ]; then + header="$header \"$line\"" + fi +done < $bench"_settings.txt" + +echo $header > $WHAT.out.header +cat $WHAT.out >> $WHAT.out.header + + +echo "set title '$WHAT'" > $WHAT.gnuplot +echo "set key autotitle columnhead outside " >> $WHAT.gnuplot +echo "set xtics rotate 1" >> $WHAT.gnuplot + +echo "set term pdf color rounded enhanced fontscale 0.35 size 7in,5in" >> $WHAT.gnuplot +echo set output "'"$WHAT.pdf"'" >> $WHAT.gnuplot + +col=`cat $bench"_settings.txt" | wc -l` +echo "plot for [col=2:$col+1] '$WHAT.out.header' using 0:col:xticlabels(1) with lines" >> $WHAT.gnuplot +echo " " >> $WHAT.gnuplot + +gnuplot -persist < $WHAT.gnuplot + +# generate a png file +# convert -background white -density 120 -rotate 90 -resize 800 +dither -colors 256 -quality 0 $WHAT.ps -background white -flatten .$WHAT.png + +# clean +rm $WHAT.out.header $WHAT.gnuplot \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/run.sh b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/run.sh new file mode 100644 index 0000000000..9d6ee40bca --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/perf_monitoring/gemm/run.sh @@ -0,0 +1,156 @@ +#!/bin/bash + +# ./run.sh gemm +# ./run.sh lazy_gemm + +# Examples of environment variables to be set: +# PREFIX="haswell-fma-" +# CXX_FLAGS="-mfma" + +# Options: +# -up : enforce the recomputation of existing data, and keep best results as a merging strategy +# -s : recompute selected changesets only and keep bests + +bench=$1 + +if echo "$*" | grep '\-up' > /dev/null; then + update=true +else + update=false +fi + +if echo "$*" | grep '\-s' > /dev/null; then + selected=true +else + selected=false +fi + +global_args="$*" + +if [ $selected == true ]; then + echo "Recompute selected changesets only and keep bests" +elif [ $update == true ]; then + echo "(Re-)Compute all changesets and keep bests" +else + echo "Skip previously computed changesets" +fi + + + +if [ ! -d "eigen_src" ]; then + hg clone https://bitbucket.org/eigen/eigen eigen_src +else + cd eigen_src + hg pull -u + cd .. +fi + +if [ ! -z '$CXX' ]; then + CXX=g++ +fi + +function make_backup +{ + if [ -f "$1.out" ]; then + mv "$1.out" "$1.backup" + fi +} + +function merge +{ + count1=`echo $1 | wc -w` + count2=`echo $2 | wc -w` + + if [ $count1 == $count2 ]; then + a=( $1 ); b=( $2 ) + res="" + for (( i=0 ; i<$count1 ; i++ )); do + ai=${a[$i]}; bi=${b[$i]} + tmp=`echo "if ($ai > $bi) $ai else $bi " | bc -l` + res="$res $tmp" + done + echo $res + + else + echo $1 + fi +} + +function test_current +{ + rev=$1 + scalar=$2 + name=$3 + + prev="" + if [ -e "$name.backup" ]; then + prev=`grep $rev "$name.backup" | cut -c 14-` + fi + res=$prev + count_rev=`echo $prev | wc -w` + count_ref=`cat $bench"_settings.txt" | wc -l` + if echo "$global_args" | grep "$rev" > /dev/null; then + rev_found=true + else + rev_found=false + fi +# echo $update et $selected et $rev_found because $rev et "$global_args" +# echo $count_rev et $count_ref + if [ $update == true ] || [ $count_rev != $count_ref ] || ([ $selected == true ] && [ $rev_found == true ]); then + if $CXX -O2 -DNDEBUG -march=native $CXX_FLAGS -I eigen_src $bench.cpp -DSCALAR=$scalar -o $name; then + curr=`./$name` + if [ $count_rev == $count_ref ]; then + echo "merge previous $prev" + echo "with new $curr" + else + echo "got $curr" + fi + res=`merge "$curr" "$prev"` +# echo $res + echo "$rev $res" >> $name.out + else + echo "Compilation failed, skip rev $rev" + fi + else + echo "Skip existing results for $rev / $name" + echo "$rev $res" >> $name.out + fi +} + +make_backup $PREFIX"s"$bench +make_backup $PREFIX"d"$bench +make_backup $PREFIX"c"$bench + +cut -f1 -d"#" < changesets.txt | grep -E '[[:alnum:]]' | while read rev +do + if [ ! -z '$rev' ]; then + echo "Testing rev $rev" + cd eigen_src + hg up -C $rev > /dev/null + actual_rev=`hg identify | cut -f1 -d' '` + cd .. + + test_current $actual_rev float $PREFIX"s"$bench + test_current $actual_rev double $PREFIX"d"$bench + test_current $actual_rev "std::complex" $PREFIX"c"$bench + fi + +done + +echo "Float:" +cat $PREFIX"s""$bench.out" +echo " " + +echo "Double:" +cat $PREFIX"d""$bench.out" +echo "" + +echo "Complex:" +cat $PREFIX"c""$bench.out" +echo "" + +./make_plot.sh $PREFIX"s"$bench $bench +./make_plot.sh $PREFIX"d"$bench $bench +./make_plot.sh $PREFIX"c"$bench $bench + + diff --git a/ground/gcs/src/libs/eigen/bench/spbench/CMakeLists.txt b/ground/gcs/src/libs/eigen/bench/spbench/CMakeLists.txt index 6e0e1b1039..8d53f4ae2c 100644 --- a/ground/gcs/src/libs/eigen/bench/spbench/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/bench/spbench/CMakeLists.txt @@ -29,7 +29,7 @@ if(UMFPACK_FOUND AND BLAS_FOUND) set(UMFPACK_ALL_LIBS ${UMFPACK_LIBRARIES} ${BLAS_LIBRARIES}) endif() -find_package(SuperLU) +find_package(SuperLU 4.0) if(SUPERLU_FOUND AND BLAS_FOUND) add_definitions("-DEIGEN_SUPERLU_SUPPORT") include_directories(${SUPERLU_INCLUDES}) diff --git a/ground/gcs/src/libs/eigen/bench/spbench/spbenchstyle.h b/ground/gcs/src/libs/eigen/bench/spbench/spbenchstyle.h index 17a05ce710..f6a981778c 100644 --- a/ground/gcs/src/libs/eigen/bench/spbench/spbenchstyle.h +++ b/ground/gcs/src/libs/eigen/bench/spbench/spbenchstyle.h @@ -91,4 +91,5 @@ void printBenchStyle(std::ofstream& out) \n\n"; } -#endif \ No newline at end of file + +#endif diff --git a/ground/gcs/src/libs/eigen/bench/tensors/README b/ground/gcs/src/libs/eigen/bench/tensors/README new file mode 100644 index 0000000000..3a5fdbe17e --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/README @@ -0,0 +1,21 @@ +The tensor benchmark suite is made of several parts. + +The first part is a generic suite, in which each benchmark comes in 2 flavors: one that runs on CPU, and one that runs on GPU. + +To compile the floating point CPU benchmarks, simply call: +g++ tensor_benchmarks_cpu.cc benchmark_main.cc -I ../../ -std=c++11 -O3 -DNDEBUG -pthread -mavx -o benchmarks_cpu + +To compile the floating point GPU benchmarks, simply call: +nvcc tensor_benchmarks_gpu.cu benchmark_main.cc -I ../../ -std=c++11 -O2 -DNDEBUG -use_fast_math -ftz=true -arch compute_35 -o benchmarks_gpu + +We also provide a version of the generic GPU tensor benchmarks that uses half floats (aka fp16) instead of regular floats. To compile these benchmarks, simply call the command line below. You'll need a recent GPU that supports compute capability 5.3 or higher to run them and nvcc 7.5 or higher to compile the code. +nvcc tensor_benchmarks_fp16_gpu.cu benchmark_main.cc -I ../../ -std=c++11 -O2 -DNDEBUG -use_fast_math -ftz=true -arch compute_53 -o benchmarks_fp16_gpu + +last but not least, we also provide a suite of benchmarks to measure the scalability of the contraction code on CPU. To compile these benchmarks, call +g++ contraction_benchmarks_cpu.cc benchmark_main.cc -I ../../ -std=c++11 -O3 -DNDEBUG -pthread -mavx -o benchmarks_cpu + +To compile the benchmark for SYCL, using ComputeCpp you currently need 2 passes (only for translation units containing device code): +1. The device compilation pass that generates the device code (SYCL kernels and referenced device functions) and glue code needed by the host compiler to reference the device code from host code. +{ComputeCpp_ROOT}/bin/compute++ -I ../../ -I {ComputeCpp_ROOT}/include/ -std=c++11 -mllvm -inline-threshold=1000 -Wno-ignored-attributes -sycl -intelspirmetadata -emit-llvm -no-serial-memop -sycl-compress-name -DBUILD_PLATFORM_SPIR -DNDBUG -O3 -c tensor_benchmarks_sycl.cc +2. The host compilation pass that generates the final host binary. +clang++-3.7 -include tensor_benchmarks_sycl.sycl benchmark_main.cc tensor_benchmarks_sycl.cc -pthread -I ../../ -I {ComputeCpp_ROOT}/include/ -L {ComputeCpp_ROOT}/lib/ -lComputeCpp -lOpenCL -D_GLIBCXX_USE_CXX11_ABI=0 -std=c++11 -o tensor_benchmark_sycl diff --git a/ground/gcs/src/libs/eigen/bench/tensors/benchmark.h b/ground/gcs/src/libs/eigen/bench/tensors/benchmark.h new file mode 100644 index 0000000000..f115b54ad1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/benchmark.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2012 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include + +namespace testing { +class Benchmark { + public: + Benchmark(const char* name, void (*fn)(int)) { + Register(name, fn, NULL); + } + Benchmark(const char* name, void (*fn_range)(int, int)) { + Register(name, NULL, fn_range); + } + Benchmark* Arg(int x); + Benchmark* Range(int lo, int hi); + const char* Name(); + bool ShouldRun(int argc, char* argv[]); + void Run(); + private: + const char* name_; + void (*fn_)(int); + void (*fn_range_)(int, int); + std::vector args_; + void Register(const char* name, void (*fn)(int), void (*fn_range)(int, int)); + void RunRepeatedlyWithArg(int iterations, int arg); + void RunWithArg(int arg); +}; +} // namespace testing +void SetBenchmarkFlopsProcessed(int64_t); +void StopBenchmarkTiming(); +void StartBenchmarkTiming(); +#define BENCHMARK(f) \ + static ::testing::Benchmark* _benchmark_##f __attribute__((unused)) = \ + (new ::testing::Benchmark(#f, f)) diff --git a/ground/gcs/src/libs/eigen/bench/tensors/benchmark_main.cc b/ground/gcs/src/libs/eigen/bench/tensors/benchmark_main.cc new file mode 100644 index 0000000000..1efa0dbad6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/benchmark_main.cc @@ -0,0 +1,237 @@ +/* + * Copyright (C) 2012 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "benchmark.h" +#include +#include +#include +#include +#include +#include +#include +#include + +static int64_t g_flops_processed; +static int64_t g_benchmark_total_time_ns; +static int64_t g_benchmark_start_time_ns; +typedef std::map BenchmarkMap; +typedef BenchmarkMap::iterator BenchmarkMapIt; + +BenchmarkMap& gBenchmarks() { + static BenchmarkMap g_benchmarks; + return g_benchmarks; +} + +static int g_name_column_width = 20; + +static int Round(int n) { + int base = 1; + while (base*10 < n) { + base *= 10; + } + if (n < 2*base) { + return 2*base; + } + if (n < 5*base) { + return 5*base; + } + return 10*base; +} + +#ifdef __APPLE__ + #include + static mach_timebase_info_data_t g_time_info; + static void __attribute__((constructor)) init_info() { + mach_timebase_info(&g_time_info); + } +#endif + +static int64_t NanoTime() { +#if defined(__APPLE__) + uint64_t t = mach_absolute_time(); + return t * g_time_info.numer / g_time_info.denom; +#else + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return static_cast(t.tv_sec) * 1000000000LL + t.tv_nsec; +#endif +} + +namespace testing { +Benchmark* Benchmark::Arg(int arg) { + args_.push_back(arg); + return this; +} + +Benchmark* Benchmark::Range(int lo, int hi) { + const int kRangeMultiplier = 8; + if (hi < lo) { + int temp = hi; + hi = lo; + lo = temp; + } + while (lo < hi) { + args_.push_back(lo); + lo *= kRangeMultiplier; + } + // We always run the hi number. + args_.push_back(hi); + return this; +} + +const char* Benchmark::Name() { + return name_; +} +bool Benchmark::ShouldRun(int argc, char* argv[]) { + if (argc == 1) { + return true; // With no arguments, we run all benchmarks. + } + // Otherwise, we interpret each argument as a regular expression and + // see if any of our benchmarks match. + for (int i = 1; i < argc; i++) { + regex_t re; + if (regcomp(&re, argv[i], 0) != 0) { + fprintf(stderr, "couldn't compile \"%s\" as a regular expression!\n", argv[i]); + exit(EXIT_FAILURE); + } + int match = regexec(&re, name_, 0, NULL, 0); + regfree(&re); + if (match != REG_NOMATCH) { + return true; + } + } + return false; +} +void Benchmark::Register(const char* name, void (*fn)(int), void (*fn_range)(int, int)) { + name_ = name; + fn_ = fn; + fn_range_ = fn_range; + if (fn_ == NULL && fn_range_ == NULL) { + fprintf(stderr, "%s: missing function\n", name_); + exit(EXIT_FAILURE); + } + gBenchmarks().insert(std::make_pair(name, this)); +} +void Benchmark::Run() { + if (fn_ != NULL) { + RunWithArg(0); + } else { + if (args_.empty()) { + fprintf(stderr, "%s: no args!\n", name_); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < args_.size(); ++i) { + RunWithArg(args_[i]); + } + } +} +void Benchmark::RunRepeatedlyWithArg(int iterations, int arg) { + g_flops_processed = 0; + g_benchmark_total_time_ns = 0; + g_benchmark_start_time_ns = NanoTime(); + if (fn_ != NULL) { + fn_(iterations); + } else { + fn_range_(iterations, arg); + } + if (g_benchmark_start_time_ns != 0) { + g_benchmark_total_time_ns += NanoTime() - g_benchmark_start_time_ns; + } +} +void Benchmark::RunWithArg(int arg) { + // run once in case it's expensive + int iterations = 1; + RunRepeatedlyWithArg(iterations, arg); + while (g_benchmark_total_time_ns < 1e9 && iterations < 1e9) { + int last = iterations; + if (g_benchmark_total_time_ns/iterations == 0) { + iterations = 1e9; + } else { + iterations = 1e9 / (g_benchmark_total_time_ns/iterations); + } + iterations = std::max(last + 1, std::min(iterations + iterations/2, 100*last)); + iterations = Round(iterations); + RunRepeatedlyWithArg(iterations, arg); + } + char throughput[100]; + throughput[0] = '\0'; + if (g_benchmark_total_time_ns > 0 && g_flops_processed > 0) { + double mflops_processed = static_cast(g_flops_processed)/1e6; + double seconds = static_cast(g_benchmark_total_time_ns)/1e9; + snprintf(throughput, sizeof(throughput), " %8.2f MFlops/s", mflops_processed/seconds); + } + char full_name[100]; + if (fn_range_ != NULL) { + if (arg >= (1<<20)) { + snprintf(full_name, sizeof(full_name), "%s/%dM", name_, arg/(1<<20)); + } else if (arg >= (1<<10)) { + snprintf(full_name, sizeof(full_name), "%s/%dK", name_, arg/(1<<10)); + } else { + snprintf(full_name, sizeof(full_name), "%s/%d", name_, arg); + } + } else { + snprintf(full_name, sizeof(full_name), "%s", name_); + } + printf("%-*s %10d %10" PRId64 "%s\n", g_name_column_width, full_name, + iterations, g_benchmark_total_time_ns/iterations, throughput); + fflush(stdout); +} +} // namespace testing +void SetBenchmarkFlopsProcessed(int64_t x) { + g_flops_processed = x; +} +void StopBenchmarkTiming() { + if (g_benchmark_start_time_ns != 0) { + g_benchmark_total_time_ns += NanoTime() - g_benchmark_start_time_ns; + } + g_benchmark_start_time_ns = 0; +} +void StartBenchmarkTiming() { + if (g_benchmark_start_time_ns == 0) { + g_benchmark_start_time_ns = NanoTime(); + } +} +int main(int argc, char* argv[]) { + if (gBenchmarks().empty()) { + fprintf(stderr, "No benchmarks registered!\n"); + exit(EXIT_FAILURE); + } + for (BenchmarkMapIt it = gBenchmarks().begin(); it != gBenchmarks().end(); ++it) { + int name_width = static_cast(strlen(it->second->Name())); + g_name_column_width = std::max(g_name_column_width, name_width); + } + bool need_header = true; + for (BenchmarkMapIt it = gBenchmarks().begin(); it != gBenchmarks().end(); ++it) { + ::testing::Benchmark* b = it->second; + if (b->ShouldRun(argc, argv)) { + if (need_header) { + printf("%-*s %10s %10s\n", g_name_column_width, "", "iterations", "ns/op"); + fflush(stdout); + need_header = false; + } + b->Run(); + } + } + if (need_header) { + fprintf(stderr, "No matching benchmarks!\n"); + fprintf(stderr, "Available benchmarks:\n"); + for (BenchmarkMapIt it = gBenchmarks().begin(); it != gBenchmarks().end(); ++it) { + fprintf(stderr, " %s\n", it->second->Name()); + } + exit(EXIT_FAILURE); + } + return 0; +} diff --git a/ground/gcs/src/libs/eigen/bench/tensors/contraction_benchmarks_cpu.cc b/ground/gcs/src/libs/eigen/bench/tensors/contraction_benchmarks_cpu.cc new file mode 100644 index 0000000000..f9e57ad474 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/contraction_benchmarks_cpu.cc @@ -0,0 +1,39 @@ +#define EIGEN_USE_THREADS + +#include + +#include "tensor_benchmarks.h" + +#define CREATE_THREAD_POOL(threads) \ +Eigen::ThreadPool pool(threads); \ +Eigen::ThreadPoolDevice device(&pool, threads); + + +// Contractions for number of threads ranging from 1 to 32 +// Dimensions are Rows, Cols, Depth +#define BM_ContractionCPU(D1, D2, D3) \ + static void BM_##Contraction##_##D1##x##D2##x##D3(int iters, int Threads) { \ + StopBenchmarkTiming(); \ + CREATE_THREAD_POOL(Threads); \ + BenchmarkSuite suite(device, D1, D2, D3); \ + suite.contraction(iters); \ + } \ + BENCHMARK_RANGE(BM_##Contraction##_##D1##x##D2##x##D3, 1, 32); + + +// Vector Matrix and Matrix Vector products +BM_ContractionCPU(1, 2000, 500); +BM_ContractionCPU(2000, 1, 500); + +// Various skinny matrices +BM_ContractionCPU(250, 3, 512); +BM_ContractionCPU(1500, 3, 512); + +BM_ContractionCPU(512, 800, 4); +BM_ContractionCPU(512, 80, 800); +BM_ContractionCPU(512, 80, 13522); +BM_ContractionCPU(1, 80, 13522); + +BM_ContractionCPU(3200, 512, 4); +BM_ContractionCPU(3200, 512, 80); +BM_ContractionCPU(3200, 80, 512); diff --git a/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks.h b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks.h new file mode 100644 index 0000000000..c2fb3dedef --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks.h @@ -0,0 +1,478 @@ +#ifndef THIRD_PARTY_EIGEN3_TENSOR_BENCHMARKS_H_ +#define THIRD_PARTY_EIGEN3_TENSOR_BENCHMARKS_H_ + +typedef int TensorIndex; +#define EIGEN_DEFAULT_DENSE_INDEX_TYPE int + +#include "unsupported/Eigen/CXX11/Tensor" +#include "benchmark.h" + +#define BENCHMARK_RANGE(bench, lo, hi) \ + BENCHMARK(bench)->Range(lo, hi) + +using Eigen::Tensor; +using Eigen::TensorMap; + +// TODO(bsteiner): also templatize on the input type since we have users +// for int8 as well as floats. +template class BenchmarkSuite { + public: + BenchmarkSuite(const Device& device, size_t m, size_t k, size_t n) + : m_(m), k_(k), n_(n), device_(device) { + initialize(); + } + + BenchmarkSuite(const Device& device, size_t m) + : m_(m), k_(m), n_(m), device_(device) { + initialize(); + } + + ~BenchmarkSuite() { + device_.deallocate(a_); + device_.deallocate(b_); + device_.deallocate(c_); + } + + void memcpy(int num_iters) { + eigen_assert(m_ == k_ && k_ == n_); + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + device_.memcpy(c_, a_, m_ * m_ * sizeof(T)); + } + // Record the number of values copied per second + finalizeBenchmark(static_cast(m_) * m_ * num_iters); + } + + void typeCasting(int num_iters) { + eigen_assert(m_ == n_); + Eigen::array sizes; + if (sizeof(T) >= sizeof(int)) { + sizes[0] = m_; + sizes[1] = k_; + } else { + sizes[0] = m_ * sizeof(T) / sizeof(int); + sizes[1] = k_ * sizeof(T) / sizeof(int); + } + const TensorMap, Eigen::Aligned> A((int*)a_, sizes); + TensorMap, Eigen::Aligned> B(b_, sizes); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + B.device(device_) = A.template cast(); + } + // Record the number of values copied per second + finalizeBenchmark(static_cast(m_) * k_ * num_iters); + } + + void random(int num_iters) { + eigen_assert(m_ == k_ && k_ == n_); + Eigen::array sizes; + sizes[0] = m_; + sizes[1] = m_; + TensorMap, Eigen::Aligned> C(c_, sizes); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = C.random(); + } + // Record the number of random numbers generated per second + finalizeBenchmark(static_cast(m_) * m_ * num_iters); + } + + void slicing(int num_iters) { + eigen_assert(m_ == k_ && k_ == n_); + Eigen::array sizes; + sizes[0] = m_; + sizes[1] = m_; + const TensorMap, Eigen::Aligned> A(a_, sizes); + const TensorMap, Eigen::Aligned> B(b_, sizes); + TensorMap, Eigen::Aligned> C(c_, sizes); + + const Eigen::DSizes quarter_sizes(m_/2, m_/2); + const Eigen::DSizes first_quadrant(0, 0); + const Eigen::DSizes second_quadrant(0, m_/2); + const Eigen::DSizes third_quadrant(m_/2, 0); + const Eigen::DSizes fourth_quadrant(m_/2, m_/2); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.slice(first_quadrant, quarter_sizes).device(device_) = + A.slice(first_quadrant, quarter_sizes); + C.slice(second_quadrant, quarter_sizes).device(device_) = + B.slice(second_quadrant, quarter_sizes); + C.slice(third_quadrant, quarter_sizes).device(device_) = + A.slice(third_quadrant, quarter_sizes); + C.slice(fourth_quadrant, quarter_sizes).device(device_) = + B.slice(fourth_quadrant, quarter_sizes); + } + // Record the number of values copied from the rhs slice to the lhs slice + // each second + finalizeBenchmark(static_cast(m_) * m_ * num_iters); + } + + void rowChip(int num_iters) { + Eigen::array input_size; + input_size[0] = k_; + input_size[1] = n_; + const TensorMap, Eigen::Aligned> B(b_, input_size); + Eigen::array output_size; + output_size[0] = n_; + TensorMap, Eigen::Aligned> C(c_, output_size); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = B.chip(iter % k_, 0); + } + // Record the number of values copied from the rhs chip to the lhs. + finalizeBenchmark(static_cast(n_) * num_iters); + } + + void colChip(int num_iters) { + Eigen::array input_size; + input_size[0] = k_; + input_size[1] = n_; + const TensorMap, Eigen::Aligned> B(b_, input_size); + Eigen::array output_size; + output_size[0] = n_; + TensorMap, Eigen::Aligned> C(c_, output_size); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = B.chip(iter % n_, 1); + } + // Record the number of values copied from the rhs chip to the lhs. + finalizeBenchmark(static_cast(n_) * num_iters); + } + + void shuffling(int num_iters) { + eigen_assert(m_ == n_); + Eigen::array size_a; + size_a[0] = m_; + size_a[1] = k_; + const TensorMap, Eigen::Aligned> A(a_, size_a); + Eigen::array size_b; + size_b[0] = k_; + size_b[1] = m_; + TensorMap, Eigen::Aligned> B(b_, size_b); + + Eigen::array shuffle; + shuffle[0] = 1; + shuffle[1] = 0; + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + B.device(device_) = A.shuffle(shuffle); + } + // Record the number of values shuffled from A and copied to B each second + finalizeBenchmark(static_cast(m_) * k_ * num_iters); + } + + void padding(int num_iters) { + eigen_assert(m_ == k_); + Eigen::array size_a; + size_a[0] = m_; + size_a[1] = k_-3; + const TensorMap, Eigen::Aligned> A(a_, size_a); + Eigen::array size_b; + size_b[0] = k_; + size_b[1] = m_; + TensorMap, Eigen::Aligned> B(b_, size_b); + +#if defined(EIGEN_HAS_INDEX_LIST) + Eigen::IndexPairList, + Eigen::type2indexpair<2, 1> > paddings; +#else + Eigen::array, 2> paddings; + paddings[0] = Eigen::IndexPair(0, 0); + paddings[1] = Eigen::IndexPair(2, 1); +#endif + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + B.device(device_) = A.pad(paddings); + } + // Record the number of values copied from the padded tensor A each second + finalizeBenchmark(static_cast(m_) * k_ * num_iters); + } + + void striding(int num_iters) { + eigen_assert(m_ == k_); + Eigen::array size_a; + size_a[0] = m_; + size_a[1] = k_; + const TensorMap, Eigen::Aligned> A(a_, size_a); + Eigen::array size_b; + size_b[0] = m_; + size_b[1] = k_/2; + TensorMap, Eigen::Aligned> B(b_, size_b); + +#ifndef EIGEN_HAS_INDEX_LIST + Eigen::array strides; + strides[0] = 1; + strides[1] = 2; +#else + // Take advantage of cxx11 to give the compiler information it can use to + // optimize the code. + Eigen::IndexList, Eigen::type2index<2> > strides; +#endif + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + B.device(device_) = A.stride(strides); + } + // Record the number of values copied from the padded tensor A each second + finalizeBenchmark(static_cast(m_) * k_ * num_iters); + } + + void broadcasting(int num_iters) { + Eigen::array size_a; + size_a[0] = m_; + size_a[1] = 1; + const TensorMap, Eigen::Aligned> A(a_, size_a); + Eigen::array size_c; + size_c[0] = m_; + size_c[1] = n_; + TensorMap, Eigen::Aligned> C(c_, size_c); + +#ifndef EIGEN_HAS_INDEX_LIST + Eigen::array broadcast; + broadcast[0] = 1; + broadcast[1] = n_; +#else + // Take advantage of cxx11 to give the compiler information it can use to + // optimize the code. + Eigen::IndexList, int> broadcast; + broadcast.set(1, n_); +#endif + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = A.broadcast(broadcast); + } + // Record the number of values broadcasted from A and copied to C each second + finalizeBenchmark(static_cast(m_) * n_ * num_iters); + } + + void coeffWiseOp(int num_iters) { + eigen_assert(m_ == k_ && k_ == n_); + Eigen::array sizes; + sizes[0] = m_; + sizes[1] = m_; + const TensorMap, Eigen::Aligned> A(a_, sizes); + const TensorMap, Eigen::Aligned> B(b_, sizes); + TensorMap, Eigen::Aligned> C(c_, sizes); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = A * A.constant(static_cast(3.14)) + B * B.constant(static_cast(2.7)); + } + // Record the number of FLOP executed per second (2 multiplications and + // 1 addition per value) + finalizeBenchmark(static_cast(3) * m_ * m_ * num_iters); + } + + void algebraicFunc(int num_iters) { + eigen_assert(m_ == k_ && k_ == n_); + Eigen::array sizes; + sizes[0] = m_; + sizes[1] = m_; + const TensorMap, Eigen::Aligned> A(a_, sizes); + const TensorMap, Eigen::Aligned> B(b_, sizes); + TensorMap, Eigen::Aligned> C(c_, sizes); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = A.rsqrt() + B.sqrt() * B.square(); + } + // Record the number of FLOP executed per second (assuming one operation + // per value) + finalizeBenchmark(static_cast(m_) * m_ * num_iters); + } + + void transcendentalFunc(int num_iters) { + eigen_assert(m_ == k_ && k_ == n_); + Eigen::array sizes; + sizes[0] = m_; + sizes[1] = m_; + const TensorMap, Eigen::Aligned> A(a_, sizes); + const TensorMap, Eigen::Aligned> B(b_, sizes); + TensorMap, Eigen::Aligned> C(c_, sizes); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = A.exp() + B.log(); + } + // Record the number of FLOP executed per second (assuming one operation + // per value) + finalizeBenchmark(static_cast(m_) * m_ * num_iters); + } + + // Row reduction + void rowReduction(int num_iters) { + Eigen::array input_size; + input_size[0] = k_; + input_size[1] = n_; + const TensorMap, Eigen::Aligned> B(b_, input_size); + Eigen::array output_size; + output_size[0] = n_; + TensorMap, Eigen::Aligned> C(c_, output_size); + +#ifndef EIGEN_HAS_INDEX_LIST + Eigen::array sum_along_dim; + sum_along_dim[0] = 0; +#else + // Take advantage of cxx11 to give the compiler information it can use to + // optimize the code. + Eigen::IndexList> sum_along_dim; +#endif + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = B.sum(sum_along_dim); + } + // Record the number of FLOP executed per second (assuming one operation + // per value) + finalizeBenchmark(static_cast(k_) * n_ * num_iters); + } + + // Column reduction + void colReduction(int num_iters) { + Eigen::array input_size; + input_size[0] = k_; + input_size[1] = n_; + const TensorMap, Eigen::Aligned> B( + b_, input_size); + Eigen::array output_size; + output_size[0] = k_; + TensorMap, Eigen::Aligned> C( + c_, output_size); + +#ifndef EIGEN_HAS_INDEX_LIST + Eigen::array sum_along_dim; + sum_along_dim[0] = 1; +#else + // Take advantage of cxx11 to give the compiler information it can use to + // optimize the code. + Eigen::IndexList> sum_along_dim; +#endif + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = B.sum(sum_along_dim); + } + // Record the number of FLOP executed per second (assuming one operation + // per value) + finalizeBenchmark(static_cast(k_) * n_ * num_iters); + } + + // Full reduction + void fullReduction(int num_iters) { + Eigen::array input_size; + input_size[0] = k_; + input_size[1] = n_; + const TensorMap, Eigen::Aligned> B( + b_, input_size); + Eigen::array output_size; + TensorMap, Eigen::Aligned> C( + c_, output_size); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = B.sum(); + } + // Record the number of FLOP executed per second (assuming one operation + // per value) + finalizeBenchmark(static_cast(k_) * n_ * num_iters); + } + + // do a contraction which is equivalent to a matrix multiplication + void contraction(int num_iters) { + Eigen::array sizeA; + sizeA[0] = m_; + sizeA[1] = k_; + Eigen::array sizeB; + sizeB[0] = k_; + sizeB[1] = n_; + Eigen::array sizeC; + sizeC[0] = m_; + sizeC[1] = n_; + + const TensorMap, Eigen::Aligned> A(a_, sizeA); + const TensorMap, Eigen::Aligned> B(b_, sizeB); + TensorMap, Eigen::Aligned> C(c_, sizeC); + + typedef typename Tensor::DimensionPair DimPair; + Eigen::array dims; + dims[0] = DimPair(1, 0); + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = A.contract(B, dims); + } + // Record the number of FLOP executed per second (size_ multiplications and + // additions for each value in the resulting tensor) + finalizeBenchmark(static_cast(2) * m_ * n_ * k_ * num_iters); + } + + void convolution(int num_iters, int kernel_x, int kernel_y) { + Eigen::array input_sizes; + input_sizes[0] = m_; + input_sizes[1] = n_; + TensorMap, Eigen::Aligned> A(a_, input_sizes); + Eigen::array kernel_sizes; + kernel_sizes[0] = kernel_x; + kernel_sizes[1] = kernel_y; + TensorMap, Eigen::Aligned> B(b_, kernel_sizes); + Eigen::array result_sizes; + result_sizes[0] = m_ - kernel_x + 1; + result_sizes[1] = n_ - kernel_y + 1; + TensorMap, Eigen::Aligned> C(c_, result_sizes); + Eigen::array dims; + dims[0] = 0; + dims[1] = 1; + + StartBenchmarkTiming(); + for (int iter = 0; iter < num_iters; ++iter) { + C.device(device_) = A.convolve(B, dims); + } + // Record the number of FLOP executed per second (kernel_size + // multiplications and additions for each value in the resulting tensor) + finalizeBenchmark(static_cast(2) * + (m_ - kernel_x + 1) * (n_ - kernel_y + 1) * kernel_x * kernel_y * num_iters); + } + + private: + void initialize() { + a_ = (T *) device_.allocate(m_ * k_ * sizeof(T)); + b_ = (T *) device_.allocate(k_ * n_ * sizeof(T)); + c_ = (T *) device_.allocate(m_ * n_ * sizeof(T)); + + // Initialize the content of the memory pools to prevent asan from + // complaining. + device_.memset(a_, 12, m_ * k_ * sizeof(T)); + device_.memset(b_, 23, k_ * n_ * sizeof(T)); + device_.memset(c_, 31, m_ * n_ * sizeof(T)); + + //BenchmarkUseRealTime(); + } + + inline void finalizeBenchmark(int64_t num_items) { +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) + if (Eigen::internal::is_same::value) { + device_.synchronize(); + } +#endif + StopBenchmarkTiming(); + SetBenchmarkFlopsProcessed(num_items); + } + + + TensorIndex m_; + TensorIndex k_; + TensorIndex n_; + T* a_; + T* b_; + T* c_; + Device device_; +}; +#endif // THIRD_PARTY_EIGEN3_TENSOR_BENCHMARKS_H_ diff --git a/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_cpu.cc b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_cpu.cc new file mode 100644 index 0000000000..8947f4b7fc --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_cpu.cc @@ -0,0 +1,168 @@ +#define EIGEN_USE_THREADS + +#include + +#include "tensor_benchmarks.h" + +#define CREATE_THREAD_POOL(threads) \ +Eigen::ThreadPool pool(threads); \ +Eigen::ThreadPoolDevice device(&pool, threads); + +// Simple functions +#define BM_FuncCPU(FUNC, THREADS) \ + static void BM_##FUNC##_##THREADS##T(int iters, int N) { \ + StopBenchmarkTiming(); \ + CREATE_THREAD_POOL(THREADS); \ + BenchmarkSuite suite(device, N); \ + suite.FUNC(iters); \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##THREADS##T, 10, 5000); + +BM_FuncCPU(memcpy, 4); +BM_FuncCPU(memcpy, 8); +BM_FuncCPU(memcpy, 12); + +BM_FuncCPU(typeCasting, 4); +BM_FuncCPU(typeCasting, 8); +BM_FuncCPU(typeCasting, 12); + +BM_FuncCPU(random, 4); +BM_FuncCPU(random, 8); +BM_FuncCPU(random, 12); + +BM_FuncCPU(slicing, 4); +BM_FuncCPU(slicing, 8); +BM_FuncCPU(slicing, 12); + +BM_FuncCPU(rowChip, 4); +BM_FuncCPU(rowChip, 8); +BM_FuncCPU(rowChip, 12); + +BM_FuncCPU(colChip, 4); +BM_FuncCPU(colChip, 8); +BM_FuncCPU(colChip, 12); + +BM_FuncCPU(shuffling, 4); +BM_FuncCPU(shuffling, 8); +BM_FuncCPU(shuffling, 12); + +BM_FuncCPU(padding, 4); +BM_FuncCPU(padding, 8); +BM_FuncCPU(padding, 12); + +BM_FuncCPU(striding, 4); +BM_FuncCPU(striding, 8); +BM_FuncCPU(striding, 12); + +BM_FuncCPU(broadcasting, 4); +BM_FuncCPU(broadcasting, 8); +BM_FuncCPU(broadcasting, 12); + +BM_FuncCPU(coeffWiseOp, 4); +BM_FuncCPU(coeffWiseOp, 8); +BM_FuncCPU(coeffWiseOp, 12); + +BM_FuncCPU(algebraicFunc, 4); +BM_FuncCPU(algebraicFunc, 8); +BM_FuncCPU(algebraicFunc, 12); + +BM_FuncCPU(transcendentalFunc, 4); +BM_FuncCPU(transcendentalFunc, 8); +BM_FuncCPU(transcendentalFunc, 12); + +BM_FuncCPU(rowReduction, 4); +BM_FuncCPU(rowReduction, 8); +BM_FuncCPU(rowReduction, 12); + +BM_FuncCPU(colReduction, 4); +BM_FuncCPU(colReduction, 8); +BM_FuncCPU(colReduction, 12); + + +// Contractions +#define BM_FuncWithInputDimsCPU(FUNC, D1, D2, D3, THREADS) \ + static void BM_##FUNC##_##D1##x##D2##x##D3##_##THREADS##T(int iters, int N) { \ + StopBenchmarkTiming(); \ + if (THREADS == 1) { \ + Eigen::DefaultDevice device; \ + BenchmarkSuite suite(device, D1, D2, D3); \ + suite.FUNC(iters); \ + } else { \ + CREATE_THREAD_POOL(THREADS); \ + BenchmarkSuite suite(device, D1, D2, D3); \ + suite.FUNC(iters); \ + } \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##D1##x##D2##x##D3##_##THREADS##T, 10, 5000); + + +BM_FuncWithInputDimsCPU(contraction, N, N, N, 1); +BM_FuncWithInputDimsCPU(contraction, N, N, N, 4); +BM_FuncWithInputDimsCPU(contraction, N, N, N, 8); +BM_FuncWithInputDimsCPU(contraction, N, N, N, 12); +BM_FuncWithInputDimsCPU(contraction, N, N, N, 16); + +BM_FuncWithInputDimsCPU(contraction, 64, N, N, 1); +BM_FuncWithInputDimsCPU(contraction, 64, N, N, 4); +BM_FuncWithInputDimsCPU(contraction, 64, N, N, 8); +BM_FuncWithInputDimsCPU(contraction, 64, N, N, 12); +BM_FuncWithInputDimsCPU(contraction, 64, N, N, 16); + +BM_FuncWithInputDimsCPU(contraction, N, 64, N, 1); +BM_FuncWithInputDimsCPU(contraction, N, 64, N, 4); +BM_FuncWithInputDimsCPU(contraction, N, 64, N, 8); +BM_FuncWithInputDimsCPU(contraction, N, 64, N, 12); +BM_FuncWithInputDimsCPU(contraction, N, 64, N, 16); + +BM_FuncWithInputDimsCPU(contraction, N, N, 64, 1); +BM_FuncWithInputDimsCPU(contraction, N, N, 64, 4); +BM_FuncWithInputDimsCPU(contraction, N, N, 64, 8); +BM_FuncWithInputDimsCPU(contraction, N, N, 64, 12); +BM_FuncWithInputDimsCPU(contraction, N, N, 64, 16); + +BM_FuncWithInputDimsCPU(contraction, 1, N, N, 1); +BM_FuncWithInputDimsCPU(contraction, 1, N, N, 4); +BM_FuncWithInputDimsCPU(contraction, 1, N, N, 8); +BM_FuncWithInputDimsCPU(contraction, 1, N, N, 12); +BM_FuncWithInputDimsCPU(contraction, 1, N, N, 16); + +BM_FuncWithInputDimsCPU(contraction, N, N, 1, 1); +BM_FuncWithInputDimsCPU(contraction, N, N, 1, 4); +BM_FuncWithInputDimsCPU(contraction, N, N, 1, 8); +BM_FuncWithInputDimsCPU(contraction, N, N, 1, 12); +BM_FuncWithInputDimsCPU(contraction, N, N, 1, 16); + + +// Convolutions +#define BM_FuncWithKernelDimsCPU(FUNC, DIM1, DIM2, THREADS) \ + static void BM_##FUNC##_##DIM1##x##DIM2##_##THREADS##T(int iters, int N) { \ + StopBenchmarkTiming(); \ + CREATE_THREAD_POOL(THREADS); \ + BenchmarkSuite suite(device, N); \ + suite.FUNC(iters, DIM1, DIM2); \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##DIM1##x##DIM2##_##THREADS##T, 128, 5000); + +BM_FuncWithKernelDimsCPU(convolution, 7, 1, 4); +BM_FuncWithKernelDimsCPU(convolution, 7, 1, 8); +BM_FuncWithKernelDimsCPU(convolution, 7, 1, 12); + +BM_FuncWithKernelDimsCPU(convolution, 1, 7, 4); +BM_FuncWithKernelDimsCPU(convolution, 1, 7, 8); +BM_FuncWithKernelDimsCPU(convolution, 1, 7, 12); + +BM_FuncWithKernelDimsCPU(convolution, 7, 4, 4); +BM_FuncWithKernelDimsCPU(convolution, 7, 4, 8); +BM_FuncWithKernelDimsCPU(convolution, 7, 4, 12); + +BM_FuncWithKernelDimsCPU(convolution, 4, 7, 4); +BM_FuncWithKernelDimsCPU(convolution, 4, 7, 8); +BM_FuncWithKernelDimsCPU(convolution, 4, 7, 12); + +BM_FuncWithKernelDimsCPU(convolution, 7, 64, 4); +BM_FuncWithKernelDimsCPU(convolution, 7, 64, 8); +BM_FuncWithKernelDimsCPU(convolution, 7, 64, 12); + +BM_FuncWithKernelDimsCPU(convolution, 64, 7, 4); +BM_FuncWithKernelDimsCPU(convolution, 64, 7, 8); +BM_FuncWithKernelDimsCPU(convolution, 64, 7, 12); diff --git a/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_fp16_gpu.cu b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_fp16_gpu.cu new file mode 100644 index 0000000000..65784d0d67 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_fp16_gpu.cu @@ -0,0 +1,77 @@ +#define EIGEN_USE_GPU + +#include +#include +#include + +#include "tensor_benchmarks.h" + +// Simple functions +#define BM_FuncGPU(FUNC) \ + static void BM_##FUNC(int iters, int N) { \ + StopBenchmarkTiming(); \ + Eigen::CudaStreamDevice stream; \ + Eigen::GpuDevice device(&stream); \ + BenchmarkSuite suite(device, N); \ + cudaDeviceSynchronize(); \ + suite.FUNC(iters); \ + } \ + BENCHMARK_RANGE(BM_##FUNC, 10, 5000); + +BM_FuncGPU(memcpy); +BM_FuncGPU(typeCasting); +//BM_FuncGPU(random); +BM_FuncGPU(slicing); +BM_FuncGPU(rowChip); +BM_FuncGPU(colChip); +BM_FuncGPU(shuffling); +BM_FuncGPU(padding); +BM_FuncGPU(striding); +BM_FuncGPU(broadcasting); +BM_FuncGPU(coeffWiseOp); +BM_FuncGPU(algebraicFunc); +BM_FuncGPU(transcendentalFunc); +BM_FuncGPU(rowReduction); +BM_FuncGPU(colReduction); +BM_FuncGPU(fullReduction); + + +// Contractions +#define BM_FuncWithInputDimsGPU(FUNC, D1, D2, D3) \ + static void BM_##FUNC##_##D1##x##D2##x##D3(int iters, int N) { \ + StopBenchmarkTiming(); \ + Eigen::CudaStreamDevice stream; \ + Eigen::GpuDevice device(&stream); \ + BenchmarkSuite suite(device, D1, D2, D3); \ + cudaDeviceSynchronize(); \ + suite.FUNC(iters); \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##D1##x##D2##x##D3, 10, 5000); + + +BM_FuncWithInputDimsGPU(contraction, N, N, N); +BM_FuncWithInputDimsGPU(contraction, 64, N, N); +BM_FuncWithInputDimsGPU(contraction, N, 64, N); +BM_FuncWithInputDimsGPU(contraction, N, N, 64); + + +// Convolutions +#define BM_FuncWithKernelDimsGPU(FUNC, DIM1, DIM2) \ + static void BM_##FUNC##_##DIM1##x##DIM2(int iters, int N) { \ + StopBenchmarkTiming(); \ + Eigen::CudaStreamDevice stream; \ + Eigen::GpuDevice device(&stream); \ + BenchmarkSuite suite(device, N); \ + cudaDeviceSynchronize(); \ + suite.FUNC(iters, DIM1, DIM2); \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##DIM1##x##DIM2, 128, 5000); + +/* +BM_FuncWithKernelDimsGPU(convolution, 7, 1); +BM_FuncWithKernelDimsGPU(convolution, 1, 7); +BM_FuncWithKernelDimsGPU(convolution, 7, 4); +BM_FuncWithKernelDimsGPU(convolution, 4, 7); +BM_FuncWithKernelDimsGPU(convolution, 7, 64); +BM_FuncWithKernelDimsGPU(convolution, 64, 7); +*/ diff --git a/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_gpu.cu b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_gpu.cu new file mode 100644 index 0000000000..76d68c5c1b --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_gpu.cu @@ -0,0 +1,75 @@ +#define EIGEN_USE_GPU + +#include +#include +#include + +#include "tensor_benchmarks.h" + +// Simple functions +#define BM_FuncGPU(FUNC) \ + static void BM_##FUNC(int iters, int N) { \ + StopBenchmarkTiming(); \ + Eigen::CudaStreamDevice stream; \ + Eigen::GpuDevice device(&stream); \ + BenchmarkSuite suite(device, N); \ + cudaDeviceSynchronize(); \ + suite.FUNC(iters); \ + } \ + BENCHMARK_RANGE(BM_##FUNC, 10, 5000); + +BM_FuncGPU(memcpy); +BM_FuncGPU(typeCasting); +BM_FuncGPU(random); +BM_FuncGPU(slicing); +BM_FuncGPU(rowChip); +BM_FuncGPU(colChip); +BM_FuncGPU(shuffling); +BM_FuncGPU(padding); +BM_FuncGPU(striding); +BM_FuncGPU(broadcasting); +BM_FuncGPU(coeffWiseOp); +BM_FuncGPU(algebraicFunc); +BM_FuncGPU(transcendentalFunc); +BM_FuncGPU(rowReduction); +BM_FuncGPU(colReduction); +BM_FuncGPU(fullReduction); + + +// Contractions +#define BM_FuncWithInputDimsGPU(FUNC, D1, D2, D3) \ + static void BM_##FUNC##_##D1##x##D2##x##D3(int iters, int N) { \ + StopBenchmarkTiming(); \ + Eigen::CudaStreamDevice stream; \ + Eigen::GpuDevice device(&stream); \ + BenchmarkSuite suite(device, D1, D2, D3); \ + cudaDeviceSynchronize(); \ + suite.FUNC(iters); \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##D1##x##D2##x##D3, 10, 5000); + + +BM_FuncWithInputDimsGPU(contraction, N, N, N); +BM_FuncWithInputDimsGPU(contraction, 64, N, N); +BM_FuncWithInputDimsGPU(contraction, N, 64, N); +BM_FuncWithInputDimsGPU(contraction, N, N, 64); + + +// Convolutions +#define BM_FuncWithKernelDimsGPU(FUNC, DIM1, DIM2) \ + static void BM_##FUNC##_##DIM1##x##DIM2(int iters, int N) { \ + StopBenchmarkTiming(); \ + Eigen::CudaStreamDevice stream; \ + Eigen::GpuDevice device(&stream); \ + BenchmarkSuite suite(device, N); \ + cudaDeviceSynchronize(); \ + suite.FUNC(iters, DIM1, DIM2); \ + } \ + BENCHMARK_RANGE(BM_##FUNC##_##DIM1##x##DIM2, 128, 5000); + +BM_FuncWithKernelDimsGPU(convolution, 7, 1); +BM_FuncWithKernelDimsGPU(convolution, 1, 7); +BM_FuncWithKernelDimsGPU(convolution, 7, 4); +BM_FuncWithKernelDimsGPU(convolution, 4, 7); +BM_FuncWithKernelDimsGPU(convolution, 7, 64); +BM_FuncWithKernelDimsGPU(convolution, 64, 7); diff --git a/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_sycl.cc b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_sycl.cc new file mode 100644 index 0000000000..7eca4d9660 --- /dev/null +++ b/ground/gcs/src/libs/eigen/bench/tensors/tensor_benchmarks_sycl.cc @@ -0,0 +1,37 @@ +#define EIGEN_USE_SYCL + +#include +#include + +#include "tensor_benchmarks.h" + +using Eigen::array; +using Eigen::SyclDevice; +using Eigen::Tensor; +using Eigen::TensorMap; +// Simple functions +template +cl::sycl::queue sycl_queue() { + return cl::sycl::queue(device_selector(), [=](cl::sycl::exception_list l) { + for (const auto& e : l) { + try { + std::rethrow_exception(e); + } catch (cl::sycl::exception e) { + std::cout << e.what() << std::endl; + } + } + }); +} + +#define BM_FuncGPU(FUNC) \ + static void BM_##FUNC(int iters, int N) { \ + StopBenchmarkTiming(); \ + cl::sycl::queue q = sycl_queue(); \ + Eigen::SyclDevice device(q); \ + BenchmarkSuite suite(device, N); \ + suite.FUNC(iters); \ + } \ + BENCHMARK_RANGE(BM_##FUNC, 10, 5000); + +BM_FuncGPU(broadcasting); +BM_FuncGPU(coeffWiseOp); diff --git a/ground/gcs/src/libs/eigen/blas/CMakeLists.txt b/ground/gcs/src/libs/eigen/blas/CMakeLists.txt index a9bc051374..d0efb41885 100644 --- a/ground/gcs/src/libs/eigen/blas/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/blas/CMakeLists.txt @@ -14,23 +14,18 @@ endif() add_custom_target(blas) -set(EigenBlas_SRCS single.cpp double.cpp complex_single.cpp complex_double.cpp xerbla.cpp) - -if(EIGEN_Fortran_COMPILER_WORKS) - -set(EigenBlas_SRCS ${EigenBlas_SRCS} - complexdots.f - srotm.f srotmg.f drotm.f drotmg.f - lsame.f dspmv.f ssbmv.f - chbmv.f sspmv.f - zhbmv.f chpmv.f dsbmv.f - zhpmv.f - dtbmv.f stbmv.f ctbmv.f ztbmv.f -) +set(EigenBlas_SRCS single.cpp double.cpp complex_single.cpp complex_double.cpp xerbla.cpp + f2c/srotm.c f2c/srotmg.c f2c/drotm.c f2c/drotmg.c + f2c/lsame.c f2c/dspmv.c f2c/ssbmv.c f2c/chbmv.c + f2c/sspmv.c f2c/zhbmv.c f2c/chpmv.c f2c/dsbmv.c + f2c/zhpmv.c f2c/dtbmv.c f2c/stbmv.c f2c/ctbmv.c + f2c/ztbmv.c f2c/d_cnjg.c f2c/r_cnjg.c + ) + +if (EIGEN_Fortran_COMPILER_WORKS) + set(EigenBlas_SRCS ${EigenBlas_SRCS} fortran/complexdots.f) else() - -message(WARNING " No fortran compiler has been detected, the blas build will be incomplete.") - + set(EigenBlas_SRCS ${EigenBlas_SRCS} f2c/complexdots.c) endif() add_library(eigen_blas_static ${EigenBlas_SRCS}) diff --git a/ground/gcs/src/libs/eigen/blas/PackedTriangularMatrixVector.h b/ground/gcs/src/libs/eigen/blas/PackedTriangularMatrixVector.h index e9886d56fb..0039536a86 100644 --- a/ground/gcs/src/libs/eigen/blas/PackedTriangularMatrixVector.h +++ b/ground/gcs/src/libs/eigen/blas/PackedTriangularMatrixVector.h @@ -18,7 +18,7 @@ struct packed_triangular_matrix_vector_product; template struct packed_triangular_matrix_vector_product { - typedef typename scalar_product_traits::ReturnType ResScalar; + typedef typename ScalarBinaryOpTraits::ReturnType ResScalar; enum { IsLower = (Mode & Lower) ==Lower, HasUnitDiag = (Mode & UnitDiag)==UnitDiag, @@ -47,7 +47,7 @@ struct packed_triangular_matrix_vector_product struct packed_triangular_matrix_vector_product { - typedef typename scalar_product_traits::ReturnType ResScalar; + typedef typename ScalarBinaryOpTraits::ReturnType ResScalar; enum { IsLower = (Mode & Lower) ==Lower, HasUnitDiag = (Mode & UnitDiag)==UnitDiag, diff --git a/ground/gcs/src/libs/eigen/blas/chbmv.f b/ground/gcs/src/libs/eigen/blas/chbmv.f deleted file mode 100644 index 1b1c330ea0..0000000000 --- a/ground/gcs/src/libs/eigen/blas/chbmv.f +++ /dev/null @@ -1,310 +0,0 @@ - SUBROUTINE CHBMV(UPLO,N,K,ALPHA,A,LDA,X,INCX,BETA,Y,INCY) -* .. Scalar Arguments .. - COMPLEX ALPHA,BETA - INTEGER INCX,INCY,K,LDA,N - CHARACTER UPLO -* .. -* .. Array Arguments .. - COMPLEX A(LDA,*),X(*),Y(*) -* .. -* -* Purpose -* ======= -* -* CHBMV performs the matrix-vector operation -* -* y := alpha*A*x + beta*y, -* -* where alpha and beta are scalars, x and y are n element vectors and -* A is an n by n hermitian band matrix, with k super-diagonals. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the upper or lower -* triangular part of the band matrix A is being supplied as -* follows: -* -* UPLO = 'U' or 'u' The upper triangular part of A is -* being supplied. -* -* UPLO = 'L' or 'l' The lower triangular part of A is -* being supplied. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* K - INTEGER. -* On entry, K specifies the number of super-diagonals of the -* matrix A. K must satisfy 0 .le. K. -* Unchanged on exit. -* -* ALPHA - COMPLEX . -* On entry, ALPHA specifies the scalar alpha. -* Unchanged on exit. -* -* A - COMPLEX array of DIMENSION ( LDA, n ). -* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) -* by n part of the array A must contain the upper triangular -* band part of the hermitian matrix, supplied column by -* column, with the leading diagonal of the matrix in row -* ( k + 1 ) of the array, the first super-diagonal starting at -* position 2 in row k, and so on. The top left k by k triangle -* of the array A is not referenced. -* The following program segment will transfer the upper -* triangular part of a hermitian band matrix from conventional -* full matrix storage to band storage: -* -* DO 20, J = 1, N -* M = K + 1 - J -* DO 10, I = MAX( 1, J - K ), J -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) -* by n part of the array A must contain the lower triangular -* band part of the hermitian matrix, supplied column by -* column, with the leading diagonal of the matrix in row 1 of -* the array, the first sub-diagonal starting at position 1 in -* row 2, and so on. The bottom right k by k triangle of the -* array A is not referenced. -* The following program segment will transfer the lower -* triangular part of a hermitian band matrix from conventional -* full matrix storage to band storage: -* -* DO 20, J = 1, N -* M = 1 - J -* DO 10, I = J, MIN( N, J + K ) -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Note that the imaginary parts of the diagonal elements need -* not be set and are assumed to be zero. -* Unchanged on exit. -* -* LDA - INTEGER. -* On entry, LDA specifies the first dimension of A as declared -* in the calling (sub) program. LDA must be at least -* ( k + 1 ). -* Unchanged on exit. -* -* X - COMPLEX array of DIMENSION at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the -* vector x. -* Unchanged on exit. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* BETA - COMPLEX . -* On entry, BETA specifies the scalar beta. -* Unchanged on exit. -* -* Y - COMPLEX array of DIMENSION at least -* ( 1 + ( n - 1 )*abs( INCY ) ). -* Before entry, the incremented array Y must contain the -* vector y. On exit, Y is overwritten by the updated vector y. -* -* INCY - INTEGER. -* On entry, INCY specifies the increment for the elements of -* Y. INCY must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - COMPLEX ONE - PARAMETER (ONE= (1.0E+0,0.0E+0)) - COMPLEX ZERO - PARAMETER (ZERO= (0.0E+0,0.0E+0)) -* .. -* .. Local Scalars .. - COMPLEX TEMP1,TEMP2 - INTEGER I,INFO,IX,IY,J,JX,JY,KPLUS1,KX,KY,L -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC CONJG,MAX,MIN,REAL -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (N.LT.0) THEN - INFO = 2 - ELSE IF (K.LT.0) THEN - INFO = 3 - ELSE IF (LDA.LT. (K+1)) THEN - INFO = 6 - ELSE IF (INCX.EQ.0) THEN - INFO = 8 - ELSE IF (INCY.EQ.0) THEN - INFO = 11 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('CHBMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF ((N.EQ.0) .OR. ((ALPHA.EQ.ZERO).AND. (BETA.EQ.ONE))) RETURN -* -* Set up the start points in X and Y. -* - IF (INCX.GT.0) THEN - KX = 1 - ELSE - KX = 1 - (N-1)*INCX - END IF - IF (INCY.GT.0) THEN - KY = 1 - ELSE - KY = 1 - (N-1)*INCY - END IF -* -* Start the operations. In this version the elements of the array A -* are accessed sequentially with one pass through A. -* -* First form y := beta*y. -* - IF (BETA.NE.ONE) THEN - IF (INCY.EQ.1) THEN - IF (BETA.EQ.ZERO) THEN - DO 10 I = 1,N - Y(I) = ZERO - 10 CONTINUE - ELSE - DO 20 I = 1,N - Y(I) = BETA*Y(I) - 20 CONTINUE - END IF - ELSE - IY = KY - IF (BETA.EQ.ZERO) THEN - DO 30 I = 1,N - Y(IY) = ZERO - IY = IY + INCY - 30 CONTINUE - ELSE - DO 40 I = 1,N - Y(IY) = BETA*Y(IY) - IY = IY + INCY - 40 CONTINUE - END IF - END IF - END IF - IF (ALPHA.EQ.ZERO) RETURN - IF (LSAME(UPLO,'U')) THEN -* -* Form y when upper triangle of A is stored. -* - KPLUS1 = K + 1 - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 60 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - L = KPLUS1 - J - DO 50 I = MAX(1,J-K),J - 1 - Y(I) = Y(I) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + CONJG(A(L+I,J))*X(I) - 50 CONTINUE - Y(J) = Y(J) + TEMP1*REAL(A(KPLUS1,J)) + ALPHA*TEMP2 - 60 CONTINUE - ELSE - JX = KX - JY = KY - DO 80 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - IX = KX - IY = KY - L = KPLUS1 - J - DO 70 I = MAX(1,J-K),J - 1 - Y(IY) = Y(IY) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + CONJG(A(L+I,J))*X(IX) - IX = IX + INCX - IY = IY + INCY - 70 CONTINUE - Y(JY) = Y(JY) + TEMP1*REAL(A(KPLUS1,J)) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - IF (J.GT.K) THEN - KX = KX + INCX - KY = KY + INCY - END IF - 80 CONTINUE - END IF - ELSE -* -* Form y when lower triangle of A is stored. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 100 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - Y(J) = Y(J) + TEMP1*REAL(A(1,J)) - L = 1 - J - DO 90 I = J + 1,MIN(N,J+K) - Y(I) = Y(I) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + CONJG(A(L+I,J))*X(I) - 90 CONTINUE - Y(J) = Y(J) + ALPHA*TEMP2 - 100 CONTINUE - ELSE - JX = KX - JY = KY - DO 120 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - Y(JY) = Y(JY) + TEMP1*REAL(A(1,J)) - L = 1 - J - IX = JX - IY = JY - DO 110 I = J + 1,MIN(N,J+K) - IX = IX + INCX - IY = IY + INCY - Y(IY) = Y(IY) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + CONJG(A(L+I,J))*X(IX) - 110 CONTINUE - Y(JY) = Y(JY) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - 120 CONTINUE - END IF - END IF -* - RETURN -* -* End of CHBMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/chpmv.f b/ground/gcs/src/libs/eigen/blas/chpmv.f deleted file mode 100644 index 158be5a7b7..0000000000 --- a/ground/gcs/src/libs/eigen/blas/chpmv.f +++ /dev/null @@ -1,272 +0,0 @@ - SUBROUTINE CHPMV(UPLO,N,ALPHA,AP,X,INCX,BETA,Y,INCY) -* .. Scalar Arguments .. - COMPLEX ALPHA,BETA - INTEGER INCX,INCY,N - CHARACTER UPLO -* .. -* .. Array Arguments .. - COMPLEX AP(*),X(*),Y(*) -* .. -* -* Purpose -* ======= -* -* CHPMV performs the matrix-vector operation -* -* y := alpha*A*x + beta*y, -* -* where alpha and beta are scalars, x and y are n element vectors and -* A is an n by n hermitian matrix, supplied in packed form. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the upper or lower -* triangular part of the matrix A is supplied in the packed -* array AP as follows: -* -* UPLO = 'U' or 'u' The upper triangular part of A is -* supplied in AP. -* -* UPLO = 'L' or 'l' The lower triangular part of A is -* supplied in AP. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* ALPHA - COMPLEX . -* On entry, ALPHA specifies the scalar alpha. -* Unchanged on exit. -* -* AP - COMPLEX array of DIMENSION at least -* ( ( n*( n + 1 ) )/2 ). -* Before entry with UPLO = 'U' or 'u', the array AP must -* contain the upper triangular part of the hermitian matrix -* packed sequentially, column by column, so that AP( 1 ) -* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) -* and a( 2, 2 ) respectively, and so on. -* Before entry with UPLO = 'L' or 'l', the array AP must -* contain the lower triangular part of the hermitian matrix -* packed sequentially, column by column, so that AP( 1 ) -* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) -* and a( 3, 1 ) respectively, and so on. -* Note that the imaginary parts of the diagonal elements need -* not be set and are assumed to be zero. -* Unchanged on exit. -* -* X - COMPLEX array of dimension at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the n -* element vector x. -* Unchanged on exit. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* BETA - COMPLEX . -* On entry, BETA specifies the scalar beta. When BETA is -* supplied as zero then Y need not be set on input. -* Unchanged on exit. -* -* Y - COMPLEX array of dimension at least -* ( 1 + ( n - 1 )*abs( INCY ) ). -* Before entry, the incremented array Y must contain the n -* element vector y. On exit, Y is overwritten by the updated -* vector y. -* -* INCY - INTEGER. -* On entry, INCY specifies the increment for the elements of -* Y. INCY must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - COMPLEX ONE - PARAMETER (ONE= (1.0E+0,0.0E+0)) - COMPLEX ZERO - PARAMETER (ZERO= (0.0E+0,0.0E+0)) -* .. -* .. Local Scalars .. - COMPLEX TEMP1,TEMP2 - INTEGER I,INFO,IX,IY,J,JX,JY,K,KK,KX,KY -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC CONJG,REAL -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (N.LT.0) THEN - INFO = 2 - ELSE IF (INCX.EQ.0) THEN - INFO = 6 - ELSE IF (INCY.EQ.0) THEN - INFO = 9 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('CHPMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF ((N.EQ.0) .OR. ((ALPHA.EQ.ZERO).AND. (BETA.EQ.ONE))) RETURN -* -* Set up the start points in X and Y. -* - IF (INCX.GT.0) THEN - KX = 1 - ELSE - KX = 1 - (N-1)*INCX - END IF - IF (INCY.GT.0) THEN - KY = 1 - ELSE - KY = 1 - (N-1)*INCY - END IF -* -* Start the operations. In this version the elements of the array AP -* are accessed sequentially with one pass through AP. -* -* First form y := beta*y. -* - IF (BETA.NE.ONE) THEN - IF (INCY.EQ.1) THEN - IF (BETA.EQ.ZERO) THEN - DO 10 I = 1,N - Y(I) = ZERO - 10 CONTINUE - ELSE - DO 20 I = 1,N - Y(I) = BETA*Y(I) - 20 CONTINUE - END IF - ELSE - IY = KY - IF (BETA.EQ.ZERO) THEN - DO 30 I = 1,N - Y(IY) = ZERO - IY = IY + INCY - 30 CONTINUE - ELSE - DO 40 I = 1,N - Y(IY) = BETA*Y(IY) - IY = IY + INCY - 40 CONTINUE - END IF - END IF - END IF - IF (ALPHA.EQ.ZERO) RETURN - KK = 1 - IF (LSAME(UPLO,'U')) THEN -* -* Form y when AP contains the upper triangle. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 60 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - K = KK - DO 50 I = 1,J - 1 - Y(I) = Y(I) + TEMP1*AP(K) - TEMP2 = TEMP2 + CONJG(AP(K))*X(I) - K = K + 1 - 50 CONTINUE - Y(J) = Y(J) + TEMP1*REAL(AP(KK+J-1)) + ALPHA*TEMP2 - KK = KK + J - 60 CONTINUE - ELSE - JX = KX - JY = KY - DO 80 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - IX = KX - IY = KY - DO 70 K = KK,KK + J - 2 - Y(IY) = Y(IY) + TEMP1*AP(K) - TEMP2 = TEMP2 + CONJG(AP(K))*X(IX) - IX = IX + INCX - IY = IY + INCY - 70 CONTINUE - Y(JY) = Y(JY) + TEMP1*REAL(AP(KK+J-1)) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - KK = KK + J - 80 CONTINUE - END IF - ELSE -* -* Form y when AP contains the lower triangle. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 100 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - Y(J) = Y(J) + TEMP1*REAL(AP(KK)) - K = KK + 1 - DO 90 I = J + 1,N - Y(I) = Y(I) + TEMP1*AP(K) - TEMP2 = TEMP2 + CONJG(AP(K))*X(I) - K = K + 1 - 90 CONTINUE - Y(J) = Y(J) + ALPHA*TEMP2 - KK = KK + (N-J+1) - 100 CONTINUE - ELSE - JX = KX - JY = KY - DO 120 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - Y(JY) = Y(JY) + TEMP1*REAL(AP(KK)) - IX = JX - IY = JY - DO 110 K = KK + 1,KK + N - J - IX = IX + INCX - IY = IY + INCY - Y(IY) = Y(IY) + TEMP1*AP(K) - TEMP2 = TEMP2 + CONJG(AP(K))*X(IX) - 110 CONTINUE - Y(JY) = Y(JY) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - KK = KK + (N-J+1) - 120 CONTINUE - END IF - END IF -* - RETURN -* -* End of CHPMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/common.h b/ground/gcs/src/libs/eigen/blas/common.h index 2bf642c6b4..61d8344d9a 100644 --- a/ground/gcs/src/libs/eigen/blas/common.h +++ b/ground/gcs/src/libs/eigen/blas/common.h @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2009-2010 Gael Guennebaud +// Copyright (C) 2009-2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -10,18 +10,16 @@ #ifndef EIGEN_BLAS_COMMON_H #define EIGEN_BLAS_COMMON_H -#include -#include +#include "../Eigen/Core" +#include "../Eigen/Jacobi" -#include #include #ifndef SCALAR #error the token SCALAR must be defined to compile this file #endif -#include - +#include "../Eigen/src/misc/blas.h" #define NOTR 0 #define TR 1 @@ -95,6 +93,7 @@ enum typedef Matrix PlainMatrixType; typedef Map, 0, OuterStride<> > MatrixType; +typedef Map, 0, OuterStride<> > ConstMatrixType; typedef Map, 0, InnerStride > StridedVectorType; typedef Map > CompactVectorType; @@ -106,26 +105,45 @@ matrix(T* data, int rows, int cols, int stride) } template -Map, 0, InnerStride > vector(T* data, int size, int incr) +Map, 0, OuterStride<> > +matrix(const T* data, int rows, int cols, int stride) +{ + return Map, 0, OuterStride<> >(data, rows, cols, OuterStride<>(stride)); +} + +template +Map, 0, InnerStride > make_vector(T* data, int size, int incr) { return Map, 0, InnerStride >(data, size, InnerStride(incr)); } template -Map > vector(T* data, int size) +Map, 0, InnerStride > make_vector(const T* data, int size, int incr) +{ + return Map, 0, InnerStride >(data, size, InnerStride(incr)); +} + +template +Map > make_vector(T* data, int size) { return Map >(data, size); } +template +Map > make_vector(const T* data, int size) +{ + return Map >(data, size); +} + template T* get_compact_vector(T* x, int n, int incx) { if(incx==1) return x; - T* ret = new Scalar[n]; - if(incx<0) vector(ret,n) = vector(x,n,-incx).reverse(); - else vector(ret,n) = vector(x,n, incx); + typename Eigen::internal::remove_const::type* ret = new Scalar[n]; + if(incx<0) make_vector(ret,n) = make_vector(x,n,-incx).reverse(); + else make_vector(ret,n) = make_vector(x,n, incx); return ret; } @@ -135,8 +153,8 @@ T* copy_back(T* x_cpy, T* x, int n, int incx) if(x_cpy==x) return 0; - if(incx<0) vector(x,n,-incx).reverse() = vector(x_cpy,n); - else vector(x,n, incx) = vector(x_cpy,n); + if(incx<0) make_vector(x,n,-incx).reverse() = make_vector(x_cpy,n); + else make_vector(x,n, incx) = make_vector(x_cpy,n); return x_cpy; } diff --git a/ground/gcs/src/libs/eigen/blas/ctbmv.f b/ground/gcs/src/libs/eigen/blas/ctbmv.f deleted file mode 100644 index 5a879fa01b..0000000000 --- a/ground/gcs/src/libs/eigen/blas/ctbmv.f +++ /dev/null @@ -1,366 +0,0 @@ - SUBROUTINE CTBMV(UPLO,TRANS,DIAG,N,K,A,LDA,X,INCX) -* .. Scalar Arguments .. - INTEGER INCX,K,LDA,N - CHARACTER DIAG,TRANS,UPLO -* .. -* .. Array Arguments .. - COMPLEX A(LDA,*),X(*) -* .. -* -* Purpose -* ======= -* -* CTBMV performs one of the matrix-vector operations -* -* x := A*x, or x := A'*x, or x := conjg( A' )*x, -* -* where x is an n element vector and A is an n by n unit, or non-unit, -* upper or lower triangular band matrix, with ( k + 1 ) diagonals. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the matrix is an upper or -* lower triangular matrix as follows: -* -* UPLO = 'U' or 'u' A is an upper triangular matrix. -* -* UPLO = 'L' or 'l' A is a lower triangular matrix. -* -* Unchanged on exit. -* -* TRANS - CHARACTER*1. -* On entry, TRANS specifies the operation to be performed as -* follows: -* -* TRANS = 'N' or 'n' x := A*x. -* -* TRANS = 'T' or 't' x := A'*x. -* -* TRANS = 'C' or 'c' x := conjg( A' )*x. -* -* Unchanged on exit. -* -* DIAG - CHARACTER*1. -* On entry, DIAG specifies whether or not A is unit -* triangular as follows: -* -* DIAG = 'U' or 'u' A is assumed to be unit triangular. -* -* DIAG = 'N' or 'n' A is not assumed to be unit -* triangular. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* K - INTEGER. -* On entry with UPLO = 'U' or 'u', K specifies the number of -* super-diagonals of the matrix A. -* On entry with UPLO = 'L' or 'l', K specifies the number of -* sub-diagonals of the matrix A. -* K must satisfy 0 .le. K. -* Unchanged on exit. -* -* A - COMPLEX array of DIMENSION ( LDA, n ). -* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) -* by n part of the array A must contain the upper triangular -* band part of the matrix of coefficients, supplied column by -* column, with the leading diagonal of the matrix in row -* ( k + 1 ) of the array, the first super-diagonal starting at -* position 2 in row k, and so on. The top left k by k triangle -* of the array A is not referenced. -* The following program segment will transfer an upper -* triangular band matrix from conventional full matrix storage -* to band storage: -* -* DO 20, J = 1, N -* M = K + 1 - J -* DO 10, I = MAX( 1, J - K ), J -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) -* by n part of the array A must contain the lower triangular -* band part of the matrix of coefficients, supplied column by -* column, with the leading diagonal of the matrix in row 1 of -* the array, the first sub-diagonal starting at position 1 in -* row 2, and so on. The bottom right k by k triangle of the -* array A is not referenced. -* The following program segment will transfer a lower -* triangular band matrix from conventional full matrix storage -* to band storage: -* -* DO 20, J = 1, N -* M = 1 - J -* DO 10, I = J, MIN( N, J + K ) -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Note that when DIAG = 'U' or 'u' the elements of the array A -* corresponding to the diagonal elements of the matrix are not -* referenced, but are assumed to be unity. -* Unchanged on exit. -* -* LDA - INTEGER. -* On entry, LDA specifies the first dimension of A as declared -* in the calling (sub) program. LDA must be at least -* ( k + 1 ). -* Unchanged on exit. -* -* X - COMPLEX array of dimension at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the n -* element vector x. On exit, X is overwritten with the -* tranformed vector x. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - COMPLEX ZERO - PARAMETER (ZERO= (0.0E+0,0.0E+0)) -* .. -* .. Local Scalars .. - COMPLEX TEMP - INTEGER I,INFO,IX,J,JX,KPLUS1,KX,L - LOGICAL NOCONJ,NOUNIT -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC CONJG,MAX,MIN -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (.NOT.LSAME(TRANS,'N') .AND. .NOT.LSAME(TRANS,'T') .AND. - + .NOT.LSAME(TRANS,'C')) THEN - INFO = 2 - ELSE IF (.NOT.LSAME(DIAG,'U') .AND. .NOT.LSAME(DIAG,'N')) THEN - INFO = 3 - ELSE IF (N.LT.0) THEN - INFO = 4 - ELSE IF (K.LT.0) THEN - INFO = 5 - ELSE IF (LDA.LT. (K+1)) THEN - INFO = 7 - ELSE IF (INCX.EQ.0) THEN - INFO = 9 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('CTBMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF (N.EQ.0) RETURN -* - NOCONJ = LSAME(TRANS,'T') - NOUNIT = LSAME(DIAG,'N') -* -* Set up the start point in X if the increment is not unity. This -* will be ( N - 1 )*INCX too small for descending loops. -* - IF (INCX.LE.0) THEN - KX = 1 - (N-1)*INCX - ELSE IF (INCX.NE.1) THEN - KX = 1 - END IF -* -* Start the operations. In this version the elements of A are -* accessed sequentially with one pass through A. -* - IF (LSAME(TRANS,'N')) THEN -* -* Form x := A*x. -* - IF (LSAME(UPLO,'U')) THEN - KPLUS1 = K + 1 - IF (INCX.EQ.1) THEN - DO 20 J = 1,N - IF (X(J).NE.ZERO) THEN - TEMP = X(J) - L = KPLUS1 - J - DO 10 I = MAX(1,J-K),J - 1 - X(I) = X(I) + TEMP*A(L+I,J) - 10 CONTINUE - IF (NOUNIT) X(J) = X(J)*A(KPLUS1,J) - END IF - 20 CONTINUE - ELSE - JX = KX - DO 40 J = 1,N - IF (X(JX).NE.ZERO) THEN - TEMP = X(JX) - IX = KX - L = KPLUS1 - J - DO 30 I = MAX(1,J-K),J - 1 - X(IX) = X(IX) + TEMP*A(L+I,J) - IX = IX + INCX - 30 CONTINUE - IF (NOUNIT) X(JX) = X(JX)*A(KPLUS1,J) - END IF - JX = JX + INCX - IF (J.GT.K) KX = KX + INCX - 40 CONTINUE - END IF - ELSE - IF (INCX.EQ.1) THEN - DO 60 J = N,1,-1 - IF (X(J).NE.ZERO) THEN - TEMP = X(J) - L = 1 - J - DO 50 I = MIN(N,J+K),J + 1,-1 - X(I) = X(I) + TEMP*A(L+I,J) - 50 CONTINUE - IF (NOUNIT) X(J) = X(J)*A(1,J) - END IF - 60 CONTINUE - ELSE - KX = KX + (N-1)*INCX - JX = KX - DO 80 J = N,1,-1 - IF (X(JX).NE.ZERO) THEN - TEMP = X(JX) - IX = KX - L = 1 - J - DO 70 I = MIN(N,J+K),J + 1,-1 - X(IX) = X(IX) + TEMP*A(L+I,J) - IX = IX - INCX - 70 CONTINUE - IF (NOUNIT) X(JX) = X(JX)*A(1,J) - END IF - JX = JX - INCX - IF ((N-J).GE.K) KX = KX - INCX - 80 CONTINUE - END IF - END IF - ELSE -* -* Form x := A'*x or x := conjg( A' )*x. -* - IF (LSAME(UPLO,'U')) THEN - KPLUS1 = K + 1 - IF (INCX.EQ.1) THEN - DO 110 J = N,1,-1 - TEMP = X(J) - L = KPLUS1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(KPLUS1,J) - DO 90 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + A(L+I,J)*X(I) - 90 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*CONJG(A(KPLUS1,J)) - DO 100 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + CONJG(A(L+I,J))*X(I) - 100 CONTINUE - END IF - X(J) = TEMP - 110 CONTINUE - ELSE - KX = KX + (N-1)*INCX - JX = KX - DO 140 J = N,1,-1 - TEMP = X(JX) - KX = KX - INCX - IX = KX - L = KPLUS1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(KPLUS1,J) - DO 120 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + A(L+I,J)*X(IX) - IX = IX - INCX - 120 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*CONJG(A(KPLUS1,J)) - DO 130 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + CONJG(A(L+I,J))*X(IX) - IX = IX - INCX - 130 CONTINUE - END IF - X(JX) = TEMP - JX = JX - INCX - 140 CONTINUE - END IF - ELSE - IF (INCX.EQ.1) THEN - DO 170 J = 1,N - TEMP = X(J) - L = 1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(1,J) - DO 150 I = J + 1,MIN(N,J+K) - TEMP = TEMP + A(L+I,J)*X(I) - 150 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*CONJG(A(1,J)) - DO 160 I = J + 1,MIN(N,J+K) - TEMP = TEMP + CONJG(A(L+I,J))*X(I) - 160 CONTINUE - END IF - X(J) = TEMP - 170 CONTINUE - ELSE - JX = KX - DO 200 J = 1,N - TEMP = X(JX) - KX = KX + INCX - IX = KX - L = 1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(1,J) - DO 180 I = J + 1,MIN(N,J+K) - TEMP = TEMP + A(L+I,J)*X(IX) - IX = IX + INCX - 180 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*CONJG(A(1,J)) - DO 190 I = J + 1,MIN(N,J+K) - TEMP = TEMP + CONJG(A(L+I,J))*X(IX) - IX = IX + INCX - 190 CONTINUE - END IF - X(JX) = TEMP - JX = JX + INCX - 200 CONTINUE - END IF - END IF - END IF -* - RETURN -* -* End of CTBMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/double.cpp b/ground/gcs/src/libs/eigen/blas/double.cpp index 8fd0709ba8..295b1d1f2a 100644 --- a/ground/gcs/src/libs/eigen/blas/double.cpp +++ b/ground/gcs/src/libs/eigen/blas/double.cpp @@ -23,11 +23,10 @@ double BLASFUNC(dsdot)(int* n, float* x, int* incx, float* y, int* incy) { if(*n<=0) return 0; - if(*incx==1 && *incy==1) return (vector(x,*n).cast().cwiseProduct(vector(y,*n).cast())).sum(); - else if(*incx>0 && *incy>0) return (vector(x,*n,*incx).cast().cwiseProduct(vector(y,*n,*incy).cast())).sum(); - else if(*incx<0 && *incy>0) return (vector(x,*n,-*incx).reverse().cast().cwiseProduct(vector(y,*n,*incy).cast())).sum(); - else if(*incx>0 && *incy<0) return (vector(x,*n,*incx).cast().cwiseProduct(vector(y,*n,-*incy).reverse().cast())).sum(); - else if(*incx<0 && *incy<0) return (vector(x,*n,-*incx).reverse().cast().cwiseProduct(vector(y,*n,-*incy).reverse().cast())).sum(); + if(*incx==1 && *incy==1) return (make_vector(x,*n).cast().cwiseProduct(make_vector(y,*n).cast())).sum(); + else if(*incx>0 && *incy>0) return (make_vector(x,*n,*incx).cast().cwiseProduct(make_vector(y,*n,*incy).cast())).sum(); + else if(*incx<0 && *incy>0) return (make_vector(x,*n,-*incx).reverse().cast().cwiseProduct(make_vector(y,*n,*incy).cast())).sum(); + else if(*incx>0 && *incy<0) return (make_vector(x,*n,*incx).cast().cwiseProduct(make_vector(y,*n,-*incy).reverse().cast())).sum(); + else if(*incx<0 && *incy<0) return (make_vector(x,*n,-*incx).reverse().cast().cwiseProduct(make_vector(y,*n,-*incy).reverse().cast())).sum(); else return 0; } - diff --git a/ground/gcs/src/libs/eigen/blas/drotm.f b/ground/gcs/src/libs/eigen/blas/drotm.f deleted file mode 100644 index 63a3b1134f..0000000000 --- a/ground/gcs/src/libs/eigen/blas/drotm.f +++ /dev/null @@ -1,147 +0,0 @@ - SUBROUTINE DROTM(N,DX,INCX,DY,INCY,DPARAM) -* .. Scalar Arguments .. - INTEGER INCX,INCY,N -* .. -* .. Array Arguments .. - DOUBLE PRECISION DPARAM(5),DX(*),DY(*) -* .. -* -* Purpose -* ======= -* -* APPLY THE MODIFIED GIVENS TRANSFORMATION, H, TO THE 2 BY N MATRIX -* -* (DX**T) , WHERE **T INDICATES TRANSPOSE. THE ELEMENTS OF DX ARE IN -* (DY**T) -* -* DX(LX+I*INCX), I = 0 TO N-1, WHERE LX = 1 IF INCX .GE. 0, ELSE -* LX = (-INCX)*N, AND SIMILARLY FOR SY USING LY AND INCY. -* WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS.. -* -* DFLAG=-1.D0 DFLAG=0.D0 DFLAG=1.D0 DFLAG=-2.D0 -* -* (DH11 DH12) (1.D0 DH12) (DH11 1.D0) (1.D0 0.D0) -* H=( ) ( ) ( ) ( ) -* (DH21 DH22), (DH21 1.D0), (-1.D0 DH22), (0.D0 1.D0). -* SEE DROTMG FOR A DESCRIPTION OF DATA STORAGE IN DPARAM. -* -* Arguments -* ========= -* -* N (input) INTEGER -* number of elements in input vector(s) -* -* DX (input/output) DOUBLE PRECISION array, dimension N -* double precision vector with N elements -* -* INCX (input) INTEGER -* storage spacing between elements of DX -* -* DY (input/output) DOUBLE PRECISION array, dimension N -* double precision vector with N elements -* -* INCY (input) INTEGER -* storage spacing between elements of DY -* -* DPARAM (input/output) DOUBLE PRECISION array, dimension 5 -* DPARAM(1)=DFLAG -* DPARAM(2)=DH11 -* DPARAM(3)=DH21 -* DPARAM(4)=DH12 -* DPARAM(5)=DH22 -* -* ===================================================================== -* -* .. Local Scalars .. - DOUBLE PRECISION DFLAG,DH11,DH12,DH21,DH22,TWO,W,Z,ZERO - INTEGER I,KX,KY,NSTEPS -* .. -* .. Data statements .. - DATA ZERO,TWO/0.D0,2.D0/ -* .. -* - DFLAG = DPARAM(1) - IF (N.LE.0 .OR. (DFLAG+TWO.EQ.ZERO)) GO TO 140 - IF (.NOT. (INCX.EQ.INCY.AND.INCX.GT.0)) GO TO 70 -* - NSTEPS = N*INCX - IF (DFLAG) 50,10,30 - 10 CONTINUE - DH12 = DPARAM(4) - DH21 = DPARAM(3) - DO 20 I = 1,NSTEPS,INCX - W = DX(I) - Z = DY(I) - DX(I) = W + Z*DH12 - DY(I) = W*DH21 + Z - 20 CONTINUE - GO TO 140 - 30 CONTINUE - DH11 = DPARAM(2) - DH22 = DPARAM(5) - DO 40 I = 1,NSTEPS,INCX - W = DX(I) - Z = DY(I) - DX(I) = W*DH11 + Z - DY(I) = -W + DH22*Z - 40 CONTINUE - GO TO 140 - 50 CONTINUE - DH11 = DPARAM(2) - DH12 = DPARAM(4) - DH21 = DPARAM(3) - DH22 = DPARAM(5) - DO 60 I = 1,NSTEPS,INCX - W = DX(I) - Z = DY(I) - DX(I) = W*DH11 + Z*DH12 - DY(I) = W*DH21 + Z*DH22 - 60 CONTINUE - GO TO 140 - 70 CONTINUE - KX = 1 - KY = 1 - IF (INCX.LT.0) KX = 1 + (1-N)*INCX - IF (INCY.LT.0) KY = 1 + (1-N)*INCY -* - IF (DFLAG) 120,80,100 - 80 CONTINUE - DH12 = DPARAM(4) - DH21 = DPARAM(3) - DO 90 I = 1,N - W = DX(KX) - Z = DY(KY) - DX(KX) = W + Z*DH12 - DY(KY) = W*DH21 + Z - KX = KX + INCX - KY = KY + INCY - 90 CONTINUE - GO TO 140 - 100 CONTINUE - DH11 = DPARAM(2) - DH22 = DPARAM(5) - DO 110 I = 1,N - W = DX(KX) - Z = DY(KY) - DX(KX) = W*DH11 + Z - DY(KY) = -W + DH22*Z - KX = KX + INCX - KY = KY + INCY - 110 CONTINUE - GO TO 140 - 120 CONTINUE - DH11 = DPARAM(2) - DH12 = DPARAM(4) - DH21 = DPARAM(3) - DH22 = DPARAM(5) - DO 130 I = 1,N - W = DX(KX) - Z = DY(KY) - DX(KX) = W*DH11 + Z*DH12 - DY(KY) = W*DH21 + Z*DH22 - KX = KX + INCX - KY = KY + INCY - 130 CONTINUE - 140 CONTINUE - RETURN - END diff --git a/ground/gcs/src/libs/eigen/blas/drotmg.f b/ground/gcs/src/libs/eigen/blas/drotmg.f deleted file mode 100644 index 3ae647b087..0000000000 --- a/ground/gcs/src/libs/eigen/blas/drotmg.f +++ /dev/null @@ -1,206 +0,0 @@ - SUBROUTINE DROTMG(DD1,DD2,DX1,DY1,DPARAM) -* .. Scalar Arguments .. - DOUBLE PRECISION DD1,DD2,DX1,DY1 -* .. -* .. Array Arguments .. - DOUBLE PRECISION DPARAM(5) -* .. -* -* Purpose -* ======= -* -* CONSTRUCT THE MODIFIED GIVENS TRANSFORMATION MATRIX H WHICH ZEROS -* THE SECOND COMPONENT OF THE 2-VECTOR (DSQRT(DD1)*DX1,DSQRT(DD2)* -* DY2)**T. -* WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS.. -* -* DFLAG=-1.D0 DFLAG=0.D0 DFLAG=1.D0 DFLAG=-2.D0 -* -* (DH11 DH12) (1.D0 DH12) (DH11 1.D0) (1.D0 0.D0) -* H=( ) ( ) ( ) ( ) -* (DH21 DH22), (DH21 1.D0), (-1.D0 DH22), (0.D0 1.D0). -* LOCATIONS 2-4 OF DPARAM CONTAIN DH11, DH21, DH12, AND DH22 -* RESPECTIVELY. (VALUES OF 1.D0, -1.D0, OR 0.D0 IMPLIED BY THE -* VALUE OF DPARAM(1) ARE NOT STORED IN DPARAM.) -* -* THE VALUES OF GAMSQ AND RGAMSQ SET IN THE DATA STATEMENT MAY BE -* INEXACT. THIS IS OK AS THEY ARE ONLY USED FOR TESTING THE SIZE -* OF DD1 AND DD2. ALL ACTUAL SCALING OF DATA IS DONE USING GAM. -* -* -* Arguments -* ========= -* -* DD1 (input/output) DOUBLE PRECISION -* -* DD2 (input/output) DOUBLE PRECISION -* -* DX1 (input/output) DOUBLE PRECISION -* -* DY1 (input) DOUBLE PRECISION -* -* DPARAM (input/output) DOUBLE PRECISION array, dimension 5 -* DPARAM(1)=DFLAG -* DPARAM(2)=DH11 -* DPARAM(3)=DH21 -* DPARAM(4)=DH12 -* DPARAM(5)=DH22 -* -* ===================================================================== -* -* .. Local Scalars .. - DOUBLE PRECISION DFLAG,DH11,DH12,DH21,DH22,DP1,DP2,DQ1,DQ2,DTEMP, - + DU,GAM,GAMSQ,ONE,RGAMSQ,TWO,ZERO - INTEGER IGO -* .. -* .. Intrinsic Functions .. - INTRINSIC DABS -* .. -* .. Data statements .. -* - DATA ZERO,ONE,TWO/0.D0,1.D0,2.D0/ - DATA GAM,GAMSQ,RGAMSQ/4096.D0,16777216.D0,5.9604645D-8/ -* .. - - IF (.NOT.DD1.LT.ZERO) GO TO 10 -* GO ZERO-H-D-AND-DX1.. - GO TO 60 - 10 CONTINUE -* CASE-DD1-NONNEGATIVE - DP2 = DD2*DY1 - IF (.NOT.DP2.EQ.ZERO) GO TO 20 - DFLAG = -TWO - GO TO 260 -* REGULAR-CASE.. - 20 CONTINUE - DP1 = DD1*DX1 - DQ2 = DP2*DY1 - DQ1 = DP1*DX1 -* - IF (.NOT.DABS(DQ1).GT.DABS(DQ2)) GO TO 40 - DH21 = -DY1/DX1 - DH12 = DP2/DP1 -* - DU = ONE - DH12*DH21 -* - IF (.NOT.DU.LE.ZERO) GO TO 30 -* GO ZERO-H-D-AND-DX1.. - GO TO 60 - 30 CONTINUE - DFLAG = ZERO - DD1 = DD1/DU - DD2 = DD2/DU - DX1 = DX1*DU -* GO SCALE-CHECK.. - GO TO 100 - 40 CONTINUE - IF (.NOT.DQ2.LT.ZERO) GO TO 50 -* GO ZERO-H-D-AND-DX1.. - GO TO 60 - 50 CONTINUE - DFLAG = ONE - DH11 = DP1/DP2 - DH22 = DX1/DY1 - DU = ONE + DH11*DH22 - DTEMP = DD2/DU - DD2 = DD1/DU - DD1 = DTEMP - DX1 = DY1*DU -* GO SCALE-CHECK - GO TO 100 -* PROCEDURE..ZERO-H-D-AND-DX1.. - 60 CONTINUE - DFLAG = -ONE - DH11 = ZERO - DH12 = ZERO - DH21 = ZERO - DH22 = ZERO -* - DD1 = ZERO - DD2 = ZERO - DX1 = ZERO -* RETURN.. - GO TO 220 -* PROCEDURE..FIX-H.. - 70 CONTINUE - IF (.NOT.DFLAG.GE.ZERO) GO TO 90 -* - IF (.NOT.DFLAG.EQ.ZERO) GO TO 80 - DH11 = ONE - DH22 = ONE - DFLAG = -ONE - GO TO 90 - 80 CONTINUE - DH21 = -ONE - DH12 = ONE - DFLAG = -ONE - 90 CONTINUE - GO TO IGO(120,150,180,210) -* PROCEDURE..SCALE-CHECK - 100 CONTINUE - 110 CONTINUE - IF (.NOT.DD1.LE.RGAMSQ) GO TO 130 - IF (DD1.EQ.ZERO) GO TO 160 - ASSIGN 120 TO IGO -* FIX-H.. - GO TO 70 - 120 CONTINUE - DD1 = DD1*GAM**2 - DX1 = DX1/GAM - DH11 = DH11/GAM - DH12 = DH12/GAM - GO TO 110 - 130 CONTINUE - 140 CONTINUE - IF (.NOT.DD1.GE.GAMSQ) GO TO 160 - ASSIGN 150 TO IGO -* FIX-H.. - GO TO 70 - 150 CONTINUE - DD1 = DD1/GAM**2 - DX1 = DX1*GAM - DH11 = DH11*GAM - DH12 = DH12*GAM - GO TO 140 - 160 CONTINUE - 170 CONTINUE - IF (.NOT.DABS(DD2).LE.RGAMSQ) GO TO 190 - IF (DD2.EQ.ZERO) GO TO 220 - ASSIGN 180 TO IGO -* FIX-H.. - GO TO 70 - 180 CONTINUE - DD2 = DD2*GAM**2 - DH21 = DH21/GAM - DH22 = DH22/GAM - GO TO 170 - 190 CONTINUE - 200 CONTINUE - IF (.NOT.DABS(DD2).GE.GAMSQ) GO TO 220 - ASSIGN 210 TO IGO -* FIX-H.. - GO TO 70 - 210 CONTINUE - DD2 = DD2/GAM**2 - DH21 = DH21*GAM - DH22 = DH22*GAM - GO TO 200 - 220 CONTINUE - IF (DFLAG) 250,230,240 - 230 CONTINUE - DPARAM(3) = DH21 - DPARAM(4) = DH12 - GO TO 260 - 240 CONTINUE - DPARAM(2) = DH11 - DPARAM(5) = DH22 - GO TO 260 - 250 CONTINUE - DPARAM(2) = DH11 - DPARAM(3) = DH21 - DPARAM(4) = DH12 - DPARAM(5) = DH22 - 260 CONTINUE - DPARAM(1) = DFLAG - RETURN - END diff --git a/ground/gcs/src/libs/eigen/blas/dsbmv.f b/ground/gcs/src/libs/eigen/blas/dsbmv.f deleted file mode 100644 index 8c82d1fa1d..0000000000 --- a/ground/gcs/src/libs/eigen/blas/dsbmv.f +++ /dev/null @@ -1,304 +0,0 @@ - SUBROUTINE DSBMV(UPLO,N,K,ALPHA,A,LDA,X,INCX,BETA,Y,INCY) -* .. Scalar Arguments .. - DOUBLE PRECISION ALPHA,BETA - INTEGER INCX,INCY,K,LDA,N - CHARACTER UPLO -* .. -* .. Array Arguments .. - DOUBLE PRECISION A(LDA,*),X(*),Y(*) -* .. -* -* Purpose -* ======= -* -* DSBMV performs the matrix-vector operation -* -* y := alpha*A*x + beta*y, -* -* where alpha and beta are scalars, x and y are n element vectors and -* A is an n by n symmetric band matrix, with k super-diagonals. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the upper or lower -* triangular part of the band matrix A is being supplied as -* follows: -* -* UPLO = 'U' or 'u' The upper triangular part of A is -* being supplied. -* -* UPLO = 'L' or 'l' The lower triangular part of A is -* being supplied. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* K - INTEGER. -* On entry, K specifies the number of super-diagonals of the -* matrix A. K must satisfy 0 .le. K. -* Unchanged on exit. -* -* ALPHA - DOUBLE PRECISION. -* On entry, ALPHA specifies the scalar alpha. -* Unchanged on exit. -* -* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). -* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) -* by n part of the array A must contain the upper triangular -* band part of the symmetric matrix, supplied column by -* column, with the leading diagonal of the matrix in row -* ( k + 1 ) of the array, the first super-diagonal starting at -* position 2 in row k, and so on. The top left k by k triangle -* of the array A is not referenced. -* The following program segment will transfer the upper -* triangular part of a symmetric band matrix from conventional -* full matrix storage to band storage: -* -* DO 20, J = 1, N -* M = K + 1 - J -* DO 10, I = MAX( 1, J - K ), J -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) -* by n part of the array A must contain the lower triangular -* band part of the symmetric matrix, supplied column by -* column, with the leading diagonal of the matrix in row 1 of -* the array, the first sub-diagonal starting at position 1 in -* row 2, and so on. The bottom right k by k triangle of the -* array A is not referenced. -* The following program segment will transfer the lower -* triangular part of a symmetric band matrix from conventional -* full matrix storage to band storage: -* -* DO 20, J = 1, N -* M = 1 - J -* DO 10, I = J, MIN( N, J + K ) -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Unchanged on exit. -* -* LDA - INTEGER. -* On entry, LDA specifies the first dimension of A as declared -* in the calling (sub) program. LDA must be at least -* ( k + 1 ). -* Unchanged on exit. -* -* X - DOUBLE PRECISION array of DIMENSION at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the -* vector x. -* Unchanged on exit. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* BETA - DOUBLE PRECISION. -* On entry, BETA specifies the scalar beta. -* Unchanged on exit. -* -* Y - DOUBLE PRECISION array of DIMENSION at least -* ( 1 + ( n - 1 )*abs( INCY ) ). -* Before entry, the incremented array Y must contain the -* vector y. On exit, Y is overwritten by the updated vector y. -* -* INCY - INTEGER. -* On entry, INCY specifies the increment for the elements of -* Y. INCY must not be zero. -* Unchanged on exit. -* -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - DOUBLE PRECISION ONE,ZERO - PARAMETER (ONE=1.0D+0,ZERO=0.0D+0) -* .. -* .. Local Scalars .. - DOUBLE PRECISION TEMP1,TEMP2 - INTEGER I,INFO,IX,IY,J,JX,JY,KPLUS1,KX,KY,L -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC MAX,MIN -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (N.LT.0) THEN - INFO = 2 - ELSE IF (K.LT.0) THEN - INFO = 3 - ELSE IF (LDA.LT. (K+1)) THEN - INFO = 6 - ELSE IF (INCX.EQ.0) THEN - INFO = 8 - ELSE IF (INCY.EQ.0) THEN - INFO = 11 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('DSBMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF ((N.EQ.0) .OR. ((ALPHA.EQ.ZERO).AND. (BETA.EQ.ONE))) RETURN -* -* Set up the start points in X and Y. -* - IF (INCX.GT.0) THEN - KX = 1 - ELSE - KX = 1 - (N-1)*INCX - END IF - IF (INCY.GT.0) THEN - KY = 1 - ELSE - KY = 1 - (N-1)*INCY - END IF -* -* Start the operations. In this version the elements of the array A -* are accessed sequentially with one pass through A. -* -* First form y := beta*y. -* - IF (BETA.NE.ONE) THEN - IF (INCY.EQ.1) THEN - IF (BETA.EQ.ZERO) THEN - DO 10 I = 1,N - Y(I) = ZERO - 10 CONTINUE - ELSE - DO 20 I = 1,N - Y(I) = BETA*Y(I) - 20 CONTINUE - END IF - ELSE - IY = KY - IF (BETA.EQ.ZERO) THEN - DO 30 I = 1,N - Y(IY) = ZERO - IY = IY + INCY - 30 CONTINUE - ELSE - DO 40 I = 1,N - Y(IY) = BETA*Y(IY) - IY = IY + INCY - 40 CONTINUE - END IF - END IF - END IF - IF (ALPHA.EQ.ZERO) RETURN - IF (LSAME(UPLO,'U')) THEN -* -* Form y when upper triangle of A is stored. -* - KPLUS1 = K + 1 - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 60 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - L = KPLUS1 - J - DO 50 I = MAX(1,J-K),J - 1 - Y(I) = Y(I) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + A(L+I,J)*X(I) - 50 CONTINUE - Y(J) = Y(J) + TEMP1*A(KPLUS1,J) + ALPHA*TEMP2 - 60 CONTINUE - ELSE - JX = KX - JY = KY - DO 80 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - IX = KX - IY = KY - L = KPLUS1 - J - DO 70 I = MAX(1,J-K),J - 1 - Y(IY) = Y(IY) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + A(L+I,J)*X(IX) - IX = IX + INCX - IY = IY + INCY - 70 CONTINUE - Y(JY) = Y(JY) + TEMP1*A(KPLUS1,J) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - IF (J.GT.K) THEN - KX = KX + INCX - KY = KY + INCY - END IF - 80 CONTINUE - END IF - ELSE -* -* Form y when lower triangle of A is stored. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 100 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - Y(J) = Y(J) + TEMP1*A(1,J) - L = 1 - J - DO 90 I = J + 1,MIN(N,J+K) - Y(I) = Y(I) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + A(L+I,J)*X(I) - 90 CONTINUE - Y(J) = Y(J) + ALPHA*TEMP2 - 100 CONTINUE - ELSE - JX = KX - JY = KY - DO 120 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - Y(JY) = Y(JY) + TEMP1*A(1,J) - L = 1 - J - IX = JX - IY = JY - DO 110 I = J + 1,MIN(N,J+K) - IX = IX + INCX - IY = IY + INCY - Y(IY) = Y(IY) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + A(L+I,J)*X(IX) - 110 CONTINUE - Y(JY) = Y(JY) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - 120 CONTINUE - END IF - END IF -* - RETURN -* -* End of DSBMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/dspmv.f b/ground/gcs/src/libs/eigen/blas/dspmv.f deleted file mode 100644 index f6e121e765..0000000000 --- a/ground/gcs/src/libs/eigen/blas/dspmv.f +++ /dev/null @@ -1,265 +0,0 @@ - SUBROUTINE DSPMV(UPLO,N,ALPHA,AP,X,INCX,BETA,Y,INCY) -* .. Scalar Arguments .. - DOUBLE PRECISION ALPHA,BETA - INTEGER INCX,INCY,N - CHARACTER UPLO -* .. -* .. Array Arguments .. - DOUBLE PRECISION AP(*),X(*),Y(*) -* .. -* -* Purpose -* ======= -* -* DSPMV performs the matrix-vector operation -* -* y := alpha*A*x + beta*y, -* -* where alpha and beta are scalars, x and y are n element vectors and -* A is an n by n symmetric matrix, supplied in packed form. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the upper or lower -* triangular part of the matrix A is supplied in the packed -* array AP as follows: -* -* UPLO = 'U' or 'u' The upper triangular part of A is -* supplied in AP. -* -* UPLO = 'L' or 'l' The lower triangular part of A is -* supplied in AP. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* ALPHA - DOUBLE PRECISION. -* On entry, ALPHA specifies the scalar alpha. -* Unchanged on exit. -* -* AP - DOUBLE PRECISION array of DIMENSION at least -* ( ( n*( n + 1 ) )/2 ). -* Before entry with UPLO = 'U' or 'u', the array AP must -* contain the upper triangular part of the symmetric matrix -* packed sequentially, column by column, so that AP( 1 ) -* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) -* and a( 2, 2 ) respectively, and so on. -* Before entry with UPLO = 'L' or 'l', the array AP must -* contain the lower triangular part of the symmetric matrix -* packed sequentially, column by column, so that AP( 1 ) -* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) -* and a( 3, 1 ) respectively, and so on. -* Unchanged on exit. -* -* X - DOUBLE PRECISION array of dimension at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the n -* element vector x. -* Unchanged on exit. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* BETA - DOUBLE PRECISION. -* On entry, BETA specifies the scalar beta. When BETA is -* supplied as zero then Y need not be set on input. -* Unchanged on exit. -* -* Y - DOUBLE PRECISION array of dimension at least -* ( 1 + ( n - 1 )*abs( INCY ) ). -* Before entry, the incremented array Y must contain the n -* element vector y. On exit, Y is overwritten by the updated -* vector y. -* -* INCY - INTEGER. -* On entry, INCY specifies the increment for the elements of -* Y. INCY must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - DOUBLE PRECISION ONE,ZERO - PARAMETER (ONE=1.0D+0,ZERO=0.0D+0) -* .. -* .. Local Scalars .. - DOUBLE PRECISION TEMP1,TEMP2 - INTEGER I,INFO,IX,IY,J,JX,JY,K,KK,KX,KY -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (N.LT.0) THEN - INFO = 2 - ELSE IF (INCX.EQ.0) THEN - INFO = 6 - ELSE IF (INCY.EQ.0) THEN - INFO = 9 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('DSPMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF ((N.EQ.0) .OR. ((ALPHA.EQ.ZERO).AND. (BETA.EQ.ONE))) RETURN -* -* Set up the start points in X and Y. -* - IF (INCX.GT.0) THEN - KX = 1 - ELSE - KX = 1 - (N-1)*INCX - END IF - IF (INCY.GT.0) THEN - KY = 1 - ELSE - KY = 1 - (N-1)*INCY - END IF -* -* Start the operations. In this version the elements of the array AP -* are accessed sequentially with one pass through AP. -* -* First form y := beta*y. -* - IF (BETA.NE.ONE) THEN - IF (INCY.EQ.1) THEN - IF (BETA.EQ.ZERO) THEN - DO 10 I = 1,N - Y(I) = ZERO - 10 CONTINUE - ELSE - DO 20 I = 1,N - Y(I) = BETA*Y(I) - 20 CONTINUE - END IF - ELSE - IY = KY - IF (BETA.EQ.ZERO) THEN - DO 30 I = 1,N - Y(IY) = ZERO - IY = IY + INCY - 30 CONTINUE - ELSE - DO 40 I = 1,N - Y(IY) = BETA*Y(IY) - IY = IY + INCY - 40 CONTINUE - END IF - END IF - END IF - IF (ALPHA.EQ.ZERO) RETURN - KK = 1 - IF (LSAME(UPLO,'U')) THEN -* -* Form y when AP contains the upper triangle. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 60 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - K = KK - DO 50 I = 1,J - 1 - Y(I) = Y(I) + TEMP1*AP(K) - TEMP2 = TEMP2 + AP(K)*X(I) - K = K + 1 - 50 CONTINUE - Y(J) = Y(J) + TEMP1*AP(KK+J-1) + ALPHA*TEMP2 - KK = KK + J - 60 CONTINUE - ELSE - JX = KX - JY = KY - DO 80 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - IX = KX - IY = KY - DO 70 K = KK,KK + J - 2 - Y(IY) = Y(IY) + TEMP1*AP(K) - TEMP2 = TEMP2 + AP(K)*X(IX) - IX = IX + INCX - IY = IY + INCY - 70 CONTINUE - Y(JY) = Y(JY) + TEMP1*AP(KK+J-1) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - KK = KK + J - 80 CONTINUE - END IF - ELSE -* -* Form y when AP contains the lower triangle. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 100 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - Y(J) = Y(J) + TEMP1*AP(KK) - K = KK + 1 - DO 90 I = J + 1,N - Y(I) = Y(I) + TEMP1*AP(K) - TEMP2 = TEMP2 + AP(K)*X(I) - K = K + 1 - 90 CONTINUE - Y(J) = Y(J) + ALPHA*TEMP2 - KK = KK + (N-J+1) - 100 CONTINUE - ELSE - JX = KX - JY = KY - DO 120 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - Y(JY) = Y(JY) + TEMP1*AP(KK) - IX = JX - IY = JY - DO 110 K = KK + 1,KK + N - J - IX = IX + INCX - IY = IY + INCY - Y(IY) = Y(IY) + TEMP1*AP(K) - TEMP2 = TEMP2 + AP(K)*X(IX) - 110 CONTINUE - Y(JY) = Y(JY) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - KK = KK + (N-J+1) - 120 CONTINUE - END IF - END IF -* - RETURN -* -* End of DSPMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/dtbmv.f b/ground/gcs/src/libs/eigen/blas/dtbmv.f deleted file mode 100644 index a87ffdeaeb..0000000000 --- a/ground/gcs/src/libs/eigen/blas/dtbmv.f +++ /dev/null @@ -1,335 +0,0 @@ - SUBROUTINE DTBMV(UPLO,TRANS,DIAG,N,K,A,LDA,X,INCX) -* .. Scalar Arguments .. - INTEGER INCX,K,LDA,N - CHARACTER DIAG,TRANS,UPLO -* .. -* .. Array Arguments .. - DOUBLE PRECISION A(LDA,*),X(*) -* .. -* -* Purpose -* ======= -* -* DTBMV performs one of the matrix-vector operations -* -* x := A*x, or x := A'*x, -* -* where x is an n element vector and A is an n by n unit, or non-unit, -* upper or lower triangular band matrix, with ( k + 1 ) diagonals. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the matrix is an upper or -* lower triangular matrix as follows: -* -* UPLO = 'U' or 'u' A is an upper triangular matrix. -* -* UPLO = 'L' or 'l' A is a lower triangular matrix. -* -* Unchanged on exit. -* -* TRANS - CHARACTER*1. -* On entry, TRANS specifies the operation to be performed as -* follows: -* -* TRANS = 'N' or 'n' x := A*x. -* -* TRANS = 'T' or 't' x := A'*x. -* -* TRANS = 'C' or 'c' x := A'*x. -* -* Unchanged on exit. -* -* DIAG - CHARACTER*1. -* On entry, DIAG specifies whether or not A is unit -* triangular as follows: -* -* DIAG = 'U' or 'u' A is assumed to be unit triangular. -* -* DIAG = 'N' or 'n' A is not assumed to be unit -* triangular. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* K - INTEGER. -* On entry with UPLO = 'U' or 'u', K specifies the number of -* super-diagonals of the matrix A. -* On entry with UPLO = 'L' or 'l', K specifies the number of -* sub-diagonals of the matrix A. -* K must satisfy 0 .le. K. -* Unchanged on exit. -* -* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). -* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) -* by n part of the array A must contain the upper triangular -* band part of the matrix of coefficients, supplied column by -* column, with the leading diagonal of the matrix in row -* ( k + 1 ) of the array, the first super-diagonal starting at -* position 2 in row k, and so on. The top left k by k triangle -* of the array A is not referenced. -* The following program segment will transfer an upper -* triangular band matrix from conventional full matrix storage -* to band storage: -* -* DO 20, J = 1, N -* M = K + 1 - J -* DO 10, I = MAX( 1, J - K ), J -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) -* by n part of the array A must contain the lower triangular -* band part of the matrix of coefficients, supplied column by -* column, with the leading diagonal of the matrix in row 1 of -* the array, the first sub-diagonal starting at position 1 in -* row 2, and so on. The bottom right k by k triangle of the -* array A is not referenced. -* The following program segment will transfer a lower -* triangular band matrix from conventional full matrix storage -* to band storage: -* -* DO 20, J = 1, N -* M = 1 - J -* DO 10, I = J, MIN( N, J + K ) -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Note that when DIAG = 'U' or 'u' the elements of the array A -* corresponding to the diagonal elements of the matrix are not -* referenced, but are assumed to be unity. -* Unchanged on exit. -* -* LDA - INTEGER. -* On entry, LDA specifies the first dimension of A as declared -* in the calling (sub) program. LDA must be at least -* ( k + 1 ). -* Unchanged on exit. -* -* X - DOUBLE PRECISION array of dimension at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the n -* element vector x. On exit, X is overwritten with the -* tranformed vector x. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - DOUBLE PRECISION ZERO - PARAMETER (ZERO=0.0D+0) -* .. -* .. Local Scalars .. - DOUBLE PRECISION TEMP - INTEGER I,INFO,IX,J,JX,KPLUS1,KX,L - LOGICAL NOUNIT -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC MAX,MIN -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (.NOT.LSAME(TRANS,'N') .AND. .NOT.LSAME(TRANS,'T') .AND. - + .NOT.LSAME(TRANS,'C')) THEN - INFO = 2 - ELSE IF (.NOT.LSAME(DIAG,'U') .AND. .NOT.LSAME(DIAG,'N')) THEN - INFO = 3 - ELSE IF (N.LT.0) THEN - INFO = 4 - ELSE IF (K.LT.0) THEN - INFO = 5 - ELSE IF (LDA.LT. (K+1)) THEN - INFO = 7 - ELSE IF (INCX.EQ.0) THEN - INFO = 9 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('DTBMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF (N.EQ.0) RETURN -* - NOUNIT = LSAME(DIAG,'N') -* -* Set up the start point in X if the increment is not unity. This -* will be ( N - 1 )*INCX too small for descending loops. -* - IF (INCX.LE.0) THEN - KX = 1 - (N-1)*INCX - ELSE IF (INCX.NE.1) THEN - KX = 1 - END IF -* -* Start the operations. In this version the elements of A are -* accessed sequentially with one pass through A. -* - IF (LSAME(TRANS,'N')) THEN -* -* Form x := A*x. -* - IF (LSAME(UPLO,'U')) THEN - KPLUS1 = K + 1 - IF (INCX.EQ.1) THEN - DO 20 J = 1,N - IF (X(J).NE.ZERO) THEN - TEMP = X(J) - L = KPLUS1 - J - DO 10 I = MAX(1,J-K),J - 1 - X(I) = X(I) + TEMP*A(L+I,J) - 10 CONTINUE - IF (NOUNIT) X(J) = X(J)*A(KPLUS1,J) - END IF - 20 CONTINUE - ELSE - JX = KX - DO 40 J = 1,N - IF (X(JX).NE.ZERO) THEN - TEMP = X(JX) - IX = KX - L = KPLUS1 - J - DO 30 I = MAX(1,J-K),J - 1 - X(IX) = X(IX) + TEMP*A(L+I,J) - IX = IX + INCX - 30 CONTINUE - IF (NOUNIT) X(JX) = X(JX)*A(KPLUS1,J) - END IF - JX = JX + INCX - IF (J.GT.K) KX = KX + INCX - 40 CONTINUE - END IF - ELSE - IF (INCX.EQ.1) THEN - DO 60 J = N,1,-1 - IF (X(J).NE.ZERO) THEN - TEMP = X(J) - L = 1 - J - DO 50 I = MIN(N,J+K),J + 1,-1 - X(I) = X(I) + TEMP*A(L+I,J) - 50 CONTINUE - IF (NOUNIT) X(J) = X(J)*A(1,J) - END IF - 60 CONTINUE - ELSE - KX = KX + (N-1)*INCX - JX = KX - DO 80 J = N,1,-1 - IF (X(JX).NE.ZERO) THEN - TEMP = X(JX) - IX = KX - L = 1 - J - DO 70 I = MIN(N,J+K),J + 1,-1 - X(IX) = X(IX) + TEMP*A(L+I,J) - IX = IX - INCX - 70 CONTINUE - IF (NOUNIT) X(JX) = X(JX)*A(1,J) - END IF - JX = JX - INCX - IF ((N-J).GE.K) KX = KX - INCX - 80 CONTINUE - END IF - END IF - ELSE -* -* Form x := A'*x. -* - IF (LSAME(UPLO,'U')) THEN - KPLUS1 = K + 1 - IF (INCX.EQ.1) THEN - DO 100 J = N,1,-1 - TEMP = X(J) - L = KPLUS1 - J - IF (NOUNIT) TEMP = TEMP*A(KPLUS1,J) - DO 90 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + A(L+I,J)*X(I) - 90 CONTINUE - X(J) = TEMP - 100 CONTINUE - ELSE - KX = KX + (N-1)*INCX - JX = KX - DO 120 J = N,1,-1 - TEMP = X(JX) - KX = KX - INCX - IX = KX - L = KPLUS1 - J - IF (NOUNIT) TEMP = TEMP*A(KPLUS1,J) - DO 110 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + A(L+I,J)*X(IX) - IX = IX - INCX - 110 CONTINUE - X(JX) = TEMP - JX = JX - INCX - 120 CONTINUE - END IF - ELSE - IF (INCX.EQ.1) THEN - DO 140 J = 1,N - TEMP = X(J) - L = 1 - J - IF (NOUNIT) TEMP = TEMP*A(1,J) - DO 130 I = J + 1,MIN(N,J+K) - TEMP = TEMP + A(L+I,J)*X(I) - 130 CONTINUE - X(J) = TEMP - 140 CONTINUE - ELSE - JX = KX - DO 160 J = 1,N - TEMP = X(JX) - KX = KX + INCX - IX = KX - L = 1 - J - IF (NOUNIT) TEMP = TEMP*A(1,J) - DO 150 I = J + 1,MIN(N,J+K) - TEMP = TEMP + A(L+I,J)*X(IX) - IX = IX + INCX - 150 CONTINUE - X(JX) = TEMP - JX = JX + INCX - 160 CONTINUE - END IF - END IF - END IF -* - RETURN -* -* End of DTBMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/f2c/chbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/chbmv.c new file mode 100644 index 0000000000..f218fe3f57 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/chbmv.c @@ -0,0 +1,487 @@ +/* chbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int chbmv_(char *uplo, integer *n, integer *k, complex * + alpha, complex *a, integer *lda, complex *x, integer *incx, complex * + beta, complex *y, integer *incy, ftnlen uplo_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4, i__5; + real r__1; + complex q__1, q__2, q__3, q__4; + + /* Builtin functions */ + void r_cnjg(complex *, complex *); + + /* Local variables */ + integer i__, j, l, ix, iy, jx, jy, kx, ky, info; + complex temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* CHBMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n hermitian band matrix, with k super-diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the band matrix A is being supplied as */ +/* follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* being supplied. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* being supplied. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry, K specifies the number of super-diagonals of the */ +/* matrix A. K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* ALPHA - COMPLEX . */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* A - COMPLEX array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the hermitian matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer the upper */ +/* triangular part of a hermitian band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the hermitian matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer the lower */ +/* triangular part of a hermitian band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Note that the imaginary parts of the diagonal elements need */ +/* not be set and are assumed to be zero. */ +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - COMPLEX array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the */ +/* vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - COMPLEX . */ +/* On entry, BETA specifies the scalar beta. */ +/* Unchanged on exit. */ + +/* Y - COMPLEX array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the */ +/* vector y. On exit, Y is overwritten by the updated vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + --y; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*k < 0) { + info = 3; + } else if (*lda < *k + 1) { + info = 6; + } else if (*incx == 0) { + info = 8; + } else if (*incy == 0) { + info = 11; + } + if (info != 0) { + xerbla_("CHBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (alpha->r == 0.f && alpha->i == 0.f && (beta->r == 1.f && + beta->i == 0.f))) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array A */ +/* are accessed sequentially with one pass through A. */ + +/* First form y := beta*y. */ + + if (beta->r != 1.f || beta->i != 0.f) { + if (*incy == 1) { + if (beta->r == 0.f && beta->i == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + y[i__2].r = 0.f, y[i__2].i = 0.f; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + i__3 = i__; + q__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + q__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; +/* L20: */ + } + } + } else { + iy = ky; + if (beta->r == 0.f && beta->i == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + y[i__2].r = 0.f, y[i__2].i = 0.f; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + i__3 = iy; + q__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + q__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + iy += *incy; +/* L40: */ + } + } + } + } + if (alpha->r == 0.f && alpha->i == 0.f) { + return 0; + } + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when upper triangle of A is stored. */ + + kplus1 = *k + 1; + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + q__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, q__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + i__2 = i__; + i__3 = i__; + i__5 = l + i__ + j * a_dim1; + q__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + q__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + q__1.r = y[i__3].r + q__2.r, q__1.i = y[i__3].i + q__2.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__2 = i__; + q__2.r = q__3.r * x[i__2].r - q__3.i * x[i__2].i, q__2.i = + q__3.r * x[i__2].i + q__3.i * x[i__2].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; +/* L50: */ + } + i__4 = j; + i__2 = j; + i__3 = kplus1 + j * a_dim1; + r__1 = a[i__3].r; + q__3.r = r__1 * temp1.r, q__3.i = r__1 * temp1.i; + q__2.r = y[i__2].r + q__3.r, q__2.i = y[i__2].i + q__3.i; + q__4.r = alpha->r * temp2.r - alpha->i * temp2.i, q__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = q__2.r + q__4.r, q__1.i = q__2.i + q__4.i; + y[i__4].r = q__1.r, y[i__4].i = q__1.i; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__4 = jx; + q__1.r = alpha->r * x[i__4].r - alpha->i * x[i__4].i, q__1.i = + alpha->r * x[i__4].i + alpha->i * x[i__4].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + ix = kx; + iy = ky; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + i__4 = iy; + i__2 = iy; + i__5 = l + i__ + j * a_dim1; + q__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + q__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + q__1.r = y[i__2].r + q__2.r, q__1.i = y[i__2].i + q__2.i; + y[i__4].r = q__1.r, y[i__4].i = q__1.i; + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__4 = ix; + q__2.r = q__3.r * x[i__4].r - q__3.i * x[i__4].i, q__2.i = + q__3.r * x[i__4].i + q__3.i * x[i__4].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; + ix += *incx; + iy += *incy; +/* L70: */ + } + i__3 = jy; + i__4 = jy; + i__2 = kplus1 + j * a_dim1; + r__1 = a[i__2].r; + q__3.r = r__1 * temp1.r, q__3.i = r__1 * temp1.i; + q__2.r = y[i__4].r + q__3.r, q__2.i = y[i__4].i + q__3.i; + q__4.r = alpha->r * temp2.r - alpha->i * temp2.i, q__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = q__2.r + q__4.r, q__1.i = q__2.i + q__4.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + jx += *incx; + jy += *incy; + if (j > *k) { + kx += *incx; + ky += *incy; + } +/* L80: */ + } + } + } else { + +/* Form y when lower triangle of A is stored. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__3 = j; + q__1.r = alpha->r * x[i__3].r - alpha->i * x[i__3].i, q__1.i = + alpha->r * x[i__3].i + alpha->i * x[i__3].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + i__3 = j; + i__4 = j; + i__2 = j * a_dim1 + 1; + r__1 = a[i__2].r; + q__2.r = r__1 * temp1.r, q__2.i = r__1 * temp1.i; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + i__4 = i__; + i__2 = i__; + i__5 = l + i__ + j * a_dim1; + q__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + q__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + q__1.r = y[i__2].r + q__2.r, q__1.i = y[i__2].i + q__2.i; + y[i__4].r = q__1.r, y[i__4].i = q__1.i; + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__4 = i__; + q__2.r = q__3.r * x[i__4].r - q__3.i * x[i__4].i, q__2.i = + q__3.r * x[i__4].i + q__3.i * x[i__4].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; +/* L90: */ + } + i__3 = j; + i__4 = j; + q__2.r = alpha->r * temp2.r - alpha->i * temp2.i, q__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__3 = jx; + q__1.r = alpha->r * x[i__3].r - alpha->i * x[i__3].i, q__1.i = + alpha->r * x[i__3].i + alpha->i * x[i__3].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + i__3 = jy; + i__4 = jy; + i__2 = j * a_dim1 + 1; + r__1 = a[i__2].r; + q__2.r = r__1 * temp1.r, q__2.i = r__1 * temp1.i; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + l = 1 - j; + ix = jx; + iy = jy; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + ix += *incx; + iy += *incy; + i__4 = iy; + i__2 = iy; + i__5 = l + i__ + j * a_dim1; + q__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + q__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + q__1.r = y[i__2].r + q__2.r, q__1.i = y[i__2].i + q__2.i; + y[i__4].r = q__1.r, y[i__4].i = q__1.i; + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__4 = ix; + q__2.r = q__3.r * x[i__4].r - q__3.i * x[i__4].i, q__2.i = + q__3.r * x[i__4].i + q__3.i * x[i__4].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; +/* L110: */ + } + i__3 = jy; + i__4 = jy; + q__2.r = alpha->r * temp2.r - alpha->i * temp2.i, q__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + jx += *incx; + jy += *incy; +/* L120: */ + } + } + } + + return 0; + +/* End of CHBMV . */ + +} /* chbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/chpmv.c b/ground/gcs/src/libs/eigen/blas/f2c/chpmv.c new file mode 100644 index 0000000000..65bab1c7fc --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/chpmv.c @@ -0,0 +1,438 @@ +/* chpmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int chpmv_(char *uplo, integer *n, complex *alpha, complex * + ap, complex *x, integer *incx, complex *beta, complex *y, integer * + incy, ftnlen uplo_len) +{ + /* System generated locals */ + integer i__1, i__2, i__3, i__4, i__5; + real r__1; + complex q__1, q__2, q__3, q__4; + + /* Builtin functions */ + void r_cnjg(complex *, complex *); + + /* Local variables */ + integer i__, j, k, kk, ix, iy, jx, jy, kx, ky, info; + complex temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* CHPMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n hermitian matrix, supplied in packed form. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the matrix A is supplied in the packed */ +/* array AP as follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* supplied in AP. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* supplied in AP. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* ALPHA - COMPLEX . */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* AP - COMPLEX array of DIMENSION at least */ +/* ( ( n*( n + 1 ) )/2 ). */ +/* Before entry with UPLO = 'U' or 'u', the array AP must */ +/* contain the upper triangular part of the hermitian matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) */ +/* and a( 2, 2 ) respectively, and so on. */ +/* Before entry with UPLO = 'L' or 'l', the array AP must */ +/* contain the lower triangular part of the hermitian matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) */ +/* and a( 3, 1 ) respectively, and so on. */ +/* Note that the imaginary parts of the diagonal elements need */ +/* not be set and are assumed to be zero. */ +/* Unchanged on exit. */ + +/* X - COMPLEX array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - COMPLEX . */ +/* On entry, BETA specifies the scalar beta. When BETA is */ +/* supplied as zero then Y need not be set on input. */ +/* Unchanged on exit. */ + +/* Y - COMPLEX array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the n */ +/* element vector y. On exit, Y is overwritten by the updated */ +/* vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + --y; + --x; + --ap; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*incx == 0) { + info = 6; + } else if (*incy == 0) { + info = 9; + } + if (info != 0) { + xerbla_("CHPMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (alpha->r == 0.f && alpha->i == 0.f && (beta->r == 1.f && + beta->i == 0.f))) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array AP */ +/* are accessed sequentially with one pass through AP. */ + +/* First form y := beta*y. */ + + if (beta->r != 1.f || beta->i != 0.f) { + if (*incy == 1) { + if (beta->r == 0.f && beta->i == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + y[i__2].r = 0.f, y[i__2].i = 0.f; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + i__3 = i__; + q__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + q__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; +/* L20: */ + } + } + } else { + iy = ky; + if (beta->r == 0.f && beta->i == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + y[i__2].r = 0.f, y[i__2].i = 0.f; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + i__3 = iy; + q__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + q__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + iy += *incy; +/* L40: */ + } + } + } + } + if (alpha->r == 0.f && alpha->i == 0.f) { + return 0; + } + kk = 1; + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when AP contains the upper triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + q__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, q__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + k = kk; + i__2 = j - 1; + for (i__ = 1; i__ <= i__2; ++i__) { + i__3 = i__; + i__4 = i__; + i__5 = k; + q__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + q__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + r_cnjg(&q__3, &ap[k]); + i__3 = i__; + q__2.r = q__3.r * x[i__3].r - q__3.i * x[i__3].i, q__2.i = + q__3.r * x[i__3].i + q__3.i * x[i__3].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; + ++k; +/* L50: */ + } + i__2 = j; + i__3 = j; + i__4 = kk + j - 1; + r__1 = ap[i__4].r; + q__3.r = r__1 * temp1.r, q__3.i = r__1 * temp1.i; + q__2.r = y[i__3].r + q__3.r, q__2.i = y[i__3].i + q__3.i; + q__4.r = alpha->r * temp2.r - alpha->i * temp2.i, q__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = q__2.r + q__4.r, q__1.i = q__2.i + q__4.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + kk += j; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = jx; + q__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, q__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + ix = kx; + iy = ky; + i__2 = kk + j - 2; + for (k = kk; k <= i__2; ++k) { + i__3 = iy; + i__4 = iy; + i__5 = k; + q__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + q__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + r_cnjg(&q__3, &ap[k]); + i__3 = ix; + q__2.r = q__3.r * x[i__3].r - q__3.i * x[i__3].i, q__2.i = + q__3.r * x[i__3].i + q__3.i * x[i__3].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; + ix += *incx; + iy += *incy; +/* L70: */ + } + i__2 = jy; + i__3 = jy; + i__4 = kk + j - 1; + r__1 = ap[i__4].r; + q__3.r = r__1 * temp1.r, q__3.i = r__1 * temp1.i; + q__2.r = y[i__3].r + q__3.r, q__2.i = y[i__3].i + q__3.i; + q__4.r = alpha->r * temp2.r - alpha->i * temp2.i, q__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = q__2.r + q__4.r, q__1.i = q__2.i + q__4.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + jx += *incx; + jy += *incy; + kk += j; +/* L80: */ + } + } + } else { + +/* Form y when AP contains the lower triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + q__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, q__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + i__2 = j; + i__3 = j; + i__4 = kk; + r__1 = ap[i__4].r; + q__2.r = r__1 * temp1.r, q__2.i = r__1 * temp1.i; + q__1.r = y[i__3].r + q__2.r, q__1.i = y[i__3].i + q__2.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + k = kk + 1; + i__2 = *n; + for (i__ = j + 1; i__ <= i__2; ++i__) { + i__3 = i__; + i__4 = i__; + i__5 = k; + q__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + q__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + r_cnjg(&q__3, &ap[k]); + i__3 = i__; + q__2.r = q__3.r * x[i__3].r - q__3.i * x[i__3].i, q__2.i = + q__3.r * x[i__3].i + q__3.i * x[i__3].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; + ++k; +/* L90: */ + } + i__2 = j; + i__3 = j; + q__2.r = alpha->r * temp2.r - alpha->i * temp2.i, q__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = y[i__3].r + q__2.r, q__1.i = y[i__3].i + q__2.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + kk += *n - j + 1; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = jx; + q__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, q__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = q__1.r, temp1.i = q__1.i; + temp2.r = 0.f, temp2.i = 0.f; + i__2 = jy; + i__3 = jy; + i__4 = kk; + r__1 = ap[i__4].r; + q__2.r = r__1 * temp1.r, q__2.i = r__1 * temp1.i; + q__1.r = y[i__3].r + q__2.r, q__1.i = y[i__3].i + q__2.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + ix = jx; + iy = jy; + i__2 = kk + *n - j; + for (k = kk + 1; k <= i__2; ++k) { + ix += *incx; + iy += *incy; + i__3 = iy; + i__4 = iy; + i__5 = k; + q__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + q__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + q__1.r = y[i__4].r + q__2.r, q__1.i = y[i__4].i + q__2.i; + y[i__3].r = q__1.r, y[i__3].i = q__1.i; + r_cnjg(&q__3, &ap[k]); + i__3 = ix; + q__2.r = q__3.r * x[i__3].r - q__3.i * x[i__3].i, q__2.i = + q__3.r * x[i__3].i + q__3.i * x[i__3].r; + q__1.r = temp2.r + q__2.r, q__1.i = temp2.i + q__2.i; + temp2.r = q__1.r, temp2.i = q__1.i; +/* L110: */ + } + i__2 = jy; + i__3 = jy; + q__2.r = alpha->r * temp2.r - alpha->i * temp2.i, q__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + q__1.r = y[i__3].r + q__2.r, q__1.i = y[i__3].i + q__2.i; + y[i__2].r = q__1.r, y[i__2].i = q__1.i; + jx += *incx; + jy += *incy; + kk += *n - j + 1; +/* L120: */ + } + } + } + + return 0; + +/* End of CHPMV . */ + +} /* chpmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/complexdots.c b/ground/gcs/src/libs/eigen/blas/f2c/complexdots.c new file mode 100644 index 0000000000..a856a231c3 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/complexdots.c @@ -0,0 +1,84 @@ +/* This file has been modified to use the standard gfortran calling + convention, rather than the f2c calling convention. + + It does not require -ff2c when compiled with gfortran. +*/ + +/* complexdots.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +complex cdotc_(integer *n, complex *cx, integer + *incx, complex *cy, integer *incy) +{ + complex res; + extern /* Subroutine */ int cdotcw_(integer *, complex *, integer *, + complex *, integer *, complex *); + + /* Parameter adjustments */ + --cy; + --cx; + + /* Function Body */ + cdotcw_(n, &cx[1], incx, &cy[1], incy, &res); + return res; +} /* cdotc_ */ + +complex cdotu_(integer *n, complex *cx, integer + *incx, complex *cy, integer *incy) +{ + complex res; + extern /* Subroutine */ int cdotuw_(integer *, complex *, integer *, + complex *, integer *, complex *); + + /* Parameter adjustments */ + --cy; + --cx; + + /* Function Body */ + cdotuw_(n, &cx[1], incx, &cy[1], incy, &res); + return res; +} /* cdotu_ */ + +doublecomplex zdotc_(integer *n, doublecomplex *cx, integer *incx, + doublecomplex *cy, integer *incy) +{ + doublecomplex res; + extern /* Subroutine */ int zdotcw_(integer *, doublecomplex *, integer *, + doublecomplex *, integer *, doublecomplex *); + + /* Parameter adjustments */ + --cy; + --cx; + + /* Function Body */ + zdotcw_(n, &cx[1], incx, &cy[1], incy, &res); + return res; +} /* zdotc_ */ + +doublecomplex zdotu_(integer *n, doublecomplex *cx, integer *incx, + doublecomplex *cy, integer *incy) +{ + doublecomplex res; + extern /* Subroutine */ int zdotuw_(integer *, doublecomplex *, integer *, + doublecomplex *, integer *, doublecomplex *); + + /* Parameter adjustments */ + --cy; + --cx; + + /* Function Body */ + zdotuw_(n, &cx[1], incx, &cy[1], incy, &res); + return res; +} /* zdotu_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/ctbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/ctbmv.c new file mode 100644 index 0000000000..790fd581fe --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/ctbmv.c @@ -0,0 +1,647 @@ +/* ctbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int ctbmv_(char *uplo, char *trans, char *diag, integer *n, + integer *k, complex *a, integer *lda, complex *x, integer *incx, + ftnlen uplo_len, ftnlen trans_len, ftnlen diag_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4, i__5; + complex q__1, q__2, q__3; + + /* Builtin functions */ + void r_cnjg(complex *, complex *); + + /* Local variables */ + integer i__, j, l, ix, jx, kx, info; + complex temp; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + logical noconj, nounit; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* CTBMV performs one of the matrix-vector operations */ + +/* x := A*x, or x := A'*x, or x := conjg( A' )*x, */ + +/* where x is an n element vector and A is an n by n unit, or non-unit, */ +/* upper or lower triangular band matrix, with ( k + 1 ) diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the matrix is an upper or */ +/* lower triangular matrix as follows: */ + +/* UPLO = 'U' or 'u' A is an upper triangular matrix. */ + +/* UPLO = 'L' or 'l' A is a lower triangular matrix. */ + +/* Unchanged on exit. */ + +/* TRANS - CHARACTER*1. */ +/* On entry, TRANS specifies the operation to be performed as */ +/* follows: */ + +/* TRANS = 'N' or 'n' x := A*x. */ + +/* TRANS = 'T' or 't' x := A'*x. */ + +/* TRANS = 'C' or 'c' x := conjg( A' )*x. */ + +/* Unchanged on exit. */ + +/* DIAG - CHARACTER*1. */ +/* On entry, DIAG specifies whether or not A is unit */ +/* triangular as follows: */ + +/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */ + +/* DIAG = 'N' or 'n' A is not assumed to be unit */ +/* triangular. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry with UPLO = 'U' or 'u', K specifies the number of */ +/* super-diagonals of the matrix A. */ +/* On entry with UPLO = 'L' or 'l', K specifies the number of */ +/* sub-diagonals of the matrix A. */ +/* K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* A - COMPLEX array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer an upper */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer a lower */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Note that when DIAG = 'U' or 'u' the elements of the array A */ +/* corresponding to the diagonal elements of the matrix are not */ +/* referenced, but are assumed to be unity. */ +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - COMPLEX array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. On exit, X is overwritten with the */ +/* tranformed vector x. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (! lsame_(trans, "N", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, + "T", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, "C", (ftnlen)1, ( + ftnlen)1)) { + info = 2; + } else if (! lsame_(diag, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(diag, + "N", (ftnlen)1, (ftnlen)1)) { + info = 3; + } else if (*n < 0) { + info = 4; + } else if (*k < 0) { + info = 5; + } else if (*lda < *k + 1) { + info = 7; + } else if (*incx == 0) { + info = 9; + } + if (info != 0) { + xerbla_("CTBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0) { + return 0; + } + + noconj = lsame_(trans, "T", (ftnlen)1, (ftnlen)1); + nounit = lsame_(diag, "N", (ftnlen)1, (ftnlen)1); + +/* Set up the start point in X if the increment is not unity. This */ +/* will be ( N - 1 )*INCX too small for descending loops. */ + + if (*incx <= 0) { + kx = 1 - (*n - 1) * *incx; + } else if (*incx != 1) { + kx = 1; + } + +/* Start the operations. In this version the elements of A are */ +/* accessed sequentially with one pass through A. */ + + if (lsame_(trans, "N", (ftnlen)1, (ftnlen)1)) { + +/* Form x := A*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + if (x[i__2].r != 0.f || x[i__2].i != 0.f) { + i__2 = j; + temp.r = x[i__2].r, temp.i = x[i__2].i; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + i__2 = i__; + i__3 = i__; + i__5 = l + i__ + j * a_dim1; + q__2.r = temp.r * a[i__5].r - temp.i * a[i__5].i, + q__2.i = temp.r * a[i__5].i + temp.i * a[ + i__5].r; + q__1.r = x[i__3].r + q__2.r, q__1.i = x[i__3].i + + q__2.i; + x[i__2].r = q__1.r, x[i__2].i = q__1.i; +/* L10: */ + } + if (nounit) { + i__4 = j; + i__2 = j; + i__3 = kplus1 + j * a_dim1; + q__1.r = x[i__2].r * a[i__3].r - x[i__2].i * a[ + i__3].i, q__1.i = x[i__2].r * a[i__3].i + + x[i__2].i * a[i__3].r; + x[i__4].r = q__1.r, x[i__4].i = q__1.i; + } + } +/* L20: */ + } + } else { + jx = kx; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__4 = jx; + if (x[i__4].r != 0.f || x[i__4].i != 0.f) { + i__4 = jx; + temp.r = x[i__4].r, temp.i = x[i__4].i; + ix = kx; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + i__4 = ix; + i__2 = ix; + i__5 = l + i__ + j * a_dim1; + q__2.r = temp.r * a[i__5].r - temp.i * a[i__5].i, + q__2.i = temp.r * a[i__5].i + temp.i * a[ + i__5].r; + q__1.r = x[i__2].r + q__2.r, q__1.i = x[i__2].i + + q__2.i; + x[i__4].r = q__1.r, x[i__4].i = q__1.i; + ix += *incx; +/* L30: */ + } + if (nounit) { + i__3 = jx; + i__4 = jx; + i__2 = kplus1 + j * a_dim1; + q__1.r = x[i__4].r * a[i__2].r - x[i__4].i * a[ + i__2].i, q__1.i = x[i__4].r * a[i__2].i + + x[i__4].i * a[i__2].r; + x[i__3].r = q__1.r, x[i__3].i = q__1.i; + } + } + jx += *incx; + if (j > *k) { + kx += *incx; + } +/* L40: */ + } + } + } else { + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + i__1 = j; + if (x[i__1].r != 0.f || x[i__1].i != 0.f) { + i__1 = j; + temp.r = x[i__1].r, temp.i = x[i__1].i; + l = 1 - j; +/* Computing MIN */ + i__1 = *n, i__3 = j + *k; + i__4 = j + 1; + for (i__ = min(i__1,i__3); i__ >= i__4; --i__) { + i__1 = i__; + i__3 = i__; + i__2 = l + i__ + j * a_dim1; + q__2.r = temp.r * a[i__2].r - temp.i * a[i__2].i, + q__2.i = temp.r * a[i__2].i + temp.i * a[ + i__2].r; + q__1.r = x[i__3].r + q__2.r, q__1.i = x[i__3].i + + q__2.i; + x[i__1].r = q__1.r, x[i__1].i = q__1.i; +/* L50: */ + } + if (nounit) { + i__4 = j; + i__1 = j; + i__3 = j * a_dim1 + 1; + q__1.r = x[i__1].r * a[i__3].r - x[i__1].i * a[ + i__3].i, q__1.i = x[i__1].r * a[i__3].i + + x[i__1].i * a[i__3].r; + x[i__4].r = q__1.r, x[i__4].i = q__1.i; + } + } +/* L60: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + i__4 = jx; + if (x[i__4].r != 0.f || x[i__4].i != 0.f) { + i__4 = jx; + temp.r = x[i__4].r, temp.i = x[i__4].i; + ix = kx; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__1 = j + *k; + i__3 = j + 1; + for (i__ = min(i__4,i__1); i__ >= i__3; --i__) { + i__4 = ix; + i__1 = ix; + i__2 = l + i__ + j * a_dim1; + q__2.r = temp.r * a[i__2].r - temp.i * a[i__2].i, + q__2.i = temp.r * a[i__2].i + temp.i * a[ + i__2].r; + q__1.r = x[i__1].r + q__2.r, q__1.i = x[i__1].i + + q__2.i; + x[i__4].r = q__1.r, x[i__4].i = q__1.i; + ix -= *incx; +/* L70: */ + } + if (nounit) { + i__3 = jx; + i__4 = jx; + i__1 = j * a_dim1 + 1; + q__1.r = x[i__4].r * a[i__1].r - x[i__4].i * a[ + i__1].i, q__1.i = x[i__4].r * a[i__1].i + + x[i__4].i * a[i__1].r; + x[i__3].r = q__1.r, x[i__3].i = q__1.i; + } + } + jx -= *incx; + if (*n - j >= *k) { + kx -= *incx; + } +/* L80: */ + } + } + } + } else { + +/* Form x := A'*x or x := conjg( A' )*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + i__3 = j; + temp.r = x[i__3].r, temp.i = x[i__3].i; + l = kplus1 - j; + if (noconj) { + if (nounit) { + i__3 = kplus1 + j * a_dim1; + q__1.r = temp.r * a[i__3].r - temp.i * a[i__3].i, + q__1.i = temp.r * a[i__3].i + temp.i * a[ + i__3].r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + i__4 = l + i__ + j * a_dim1; + i__1 = i__; + q__2.r = a[i__4].r * x[i__1].r - a[i__4].i * x[ + i__1].i, q__2.i = a[i__4].r * x[i__1].i + + a[i__4].i * x[i__1].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; +/* L90: */ + } + } else { + if (nounit) { + r_cnjg(&q__2, &a[kplus1 + j * a_dim1]); + q__1.r = temp.r * q__2.r - temp.i * q__2.i, + q__1.i = temp.r * q__2.i + temp.i * + q__2.r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__4 = i__; + q__2.r = q__3.r * x[i__4].r - q__3.i * x[i__4].i, + q__2.i = q__3.r * x[i__4].i + q__3.i * x[ + i__4].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; +/* L100: */ + } + } + i__3 = j; + x[i__3].r = temp.r, x[i__3].i = temp.i; +/* L110: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + i__3 = jx; + temp.r = x[i__3].r, temp.i = x[i__3].i; + kx -= *incx; + ix = kx; + l = kplus1 - j; + if (noconj) { + if (nounit) { + i__3 = kplus1 + j * a_dim1; + q__1.r = temp.r * a[i__3].r - temp.i * a[i__3].i, + q__1.i = temp.r * a[i__3].i + temp.i * a[ + i__3].r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + i__4 = l + i__ + j * a_dim1; + i__1 = ix; + q__2.r = a[i__4].r * x[i__1].r - a[i__4].i * x[ + i__1].i, q__2.i = a[i__4].r * x[i__1].i + + a[i__4].i * x[i__1].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; + ix -= *incx; +/* L120: */ + } + } else { + if (nounit) { + r_cnjg(&q__2, &a[kplus1 + j * a_dim1]); + q__1.r = temp.r * q__2.r - temp.i * q__2.i, + q__1.i = temp.r * q__2.i + temp.i * + q__2.r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__4 = ix; + q__2.r = q__3.r * x[i__4].r - q__3.i * x[i__4].i, + q__2.i = q__3.r * x[i__4].i + q__3.i * x[ + i__4].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; + ix -= *incx; +/* L130: */ + } + } + i__3 = jx; + x[i__3].r = temp.r, x[i__3].i = temp.i; + jx -= *incx; +/* L140: */ + } + } + } else { + if (*incx == 1) { + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + i__4 = j; + temp.r = x[i__4].r, temp.i = x[i__4].i; + l = 1 - j; + if (noconj) { + if (nounit) { + i__4 = j * a_dim1 + 1; + q__1.r = temp.r * a[i__4].r - temp.i * a[i__4].i, + q__1.i = temp.r * a[i__4].i + temp.i * a[ + i__4].r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + i__1 = l + i__ + j * a_dim1; + i__2 = i__; + q__2.r = a[i__1].r * x[i__2].r - a[i__1].i * x[ + i__2].i, q__2.i = a[i__1].r * x[i__2].i + + a[i__1].i * x[i__2].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; +/* L150: */ + } + } else { + if (nounit) { + r_cnjg(&q__2, &a[j * a_dim1 + 1]); + q__1.r = temp.r * q__2.r - temp.i * q__2.i, + q__1.i = temp.r * q__2.i + temp.i * + q__2.r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__1 = i__; + q__2.r = q__3.r * x[i__1].r - q__3.i * x[i__1].i, + q__2.i = q__3.r * x[i__1].i + q__3.i * x[ + i__1].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; +/* L160: */ + } + } + i__4 = j; + x[i__4].r = temp.r, x[i__4].i = temp.i; +/* L170: */ + } + } else { + jx = kx; + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + i__4 = jx; + temp.r = x[i__4].r, temp.i = x[i__4].i; + kx += *incx; + ix = kx; + l = 1 - j; + if (noconj) { + if (nounit) { + i__4 = j * a_dim1 + 1; + q__1.r = temp.r * a[i__4].r - temp.i * a[i__4].i, + q__1.i = temp.r * a[i__4].i + temp.i * a[ + i__4].r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + i__1 = l + i__ + j * a_dim1; + i__2 = ix; + q__2.r = a[i__1].r * x[i__2].r - a[i__1].i * x[ + i__2].i, q__2.i = a[i__1].r * x[i__2].i + + a[i__1].i * x[i__2].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; + ix += *incx; +/* L180: */ + } + } else { + if (nounit) { + r_cnjg(&q__2, &a[j * a_dim1 + 1]); + q__1.r = temp.r * q__2.r - temp.i * q__2.i, + q__1.i = temp.r * q__2.i + temp.i * + q__2.r; + temp.r = q__1.r, temp.i = q__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + r_cnjg(&q__3, &a[l + i__ + j * a_dim1]); + i__1 = ix; + q__2.r = q__3.r * x[i__1].r - q__3.i * x[i__1].i, + q__2.i = q__3.r * x[i__1].i + q__3.i * x[ + i__1].r; + q__1.r = temp.r + q__2.r, q__1.i = temp.i + + q__2.i; + temp.r = q__1.r, temp.i = q__1.i; + ix += *incx; +/* L190: */ + } + } + i__4 = jx; + x[i__4].r = temp.r, x[i__4].i = temp.i; + jx += *incx; +/* L200: */ + } + } + } + } + + return 0; + +/* End of CTBMV . */ + +} /* ctbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/d_cnjg.c b/ground/gcs/src/libs/eigen/blas/f2c/d_cnjg.c new file mode 100644 index 0000000000..623090c6b0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/d_cnjg.c @@ -0,0 +1,6 @@ +#include "datatypes.h" + +void d_cnjg(doublecomplex *r, doublecomplex *z) { + r->r = z->r; + r->i = -(z->i); +} diff --git a/ground/gcs/src/libs/eigen/blas/f2c/datatypes.h b/ground/gcs/src/libs/eigen/blas/f2c/datatypes.h new file mode 100644 index 0000000000..63232b246a --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/datatypes.h @@ -0,0 +1,24 @@ +/* This contains a limited subset of the typedefs exposed by f2c + for use by the Eigen BLAS C-only implementation. +*/ + +#ifndef __EIGEN_DATATYPES_H__ +#define __EIGEN_DATATYPES_H__ + +typedef int integer; +typedef unsigned int uinteger; +typedef float real; +typedef double doublereal; +typedef struct { real r, i; } complex; +typedef struct { doublereal r, i; } doublecomplex; +typedef int ftnlen; +typedef int logical; + +#define abs(x) ((x) >= 0 ? (x) : -(x)) +#define dabs(x) (doublereal)abs(x) +#define min(a,b) ((a) <= (b) ? (a) : (b)) +#define max(a,b) ((a) >= (b) ? (a) : (b)) +#define dmin(a,b) (doublereal)min(a,b) +#define dmax(a,b) (doublereal)max(a,b) + +#endif diff --git a/ground/gcs/src/libs/eigen/blas/f2c/drotm.c b/ground/gcs/src/libs/eigen/blas/f2c/drotm.c new file mode 100644 index 0000000000..17a779b74b --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/drotm.c @@ -0,0 +1,215 @@ +/* drotm.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int drotm_(integer *n, doublereal *dx, integer *incx, + doublereal *dy, integer *incy, doublereal *dparam) +{ + /* Initialized data */ + + static doublereal zero = 0.; + static doublereal two = 2.; + + /* System generated locals */ + integer i__1, i__2; + + /* Local variables */ + integer i__; + doublereal w, z__; + integer kx, ky; + doublereal dh11, dh12, dh21, dh22, dflag; + integer nsteps; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* APPLY THE MODIFIED GIVENS TRANSFORMATION, H, TO THE 2 BY N MATRIX */ + +/* (DX**T) , WHERE **T INDICATES TRANSPOSE. THE ELEMENTS OF DX ARE IN */ +/* (DY**T) */ + +/* DX(LX+I*INCX), I = 0 TO N-1, WHERE LX = 1 IF INCX .GE. 0, ELSE */ +/* LX = (-INCX)*N, AND SIMILARLY FOR SY USING LY AND INCY. */ +/* WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS.. */ + +/* DFLAG=-1.D0 DFLAG=0.D0 DFLAG=1.D0 DFLAG=-2.D0 */ + +/* (DH11 DH12) (1.D0 DH12) (DH11 1.D0) (1.D0 0.D0) */ +/* H=( ) ( ) ( ) ( ) */ +/* (DH21 DH22), (DH21 1.D0), (-1.D0 DH22), (0.D0 1.D0). */ +/* SEE DROTMG FOR A DESCRIPTION OF DATA STORAGE IN DPARAM. */ + +/* Arguments */ +/* ========= */ + +/* N (input) INTEGER */ +/* number of elements in input vector(s) */ + +/* DX (input/output) DOUBLE PRECISION array, dimension N */ +/* double precision vector with N elements */ + +/* INCX (input) INTEGER */ +/* storage spacing between elements of DX */ + +/* DY (input/output) DOUBLE PRECISION array, dimension N */ +/* double precision vector with N elements */ + +/* INCY (input) INTEGER */ +/* storage spacing between elements of DY */ + +/* DPARAM (input/output) DOUBLE PRECISION array, dimension 5 */ +/* DPARAM(1)=DFLAG */ +/* DPARAM(2)=DH11 */ +/* DPARAM(3)=DH21 */ +/* DPARAM(4)=DH12 */ +/* DPARAM(5)=DH22 */ + +/* ===================================================================== */ + +/* .. Local Scalars .. */ +/* .. */ +/* .. Data statements .. */ + /* Parameter adjustments */ + --dparam; + --dy; + --dx; + + /* Function Body */ +/* .. */ + + dflag = dparam[1]; + if (*n <= 0 || dflag + two == zero) { + goto L140; + } + if (! (*incx == *incy && *incx > 0)) { + goto L70; + } + + nsteps = *n * *incx; + if (dflag < 0.) { + goto L50; + } else if (dflag == 0) { + goto L10; + } else { + goto L30; + } +L10: + dh12 = dparam[4]; + dh21 = dparam[3]; + i__1 = nsteps; + i__2 = *incx; + for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) { + w = dx[i__]; + z__ = dy[i__]; + dx[i__] = w + z__ * dh12; + dy[i__] = w * dh21 + z__; +/* L20: */ + } + goto L140; +L30: + dh11 = dparam[2]; + dh22 = dparam[5]; + i__2 = nsteps; + i__1 = *incx; + for (i__ = 1; i__1 < 0 ? i__ >= i__2 : i__ <= i__2; i__ += i__1) { + w = dx[i__]; + z__ = dy[i__]; + dx[i__] = w * dh11 + z__; + dy[i__] = -w + dh22 * z__; +/* L40: */ + } + goto L140; +L50: + dh11 = dparam[2]; + dh12 = dparam[4]; + dh21 = dparam[3]; + dh22 = dparam[5]; + i__1 = nsteps; + i__2 = *incx; + for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) { + w = dx[i__]; + z__ = dy[i__]; + dx[i__] = w * dh11 + z__ * dh12; + dy[i__] = w * dh21 + z__ * dh22; +/* L60: */ + } + goto L140; +L70: + kx = 1; + ky = 1; + if (*incx < 0) { + kx = (1 - *n) * *incx + 1; + } + if (*incy < 0) { + ky = (1 - *n) * *incy + 1; + } + + if (dflag < 0.) { + goto L120; + } else if (dflag == 0) { + goto L80; + } else { + goto L100; + } +L80: + dh12 = dparam[4]; + dh21 = dparam[3]; + i__2 = *n; + for (i__ = 1; i__ <= i__2; ++i__) { + w = dx[kx]; + z__ = dy[ky]; + dx[kx] = w + z__ * dh12; + dy[ky] = w * dh21 + z__; + kx += *incx; + ky += *incy; +/* L90: */ + } + goto L140; +L100: + dh11 = dparam[2]; + dh22 = dparam[5]; + i__2 = *n; + for (i__ = 1; i__ <= i__2; ++i__) { + w = dx[kx]; + z__ = dy[ky]; + dx[kx] = w * dh11 + z__; + dy[ky] = -w + dh22 * z__; + kx += *incx; + ky += *incy; +/* L110: */ + } + goto L140; +L120: + dh11 = dparam[2]; + dh12 = dparam[4]; + dh21 = dparam[3]; + dh22 = dparam[5]; + i__2 = *n; + for (i__ = 1; i__ <= i__2; ++i__) { + w = dx[kx]; + z__ = dy[ky]; + dx[kx] = w * dh11 + z__ * dh12; + dy[ky] = w * dh21 + z__ * dh22; + kx += *incx; + ky += *incy; +/* L130: */ + } +L140: + return 0; +} /* drotm_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/drotmg.c b/ground/gcs/src/libs/eigen/blas/f2c/drotmg.c new file mode 100644 index 0000000000..a63eb10834 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/drotmg.c @@ -0,0 +1,293 @@ +/* drotmg.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int drotmg_(doublereal *dd1, doublereal *dd2, doublereal * + dx1, doublereal *dy1, doublereal *dparam) +{ + /* Initialized data */ + + static doublereal zero = 0.; + static doublereal one = 1.; + static doublereal two = 2.; + static doublereal gam = 4096.; + static doublereal gamsq = 16777216.; + static doublereal rgamsq = 5.9604645e-8; + + /* Format strings */ + static char fmt_120[] = ""; + static char fmt_150[] = ""; + static char fmt_180[] = ""; + static char fmt_210[] = ""; + + /* System generated locals */ + doublereal d__1; + + /* Local variables */ + doublereal du, dp1, dp2, dq1, dq2, dh11, dh12, dh21, dh22; + integer igo; + doublereal dflag, dtemp; + + /* Assigned format variables */ + static char *igo_fmt; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* CONSTRUCT THE MODIFIED GIVENS TRANSFORMATION MATRIX H WHICH ZEROS */ +/* THE SECOND COMPONENT OF THE 2-VECTOR (DSQRT(DD1)*DX1,DSQRT(DD2)* */ +/* DY2)**T. */ +/* WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS.. */ + +/* DFLAG=-1.D0 DFLAG=0.D0 DFLAG=1.D0 DFLAG=-2.D0 */ + +/* (DH11 DH12) (1.D0 DH12) (DH11 1.D0) (1.D0 0.D0) */ +/* H=( ) ( ) ( ) ( ) */ +/* (DH21 DH22), (DH21 1.D0), (-1.D0 DH22), (0.D0 1.D0). */ +/* LOCATIONS 2-4 OF DPARAM CONTAIN DH11, DH21, DH12, AND DH22 */ +/* RESPECTIVELY. (VALUES OF 1.D0, -1.D0, OR 0.D0 IMPLIED BY THE */ +/* VALUE OF DPARAM(1) ARE NOT STORED IN DPARAM.) */ + +/* THE VALUES OF GAMSQ AND RGAMSQ SET IN THE DATA STATEMENT MAY BE */ +/* INEXACT. THIS IS OK AS THEY ARE ONLY USED FOR TESTING THE SIZE */ +/* OF DD1 AND DD2. ALL ACTUAL SCALING OF DATA IS DONE USING GAM. */ + + +/* Arguments */ +/* ========= */ + +/* DD1 (input/output) DOUBLE PRECISION */ + +/* DD2 (input/output) DOUBLE PRECISION */ + +/* DX1 (input/output) DOUBLE PRECISION */ + +/* DY1 (input) DOUBLE PRECISION */ + +/* DPARAM (input/output) DOUBLE PRECISION array, dimension 5 */ +/* DPARAM(1)=DFLAG */ +/* DPARAM(2)=DH11 */ +/* DPARAM(3)=DH21 */ +/* DPARAM(4)=DH12 */ +/* DPARAM(5)=DH22 */ + +/* ===================================================================== */ + +/* .. Local Scalars .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ +/* .. Data statements .. */ + + /* Parameter adjustments */ + --dparam; + + /* Function Body */ +/* .. */ + if (! (*dd1 < zero)) { + goto L10; + } +/* GO ZERO-H-D-AND-DX1.. */ + goto L60; +L10: +/* CASE-DD1-NONNEGATIVE */ + dp2 = *dd2 * *dy1; + if (! (dp2 == zero)) { + goto L20; + } + dflag = -two; + goto L260; +/* REGULAR-CASE.. */ +L20: + dp1 = *dd1 * *dx1; + dq2 = dp2 * *dy1; + dq1 = dp1 * *dx1; + + if (! (abs(dq1) > abs(dq2))) { + goto L40; + } + dh21 = -(*dy1) / *dx1; + dh12 = dp2 / dp1; + + du = one - dh12 * dh21; + + if (! (du <= zero)) { + goto L30; + } +/* GO ZERO-H-D-AND-DX1.. */ + goto L60; +L30: + dflag = zero; + *dd1 /= du; + *dd2 /= du; + *dx1 *= du; +/* GO SCALE-CHECK.. */ + goto L100; +L40: + if (! (dq2 < zero)) { + goto L50; + } +/* GO ZERO-H-D-AND-DX1.. */ + goto L60; +L50: + dflag = one; + dh11 = dp1 / dp2; + dh22 = *dx1 / *dy1; + du = one + dh11 * dh22; + dtemp = *dd2 / du; + *dd2 = *dd1 / du; + *dd1 = dtemp; + *dx1 = *dy1 * du; +/* GO SCALE-CHECK */ + goto L100; +/* PROCEDURE..ZERO-H-D-AND-DX1.. */ +L60: + dflag = -one; + dh11 = zero; + dh12 = zero; + dh21 = zero; + dh22 = zero; + + *dd1 = zero; + *dd2 = zero; + *dx1 = zero; +/* RETURN.. */ + goto L220; +/* PROCEDURE..FIX-H.. */ +L70: + if (! (dflag >= zero)) { + goto L90; + } + + if (! (dflag == zero)) { + goto L80; + } + dh11 = one; + dh22 = one; + dflag = -one; + goto L90; +L80: + dh21 = -one; + dh12 = one; + dflag = -one; +L90: + switch (igo) { + case 0: goto L120; + case 1: goto L150; + case 2: goto L180; + case 3: goto L210; + } +/* PROCEDURE..SCALE-CHECK */ +L100: +L110: + if (! (*dd1 <= rgamsq)) { + goto L130; + } + if (*dd1 == zero) { + goto L160; + } + igo = 0; + igo_fmt = fmt_120; +/* FIX-H.. */ + goto L70; +L120: +/* Computing 2nd power */ + d__1 = gam; + *dd1 *= d__1 * d__1; + *dx1 /= gam; + dh11 /= gam; + dh12 /= gam; + goto L110; +L130: +L140: + if (! (*dd1 >= gamsq)) { + goto L160; + } + igo = 1; + igo_fmt = fmt_150; +/* FIX-H.. */ + goto L70; +L150: +/* Computing 2nd power */ + d__1 = gam; + *dd1 /= d__1 * d__1; + *dx1 *= gam; + dh11 *= gam; + dh12 *= gam; + goto L140; +L160: +L170: + if (! (abs(*dd2) <= rgamsq)) { + goto L190; + } + if (*dd2 == zero) { + goto L220; + } + igo = 2; + igo_fmt = fmt_180; +/* FIX-H.. */ + goto L70; +L180: +/* Computing 2nd power */ + d__1 = gam; + *dd2 *= d__1 * d__1; + dh21 /= gam; + dh22 /= gam; + goto L170; +L190: +L200: + if (! (abs(*dd2) >= gamsq)) { + goto L220; + } + igo = 3; + igo_fmt = fmt_210; +/* FIX-H.. */ + goto L70; +L210: +/* Computing 2nd power */ + d__1 = gam; + *dd2 /= d__1 * d__1; + dh21 *= gam; + dh22 *= gam; + goto L200; +L220: + if (dflag < 0.) { + goto L250; + } else if (dflag == 0) { + goto L230; + } else { + goto L240; + } +L230: + dparam[3] = dh21; + dparam[4] = dh12; + goto L260; +L240: + dparam[2] = dh11; + dparam[5] = dh22; + goto L260; +L250: + dparam[2] = dh11; + dparam[3] = dh21; + dparam[4] = dh12; + dparam[5] = dh22; +L260: + dparam[1] = dflag; + return 0; +} /* drotmg_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/dsbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/dsbmv.c new file mode 100644 index 0000000000..c6b4b21d65 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/dsbmv.c @@ -0,0 +1,366 @@ +/* dsbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int dsbmv_(char *uplo, integer *n, integer *k, doublereal * + alpha, doublereal *a, integer *lda, doublereal *x, integer *incx, + doublereal *beta, doublereal *y, integer *incy, ftnlen uplo_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4; + + /* Local variables */ + integer i__, j, l, ix, iy, jx, jy, kx, ky, info; + doublereal temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* DSBMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n symmetric band matrix, with k super-diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the band matrix A is being supplied as */ +/* follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* being supplied. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* being supplied. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry, K specifies the number of super-diagonals of the */ +/* matrix A. K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* ALPHA - DOUBLE PRECISION. */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the symmetric matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer the upper */ +/* triangular part of a symmetric band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the symmetric matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer the lower */ +/* triangular part of a symmetric band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - DOUBLE PRECISION array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the */ +/* vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - DOUBLE PRECISION. */ +/* On entry, BETA specifies the scalar beta. */ +/* Unchanged on exit. */ + +/* Y - DOUBLE PRECISION array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the */ +/* vector y. On exit, Y is overwritten by the updated vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + --y; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*k < 0) { + info = 3; + } else if (*lda < *k + 1) { + info = 6; + } else if (*incx == 0) { + info = 8; + } else if (*incy == 0) { + info = 11; + } + if (info != 0) { + xerbla_("DSBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (*alpha == 0. && *beta == 1.)) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array A */ +/* are accessed sequentially with one pass through A. */ + +/* First form y := beta*y. */ + + if (*beta != 1.) { + if (*incy == 1) { + if (*beta == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = 0.; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = *beta * y[i__]; +/* L20: */ + } + } + } else { + iy = ky; + if (*beta == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = 0.; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = *beta * y[iy]; + iy += *incy; +/* L40: */ + } + } + } + } + if (*alpha == 0.) { + return 0; + } + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when upper triangle of A is stored. */ + + kplus1 = *k + 1; + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + y[i__] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[i__]; +/* L50: */ + } + y[j] = y[j] + temp1 * a[kplus1 + j * a_dim1] + *alpha * temp2; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.; + ix = kx; + iy = ky; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + y[iy] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[ix]; + ix += *incx; + iy += *incy; +/* L70: */ + } + y[jy] = y[jy] + temp1 * a[kplus1 + j * a_dim1] + *alpha * + temp2; + jx += *incx; + jy += *incy; + if (j > *k) { + kx += *incx; + ky += *incy; + } +/* L80: */ + } + } + } else { + +/* Form y when lower triangle of A is stored. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.; + y[j] += temp1 * a[j * a_dim1 + 1]; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + y[i__] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[i__]; +/* L90: */ + } + y[j] += *alpha * temp2; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.; + y[jy] += temp1 * a[j * a_dim1 + 1]; + l = 1 - j; + ix = jx; + iy = jy; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + ix += *incx; + iy += *incy; + y[iy] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[ix]; +/* L110: */ + } + y[jy] += *alpha * temp2; + jx += *incx; + jy += *incy; +/* L120: */ + } + } + } + + return 0; + +/* End of DSBMV . */ + +} /* dsbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/dspmv.c b/ground/gcs/src/libs/eigen/blas/f2c/dspmv.c new file mode 100644 index 0000000000..0b4e92d5cf --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/dspmv.c @@ -0,0 +1,316 @@ +/* dspmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int dspmv_(char *uplo, integer *n, doublereal *alpha, + doublereal *ap, doublereal *x, integer *incx, doublereal *beta, + doublereal *y, integer *incy, ftnlen uplo_len) +{ + /* System generated locals */ + integer i__1, i__2; + + /* Local variables */ + integer i__, j, k, kk, ix, iy, jx, jy, kx, ky, info; + doublereal temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* DSPMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n symmetric matrix, supplied in packed form. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the matrix A is supplied in the packed */ +/* array AP as follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* supplied in AP. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* supplied in AP. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* ALPHA - DOUBLE PRECISION. */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* AP - DOUBLE PRECISION array of DIMENSION at least */ +/* ( ( n*( n + 1 ) )/2 ). */ +/* Before entry with UPLO = 'U' or 'u', the array AP must */ +/* contain the upper triangular part of the symmetric matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) */ +/* and a( 2, 2 ) respectively, and so on. */ +/* Before entry with UPLO = 'L' or 'l', the array AP must */ +/* contain the lower triangular part of the symmetric matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) */ +/* and a( 3, 1 ) respectively, and so on. */ +/* Unchanged on exit. */ + +/* X - DOUBLE PRECISION array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - DOUBLE PRECISION. */ +/* On entry, BETA specifies the scalar beta. When BETA is */ +/* supplied as zero then Y need not be set on input. */ +/* Unchanged on exit. */ + +/* Y - DOUBLE PRECISION array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the n */ +/* element vector y. On exit, Y is overwritten by the updated */ +/* vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + --y; + --x; + --ap; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*incx == 0) { + info = 6; + } else if (*incy == 0) { + info = 9; + } + if (info != 0) { + xerbla_("DSPMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (*alpha == 0. && *beta == 1.)) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array AP */ +/* are accessed sequentially with one pass through AP. */ + +/* First form y := beta*y. */ + + if (*beta != 1.) { + if (*incy == 1) { + if (*beta == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = 0.; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = *beta * y[i__]; +/* L20: */ + } + } + } else { + iy = ky; + if (*beta == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = 0.; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = *beta * y[iy]; + iy += *incy; +/* L40: */ + } + } + } + } + if (*alpha == 0.) { + return 0; + } + kk = 1; + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when AP contains the upper triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.; + k = kk; + i__2 = j - 1; + for (i__ = 1; i__ <= i__2; ++i__) { + y[i__] += temp1 * ap[k]; + temp2 += ap[k] * x[i__]; + ++k; +/* L50: */ + } + y[j] = y[j] + temp1 * ap[kk + j - 1] + *alpha * temp2; + kk += j; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.; + ix = kx; + iy = ky; + i__2 = kk + j - 2; + for (k = kk; k <= i__2; ++k) { + y[iy] += temp1 * ap[k]; + temp2 += ap[k] * x[ix]; + ix += *incx; + iy += *incy; +/* L70: */ + } + y[jy] = y[jy] + temp1 * ap[kk + j - 1] + *alpha * temp2; + jx += *incx; + jy += *incy; + kk += j; +/* L80: */ + } + } + } else { + +/* Form y when AP contains the lower triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.; + y[j] += temp1 * ap[kk]; + k = kk + 1; + i__2 = *n; + for (i__ = j + 1; i__ <= i__2; ++i__) { + y[i__] += temp1 * ap[k]; + temp2 += ap[k] * x[i__]; + ++k; +/* L90: */ + } + y[j] += *alpha * temp2; + kk += *n - j + 1; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.; + y[jy] += temp1 * ap[kk]; + ix = jx; + iy = jy; + i__2 = kk + *n - j; + for (k = kk + 1; k <= i__2; ++k) { + ix += *incx; + iy += *incy; + y[iy] += temp1 * ap[k]; + temp2 += ap[k] * x[ix]; +/* L110: */ + } + y[jy] += *alpha * temp2; + jx += *incx; + jy += *incy; + kk += *n - j + 1; +/* L120: */ + } + } + } + + return 0; + +/* End of DSPMV . */ + +} /* dspmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/dtbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/dtbmv.c new file mode 100644 index 0000000000..fdf73ebb52 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/dtbmv.c @@ -0,0 +1,428 @@ +/* dtbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int dtbmv_(char *uplo, char *trans, char *diag, integer *n, + integer *k, doublereal *a, integer *lda, doublereal *x, integer *incx, + ftnlen uplo_len, ftnlen trans_len, ftnlen diag_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4; + + /* Local variables */ + integer i__, j, l, ix, jx, kx, info; + doublereal temp; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + logical nounit; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* DTBMV performs one of the matrix-vector operations */ + +/* x := A*x, or x := A'*x, */ + +/* where x is an n element vector and A is an n by n unit, or non-unit, */ +/* upper or lower triangular band matrix, with ( k + 1 ) diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the matrix is an upper or */ +/* lower triangular matrix as follows: */ + +/* UPLO = 'U' or 'u' A is an upper triangular matrix. */ + +/* UPLO = 'L' or 'l' A is a lower triangular matrix. */ + +/* Unchanged on exit. */ + +/* TRANS - CHARACTER*1. */ +/* On entry, TRANS specifies the operation to be performed as */ +/* follows: */ + +/* TRANS = 'N' or 'n' x := A*x. */ + +/* TRANS = 'T' or 't' x := A'*x. */ + +/* TRANS = 'C' or 'c' x := A'*x. */ + +/* Unchanged on exit. */ + +/* DIAG - CHARACTER*1. */ +/* On entry, DIAG specifies whether or not A is unit */ +/* triangular as follows: */ + +/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */ + +/* DIAG = 'N' or 'n' A is not assumed to be unit */ +/* triangular. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry with UPLO = 'U' or 'u', K specifies the number of */ +/* super-diagonals of the matrix A. */ +/* On entry with UPLO = 'L' or 'l', K specifies the number of */ +/* sub-diagonals of the matrix A. */ +/* K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer an upper */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer a lower */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Note that when DIAG = 'U' or 'u' the elements of the array A */ +/* corresponding to the diagonal elements of the matrix are not */ +/* referenced, but are assumed to be unity. */ +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - DOUBLE PRECISION array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. On exit, X is overwritten with the */ +/* tranformed vector x. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (! lsame_(trans, "N", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, + "T", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, "C", (ftnlen)1, ( + ftnlen)1)) { + info = 2; + } else if (! lsame_(diag, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(diag, + "N", (ftnlen)1, (ftnlen)1)) { + info = 3; + } else if (*n < 0) { + info = 4; + } else if (*k < 0) { + info = 5; + } else if (*lda < *k + 1) { + info = 7; + } else if (*incx == 0) { + info = 9; + } + if (info != 0) { + xerbla_("DTBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0) { + return 0; + } + + nounit = lsame_(diag, "N", (ftnlen)1, (ftnlen)1); + +/* Set up the start point in X if the increment is not unity. This */ +/* will be ( N - 1 )*INCX too small for descending loops. */ + + if (*incx <= 0) { + kx = 1 - (*n - 1) * *incx; + } else if (*incx != 1) { + kx = 1; + } + +/* Start the operations. In this version the elements of A are */ +/* accessed sequentially with one pass through A. */ + + if (lsame_(trans, "N", (ftnlen)1, (ftnlen)1)) { + +/* Form x := A*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (x[j] != 0.) { + temp = x[j]; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + x[i__] += temp * a[l + i__ + j * a_dim1]; +/* L10: */ + } + if (nounit) { + x[j] *= a[kplus1 + j * a_dim1]; + } + } +/* L20: */ + } + } else { + jx = kx; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (x[jx] != 0.) { + temp = x[jx]; + ix = kx; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + x[ix] += temp * a[l + i__ + j * a_dim1]; + ix += *incx; +/* L30: */ + } + if (nounit) { + x[jx] *= a[kplus1 + j * a_dim1]; + } + } + jx += *incx; + if (j > *k) { + kx += *incx; + } +/* L40: */ + } + } + } else { + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + if (x[j] != 0.) { + temp = x[j]; + l = 1 - j; +/* Computing MIN */ + i__1 = *n, i__3 = j + *k; + i__4 = j + 1; + for (i__ = min(i__1,i__3); i__ >= i__4; --i__) { + x[i__] += temp * a[l + i__ + j * a_dim1]; +/* L50: */ + } + if (nounit) { + x[j] *= a[j * a_dim1 + 1]; + } + } +/* L60: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + if (x[jx] != 0.) { + temp = x[jx]; + ix = kx; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__1 = j + *k; + i__3 = j + 1; + for (i__ = min(i__4,i__1); i__ >= i__3; --i__) { + x[ix] += temp * a[l + i__ + j * a_dim1]; + ix -= *incx; +/* L70: */ + } + if (nounit) { + x[jx] *= a[j * a_dim1 + 1]; + } + } + jx -= *incx; + if (*n - j >= *k) { + kx -= *incx; + } +/* L80: */ + } + } + } + } else { + +/* Form x := A'*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + temp = x[j]; + l = kplus1 - j; + if (nounit) { + temp *= a[kplus1 + j * a_dim1]; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + temp += a[l + i__ + j * a_dim1] * x[i__]; +/* L90: */ + } + x[j] = temp; +/* L100: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + temp = x[jx]; + kx -= *incx; + ix = kx; + l = kplus1 - j; + if (nounit) { + temp *= a[kplus1 + j * a_dim1]; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + temp += a[l + i__ + j * a_dim1] * x[ix]; + ix -= *incx; +/* L110: */ + } + x[jx] = temp; + jx -= *incx; +/* L120: */ + } + } + } else { + if (*incx == 1) { + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + temp = x[j]; + l = 1 - j; + if (nounit) { + temp *= a[j * a_dim1 + 1]; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + temp += a[l + i__ + j * a_dim1] * x[i__]; +/* L130: */ + } + x[j] = temp; +/* L140: */ + } + } else { + jx = kx; + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + temp = x[jx]; + kx += *incx; + ix = kx; + l = 1 - j; + if (nounit) { + temp *= a[j * a_dim1 + 1]; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + temp += a[l + i__ + j * a_dim1] * x[ix]; + ix += *incx; +/* L150: */ + } + x[jx] = temp; + jx += *incx; +/* L160: */ + } + } + } + } + + return 0; + +/* End of DTBMV . */ + +} /* dtbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/lsame.c b/ground/gcs/src/libs/eigen/blas/f2c/lsame.c new file mode 100644 index 0000000000..46324d9168 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/lsame.c @@ -0,0 +1,117 @@ +/* lsame.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +logical lsame_(char *ca, char *cb, ftnlen ca_len, ftnlen cb_len) +{ + /* System generated locals */ + logical ret_val; + + /* Local variables */ + integer inta, intb, zcode; + + +/* -- LAPACK auxiliary routine (version 3.1) -- */ +/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */ +/* November 2006 */ + +/* .. Scalar Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* LSAME returns .TRUE. if CA is the same letter as CB regardless of */ +/* case. */ + +/* Arguments */ +/* ========= */ + +/* CA (input) CHARACTER*1 */ + +/* CB (input) CHARACTER*1 */ +/* CA and CB specify the single characters to be compared. */ + +/* ===================================================================== */ + +/* .. Intrinsic Functions .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ + +/* Test if the characters are equal */ + + ret_val = *(unsigned char *)ca == *(unsigned char *)cb; + if (ret_val) { + return ret_val; + } + +/* Now test for equivalence if both characters are alphabetic. */ + + zcode = 'Z'; + +/* Use 'Z' rather than 'A' so that ASCII can be detected on Prime */ +/* machines, on which ICHAR returns a value with bit 8 set. */ +/* ICHAR('A') on Prime machines returns 193 which is the same as */ +/* ICHAR('A') on an EBCDIC machine. */ + + inta = *(unsigned char *)ca; + intb = *(unsigned char *)cb; + + if (zcode == 90 || zcode == 122) { + +/* ASCII is assumed - ZCODE is the ASCII code of either lower or */ +/* upper case 'Z'. */ + + if (inta >= 97 && inta <= 122) { + inta += -32; + } + if (intb >= 97 && intb <= 122) { + intb += -32; + } + + } else if (zcode == 233 || zcode == 169) { + +/* EBCDIC is assumed - ZCODE is the EBCDIC code of either lower or */ +/* upper case 'Z'. */ + + if ((inta >= 129 && inta <= 137) || (inta >= 145 && inta <= 153) || + (inta >= 162 && inta <= 169)) { + inta += 64; + } + if ((intb >= 129 && intb <= 137) || (intb >= 145 && intb <= 153) || + (intb >= 162 && intb <= 169)) { + intb += 64; + } + + } else if (zcode == 218 || zcode == 250) { + +/* ASCII is assumed, on Prime machines - ZCODE is the ASCII code */ +/* plus 128 of either lower or upper case 'Z'. */ + + if (inta >= 225 && inta <= 250) { + inta += -32; + } + if (intb >= 225 && intb <= 250) { + intb += -32; + } + } + ret_val = inta == intb; + +/* RETURN */ + +/* End of LSAME */ + + return ret_val; +} /* lsame_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/r_cnjg.c b/ground/gcs/src/libs/eigen/blas/f2c/r_cnjg.c new file mode 100644 index 0000000000..c08182f88e --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/r_cnjg.c @@ -0,0 +1,6 @@ +#include "datatypes.h" + +void r_cnjg(complex *r, complex *z) { + r->r = z->r; + r->i = -(z->i); +} diff --git a/ground/gcs/src/libs/eigen/blas/f2c/srotm.c b/ground/gcs/src/libs/eigen/blas/f2c/srotm.c new file mode 100644 index 0000000000..bd5944a99d --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/srotm.c @@ -0,0 +1,216 @@ +/* srotm.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int srotm_(integer *n, real *sx, integer *incx, real *sy, + integer *incy, real *sparam) +{ + /* Initialized data */ + + static real zero = 0.f; + static real two = 2.f; + + /* System generated locals */ + integer i__1, i__2; + + /* Local variables */ + integer i__; + real w, z__; + integer kx, ky; + real sh11, sh12, sh21, sh22, sflag; + integer nsteps; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* APPLY THE MODIFIED GIVENS TRANSFORMATION, H, TO THE 2 BY N MATRIX */ + +/* (SX**T) , WHERE **T INDICATES TRANSPOSE. THE ELEMENTS OF SX ARE IN */ +/* (DX**T) */ + +/* SX(LX+I*INCX), I = 0 TO N-1, WHERE LX = 1 IF INCX .GE. 0, ELSE */ +/* LX = (-INCX)*N, AND SIMILARLY FOR SY USING USING LY AND INCY. */ +/* WITH SPARAM(1)=SFLAG, H HAS ONE OF THE FOLLOWING FORMS.. */ + +/* SFLAG=-1.E0 SFLAG=0.E0 SFLAG=1.E0 SFLAG=-2.E0 */ + +/* (SH11 SH12) (1.E0 SH12) (SH11 1.E0) (1.E0 0.E0) */ +/* H=( ) ( ) ( ) ( ) */ +/* (SH21 SH22), (SH21 1.E0), (-1.E0 SH22), (0.E0 1.E0). */ +/* SEE SROTMG FOR A DESCRIPTION OF DATA STORAGE IN SPARAM. */ + + +/* Arguments */ +/* ========= */ + +/* N (input) INTEGER */ +/* number of elements in input vector(s) */ + +/* SX (input/output) REAL array, dimension N */ +/* double precision vector with N elements */ + +/* INCX (input) INTEGER */ +/* storage spacing between elements of SX */ + +/* SY (input/output) REAL array, dimension N */ +/* double precision vector with N elements */ + +/* INCY (input) INTEGER */ +/* storage spacing between elements of SY */ + +/* SPARAM (input/output) REAL array, dimension 5 */ +/* SPARAM(1)=SFLAG */ +/* SPARAM(2)=SH11 */ +/* SPARAM(3)=SH21 */ +/* SPARAM(4)=SH12 */ +/* SPARAM(5)=SH22 */ + +/* ===================================================================== */ + +/* .. Local Scalars .. */ +/* .. */ +/* .. Data statements .. */ + /* Parameter adjustments */ + --sparam; + --sy; + --sx; + + /* Function Body */ +/* .. */ + + sflag = sparam[1]; + if (*n <= 0 || sflag + two == zero) { + goto L140; + } + if (! (*incx == *incy && *incx > 0)) { + goto L70; + } + + nsteps = *n * *incx; + if (sflag < 0.f) { + goto L50; + } else if (sflag == 0) { + goto L10; + } else { + goto L30; + } +L10: + sh12 = sparam[4]; + sh21 = sparam[3]; + i__1 = nsteps; + i__2 = *incx; + for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) { + w = sx[i__]; + z__ = sy[i__]; + sx[i__] = w + z__ * sh12; + sy[i__] = w * sh21 + z__; +/* L20: */ + } + goto L140; +L30: + sh11 = sparam[2]; + sh22 = sparam[5]; + i__2 = nsteps; + i__1 = *incx; + for (i__ = 1; i__1 < 0 ? i__ >= i__2 : i__ <= i__2; i__ += i__1) { + w = sx[i__]; + z__ = sy[i__]; + sx[i__] = w * sh11 + z__; + sy[i__] = -w + sh22 * z__; +/* L40: */ + } + goto L140; +L50: + sh11 = sparam[2]; + sh12 = sparam[4]; + sh21 = sparam[3]; + sh22 = sparam[5]; + i__1 = nsteps; + i__2 = *incx; + for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) { + w = sx[i__]; + z__ = sy[i__]; + sx[i__] = w * sh11 + z__ * sh12; + sy[i__] = w * sh21 + z__ * sh22; +/* L60: */ + } + goto L140; +L70: + kx = 1; + ky = 1; + if (*incx < 0) { + kx = (1 - *n) * *incx + 1; + } + if (*incy < 0) { + ky = (1 - *n) * *incy + 1; + } + + if (sflag < 0.f) { + goto L120; + } else if (sflag == 0) { + goto L80; + } else { + goto L100; + } +L80: + sh12 = sparam[4]; + sh21 = sparam[3]; + i__2 = *n; + for (i__ = 1; i__ <= i__2; ++i__) { + w = sx[kx]; + z__ = sy[ky]; + sx[kx] = w + z__ * sh12; + sy[ky] = w * sh21 + z__; + kx += *incx; + ky += *incy; +/* L90: */ + } + goto L140; +L100: + sh11 = sparam[2]; + sh22 = sparam[5]; + i__2 = *n; + for (i__ = 1; i__ <= i__2; ++i__) { + w = sx[kx]; + z__ = sy[ky]; + sx[kx] = w * sh11 + z__; + sy[ky] = -w + sh22 * z__; + kx += *incx; + ky += *incy; +/* L110: */ + } + goto L140; +L120: + sh11 = sparam[2]; + sh12 = sparam[4]; + sh21 = sparam[3]; + sh22 = sparam[5]; + i__2 = *n; + for (i__ = 1; i__ <= i__2; ++i__) { + w = sx[kx]; + z__ = sy[ky]; + sx[kx] = w * sh11 + z__ * sh12; + sy[ky] = w * sh21 + z__ * sh22; + kx += *incx; + ky += *incy; +/* L130: */ + } +L140: + return 0; +} /* srotm_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/srotmg.c b/ground/gcs/src/libs/eigen/blas/f2c/srotmg.c new file mode 100644 index 0000000000..75f789fe28 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/srotmg.c @@ -0,0 +1,295 @@ +/* srotmg.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int srotmg_(real *sd1, real *sd2, real *sx1, real *sy1, real + *sparam) +{ + /* Initialized data */ + + static real zero = 0.f; + static real one = 1.f; + static real two = 2.f; + static real gam = 4096.f; + static real gamsq = 16777200.f; + static real rgamsq = 5.96046e-8f; + + /* Format strings */ + static char fmt_120[] = ""; + static char fmt_150[] = ""; + static char fmt_180[] = ""; + static char fmt_210[] = ""; + + /* System generated locals */ + real r__1; + + /* Local variables */ + real su, sp1, sp2, sq1, sq2, sh11, sh12, sh21, sh22; + integer igo; + real sflag, stemp; + + /* Assigned format variables */ + static char *igo_fmt; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* CONSTRUCT THE MODIFIED GIVENS TRANSFORMATION MATRIX H WHICH ZEROS */ +/* THE SECOND COMPONENT OF THE 2-VECTOR (SQRT(SD1)*SX1,SQRT(SD2)* */ +/* SY2)**T. */ +/* WITH SPARAM(1)=SFLAG, H HAS ONE OF THE FOLLOWING FORMS.. */ + +/* SFLAG=-1.E0 SFLAG=0.E0 SFLAG=1.E0 SFLAG=-2.E0 */ + +/* (SH11 SH12) (1.E0 SH12) (SH11 1.E0) (1.E0 0.E0) */ +/* H=( ) ( ) ( ) ( ) */ +/* (SH21 SH22), (SH21 1.E0), (-1.E0 SH22), (0.E0 1.E0). */ +/* LOCATIONS 2-4 OF SPARAM CONTAIN SH11,SH21,SH12, AND SH22 */ +/* RESPECTIVELY. (VALUES OF 1.E0, -1.E0, OR 0.E0 IMPLIED BY THE */ +/* VALUE OF SPARAM(1) ARE NOT STORED IN SPARAM.) */ + +/* THE VALUES OF GAMSQ AND RGAMSQ SET IN THE DATA STATEMENT MAY BE */ +/* INEXACT. THIS IS OK AS THEY ARE ONLY USED FOR TESTING THE SIZE */ +/* OF SD1 AND SD2. ALL ACTUAL SCALING OF DATA IS DONE USING GAM. */ + + +/* Arguments */ +/* ========= */ + + +/* SD1 (input/output) REAL */ + +/* SD2 (input/output) REAL */ + +/* SX1 (input/output) REAL */ + +/* SY1 (input) REAL */ + + +/* SPARAM (input/output) REAL array, dimension 5 */ +/* SPARAM(1)=SFLAG */ +/* SPARAM(2)=SH11 */ +/* SPARAM(3)=SH21 */ +/* SPARAM(4)=SH12 */ +/* SPARAM(5)=SH22 */ + +/* ===================================================================== */ + +/* .. Local Scalars .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ +/* .. Data statements .. */ + + /* Parameter adjustments */ + --sparam; + + /* Function Body */ +/* .. */ + if (! (*sd1 < zero)) { + goto L10; + } +/* GO ZERO-H-D-AND-SX1.. */ + goto L60; +L10: +/* CASE-SD1-NONNEGATIVE */ + sp2 = *sd2 * *sy1; + if (! (sp2 == zero)) { + goto L20; + } + sflag = -two; + goto L260; +/* REGULAR-CASE.. */ +L20: + sp1 = *sd1 * *sx1; + sq2 = sp2 * *sy1; + sq1 = sp1 * *sx1; + + if (! (dabs(sq1) > dabs(sq2))) { + goto L40; + } + sh21 = -(*sy1) / *sx1; + sh12 = sp2 / sp1; + + su = one - sh12 * sh21; + + if (! (su <= zero)) { + goto L30; + } +/* GO ZERO-H-D-AND-SX1.. */ + goto L60; +L30: + sflag = zero; + *sd1 /= su; + *sd2 /= su; + *sx1 *= su; +/* GO SCALE-CHECK.. */ + goto L100; +L40: + if (! (sq2 < zero)) { + goto L50; + } +/* GO ZERO-H-D-AND-SX1.. */ + goto L60; +L50: + sflag = one; + sh11 = sp1 / sp2; + sh22 = *sx1 / *sy1; + su = one + sh11 * sh22; + stemp = *sd2 / su; + *sd2 = *sd1 / su; + *sd1 = stemp; + *sx1 = *sy1 * su; +/* GO SCALE-CHECK */ + goto L100; +/* PROCEDURE..ZERO-H-D-AND-SX1.. */ +L60: + sflag = -one; + sh11 = zero; + sh12 = zero; + sh21 = zero; + sh22 = zero; + + *sd1 = zero; + *sd2 = zero; + *sx1 = zero; +/* RETURN.. */ + goto L220; +/* PROCEDURE..FIX-H.. */ +L70: + if (! (sflag >= zero)) { + goto L90; + } + + if (! (sflag == zero)) { + goto L80; + } + sh11 = one; + sh22 = one; + sflag = -one; + goto L90; +L80: + sh21 = -one; + sh12 = one; + sflag = -one; +L90: + switch (igo) { + case 0: goto L120; + case 1: goto L150; + case 2: goto L180; + case 3: goto L210; + } +/* PROCEDURE..SCALE-CHECK */ +L100: +L110: + if (! (*sd1 <= rgamsq)) { + goto L130; + } + if (*sd1 == zero) { + goto L160; + } + igo = 0; + igo_fmt = fmt_120; +/* FIX-H.. */ + goto L70; +L120: +/* Computing 2nd power */ + r__1 = gam; + *sd1 *= r__1 * r__1; + *sx1 /= gam; + sh11 /= gam; + sh12 /= gam; + goto L110; +L130: +L140: + if (! (*sd1 >= gamsq)) { + goto L160; + } + igo = 1; + igo_fmt = fmt_150; +/* FIX-H.. */ + goto L70; +L150: +/* Computing 2nd power */ + r__1 = gam; + *sd1 /= r__1 * r__1; + *sx1 *= gam; + sh11 *= gam; + sh12 *= gam; + goto L140; +L160: +L170: + if (! (dabs(*sd2) <= rgamsq)) { + goto L190; + } + if (*sd2 == zero) { + goto L220; + } + igo = 2; + igo_fmt = fmt_180; +/* FIX-H.. */ + goto L70; +L180: +/* Computing 2nd power */ + r__1 = gam; + *sd2 *= r__1 * r__1; + sh21 /= gam; + sh22 /= gam; + goto L170; +L190: +L200: + if (! (dabs(*sd2) >= gamsq)) { + goto L220; + } + igo = 3; + igo_fmt = fmt_210; +/* FIX-H.. */ + goto L70; +L210: +/* Computing 2nd power */ + r__1 = gam; + *sd2 /= r__1 * r__1; + sh21 *= gam; + sh22 *= gam; + goto L200; +L220: + if (sflag < 0.f) { + goto L250; + } else if (sflag == 0) { + goto L230; + } else { + goto L240; + } +L230: + sparam[3] = sh21; + sparam[4] = sh12; + goto L260; +L240: + sparam[2] = sh11; + sparam[5] = sh22; + goto L260; +L250: + sparam[2] = sh11; + sparam[3] = sh21; + sparam[4] = sh12; + sparam[5] = sh22; +L260: + sparam[1] = sflag; + return 0; +} /* srotmg_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/ssbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/ssbmv.c new file mode 100644 index 0000000000..8599325f27 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/ssbmv.c @@ -0,0 +1,368 @@ +/* ssbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int ssbmv_(char *uplo, integer *n, integer *k, real *alpha, + real *a, integer *lda, real *x, integer *incx, real *beta, real *y, + integer *incy, ftnlen uplo_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4; + + /* Local variables */ + integer i__, j, l, ix, iy, jx, jy, kx, ky, info; + real temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* SSBMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n symmetric band matrix, with k super-diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the band matrix A is being supplied as */ +/* follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* being supplied. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* being supplied. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry, K specifies the number of super-diagonals of the */ +/* matrix A. K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* ALPHA - REAL . */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* A - REAL array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the symmetric matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer the upper */ +/* triangular part of a symmetric band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the symmetric matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer the lower */ +/* triangular part of a symmetric band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - REAL array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the */ +/* vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - REAL . */ +/* On entry, BETA specifies the scalar beta. */ +/* Unchanged on exit. */ + +/* Y - REAL array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the */ +/* vector y. On exit, Y is overwritten by the updated vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + --y; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*k < 0) { + info = 3; + } else if (*lda < *k + 1) { + info = 6; + } else if (*incx == 0) { + info = 8; + } else if (*incy == 0) { + info = 11; + } + if (info != 0) { + xerbla_("SSBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (*alpha == 0.f && *beta == 1.f)) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array A */ +/* are accessed sequentially with one pass through A. */ + +/* First form y := beta*y. */ + + if (*beta != 1.f) { + if (*incy == 1) { + if (*beta == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = 0.f; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = *beta * y[i__]; +/* L20: */ + } + } + } else { + iy = ky; + if (*beta == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = 0.f; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = *beta * y[iy]; + iy += *incy; +/* L40: */ + } + } + } + } + if (*alpha == 0.f) { + return 0; + } + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when upper triangle of A is stored. */ + + kplus1 = *k + 1; + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.f; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + y[i__] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[i__]; +/* L50: */ + } + y[j] = y[j] + temp1 * a[kplus1 + j * a_dim1] + *alpha * temp2; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.f; + ix = kx; + iy = ky; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + y[iy] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[ix]; + ix += *incx; + iy += *incy; +/* L70: */ + } + y[jy] = y[jy] + temp1 * a[kplus1 + j * a_dim1] + *alpha * + temp2; + jx += *incx; + jy += *incy; + if (j > *k) { + kx += *incx; + ky += *incy; + } +/* L80: */ + } + } + } else { + +/* Form y when lower triangle of A is stored. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.f; + y[j] += temp1 * a[j * a_dim1 + 1]; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + y[i__] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[i__]; +/* L90: */ + } + y[j] += *alpha * temp2; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.f; + y[jy] += temp1 * a[j * a_dim1 + 1]; + l = 1 - j; + ix = jx; + iy = jy; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + ix += *incx; + iy += *incy; + y[iy] += temp1 * a[l + i__ + j * a_dim1]; + temp2 += a[l + i__ + j * a_dim1] * x[ix]; +/* L110: */ + } + y[jy] += *alpha * temp2; + jx += *incx; + jy += *incy; +/* L120: */ + } + } + } + + return 0; + +/* End of SSBMV . */ + +} /* ssbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/sspmv.c b/ground/gcs/src/libs/eigen/blas/f2c/sspmv.c new file mode 100644 index 0000000000..47858ec6c1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/sspmv.c @@ -0,0 +1,316 @@ +/* sspmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int sspmv_(char *uplo, integer *n, real *alpha, real *ap, + real *x, integer *incx, real *beta, real *y, integer *incy, ftnlen + uplo_len) +{ + /* System generated locals */ + integer i__1, i__2; + + /* Local variables */ + integer i__, j, k, kk, ix, iy, jx, jy, kx, ky, info; + real temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* SSPMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n symmetric matrix, supplied in packed form. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the matrix A is supplied in the packed */ +/* array AP as follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* supplied in AP. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* supplied in AP. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* ALPHA - REAL . */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* AP - REAL array of DIMENSION at least */ +/* ( ( n*( n + 1 ) )/2 ). */ +/* Before entry with UPLO = 'U' or 'u', the array AP must */ +/* contain the upper triangular part of the symmetric matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) */ +/* and a( 2, 2 ) respectively, and so on. */ +/* Before entry with UPLO = 'L' or 'l', the array AP must */ +/* contain the lower triangular part of the symmetric matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) */ +/* and a( 3, 1 ) respectively, and so on. */ +/* Unchanged on exit. */ + +/* X - REAL array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - REAL . */ +/* On entry, BETA specifies the scalar beta. When BETA is */ +/* supplied as zero then Y need not be set on input. */ +/* Unchanged on exit. */ + +/* Y - REAL array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the n */ +/* element vector y. On exit, Y is overwritten by the updated */ +/* vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + --y; + --x; + --ap; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*incx == 0) { + info = 6; + } else if (*incy == 0) { + info = 9; + } + if (info != 0) { + xerbla_("SSPMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (*alpha == 0.f && *beta == 1.f)) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array AP */ +/* are accessed sequentially with one pass through AP. */ + +/* First form y := beta*y. */ + + if (*beta != 1.f) { + if (*incy == 1) { + if (*beta == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = 0.f; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[i__] = *beta * y[i__]; +/* L20: */ + } + } + } else { + iy = ky; + if (*beta == 0.f) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = 0.f; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + y[iy] = *beta * y[iy]; + iy += *incy; +/* L40: */ + } + } + } + } + if (*alpha == 0.f) { + return 0; + } + kk = 1; + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when AP contains the upper triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.f; + k = kk; + i__2 = j - 1; + for (i__ = 1; i__ <= i__2; ++i__) { + y[i__] += temp1 * ap[k]; + temp2 += ap[k] * x[i__]; + ++k; +/* L50: */ + } + y[j] = y[j] + temp1 * ap[kk + j - 1] + *alpha * temp2; + kk += j; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.f; + ix = kx; + iy = ky; + i__2 = kk + j - 2; + for (k = kk; k <= i__2; ++k) { + y[iy] += temp1 * ap[k]; + temp2 += ap[k] * x[ix]; + ix += *incx; + iy += *incy; +/* L70: */ + } + y[jy] = y[jy] + temp1 * ap[kk + j - 1] + *alpha * temp2; + jx += *incx; + jy += *incy; + kk += j; +/* L80: */ + } + } + } else { + +/* Form y when AP contains the lower triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[j]; + temp2 = 0.f; + y[j] += temp1 * ap[kk]; + k = kk + 1; + i__2 = *n; + for (i__ = j + 1; i__ <= i__2; ++i__) { + y[i__] += temp1 * ap[k]; + temp2 += ap[k] * x[i__]; + ++k; +/* L90: */ + } + y[j] += *alpha * temp2; + kk += *n - j + 1; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp1 = *alpha * x[jx]; + temp2 = 0.f; + y[jy] += temp1 * ap[kk]; + ix = jx; + iy = jy; + i__2 = kk + *n - j; + for (k = kk + 1; k <= i__2; ++k) { + ix += *incx; + iy += *incy; + y[iy] += temp1 * ap[k]; + temp2 += ap[k] * x[ix]; +/* L110: */ + } + y[jy] += *alpha * temp2; + jx += *incx; + jy += *incy; + kk += *n - j + 1; +/* L120: */ + } + } + } + + return 0; + +/* End of SSPMV . */ + +} /* sspmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/stbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/stbmv.c new file mode 100644 index 0000000000..fcf9ce336f --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/stbmv.c @@ -0,0 +1,428 @@ +/* stbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int stbmv_(char *uplo, char *trans, char *diag, integer *n, + integer *k, real *a, integer *lda, real *x, integer *incx, ftnlen + uplo_len, ftnlen trans_len, ftnlen diag_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4; + + /* Local variables */ + integer i__, j, l, ix, jx, kx, info; + real temp; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + logical nounit; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* STBMV performs one of the matrix-vector operations */ + +/* x := A*x, or x := A'*x, */ + +/* where x is an n element vector and A is an n by n unit, or non-unit, */ +/* upper or lower triangular band matrix, with ( k + 1 ) diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the matrix is an upper or */ +/* lower triangular matrix as follows: */ + +/* UPLO = 'U' or 'u' A is an upper triangular matrix. */ + +/* UPLO = 'L' or 'l' A is a lower triangular matrix. */ + +/* Unchanged on exit. */ + +/* TRANS - CHARACTER*1. */ +/* On entry, TRANS specifies the operation to be performed as */ +/* follows: */ + +/* TRANS = 'N' or 'n' x := A*x. */ + +/* TRANS = 'T' or 't' x := A'*x. */ + +/* TRANS = 'C' or 'c' x := A'*x. */ + +/* Unchanged on exit. */ + +/* DIAG - CHARACTER*1. */ +/* On entry, DIAG specifies whether or not A is unit */ +/* triangular as follows: */ + +/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */ + +/* DIAG = 'N' or 'n' A is not assumed to be unit */ +/* triangular. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry with UPLO = 'U' or 'u', K specifies the number of */ +/* super-diagonals of the matrix A. */ +/* On entry with UPLO = 'L' or 'l', K specifies the number of */ +/* sub-diagonals of the matrix A. */ +/* K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* A - REAL array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer an upper */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer a lower */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Note that when DIAG = 'U' or 'u' the elements of the array A */ +/* corresponding to the diagonal elements of the matrix are not */ +/* referenced, but are assumed to be unity. */ +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - REAL array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. On exit, X is overwritten with the */ +/* tranformed vector x. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (! lsame_(trans, "N", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, + "T", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, "C", (ftnlen)1, ( + ftnlen)1)) { + info = 2; + } else if (! lsame_(diag, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(diag, + "N", (ftnlen)1, (ftnlen)1)) { + info = 3; + } else if (*n < 0) { + info = 4; + } else if (*k < 0) { + info = 5; + } else if (*lda < *k + 1) { + info = 7; + } else if (*incx == 0) { + info = 9; + } + if (info != 0) { + xerbla_("STBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0) { + return 0; + } + + nounit = lsame_(diag, "N", (ftnlen)1, (ftnlen)1); + +/* Set up the start point in X if the increment is not unity. This */ +/* will be ( N - 1 )*INCX too small for descending loops. */ + + if (*incx <= 0) { + kx = 1 - (*n - 1) * *incx; + } else if (*incx != 1) { + kx = 1; + } + +/* Start the operations. In this version the elements of A are */ +/* accessed sequentially with one pass through A. */ + + if (lsame_(trans, "N", (ftnlen)1, (ftnlen)1)) { + +/* Form x := A*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (x[j] != 0.f) { + temp = x[j]; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + x[i__] += temp * a[l + i__ + j * a_dim1]; +/* L10: */ + } + if (nounit) { + x[j] *= a[kplus1 + j * a_dim1]; + } + } +/* L20: */ + } + } else { + jx = kx; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (x[jx] != 0.f) { + temp = x[jx]; + ix = kx; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + x[ix] += temp * a[l + i__ + j * a_dim1]; + ix += *incx; +/* L30: */ + } + if (nounit) { + x[jx] *= a[kplus1 + j * a_dim1]; + } + } + jx += *incx; + if (j > *k) { + kx += *incx; + } +/* L40: */ + } + } + } else { + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + if (x[j] != 0.f) { + temp = x[j]; + l = 1 - j; +/* Computing MIN */ + i__1 = *n, i__3 = j + *k; + i__4 = j + 1; + for (i__ = min(i__1,i__3); i__ >= i__4; --i__) { + x[i__] += temp * a[l + i__ + j * a_dim1]; +/* L50: */ + } + if (nounit) { + x[j] *= a[j * a_dim1 + 1]; + } + } +/* L60: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + if (x[jx] != 0.f) { + temp = x[jx]; + ix = kx; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__1 = j + *k; + i__3 = j + 1; + for (i__ = min(i__4,i__1); i__ >= i__3; --i__) { + x[ix] += temp * a[l + i__ + j * a_dim1]; + ix -= *incx; +/* L70: */ + } + if (nounit) { + x[jx] *= a[j * a_dim1 + 1]; + } + } + jx -= *incx; + if (*n - j >= *k) { + kx -= *incx; + } +/* L80: */ + } + } + } + } else { + +/* Form x := A'*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + temp = x[j]; + l = kplus1 - j; + if (nounit) { + temp *= a[kplus1 + j * a_dim1]; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + temp += a[l + i__ + j * a_dim1] * x[i__]; +/* L90: */ + } + x[j] = temp; +/* L100: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + temp = x[jx]; + kx -= *incx; + ix = kx; + l = kplus1 - j; + if (nounit) { + temp *= a[kplus1 + j * a_dim1]; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + temp += a[l + i__ + j * a_dim1] * x[ix]; + ix -= *incx; +/* L110: */ + } + x[jx] = temp; + jx -= *incx; +/* L120: */ + } + } + } else { + if (*incx == 1) { + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + temp = x[j]; + l = 1 - j; + if (nounit) { + temp *= a[j * a_dim1 + 1]; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + temp += a[l + i__ + j * a_dim1] * x[i__]; +/* L130: */ + } + x[j] = temp; +/* L140: */ + } + } else { + jx = kx; + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + temp = x[jx]; + kx += *incx; + ix = kx; + l = 1 - j; + if (nounit) { + temp *= a[j * a_dim1 + 1]; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + temp += a[l + i__ + j * a_dim1] * x[ix]; + ix += *incx; +/* L150: */ + } + x[jx] = temp; + jx += *incx; +/* L160: */ + } + } + } + } + + return 0; + +/* End of STBMV . */ + +} /* stbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/zhbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/zhbmv.c new file mode 100644 index 0000000000..42da13dbba --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/zhbmv.c @@ -0,0 +1,488 @@ +/* zhbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int zhbmv_(char *uplo, integer *n, integer *k, doublecomplex + *alpha, doublecomplex *a, integer *lda, doublecomplex *x, integer * + incx, doublecomplex *beta, doublecomplex *y, integer *incy, ftnlen + uplo_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4, i__5; + doublereal d__1; + doublecomplex z__1, z__2, z__3, z__4; + + /* Builtin functions */ + void d_cnjg(doublecomplex *, doublecomplex *); + + /* Local variables */ + integer i__, j, l, ix, iy, jx, jy, kx, ky, info; + doublecomplex temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* ZHBMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n hermitian band matrix, with k super-diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the band matrix A is being supplied as */ +/* follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* being supplied. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* being supplied. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry, K specifies the number of super-diagonals of the */ +/* matrix A. K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* ALPHA - COMPLEX*16 . */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* A - COMPLEX*16 array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the hermitian matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer the upper */ +/* triangular part of a hermitian band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the hermitian matrix, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer the lower */ +/* triangular part of a hermitian band matrix from conventional */ +/* full matrix storage to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Note that the imaginary parts of the diagonal elements need */ +/* not be set and are assumed to be zero. */ +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - COMPLEX*16 array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the */ +/* vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - COMPLEX*16 . */ +/* On entry, BETA specifies the scalar beta. */ +/* Unchanged on exit. */ + +/* Y - COMPLEX*16 array of DIMENSION at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the */ +/* vector y. On exit, Y is overwritten by the updated vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + --y; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*k < 0) { + info = 3; + } else if (*lda < *k + 1) { + info = 6; + } else if (*incx == 0) { + info = 8; + } else if (*incy == 0) { + info = 11; + } + if (info != 0) { + xerbla_("ZHBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (alpha->r == 0. && alpha->i == 0. && (beta->r == 1. && + beta->i == 0.))) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array A */ +/* are accessed sequentially with one pass through A. */ + +/* First form y := beta*y. */ + + if (beta->r != 1. || beta->i != 0.) { + if (*incy == 1) { + if (beta->r == 0. && beta->i == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + y[i__2].r = 0., y[i__2].i = 0.; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + i__3 = i__; + z__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + z__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; +/* L20: */ + } + } + } else { + iy = ky; + if (beta->r == 0. && beta->i == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + y[i__2].r = 0., y[i__2].i = 0.; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + i__3 = iy; + z__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + z__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + iy += *incy; +/* L40: */ + } + } + } + } + if (alpha->r == 0. && alpha->i == 0.) { + return 0; + } + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when upper triangle of A is stored. */ + + kplus1 = *k + 1; + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + z__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, z__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + i__2 = i__; + i__3 = i__; + i__5 = l + i__ + j * a_dim1; + z__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + z__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + z__1.r = y[i__3].r + z__2.r, z__1.i = y[i__3].i + z__2.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__2 = i__; + z__2.r = z__3.r * x[i__2].r - z__3.i * x[i__2].i, z__2.i = + z__3.r * x[i__2].i + z__3.i * x[i__2].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; +/* L50: */ + } + i__4 = j; + i__2 = j; + i__3 = kplus1 + j * a_dim1; + d__1 = a[i__3].r; + z__3.r = d__1 * temp1.r, z__3.i = d__1 * temp1.i; + z__2.r = y[i__2].r + z__3.r, z__2.i = y[i__2].i + z__3.i; + z__4.r = alpha->r * temp2.r - alpha->i * temp2.i, z__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = z__2.r + z__4.r, z__1.i = z__2.i + z__4.i; + y[i__4].r = z__1.r, y[i__4].i = z__1.i; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__4 = jx; + z__1.r = alpha->r * x[i__4].r - alpha->i * x[i__4].i, z__1.i = + alpha->r * x[i__4].i + alpha->i * x[i__4].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + ix = kx; + iy = ky; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + i__4 = iy; + i__2 = iy; + i__5 = l + i__ + j * a_dim1; + z__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + z__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + z__1.r = y[i__2].r + z__2.r, z__1.i = y[i__2].i + z__2.i; + y[i__4].r = z__1.r, y[i__4].i = z__1.i; + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__4 = ix; + z__2.r = z__3.r * x[i__4].r - z__3.i * x[i__4].i, z__2.i = + z__3.r * x[i__4].i + z__3.i * x[i__4].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; + ix += *incx; + iy += *incy; +/* L70: */ + } + i__3 = jy; + i__4 = jy; + i__2 = kplus1 + j * a_dim1; + d__1 = a[i__2].r; + z__3.r = d__1 * temp1.r, z__3.i = d__1 * temp1.i; + z__2.r = y[i__4].r + z__3.r, z__2.i = y[i__4].i + z__3.i; + z__4.r = alpha->r * temp2.r - alpha->i * temp2.i, z__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = z__2.r + z__4.r, z__1.i = z__2.i + z__4.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + jx += *incx; + jy += *incy; + if (j > *k) { + kx += *incx; + ky += *incy; + } +/* L80: */ + } + } + } else { + +/* Form y when lower triangle of A is stored. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__3 = j; + z__1.r = alpha->r * x[i__3].r - alpha->i * x[i__3].i, z__1.i = + alpha->r * x[i__3].i + alpha->i * x[i__3].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + i__3 = j; + i__4 = j; + i__2 = j * a_dim1 + 1; + d__1 = a[i__2].r; + z__2.r = d__1 * temp1.r, z__2.i = d__1 * temp1.i; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + i__4 = i__; + i__2 = i__; + i__5 = l + i__ + j * a_dim1; + z__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + z__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + z__1.r = y[i__2].r + z__2.r, z__1.i = y[i__2].i + z__2.i; + y[i__4].r = z__1.r, y[i__4].i = z__1.i; + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__4 = i__; + z__2.r = z__3.r * x[i__4].r - z__3.i * x[i__4].i, z__2.i = + z__3.r * x[i__4].i + z__3.i * x[i__4].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; +/* L90: */ + } + i__3 = j; + i__4 = j; + z__2.r = alpha->r * temp2.r - alpha->i * temp2.i, z__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__3 = jx; + z__1.r = alpha->r * x[i__3].r - alpha->i * x[i__3].i, z__1.i = + alpha->r * x[i__3].i + alpha->i * x[i__3].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + i__3 = jy; + i__4 = jy; + i__2 = j * a_dim1 + 1; + d__1 = a[i__2].r; + z__2.r = d__1 * temp1.r, z__2.i = d__1 * temp1.i; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + l = 1 - j; + ix = jx; + iy = jy; +/* Computing MIN */ + i__4 = *n, i__2 = j + *k; + i__3 = min(i__4,i__2); + for (i__ = j + 1; i__ <= i__3; ++i__) { + ix += *incx; + iy += *incy; + i__4 = iy; + i__2 = iy; + i__5 = l + i__ + j * a_dim1; + z__2.r = temp1.r * a[i__5].r - temp1.i * a[i__5].i, + z__2.i = temp1.r * a[i__5].i + temp1.i * a[i__5] + .r; + z__1.r = y[i__2].r + z__2.r, z__1.i = y[i__2].i + z__2.i; + y[i__4].r = z__1.r, y[i__4].i = z__1.i; + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__4 = ix; + z__2.r = z__3.r * x[i__4].r - z__3.i * x[i__4].i, z__2.i = + z__3.r * x[i__4].i + z__3.i * x[i__4].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; +/* L110: */ + } + i__3 = jy; + i__4 = jy; + z__2.r = alpha->r * temp2.r - alpha->i * temp2.i, z__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + jx += *incx; + jy += *incy; +/* L120: */ + } + } + } + + return 0; + +/* End of ZHBMV . */ + +} /* zhbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/zhpmv.c b/ground/gcs/src/libs/eigen/blas/f2c/zhpmv.c new file mode 100644 index 0000000000..fbe2f42b3d --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/zhpmv.c @@ -0,0 +1,438 @@ +/* zhpmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int zhpmv_(char *uplo, integer *n, doublecomplex *alpha, + doublecomplex *ap, doublecomplex *x, integer *incx, doublecomplex * + beta, doublecomplex *y, integer *incy, ftnlen uplo_len) +{ + /* System generated locals */ + integer i__1, i__2, i__3, i__4, i__5; + doublereal d__1; + doublecomplex z__1, z__2, z__3, z__4; + + /* Builtin functions */ + void d_cnjg(doublecomplex *, doublecomplex *); + + /* Local variables */ + integer i__, j, k, kk, ix, iy, jx, jy, kx, ky, info; + doublecomplex temp1, temp2; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* ZHPMV performs the matrix-vector operation */ + +/* y := alpha*A*x + beta*y, */ + +/* where alpha and beta are scalars, x and y are n element vectors and */ +/* A is an n by n hermitian matrix, supplied in packed form. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the upper or lower */ +/* triangular part of the matrix A is supplied in the packed */ +/* array AP as follows: */ + +/* UPLO = 'U' or 'u' The upper triangular part of A is */ +/* supplied in AP. */ + +/* UPLO = 'L' or 'l' The lower triangular part of A is */ +/* supplied in AP. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* ALPHA - COMPLEX*16 . */ +/* On entry, ALPHA specifies the scalar alpha. */ +/* Unchanged on exit. */ + +/* AP - COMPLEX*16 array of DIMENSION at least */ +/* ( ( n*( n + 1 ) )/2 ). */ +/* Before entry with UPLO = 'U' or 'u', the array AP must */ +/* contain the upper triangular part of the hermitian matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) */ +/* and a( 2, 2 ) respectively, and so on. */ +/* Before entry with UPLO = 'L' or 'l', the array AP must */ +/* contain the lower triangular part of the hermitian matrix */ +/* packed sequentially, column by column, so that AP( 1 ) */ +/* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) */ +/* and a( 3, 1 ) respectively, and so on. */ +/* Note that the imaginary parts of the diagonal elements need */ +/* not be set and are assumed to be zero. */ +/* Unchanged on exit. */ + +/* X - COMPLEX*16 array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. */ +/* Unchanged on exit. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* BETA - COMPLEX*16 . */ +/* On entry, BETA specifies the scalar beta. When BETA is */ +/* supplied as zero then Y need not be set on input. */ +/* Unchanged on exit. */ + +/* Y - COMPLEX*16 array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCY ) ). */ +/* Before entry, the incremented array Y must contain the n */ +/* element vector y. On exit, Y is overwritten by the updated */ +/* vector y. */ + +/* INCY - INTEGER. */ +/* On entry, INCY specifies the increment for the elements of */ +/* Y. INCY must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + --y; + --x; + --ap; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (*n < 0) { + info = 2; + } else if (*incx == 0) { + info = 6; + } else if (*incy == 0) { + info = 9; + } + if (info != 0) { + xerbla_("ZHPMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0 || (alpha->r == 0. && alpha->i == 0. && (beta->r == 1. && + beta->i == 0.))) { + return 0; + } + +/* Set up the start points in X and Y. */ + + if (*incx > 0) { + kx = 1; + } else { + kx = 1 - (*n - 1) * *incx; + } + if (*incy > 0) { + ky = 1; + } else { + ky = 1 - (*n - 1) * *incy; + } + +/* Start the operations. In this version the elements of the array AP */ +/* are accessed sequentially with one pass through AP. */ + +/* First form y := beta*y. */ + + if (beta->r != 1. || beta->i != 0.) { + if (*incy == 1) { + if (beta->r == 0. && beta->i == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + y[i__2].r = 0., y[i__2].i = 0.; +/* L10: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = i__; + i__3 = i__; + z__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + z__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; +/* L20: */ + } + } + } else { + iy = ky; + if (beta->r == 0. && beta->i == 0.) { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + y[i__2].r = 0., y[i__2].i = 0.; + iy += *incy; +/* L30: */ + } + } else { + i__1 = *n; + for (i__ = 1; i__ <= i__1; ++i__) { + i__2 = iy; + i__3 = iy; + z__1.r = beta->r * y[i__3].r - beta->i * y[i__3].i, + z__1.i = beta->r * y[i__3].i + beta->i * y[i__3] + .r; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + iy += *incy; +/* L40: */ + } + } + } + } + if (alpha->r == 0. && alpha->i == 0.) { + return 0; + } + kk = 1; + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + +/* Form y when AP contains the upper triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + z__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, z__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + k = kk; + i__2 = j - 1; + for (i__ = 1; i__ <= i__2; ++i__) { + i__3 = i__; + i__4 = i__; + i__5 = k; + z__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + z__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + d_cnjg(&z__3, &ap[k]); + i__3 = i__; + z__2.r = z__3.r * x[i__3].r - z__3.i * x[i__3].i, z__2.i = + z__3.r * x[i__3].i + z__3.i * x[i__3].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; + ++k; +/* L50: */ + } + i__2 = j; + i__3 = j; + i__4 = kk + j - 1; + d__1 = ap[i__4].r; + z__3.r = d__1 * temp1.r, z__3.i = d__1 * temp1.i; + z__2.r = y[i__3].r + z__3.r, z__2.i = y[i__3].i + z__3.i; + z__4.r = alpha->r * temp2.r - alpha->i * temp2.i, z__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = z__2.r + z__4.r, z__1.i = z__2.i + z__4.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + kk += j; +/* L60: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = jx; + z__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, z__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + ix = kx; + iy = ky; + i__2 = kk + j - 2; + for (k = kk; k <= i__2; ++k) { + i__3 = iy; + i__4 = iy; + i__5 = k; + z__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + z__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + d_cnjg(&z__3, &ap[k]); + i__3 = ix; + z__2.r = z__3.r * x[i__3].r - z__3.i * x[i__3].i, z__2.i = + z__3.r * x[i__3].i + z__3.i * x[i__3].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; + ix += *incx; + iy += *incy; +/* L70: */ + } + i__2 = jy; + i__3 = jy; + i__4 = kk + j - 1; + d__1 = ap[i__4].r; + z__3.r = d__1 * temp1.r, z__3.i = d__1 * temp1.i; + z__2.r = y[i__3].r + z__3.r, z__2.i = y[i__3].i + z__3.i; + z__4.r = alpha->r * temp2.r - alpha->i * temp2.i, z__4.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = z__2.r + z__4.r, z__1.i = z__2.i + z__4.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + jx += *incx; + jy += *incy; + kk += j; +/* L80: */ + } + } + } else { + +/* Form y when AP contains the lower triangle. */ + + if (*incx == 1 && *incy == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + z__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, z__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + i__2 = j; + i__3 = j; + i__4 = kk; + d__1 = ap[i__4].r; + z__2.r = d__1 * temp1.r, z__2.i = d__1 * temp1.i; + z__1.r = y[i__3].r + z__2.r, z__1.i = y[i__3].i + z__2.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + k = kk + 1; + i__2 = *n; + for (i__ = j + 1; i__ <= i__2; ++i__) { + i__3 = i__; + i__4 = i__; + i__5 = k; + z__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + z__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + d_cnjg(&z__3, &ap[k]); + i__3 = i__; + z__2.r = z__3.r * x[i__3].r - z__3.i * x[i__3].i, z__2.i = + z__3.r * x[i__3].i + z__3.i * x[i__3].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; + ++k; +/* L90: */ + } + i__2 = j; + i__3 = j; + z__2.r = alpha->r * temp2.r - alpha->i * temp2.i, z__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = y[i__3].r + z__2.r, z__1.i = y[i__3].i + z__2.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + kk += *n - j + 1; +/* L100: */ + } + } else { + jx = kx; + jy = ky; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = jx; + z__1.r = alpha->r * x[i__2].r - alpha->i * x[i__2].i, z__1.i = + alpha->r * x[i__2].i + alpha->i * x[i__2].r; + temp1.r = z__1.r, temp1.i = z__1.i; + temp2.r = 0., temp2.i = 0.; + i__2 = jy; + i__3 = jy; + i__4 = kk; + d__1 = ap[i__4].r; + z__2.r = d__1 * temp1.r, z__2.i = d__1 * temp1.i; + z__1.r = y[i__3].r + z__2.r, z__1.i = y[i__3].i + z__2.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + ix = jx; + iy = jy; + i__2 = kk + *n - j; + for (k = kk + 1; k <= i__2; ++k) { + ix += *incx; + iy += *incy; + i__3 = iy; + i__4 = iy; + i__5 = k; + z__2.r = temp1.r * ap[i__5].r - temp1.i * ap[i__5].i, + z__2.i = temp1.r * ap[i__5].i + temp1.i * ap[i__5] + .r; + z__1.r = y[i__4].r + z__2.r, z__1.i = y[i__4].i + z__2.i; + y[i__3].r = z__1.r, y[i__3].i = z__1.i; + d_cnjg(&z__3, &ap[k]); + i__3 = ix; + z__2.r = z__3.r * x[i__3].r - z__3.i * x[i__3].i, z__2.i = + z__3.r * x[i__3].i + z__3.i * x[i__3].r; + z__1.r = temp2.r + z__2.r, z__1.i = temp2.i + z__2.i; + temp2.r = z__1.r, temp2.i = z__1.i; +/* L110: */ + } + i__2 = jy; + i__3 = jy; + z__2.r = alpha->r * temp2.r - alpha->i * temp2.i, z__2.i = + alpha->r * temp2.i + alpha->i * temp2.r; + z__1.r = y[i__3].r + z__2.r, z__1.i = y[i__3].i + z__2.i; + y[i__2].r = z__1.r, y[i__2].i = z__1.i; + jx += *incx; + jy += *incy; + kk += *n - j + 1; +/* L120: */ + } + } + } + + return 0; + +/* End of ZHPMV . */ + +} /* zhpmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/f2c/ztbmv.c b/ground/gcs/src/libs/eigen/blas/f2c/ztbmv.c new file mode 100644 index 0000000000..4cdcd7f889 --- /dev/null +++ b/ground/gcs/src/libs/eigen/blas/f2c/ztbmv.c @@ -0,0 +1,647 @@ +/* ztbmv.f -- translated by f2c (version 20100827). + You must link the resulting object file with libf2c: + on Microsoft Windows system, link with libf2c.lib; + on Linux or Unix systems, link with .../path/to/libf2c.a -lm + or, if you install libf2c.a in a standard place, with -lf2c -lm + -- in that order, at the end of the command line, as in + cc *.o -lf2c -lm + Source for libf2c is in /netlib/f2c/libf2c.zip, e.g., + + http://www.netlib.org/f2c/libf2c.zip +*/ + +#include "datatypes.h" + +/* Subroutine */ int ztbmv_(char *uplo, char *trans, char *diag, integer *n, + integer *k, doublecomplex *a, integer *lda, doublecomplex *x, integer + *incx, ftnlen uplo_len, ftnlen trans_len, ftnlen diag_len) +{ + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3, i__4, i__5; + doublecomplex z__1, z__2, z__3; + + /* Builtin functions */ + void d_cnjg(doublecomplex *, doublecomplex *); + + /* Local variables */ + integer i__, j, l, ix, jx, kx, info; + doublecomplex temp; + extern logical lsame_(char *, char *, ftnlen, ftnlen); + integer kplus1; + extern /* Subroutine */ int xerbla_(char *, integer *, ftnlen); + logical noconj, nounit; + +/* .. Scalar Arguments .. */ +/* .. */ +/* .. Array Arguments .. */ +/* .. */ + +/* Purpose */ +/* ======= */ + +/* ZTBMV performs one of the matrix-vector operations */ + +/* x := A*x, or x := A'*x, or x := conjg( A' )*x, */ + +/* where x is an n element vector and A is an n by n unit, or non-unit, */ +/* upper or lower triangular band matrix, with ( k + 1 ) diagonals. */ + +/* Arguments */ +/* ========== */ + +/* UPLO - CHARACTER*1. */ +/* On entry, UPLO specifies whether the matrix is an upper or */ +/* lower triangular matrix as follows: */ + +/* UPLO = 'U' or 'u' A is an upper triangular matrix. */ + +/* UPLO = 'L' or 'l' A is a lower triangular matrix. */ + +/* Unchanged on exit. */ + +/* TRANS - CHARACTER*1. */ +/* On entry, TRANS specifies the operation to be performed as */ +/* follows: */ + +/* TRANS = 'N' or 'n' x := A*x. */ + +/* TRANS = 'T' or 't' x := A'*x. */ + +/* TRANS = 'C' or 'c' x := conjg( A' )*x. */ + +/* Unchanged on exit. */ + +/* DIAG - CHARACTER*1. */ +/* On entry, DIAG specifies whether or not A is unit */ +/* triangular as follows: */ + +/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */ + +/* DIAG = 'N' or 'n' A is not assumed to be unit */ +/* triangular. */ + +/* Unchanged on exit. */ + +/* N - INTEGER. */ +/* On entry, N specifies the order of the matrix A. */ +/* N must be at least zero. */ +/* Unchanged on exit. */ + +/* K - INTEGER. */ +/* On entry with UPLO = 'U' or 'u', K specifies the number of */ +/* super-diagonals of the matrix A. */ +/* On entry with UPLO = 'L' or 'l', K specifies the number of */ +/* sub-diagonals of the matrix A. */ +/* K must satisfy 0 .le. K. */ +/* Unchanged on exit. */ + +/* A - COMPLEX*16 array of DIMENSION ( LDA, n ). */ +/* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) */ +/* by n part of the array A must contain the upper triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row */ +/* ( k + 1 ) of the array, the first super-diagonal starting at */ +/* position 2 in row k, and so on. The top left k by k triangle */ +/* of the array A is not referenced. */ +/* The following program segment will transfer an upper */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = K + 1 - J */ +/* DO 10, I = MAX( 1, J - K ), J */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) */ +/* by n part of the array A must contain the lower triangular */ +/* band part of the matrix of coefficients, supplied column by */ +/* column, with the leading diagonal of the matrix in row 1 of */ +/* the array, the first sub-diagonal starting at position 1 in */ +/* row 2, and so on. The bottom right k by k triangle of the */ +/* array A is not referenced. */ +/* The following program segment will transfer a lower */ +/* triangular band matrix from conventional full matrix storage */ +/* to band storage: */ + +/* DO 20, J = 1, N */ +/* M = 1 - J */ +/* DO 10, I = J, MIN( N, J + K ) */ +/* A( M + I, J ) = matrix( I, J ) */ +/* 10 CONTINUE */ +/* 20 CONTINUE */ + +/* Note that when DIAG = 'U' or 'u' the elements of the array A */ +/* corresponding to the diagonal elements of the matrix are not */ +/* referenced, but are assumed to be unity. */ +/* Unchanged on exit. */ + +/* LDA - INTEGER. */ +/* On entry, LDA specifies the first dimension of A as declared */ +/* in the calling (sub) program. LDA must be at least */ +/* ( k + 1 ). */ +/* Unchanged on exit. */ + +/* X - COMPLEX*16 array of dimension at least */ +/* ( 1 + ( n - 1 )*abs( INCX ) ). */ +/* Before entry, the incremented array X must contain the n */ +/* element vector x. On exit, X is overwritten with the */ +/* tranformed vector x. */ + +/* INCX - INTEGER. */ +/* On entry, INCX specifies the increment for the elements of */ +/* X. INCX must not be zero. */ +/* Unchanged on exit. */ + +/* Further Details */ +/* =============== */ + +/* Level 2 Blas routine. */ + +/* -- Written on 22-October-1986. */ +/* Jack Dongarra, Argonne National Lab. */ +/* Jeremy Du Croz, Nag Central Office. */ +/* Sven Hammarling, Nag Central Office. */ +/* Richard Hanson, Sandia National Labs. */ + +/* ===================================================================== */ + +/* .. Parameters .. */ +/* .. */ +/* .. Local Scalars .. */ +/* .. */ +/* .. External Functions .. */ +/* .. */ +/* .. External Subroutines .. */ +/* .. */ +/* .. Intrinsic Functions .. */ +/* .. */ + +/* Test the input parameters. */ + + /* Parameter adjustments */ + a_dim1 = *lda; + a_offset = 1 + a_dim1; + a -= a_offset; + --x; + + /* Function Body */ + info = 0; + if (! lsame_(uplo, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(uplo, "L", ( + ftnlen)1, (ftnlen)1)) { + info = 1; + } else if (! lsame_(trans, "N", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, + "T", (ftnlen)1, (ftnlen)1) && ! lsame_(trans, "C", (ftnlen)1, ( + ftnlen)1)) { + info = 2; + } else if (! lsame_(diag, "U", (ftnlen)1, (ftnlen)1) && ! lsame_(diag, + "N", (ftnlen)1, (ftnlen)1)) { + info = 3; + } else if (*n < 0) { + info = 4; + } else if (*k < 0) { + info = 5; + } else if (*lda < *k + 1) { + info = 7; + } else if (*incx == 0) { + info = 9; + } + if (info != 0) { + xerbla_("ZTBMV ", &info, (ftnlen)6); + return 0; + } + +/* Quick return if possible. */ + + if (*n == 0) { + return 0; + } + + noconj = lsame_(trans, "T", (ftnlen)1, (ftnlen)1); + nounit = lsame_(diag, "N", (ftnlen)1, (ftnlen)1); + +/* Set up the start point in X if the increment is not unity. This */ +/* will be ( N - 1 )*INCX too small for descending loops. */ + + if (*incx <= 0) { + kx = 1 - (*n - 1) * *incx; + } else if (*incx != 1) { + kx = 1; + } + +/* Start the operations. In this version the elements of A are */ +/* accessed sequentially with one pass through A. */ + + if (lsame_(trans, "N", (ftnlen)1, (ftnlen)1)) { + +/* Form x := A*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = j; + if (x[i__2].r != 0. || x[i__2].i != 0.) { + i__2 = j; + temp.r = x[i__2].r, temp.i = x[i__2].i; + l = kplus1 - j; +/* Computing MAX */ + i__2 = 1, i__3 = j - *k; + i__4 = j - 1; + for (i__ = max(i__2,i__3); i__ <= i__4; ++i__) { + i__2 = i__; + i__3 = i__; + i__5 = l + i__ + j * a_dim1; + z__2.r = temp.r * a[i__5].r - temp.i * a[i__5].i, + z__2.i = temp.r * a[i__5].i + temp.i * a[ + i__5].r; + z__1.r = x[i__3].r + z__2.r, z__1.i = x[i__3].i + + z__2.i; + x[i__2].r = z__1.r, x[i__2].i = z__1.i; +/* L10: */ + } + if (nounit) { + i__4 = j; + i__2 = j; + i__3 = kplus1 + j * a_dim1; + z__1.r = x[i__2].r * a[i__3].r - x[i__2].i * a[ + i__3].i, z__1.i = x[i__2].r * a[i__3].i + + x[i__2].i * a[i__3].r; + x[i__4].r = z__1.r, x[i__4].i = z__1.i; + } + } +/* L20: */ + } + } else { + jx = kx; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__4 = jx; + if (x[i__4].r != 0. || x[i__4].i != 0.) { + i__4 = jx; + temp.r = x[i__4].r, temp.i = x[i__4].i; + ix = kx; + l = kplus1 - j; +/* Computing MAX */ + i__4 = 1, i__2 = j - *k; + i__3 = j - 1; + for (i__ = max(i__4,i__2); i__ <= i__3; ++i__) { + i__4 = ix; + i__2 = ix; + i__5 = l + i__ + j * a_dim1; + z__2.r = temp.r * a[i__5].r - temp.i * a[i__5].i, + z__2.i = temp.r * a[i__5].i + temp.i * a[ + i__5].r; + z__1.r = x[i__2].r + z__2.r, z__1.i = x[i__2].i + + z__2.i; + x[i__4].r = z__1.r, x[i__4].i = z__1.i; + ix += *incx; +/* L30: */ + } + if (nounit) { + i__3 = jx; + i__4 = jx; + i__2 = kplus1 + j * a_dim1; + z__1.r = x[i__4].r * a[i__2].r - x[i__4].i * a[ + i__2].i, z__1.i = x[i__4].r * a[i__2].i + + x[i__4].i * a[i__2].r; + x[i__3].r = z__1.r, x[i__3].i = z__1.i; + } + } + jx += *incx; + if (j > *k) { + kx += *incx; + } +/* L40: */ + } + } + } else { + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + i__1 = j; + if (x[i__1].r != 0. || x[i__1].i != 0.) { + i__1 = j; + temp.r = x[i__1].r, temp.i = x[i__1].i; + l = 1 - j; +/* Computing MIN */ + i__1 = *n, i__3 = j + *k; + i__4 = j + 1; + for (i__ = min(i__1,i__3); i__ >= i__4; --i__) { + i__1 = i__; + i__3 = i__; + i__2 = l + i__ + j * a_dim1; + z__2.r = temp.r * a[i__2].r - temp.i * a[i__2].i, + z__2.i = temp.r * a[i__2].i + temp.i * a[ + i__2].r; + z__1.r = x[i__3].r + z__2.r, z__1.i = x[i__3].i + + z__2.i; + x[i__1].r = z__1.r, x[i__1].i = z__1.i; +/* L50: */ + } + if (nounit) { + i__4 = j; + i__1 = j; + i__3 = j * a_dim1 + 1; + z__1.r = x[i__1].r * a[i__3].r - x[i__1].i * a[ + i__3].i, z__1.i = x[i__1].r * a[i__3].i + + x[i__1].i * a[i__3].r; + x[i__4].r = z__1.r, x[i__4].i = z__1.i; + } + } +/* L60: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + i__4 = jx; + if (x[i__4].r != 0. || x[i__4].i != 0.) { + i__4 = jx; + temp.r = x[i__4].r, temp.i = x[i__4].i; + ix = kx; + l = 1 - j; +/* Computing MIN */ + i__4 = *n, i__1 = j + *k; + i__3 = j + 1; + for (i__ = min(i__4,i__1); i__ >= i__3; --i__) { + i__4 = ix; + i__1 = ix; + i__2 = l + i__ + j * a_dim1; + z__2.r = temp.r * a[i__2].r - temp.i * a[i__2].i, + z__2.i = temp.r * a[i__2].i + temp.i * a[ + i__2].r; + z__1.r = x[i__1].r + z__2.r, z__1.i = x[i__1].i + + z__2.i; + x[i__4].r = z__1.r, x[i__4].i = z__1.i; + ix -= *incx; +/* L70: */ + } + if (nounit) { + i__3 = jx; + i__4 = jx; + i__1 = j * a_dim1 + 1; + z__1.r = x[i__4].r * a[i__1].r - x[i__4].i * a[ + i__1].i, z__1.i = x[i__4].r * a[i__1].i + + x[i__4].i * a[i__1].r; + x[i__3].r = z__1.r, x[i__3].i = z__1.i; + } + } + jx -= *incx; + if (*n - j >= *k) { + kx -= *incx; + } +/* L80: */ + } + } + } + } else { + +/* Form x := A'*x or x := conjg( A' )*x. */ + + if (lsame_(uplo, "U", (ftnlen)1, (ftnlen)1)) { + kplus1 = *k + 1; + if (*incx == 1) { + for (j = *n; j >= 1; --j) { + i__3 = j; + temp.r = x[i__3].r, temp.i = x[i__3].i; + l = kplus1 - j; + if (noconj) { + if (nounit) { + i__3 = kplus1 + j * a_dim1; + z__1.r = temp.r * a[i__3].r - temp.i * a[i__3].i, + z__1.i = temp.r * a[i__3].i + temp.i * a[ + i__3].r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + i__4 = l + i__ + j * a_dim1; + i__1 = i__; + z__2.r = a[i__4].r * x[i__1].r - a[i__4].i * x[ + i__1].i, z__2.i = a[i__4].r * x[i__1].i + + a[i__4].i * x[i__1].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; +/* L90: */ + } + } else { + if (nounit) { + d_cnjg(&z__2, &a[kplus1 + j * a_dim1]); + z__1.r = temp.r * z__2.r - temp.i * z__2.i, + z__1.i = temp.r * z__2.i + temp.i * + z__2.r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__4 = i__; + z__2.r = z__3.r * x[i__4].r - z__3.i * x[i__4].i, + z__2.i = z__3.r * x[i__4].i + z__3.i * x[ + i__4].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; +/* L100: */ + } + } + i__3 = j; + x[i__3].r = temp.r, x[i__3].i = temp.i; +/* L110: */ + } + } else { + kx += (*n - 1) * *incx; + jx = kx; + for (j = *n; j >= 1; --j) { + i__3 = jx; + temp.r = x[i__3].r, temp.i = x[i__3].i; + kx -= *incx; + ix = kx; + l = kplus1 - j; + if (noconj) { + if (nounit) { + i__3 = kplus1 + j * a_dim1; + z__1.r = temp.r * a[i__3].r - temp.i * a[i__3].i, + z__1.i = temp.r * a[i__3].i + temp.i * a[ + i__3].r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + i__4 = l + i__ + j * a_dim1; + i__1 = ix; + z__2.r = a[i__4].r * x[i__1].r - a[i__4].i * x[ + i__1].i, z__2.i = a[i__4].r * x[i__1].i + + a[i__4].i * x[i__1].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; + ix -= *incx; +/* L120: */ + } + } else { + if (nounit) { + d_cnjg(&z__2, &a[kplus1 + j * a_dim1]); + z__1.r = temp.r * z__2.r - temp.i * z__2.i, + z__1.i = temp.r * z__2.i + temp.i * + z__2.r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MAX */ + i__4 = 1, i__1 = j - *k; + i__3 = max(i__4,i__1); + for (i__ = j - 1; i__ >= i__3; --i__) { + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__4 = ix; + z__2.r = z__3.r * x[i__4].r - z__3.i * x[i__4].i, + z__2.i = z__3.r * x[i__4].i + z__3.i * x[ + i__4].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; + ix -= *incx; +/* L130: */ + } + } + i__3 = jx; + x[i__3].r = temp.r, x[i__3].i = temp.i; + jx -= *incx; +/* L140: */ + } + } + } else { + if (*incx == 1) { + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + i__4 = j; + temp.r = x[i__4].r, temp.i = x[i__4].i; + l = 1 - j; + if (noconj) { + if (nounit) { + i__4 = j * a_dim1 + 1; + z__1.r = temp.r * a[i__4].r - temp.i * a[i__4].i, + z__1.i = temp.r * a[i__4].i + temp.i * a[ + i__4].r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + i__1 = l + i__ + j * a_dim1; + i__2 = i__; + z__2.r = a[i__1].r * x[i__2].r - a[i__1].i * x[ + i__2].i, z__2.i = a[i__1].r * x[i__2].i + + a[i__1].i * x[i__2].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; +/* L150: */ + } + } else { + if (nounit) { + d_cnjg(&z__2, &a[j * a_dim1 + 1]); + z__1.r = temp.r * z__2.r - temp.i * z__2.i, + z__1.i = temp.r * z__2.i + temp.i * + z__2.r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__1 = i__; + z__2.r = z__3.r * x[i__1].r - z__3.i * x[i__1].i, + z__2.i = z__3.r * x[i__1].i + z__3.i * x[ + i__1].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; +/* L160: */ + } + } + i__4 = j; + x[i__4].r = temp.r, x[i__4].i = temp.i; +/* L170: */ + } + } else { + jx = kx; + i__3 = *n; + for (j = 1; j <= i__3; ++j) { + i__4 = jx; + temp.r = x[i__4].r, temp.i = x[i__4].i; + kx += *incx; + ix = kx; + l = 1 - j; + if (noconj) { + if (nounit) { + i__4 = j * a_dim1 + 1; + z__1.r = temp.r * a[i__4].r - temp.i * a[i__4].i, + z__1.i = temp.r * a[i__4].i + temp.i * a[ + i__4].r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + i__1 = l + i__ + j * a_dim1; + i__2 = ix; + z__2.r = a[i__1].r * x[i__2].r - a[i__1].i * x[ + i__2].i, z__2.i = a[i__1].r * x[i__2].i + + a[i__1].i * x[i__2].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; + ix += *incx; +/* L180: */ + } + } else { + if (nounit) { + d_cnjg(&z__2, &a[j * a_dim1 + 1]); + z__1.r = temp.r * z__2.r - temp.i * z__2.i, + z__1.i = temp.r * z__2.i + temp.i * + z__2.r; + temp.r = z__1.r, temp.i = z__1.i; + } +/* Computing MIN */ + i__1 = *n, i__2 = j + *k; + i__4 = min(i__1,i__2); + for (i__ = j + 1; i__ <= i__4; ++i__) { + d_cnjg(&z__3, &a[l + i__ + j * a_dim1]); + i__1 = ix; + z__2.r = z__3.r * x[i__1].r - z__3.i * x[i__1].i, + z__2.i = z__3.r * x[i__1].i + z__3.i * x[ + i__1].r; + z__1.r = temp.r + z__2.r, z__1.i = temp.i + + z__2.i; + temp.r = z__1.r, temp.i = z__1.i; + ix += *incx; +/* L190: */ + } + } + i__4 = jx; + x[i__4].r = temp.r, x[i__4].i = temp.i; + jx += *incx; +/* L200: */ + } + } + } + } + + return 0; + +/* End of ZTBMV . */ + +} /* ztbmv_ */ + diff --git a/ground/gcs/src/libs/eigen/blas/complexdots.f b/ground/gcs/src/libs/eigen/blas/fortran/complexdots.f similarity index 100% rename from ground/gcs/src/libs/eigen/blas/complexdots.f rename to ground/gcs/src/libs/eigen/blas/fortran/complexdots.f diff --git a/ground/gcs/src/libs/eigen/blas/level1_cplx_impl.h b/ground/gcs/src/libs/eigen/blas/level1_cplx_impl.h index 283b9f827b..719f5bac91 100644 --- a/ground/gcs/src/libs/eigen/blas/level1_cplx_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level1_cplx_impl.h @@ -32,45 +32,52 @@ RealScalar EIGEN_CAT(EIGEN_CAT(REAL_SCALAR_SUFFIX,SCALAR_SUFFIX),asum_)(int *n, if(*n<=0) return 0; - if(*incx==1) return vector(x,*n).unaryExpr().sum(); - else return vector(x,*n,std::abs(*incx)).unaryExpr().sum(); + if(*incx==1) return make_vector(x,*n).unaryExpr().sum(); + else return make_vector(x,*n,std::abs(*incx)).unaryExpr().sum(); } // computes a dot product of a conjugated vector with another vector. int EIGEN_BLAS_FUNC(dotcw)(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar* pres) { // std::cerr << "_dotc " << *n << " " << *incx << " " << *incy << "\n"; + Scalar* res = reinterpret_cast(pres); - if(*n<=0) return 0; + if(*n<=0) + { + *res = Scalar(0); + return 0; + } Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - Scalar* res = reinterpret_cast(pres); - if(*incx==1 && *incy==1) *res = (vector(x,*n).dot(vector(y,*n))); - else if(*incx>0 && *incy>0) *res = (vector(x,*n,*incx).dot(vector(y,*n,*incy))); - else if(*incx<0 && *incy>0) *res = (vector(x,*n,-*incx).reverse().dot(vector(y,*n,*incy))); - else if(*incx>0 && *incy<0) *res = (vector(x,*n,*incx).dot(vector(y,*n,-*incy).reverse())); - else if(*incx<0 && *incy<0) *res = (vector(x,*n,-*incx).reverse().dot(vector(y,*n,-*incy).reverse())); + if(*incx==1 && *incy==1) *res = (make_vector(x,*n).dot(make_vector(y,*n))); + else if(*incx>0 && *incy>0) *res = (make_vector(x,*n,*incx).dot(make_vector(y,*n,*incy))); + else if(*incx<0 && *incy>0) *res = (make_vector(x,*n,-*incx).reverse().dot(make_vector(y,*n,*incy))); + else if(*incx>0 && *incy<0) *res = (make_vector(x,*n,*incx).dot(make_vector(y,*n,-*incy).reverse())); + else if(*incx<0 && *incy<0) *res = (make_vector(x,*n,-*incx).reverse().dot(make_vector(y,*n,-*incy).reverse())); return 0; } // computes a vector-vector dot product without complex conjugation. int EIGEN_BLAS_FUNC(dotuw)(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar* pres) { -// std::cerr << "_dotu " << *n << " " << *incx << " " << *incy << "\n"; + Scalar* res = reinterpret_cast(pres); - if(*n<=0) return 0; + if(*n<=0) + { + *res = Scalar(0); + return 0; + } Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - Scalar* res = reinterpret_cast(pres); - if(*incx==1 && *incy==1) *res = (vector(x,*n).cwiseProduct(vector(y,*n))).sum(); - else if(*incx>0 && *incy>0) *res = (vector(x,*n,*incx).cwiseProduct(vector(y,*n,*incy))).sum(); - else if(*incx<0 && *incy>0) *res = (vector(x,*n,-*incx).reverse().cwiseProduct(vector(y,*n,*incy))).sum(); - else if(*incx>0 && *incy<0) *res = (vector(x,*n,*incx).cwiseProduct(vector(y,*n,-*incy).reverse())).sum(); - else if(*incx<0 && *incy<0) *res = (vector(x,*n,-*incx).reverse().cwiseProduct(vector(y,*n,-*incy).reverse())).sum(); + if(*incx==1 && *incy==1) *res = (make_vector(x,*n).cwiseProduct(make_vector(y,*n))).sum(); + else if(*incx>0 && *incy>0) *res = (make_vector(x,*n,*incx).cwiseProduct(make_vector(y,*n,*incy))).sum(); + else if(*incx<0 && *incy>0) *res = (make_vector(x,*n,-*incx).reverse().cwiseProduct(make_vector(y,*n,*incy))).sum(); + else if(*incx>0 && *incy<0) *res = (make_vector(x,*n,*incx).cwiseProduct(make_vector(y,*n,-*incy).reverse())).sum(); + else if(*incx<0 && *incy<0) *res = (make_vector(x,*n,-*incx).reverse().cwiseProduct(make_vector(y,*n,-*incy).reverse())).sum(); return 0; } @@ -82,9 +89,9 @@ RealScalar EIGEN_CAT(EIGEN_CAT(REAL_SCALAR_SUFFIX,SCALAR_SUFFIX),nrm2_)(int *n, Scalar* x = reinterpret_cast(px); if(*incx==1) - return vector(x,*n).stableNorm(); + return make_vector(x,*n).stableNorm(); - return vector(x,*n,*incx).stableNorm(); + return make_vector(x,*n,*incx).stableNorm(); } int EIGEN_CAT(EIGEN_CAT(SCALAR_SUFFIX,REAL_SCALAR_SUFFIX),rot_)(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps) @@ -96,8 +103,8 @@ int EIGEN_CAT(EIGEN_CAT(SCALAR_SUFFIX,REAL_SCALAR_SUFFIX),rot_)(int *n, RealScal RealScalar c = *pc; RealScalar s = *ps; - StridedVectorType vx(vector(x,*n,std::abs(*incx))); - StridedVectorType vy(vector(y,*n,std::abs(*incy))); + StridedVectorType vx(make_vector(x,*n,std::abs(*incx))); + StridedVectorType vy(make_vector(y,*n,std::abs(*incy))); Reverse rvx(vx); Reverse rvy(vy); @@ -119,9 +126,8 @@ int EIGEN_CAT(EIGEN_CAT(SCALAR_SUFFIX,REAL_SCALAR_SUFFIX),scal_)(int *n, RealSca // std::cerr << "__scal " << *n << " " << alpha << " " << *incx << "\n"; - if(*incx==1) vector(x,*n) *= alpha; - else vector(x,*n,std::abs(*incx)) *= alpha; + if(*incx==1) make_vector(x,*n) *= alpha; + else make_vector(x,*n,std::abs(*incx)) *= alpha; return 0; } - diff --git a/ground/gcs/src/libs/eigen/blas/level1_impl.h b/ground/gcs/src/libs/eigen/blas/level1_impl.h index b08c2f6bed..f857bfa20c 100644 --- a/ground/gcs/src/libs/eigen/blas/level1_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level1_impl.h @@ -9,19 +9,19 @@ #include "common.h" -int EIGEN_BLAS_FUNC(axpy)(int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *py, int *incy) +int EIGEN_BLAS_FUNC(axpy)(const int *n, const RealScalar *palpha, const RealScalar *px, const int *incx, RealScalar *py, const int *incy) { - Scalar* x = reinterpret_cast(px); + const Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - Scalar alpha = *reinterpret_cast(palpha); + Scalar alpha = *reinterpret_cast(palpha); if(*n<=0) return 0; - if(*incx==1 && *incy==1) vector(y,*n) += alpha * vector(x,*n); - else if(*incx>0 && *incy>0) vector(y,*n,*incy) += alpha * vector(x,*n,*incx); - else if(*incx>0 && *incy<0) vector(y,*n,-*incy).reverse() += alpha * vector(x,*n,*incx); - else if(*incx<0 && *incy>0) vector(y,*n,*incy) += alpha * vector(x,*n,-*incx).reverse(); - else if(*incx<0 && *incy<0) vector(y,*n,-*incy).reverse() += alpha * vector(x,*n,-*incx).reverse(); + if(*incx==1 && *incy==1) make_vector(y,*n) += alpha * make_vector(x,*n); + else if(*incx>0 && *incy>0) make_vector(y,*n,*incy) += alpha * make_vector(x,*n,*incx); + else if(*incx>0 && *incy<0) make_vector(y,*n,-*incy).reverse() += alpha * make_vector(x,*n,*incx); + else if(*incx<0 && *incy>0) make_vector(y,*n,*incy) += alpha * make_vector(x,*n,-*incx).reverse(); + else if(*incx<0 && *incy<0) make_vector(y,*n,-*incy).reverse() += alpha * make_vector(x,*n,-*incx).reverse(); return 0; } @@ -35,7 +35,7 @@ int EIGEN_BLAS_FUNC(copy)(int *n, RealScalar *px, int *incx, RealScalar *py, int // be carefull, *incx==0 is allowed !! if(*incx==1 && *incy==1) - vector(y,*n) = vector(x,*n); + make_vector(y,*n) = make_vector(x,*n); else { if(*incx<0) x = x - (*n-1)*(*incx); @@ -57,27 +57,27 @@ int EIGEN_CAT(EIGEN_CAT(i,SCALAR_SUFFIX),amax_)(int *n, RealScalar *px, int *inc Scalar* x = reinterpret_cast(px); DenseIndex ret; - if(*incx==1) vector(x,*n).cwiseAbs().maxCoeff(&ret); - else vector(x,*n,std::abs(*incx)).cwiseAbs().maxCoeff(&ret); - return ret+1; + if(*incx==1) make_vector(x,*n).cwiseAbs().maxCoeff(&ret); + else make_vector(x,*n,std::abs(*incx)).cwiseAbs().maxCoeff(&ret); + return int(ret)+1; } int EIGEN_CAT(EIGEN_CAT(i,SCALAR_SUFFIX),amin_)(int *n, RealScalar *px, int *incx) { if(*n<=0) return 0; Scalar* x = reinterpret_cast(px); - + DenseIndex ret; - if(*incx==1) vector(x,*n).cwiseAbs().minCoeff(&ret); - else vector(x,*n,std::abs(*incx)).cwiseAbs().minCoeff(&ret); - return ret+1; + if(*incx==1) make_vector(x,*n).cwiseAbs().minCoeff(&ret); + else make_vector(x,*n,std::abs(*incx)).cwiseAbs().minCoeff(&ret); + return int(ret)+1; } int EIGEN_BLAS_FUNC(rotg)(RealScalar *pa, RealScalar *pb, RealScalar *pc, RealScalar *ps) { using std::sqrt; using std::abs; - + Scalar& a = *reinterpret_cast(pa); Scalar& b = *reinterpret_cast(pb); RealScalar* c = pc; @@ -143,8 +143,8 @@ int EIGEN_BLAS_FUNC(scal)(int *n, RealScalar *palpha, RealScalar *px, int *incx) Scalar* x = reinterpret_cast(px); Scalar alpha = *reinterpret_cast(palpha); - if(*incx==1) vector(x,*n) *= alpha; - else vector(x,*n,std::abs(*incx)) *= alpha; + if(*incx==1) make_vector(x,*n) *= alpha; + else make_vector(x,*n,std::abs(*incx)) *= alpha; return 0; } @@ -156,12 +156,11 @@ int EIGEN_BLAS_FUNC(swap)(int *n, RealScalar *px, int *incx, RealScalar *py, int Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - if(*incx==1 && *incy==1) vector(y,*n).swap(vector(x,*n)); - else if(*incx>0 && *incy>0) vector(y,*n,*incy).swap(vector(x,*n,*incx)); - else if(*incx>0 && *incy<0) vector(y,*n,-*incy).reverse().swap(vector(x,*n,*incx)); - else if(*incx<0 && *incy>0) vector(y,*n,*incy).swap(vector(x,*n,-*incx).reverse()); - else if(*incx<0 && *incy<0) vector(y,*n,-*incy).reverse().swap(vector(x,*n,-*incx).reverse()); + if(*incx==1 && *incy==1) make_vector(y,*n).swap(make_vector(x,*n)); + else if(*incx>0 && *incy>0) make_vector(y,*n,*incy).swap(make_vector(x,*n,*incx)); + else if(*incx>0 && *incy<0) make_vector(y,*n,-*incy).reverse().swap(make_vector(x,*n,*incx)); + else if(*incx<0 && *incy>0) make_vector(y,*n,*incy).swap(make_vector(x,*n,-*incx).reverse()); + else if(*incx<0 && *incy<0) make_vector(y,*n,-*incy).reverse().swap(make_vector(x,*n,-*incx).reverse()); return 1; } - diff --git a/ground/gcs/src/libs/eigen/blas/level1_real_impl.h b/ground/gcs/src/libs/eigen/blas/level1_real_impl.h index 8acecdfc6d..02586d5195 100644 --- a/ground/gcs/src/libs/eigen/blas/level1_real_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level1_real_impl.h @@ -19,8 +19,8 @@ RealScalar EIGEN_BLAS_FUNC(asum)(int *n, RealScalar *px, int *incx) if(*n<=0) return 0; - if(*incx==1) return vector(x,*n).cwiseAbs().sum(); - else return vector(x,*n,std::abs(*incx)).cwiseAbs().sum(); + if(*incx==1) return make_vector(x,*n).cwiseAbs().sum(); + else return make_vector(x,*n,std::abs(*incx)).cwiseAbs().sum(); } // computes a vector-vector dot product. @@ -33,11 +33,11 @@ Scalar EIGEN_BLAS_FUNC(dot)(int *n, RealScalar *px, int *incx, RealScalar *py, i Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - if(*incx==1 && *incy==1) return (vector(x,*n).cwiseProduct(vector(y,*n))).sum(); - else if(*incx>0 && *incy>0) return (vector(x,*n,*incx).cwiseProduct(vector(y,*n,*incy))).sum(); - else if(*incx<0 && *incy>0) return (vector(x,*n,-*incx).reverse().cwiseProduct(vector(y,*n,*incy))).sum(); - else if(*incx>0 && *incy<0) return (vector(x,*n,*incx).cwiseProduct(vector(y,*n,-*incy).reverse())).sum(); - else if(*incx<0 && *incy<0) return (vector(x,*n,-*incx).reverse().cwiseProduct(vector(y,*n,-*incy).reverse())).sum(); + if(*incx==1 && *incy==1) return (make_vector(x,*n).cwiseProduct(make_vector(y,*n))).sum(); + else if(*incx>0 && *incy>0) return (make_vector(x,*n,*incx).cwiseProduct(make_vector(y,*n,*incy))).sum(); + else if(*incx<0 && *incy>0) return (make_vector(x,*n,-*incx).reverse().cwiseProduct(make_vector(y,*n,*incy))).sum(); + else if(*incx>0 && *incy<0) return (make_vector(x,*n,*incx).cwiseProduct(make_vector(y,*n,-*incy).reverse())).sum(); + else if(*incx<0 && *incy<0) return (make_vector(x,*n,-*incx).reverse().cwiseProduct(make_vector(y,*n,-*incy).reverse())).sum(); else return 0; } @@ -50,8 +50,8 @@ Scalar EIGEN_BLAS_FUNC(nrm2)(int *n, RealScalar *px, int *incx) Scalar* x = reinterpret_cast(px); - if(*incx==1) return vector(x,*n).stableNorm(); - else return vector(x,*n,std::abs(*incx)).stableNorm(); + if(*incx==1) return make_vector(x,*n).stableNorm(); + else return make_vector(x,*n,std::abs(*incx)).stableNorm(); } int EIGEN_BLAS_FUNC(rot)(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps) @@ -64,8 +64,8 @@ int EIGEN_BLAS_FUNC(rot)(int *n, RealScalar *px, int *incx, RealScalar *py, int Scalar c = *reinterpret_cast(pc); Scalar s = *reinterpret_cast(ps); - StridedVectorType vx(vector(x,*n,std::abs(*incx))); - StridedVectorType vy(vector(y,*n,std::abs(*incy))); + StridedVectorType vx(make_vector(x,*n,std::abs(*incx))); + StridedVectorType vy(make_vector(y,*n,std::abs(*incy))); Reverse rvx(vx); Reverse rvy(vy); diff --git a/ground/gcs/src/libs/eigen/blas/level2_cplx_impl.h b/ground/gcs/src/libs/eigen/blas/level2_cplx_impl.h index b850b6cd1b..e3ce61435e 100644 --- a/ground/gcs/src/libs/eigen/blas/level2_cplx_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level2_cplx_impl.h @@ -16,28 +16,22 @@ * where alpha and beta are scalars, x and y are n element vectors and * A is an n by n hermitian matrix. */ -int EIGEN_BLAS_FUNC(hemv)(char *uplo, int *n, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *px, int *incx, RealScalar *pbeta, RealScalar *py, int *incy) +int EIGEN_BLAS_FUNC(hemv)(const char *uplo, const int *n, const RealScalar *palpha, const RealScalar *pa, const int *lda, + const RealScalar *px, const int *incx, const RealScalar *pbeta, RealScalar *py, const int *incy) { - typedef void (*functype)(int, const Scalar*, int, const Scalar*, int, Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::selfadjoint_matrix_vector_product::run); - func[LO] = (internal::selfadjoint_matrix_vector_product::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); - Scalar* x = reinterpret_cast(px); + typedef void (*functype)(int, const Scalar*, int, const Scalar*, Scalar*, Scalar); + static const functype func[2] = { + // array index: UP + (internal::selfadjoint_matrix_vector_product::run), + // array index: LO + (internal::selfadjoint_matrix_vector_product::run), + }; + + const Scalar* a = reinterpret_cast(pa); + const Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); // check arguments int info = 0; @@ -52,13 +46,13 @@ int EIGEN_BLAS_FUNC(hemv)(char *uplo, int *n, RealScalar *palpha, RealScalar *pa if(*n==0) return 1; - Scalar* actual_x = get_compact_vector(x,*n,*incx); + const Scalar* actual_x = get_compact_vector(x,*n,*incx); Scalar* actual_y = get_compact_vector(y,*n,*incy); if(beta!=Scalar(1)) { - if(beta==Scalar(0)) vector(actual_y, *n).setZero(); - else vector(actual_y, *n) *= beta; + if(beta==Scalar(0)) make_vector(actual_y, *n).setZero(); + else make_vector(actual_y, *n) *= beta; } if(alpha!=Scalar(0)) @@ -67,7 +61,7 @@ int EIGEN_BLAS_FUNC(hemv)(char *uplo, int *n, RealScalar *palpha, RealScalar *pa if(code>=2 || func[code]==0) return 0; - func[code](*n, a, *lda, actual_x, 1, actual_y, alpha); + func[code](*n, a, *lda, actual_x, actual_y, alpha); } if(actual_x!=x) delete[] actual_x; @@ -111,19 +105,12 @@ int EIGEN_BLAS_FUNC(hemv)(char *uplo, int *n, RealScalar *palpha, RealScalar *pa int EIGEN_BLAS_FUNC(hpr)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *pap) { typedef void (*functype)(int, Scalar*, const Scalar*, RealScalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::selfadjoint_packed_rank1_update::run); - func[LO] = (internal::selfadjoint_packed_rank1_update::run); - - init = true; - } + static const functype func[2] = { + // array index: UP + (internal::selfadjoint_packed_rank1_update::run), + // array index: LO + (internal::selfadjoint_packed_rank1_update::run), + }; Scalar* x = reinterpret_cast(px); Scalar* ap = reinterpret_cast(pap); @@ -162,19 +149,12 @@ int EIGEN_BLAS_FUNC(hpr)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int EIGEN_BLAS_FUNC(hpr2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pap) { typedef void (*functype)(int, Scalar*, const Scalar*, const Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::packed_rank2_update_selector::run); - func[LO] = (internal::packed_rank2_update_selector::run); - - init = true; - } + static const functype func[2] = { + // array index: UP + (internal::packed_rank2_update_selector::run), + // array index: LO + (internal::packed_rank2_update_selector::run), + }; Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); @@ -217,19 +197,12 @@ int EIGEN_BLAS_FUNC(hpr2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px int EIGEN_BLAS_FUNC(her)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *pa, int *lda) { typedef void (*functype)(int, Scalar*, int, const Scalar*, const Scalar*, const Scalar&); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (selfadjoint_rank1_update::run); - func[LO] = (selfadjoint_rank1_update::run); - - init = true; - } + static const functype func[2] = { + // array index: UP + (selfadjoint_rank1_update::run), + // array index: LO + (selfadjoint_rank1_update::run), + }; Scalar* x = reinterpret_cast(px); Scalar* a = reinterpret_cast(pa); @@ -271,19 +244,12 @@ int EIGEN_BLAS_FUNC(her)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int EIGEN_BLAS_FUNC(her2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pa, int *lda) { typedef void (*functype)(int, Scalar*, int, const Scalar*, const Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::rank2_update_selector::run); - func[LO] = (internal::rank2_update_selector::run); - - init = true; - } + static const functype func[2] = { + // array index: UP + (internal::rank2_update_selector::run), + // array index: LO + (internal::rank2_update_selector::run), + }; Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); diff --git a/ground/gcs/src/libs/eigen/blas/level2_impl.h b/ground/gcs/src/libs/eigen/blas/level2_impl.h index 5f39419759..173f40b441 100644 --- a/ground/gcs/src/libs/eigen/blas/level2_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level2_impl.h @@ -9,29 +9,39 @@ #include "common.h" -int EIGEN_BLAS_FUNC(gemv)(char *opa, int *m, int *n, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *incb, RealScalar *pbeta, RealScalar *pc, int *incc) +template +struct general_matrix_vector_product_wrapper { - typedef void (*functype)(int, int, const Scalar *, int, const Scalar *, int , Scalar *, int, Scalar); - static functype func[4]; - - static bool init = false; - if(!init) + static void run(Index rows, Index cols,const Scalar *lhs, Index lhsStride, const Scalar *rhs, Index rhsIncr, Scalar* res, Index resIncr, Scalar alpha) { - for(int k=0; k<4; ++k) - func[k] = 0; - - func[NOTR] = (internal::general_matrix_vector_product::run); - func[TR ] = (internal::general_matrix_vector_product::run); - func[ADJ ] = (internal::general_matrix_vector_product::run); - - init = true; + typedef internal::const_blas_data_mapper LhsMapper; + typedef internal::const_blas_data_mapper RhsMapper; + + internal::general_matrix_vector_product + ::run( + rows, cols, LhsMapper(lhs, lhsStride), RhsMapper(rhs, rhsIncr), res, resIncr, alpha); } +}; - Scalar* a = reinterpret_cast(pa); - Scalar* b = reinterpret_cast(pb); +int EIGEN_BLAS_FUNC(gemv)(const char *opa, const int *m, const int *n, const RealScalar *palpha, + const RealScalar *pa, const int *lda, const RealScalar *pb, const int *incb, const RealScalar *pbeta, RealScalar *pc, const int *incc) +{ + typedef void (*functype)(int, int, const Scalar *, int, const Scalar *, int , Scalar *, int, Scalar); + static const functype func[4] = { + // array index: NOTR + (general_matrix_vector_product_wrapper::run), + // array index: TR + (general_matrix_vector_product_wrapper::run), + // array index: ADJ + (general_matrix_vector_product_wrapper::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); + const Scalar* b = reinterpret_cast(pb); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); // check arguments int info = 0; @@ -53,13 +63,13 @@ int EIGEN_BLAS_FUNC(gemv)(char *opa, int *m, int *n, RealScalar *palpha, RealSca if(code!=NOTR) std::swap(actual_m,actual_n); - Scalar* actual_b = get_compact_vector(b,actual_n,*incb); + const Scalar* actual_b = get_compact_vector(b,actual_n,*incb); Scalar* actual_c = get_compact_vector(c,actual_m,*incc); if(beta!=Scalar(1)) { - if(beta==Scalar(0)) vector(actual_c, actual_m).setZero(); - else vector(actual_c, actual_m) *= beta; + if(beta==Scalar(0)) make_vector(actual_c, actual_m).setZero(); + else make_vector(actual_c, actual_m) *= beta; } if(code>=4 || func[code]==0) @@ -73,37 +83,41 @@ int EIGEN_BLAS_FUNC(gemv)(char *opa, int *m, int *n, RealScalar *palpha, RealSca return 1; } -int EIGEN_BLAS_FUNC(trsv)(char *uplo, char *opa, char *diag, int *n, RealScalar *pa, int *lda, RealScalar *pb, int *incb) +int EIGEN_BLAS_FUNC(trsv)(const char *uplo, const char *opa, const char *diag, const int *n, const RealScalar *pa, const int *lda, RealScalar *pb, const int *incb) { typedef void (*functype)(int, const Scalar *, int, Scalar *); - static functype func[16]; - - static bool init = false; - if(!init) - { - for(int k=0; k<16; ++k) - func[k] = 0; - - func[NOTR | (UP << 2) | (NUNIT << 3)] = (internal::triangular_solve_vector::run); - func[TR | (UP << 2) | (NUNIT << 3)] = (internal::triangular_solve_vector::run); - func[ADJ | (UP << 2) | (NUNIT << 3)] = (internal::triangular_solve_vector::run); - - func[NOTR | (LO << 2) | (NUNIT << 3)] = (internal::triangular_solve_vector::run); - func[TR | (LO << 2) | (NUNIT << 3)] = (internal::triangular_solve_vector::run); - func[ADJ | (LO << 2) | (NUNIT << 3)] = (internal::triangular_solve_vector::run); - - func[NOTR | (UP << 2) | (UNIT << 3)] = (internal::triangular_solve_vector::run); - func[TR | (UP << 2) | (UNIT << 3)] = (internal::triangular_solve_vector::run); - func[ADJ | (UP << 2) | (UNIT << 3)] = (internal::triangular_solve_vector::run); - - func[NOTR | (LO << 2) | (UNIT << 3)] = (internal::triangular_solve_vector::run); - func[TR | (LO << 2) | (UNIT << 3)] = (internal::triangular_solve_vector::run); - func[ADJ | (LO << 2) | (UNIT << 3)] = (internal::triangular_solve_vector::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); + static const functype func[16] = { + // array index: NOTR | (UP << 2) | (NUNIT << 3) + (internal::triangular_solve_vector::run), + // array index: TR | (UP << 2) | (NUNIT << 3) + (internal::triangular_solve_vector::run), + // array index: ADJ | (UP << 2) | (NUNIT << 3) + (internal::triangular_solve_vector::run), + 0, + // array index: NOTR | (LO << 2) | (NUNIT << 3) + (internal::triangular_solve_vector::run), + // array index: TR | (LO << 2) | (NUNIT << 3) + (internal::triangular_solve_vector::run), + // array index: ADJ | (LO << 2) | (NUNIT << 3) + (internal::triangular_solve_vector::run), + 0, + // array index: NOTR | (UP << 2) | (UNIT << 3) + (internal::triangular_solve_vector::run), + // array index: TR | (UP << 2) | (UNIT << 3) + (internal::triangular_solve_vector::run), + // array index: ADJ | (UP << 2) | (UNIT << 3) + (internal::triangular_solve_vector::run), + 0, + // array index: NOTR | (LO << 2) | (UNIT << 3) + (internal::triangular_solve_vector::run), + // array index: TR | (LO << 2) | (UNIT << 3) + (internal::triangular_solve_vector::run), + // array index: ADJ | (LO << 2) | (UNIT << 3) + (internal::triangular_solve_vector::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); Scalar* b = reinterpret_cast(pb); int info = 0; @@ -128,37 +142,41 @@ int EIGEN_BLAS_FUNC(trsv)(char *uplo, char *opa, char *diag, int *n, RealScalar -int EIGEN_BLAS_FUNC(trmv)(char *uplo, char *opa, char *diag, int *n, RealScalar *pa, int *lda, RealScalar *pb, int *incb) +int EIGEN_BLAS_FUNC(trmv)(const char *uplo, const char *opa, const char *diag, const int *n, const RealScalar *pa, const int *lda, RealScalar *pb, const int *incb) { typedef void (*functype)(int, int, const Scalar *, int, const Scalar *, int, Scalar *, int, const Scalar&); - static functype func[16]; - - static bool init = false; - if(!init) - { - for(int k=0; k<16; ++k) - func[k] = 0; - - func[NOTR | (UP << 2) | (NUNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[TR | (UP << 2) | (NUNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[ADJ | (UP << 2) | (NUNIT << 3)] = (internal::triangular_matrix_vector_product::run); - - func[NOTR | (LO << 2) | (NUNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[TR | (LO << 2) | (NUNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[ADJ | (LO << 2) | (NUNIT << 3)] = (internal::triangular_matrix_vector_product::run); - - func[NOTR | (UP << 2) | (UNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[TR | (UP << 2) | (UNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[ADJ | (UP << 2) | (UNIT << 3)] = (internal::triangular_matrix_vector_product::run); - - func[NOTR | (LO << 2) | (UNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[TR | (LO << 2) | (UNIT << 3)] = (internal::triangular_matrix_vector_product::run); - func[ADJ | (LO << 2) | (UNIT << 3)] = (internal::triangular_matrix_vector_product::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); + static const functype func[16] = { + // array index: NOTR | (UP << 2) | (NUNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: TR | (UP << 2) | (NUNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: ADJ | (UP << 2) | (NUNIT << 3) + (internal::triangular_matrix_vector_product::run), + 0, + // array index: NOTR | (LO << 2) | (NUNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: TR | (LO << 2) | (NUNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: ADJ | (LO << 2) | (NUNIT << 3) + (internal::triangular_matrix_vector_product::run), + 0, + // array index: NOTR | (UP << 2) | (UNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: TR | (UP << 2) | (UNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: ADJ | (UP << 2) | (UNIT << 3) + (internal::triangular_matrix_vector_product::run), + 0, + // array index: NOTR | (LO << 2) | (UNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: TR | (LO << 2) | (UNIT << 3) + (internal::triangular_matrix_vector_product::run), + // array index: ADJ | (LO << 2) | (UNIT << 3) + (internal::triangular_matrix_vector_product::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); Scalar* b = reinterpret_cast(pb); int info = 0; @@ -200,13 +218,13 @@ int EIGEN_BLAS_FUNC(trmv)(char *uplo, char *opa, char *diag, int *n, RealScalar int EIGEN_BLAS_FUNC(gbmv)(char *trans, int *m, int *n, int *kl, int *ku, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *px, int *incx, RealScalar *pbeta, RealScalar *py, int *incy) { - Scalar* a = reinterpret_cast(pa); - Scalar* x = reinterpret_cast(px); + const Scalar* a = reinterpret_cast(pa); + const Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); int coeff_rows = *kl+*ku+1; - + int info = 0; if(OP(*trans)==INVALID) info = 1; else if(*m<0) info = 2; @@ -218,26 +236,26 @@ int EIGEN_BLAS_FUNC(gbmv)(char *trans, int *m, int *n, int *kl, int *ku, RealSca else if(*incy==0) info = 13; if(info) return xerbla_(SCALAR_SUFFIX_UP"GBMV ",&info,6); - + if(*m==0 || *n==0 || (alpha==Scalar(0) && beta==Scalar(1))) return 0; - + int actual_m = *m; int actual_n = *n; if(OP(*trans)!=NOTR) std::swap(actual_m,actual_n); - - Scalar* actual_x = get_compact_vector(x,actual_n,*incx); + + const Scalar* actual_x = get_compact_vector(x,actual_n,*incx); Scalar* actual_y = get_compact_vector(y,actual_m,*incy); - + if(beta!=Scalar(1)) { - if(beta==Scalar(0)) vector(actual_y, actual_m).setZero(); - else vector(actual_y, actual_m) *= beta; + if(beta==Scalar(0)) make_vector(actual_y, actual_m).setZero(); + else make_vector(actual_y, actual_m) *= beta; } - - MatrixType mat_coeffs(a,coeff_rows,*n,*lda); - + + ConstMatrixType mat_coeffs(a,coeff_rows,*n,*lda); + int nb = std::min(*n,(*m)+(*ku)); for(int j=0; j(pa); Scalar* x = reinterpret_cast(px); int coeff_rows = *k + 1; - + int info = 0; if(UPLO(*uplo)==INVALID) info = 1; else if(OP(*opa)==INVALID) info = 2; @@ -283,37 +301,37 @@ int EIGEN_BLAS_FUNC(tbmv)(char *uplo, char *opa, char *diag, int *n, int *k, Rea else if(*incx==0) info = 9; if(info) return xerbla_(SCALAR_SUFFIX_UP"TBMV ",&info,6); - + if(*n==0) return 0; - + int actual_n = *n; - + Scalar* actual_x = get_compact_vector(x,actual_n,*incx); - + MatrixType mat_coeffs(a,coeff_rows,*n,*lda); - + int ku = UPLO(*uplo)==UPPER ? *k : 0; int kl = UPLO(*uplo)==LOWER ? *k : 0; - + for(int j=0; j<*n; ++j) { int start = std::max(0,j - ku); int end = std::min((*m)-1,j + kl); int len = end - start + 1; int offset = (ku) - j + start; - + if(OP(*trans)==NOTR) - vector(actual_y+start,len) += (alpha*actual_x[j]) * mat_coeffs.col(j).segment(offset,len); + make_vector(actual_y+start,len) += (alpha*actual_x[j]) * mat_coeffs.col(j).segment(offset,len); else if(OP(*trans)==TR) - actual_y[j] += alpha * ( mat_coeffs.col(j).segment(offset,len).transpose() * vector(actual_x+start,len) ).value(); + actual_y[j] += alpha * ( mat_coeffs.col(j).segment(offset,len).transpose() * make_vector(actual_x+start,len) ).value(); else - actual_y[j] += alpha * ( mat_coeffs.col(j).segment(offset,len).adjoint() * vector(actual_x+start,len) ).value(); - } - + actual_y[j] += alpha * ( mat_coeffs.col(j).segment(offset,len).adjoint() * make_vector(actual_x+start,len) ).value(); + } + if(actual_x!=x) delete[] actual_x; if(actual_y!=y) delete[] copy_back(actual_y,y,actual_m,*incy); - + return 0; } #endif @@ -332,37 +350,41 @@ int EIGEN_BLAS_FUNC(tbmv)(char *uplo, char *opa, char *diag, int *n, int *k, Rea int EIGEN_BLAS_FUNC(tbsv)(char *uplo, char *op, char *diag, int *n, int *k, RealScalar *pa, int *lda, RealScalar *px, int *incx) { typedef void (*functype)(int, int, const Scalar *, int, Scalar *); - static functype func[16]; - - static bool init = false; - if(!init) - { - for(int k=0; k<16; ++k) - func[k] = 0; - - func[NOTR | (UP << 2) | (NUNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[TR | (UP << 2) | (NUNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[ADJ | (UP << 2) | (NUNIT << 3)] = (internal::band_solve_triangular_selector::run); - - func[NOTR | (LO << 2) | (NUNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[TR | (LO << 2) | (NUNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[ADJ | (LO << 2) | (NUNIT << 3)] = (internal::band_solve_triangular_selector::run); - - func[NOTR | (UP << 2) | (UNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[TR | (UP << 2) | (UNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[ADJ | (UP << 2) | (UNIT << 3)] = (internal::band_solve_triangular_selector::run); - - func[NOTR | (LO << 2) | (UNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[TR | (LO << 2) | (UNIT << 3)] = (internal::band_solve_triangular_selector::run); - func[ADJ | (LO << 2) | (UNIT << 3)] = (internal::band_solve_triangular_selector::run); - - init = true; - } + static const functype func[16] = { + // array index: NOTR | (UP << 2) | (NUNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: TR | (UP << 2) | (NUNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: ADJ | (UP << 2) | (NUNIT << 3) + (internal::band_solve_triangular_selector::run), + 0, + // array index: NOTR | (LO << 2) | (NUNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: TR | (LO << 2) | (NUNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: ADJ | (LO << 2) | (NUNIT << 3) + (internal::band_solve_triangular_selector::run), + 0, + // array index: NOTR | (UP << 2) | (UNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: TR | (UP << 2) | (UNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: ADJ | (UP << 2) | (UNIT << 3) + (internal::band_solve_triangular_selector::run), + 0, + // array index: NOTR | (LO << 2) | (UNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: TR | (LO << 2) | (UNIT << 3) + (internal::band_solve_triangular_selector::run), + // array index: ADJ | (LO << 2) | (UNIT << 3) + (internal::band_solve_triangular_selector::run), + 0, + }; Scalar* a = reinterpret_cast(pa); Scalar* x = reinterpret_cast(px); int coeff_rows = *k+1; - + int info = 0; if(UPLO(*uplo)==INVALID) info = 1; else if(OP(*op)==INVALID) info = 2; @@ -373,22 +395,22 @@ int EIGEN_BLAS_FUNC(tbsv)(char *uplo, char *op, char *diag, int *n, int *k, Real else if(*incx==0) info = 9; if(info) return xerbla_(SCALAR_SUFFIX_UP"TBSV ",&info,6); - + if(*n==0 || (*k==0 && DIAG(*diag)==UNIT)) return 0; - + int actual_n = *n; - + Scalar* actual_x = get_compact_vector(x,actual_n,*incx); - + int code = OP(*op) | (UPLO(*uplo) << 2) | (DIAG(*diag) << 3); if(code>=16 || func[code]==0) return 0; func[code](*n, *k, a, *lda, actual_x); - + if(actual_x!=x) delete[] copy_back(actual_x,x,actual_n,*incx); - + return 0; } @@ -402,32 +424,36 @@ int EIGEN_BLAS_FUNC(tbsv)(char *uplo, char *op, char *diag, int *n, int *k, Real int EIGEN_BLAS_FUNC(tpmv)(char *uplo, char *opa, char *diag, int *n, RealScalar *pap, RealScalar *px, int *incx) { typedef void (*functype)(int, const Scalar*, const Scalar*, Scalar*, Scalar); - static functype func[16]; - - static bool init = false; - if(!init) - { - for(int k=0; k<16; ++k) - func[k] = 0; - - func[NOTR | (UP << 2) | (NUNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[TR | (UP << 2) | (NUNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[ADJ | (UP << 2) | (NUNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - - func[NOTR | (LO << 2) | (NUNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[TR | (LO << 2) | (NUNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[ADJ | (LO << 2) | (NUNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - - func[NOTR | (UP << 2) | (UNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[TR | (UP << 2) | (UNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[ADJ | (UP << 2) | (UNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - - func[NOTR | (LO << 2) | (UNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[TR | (LO << 2) | (UNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - func[ADJ | (LO << 2) | (UNIT << 3)] = (internal::packed_triangular_matrix_vector_product::run); - - init = true; - } + static const functype func[16] = { + // array index: NOTR | (UP << 2) | (NUNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: TR | (UP << 2) | (NUNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: ADJ | (UP << 2) | (NUNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + 0, + // array index: NOTR | (LO << 2) | (NUNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: TR | (LO << 2) | (NUNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: ADJ | (LO << 2) | (NUNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + 0, + // array index: NOTR | (UP << 2) | (UNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: TR | (UP << 2) | (UNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: ADJ | (UP << 2) | (UNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + 0, + // array index: NOTR | (LO << 2) | (UNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: TR | (LO << 2) | (UNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + // array index: ADJ | (LO << 2) | (UNIT << 3) + (internal::packed_triangular_matrix_vector_product::run), + 0 + }; Scalar* ap = reinterpret_cast(pap); Scalar* x = reinterpret_cast(px); @@ -473,32 +499,36 @@ int EIGEN_BLAS_FUNC(tpmv)(char *uplo, char *opa, char *diag, int *n, RealScalar int EIGEN_BLAS_FUNC(tpsv)(char *uplo, char *opa, char *diag, int *n, RealScalar *pap, RealScalar *px, int *incx) { typedef void (*functype)(int, const Scalar*, Scalar*); - static functype func[16]; - - static bool init = false; - if(!init) - { - for(int k=0; k<16; ++k) - func[k] = 0; - - func[NOTR | (UP << 2) | (NUNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[TR | (UP << 2) | (NUNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[ADJ | (UP << 2) | (NUNIT << 3)] = (internal::packed_triangular_solve_vector::run); - - func[NOTR | (LO << 2) | (NUNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[TR | (LO << 2) | (NUNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[ADJ | (LO << 2) | (NUNIT << 3)] = (internal::packed_triangular_solve_vector::run); - - func[NOTR | (UP << 2) | (UNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[TR | (UP << 2) | (UNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[ADJ | (UP << 2) | (UNIT << 3)] = (internal::packed_triangular_solve_vector::run); - - func[NOTR | (LO << 2) | (UNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[TR | (LO << 2) | (UNIT << 3)] = (internal::packed_triangular_solve_vector::run); - func[ADJ | (LO << 2) | (UNIT << 3)] = (internal::packed_triangular_solve_vector::run); - - init = true; - } + static const functype func[16] = { + // array index: NOTR | (UP << 2) | (NUNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: TR | (UP << 2) | (NUNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: ADJ | (UP << 2) | (NUNIT << 3) + (internal::packed_triangular_solve_vector::run), + 0, + // array index: NOTR | (LO << 2) | (NUNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: TR | (LO << 2) | (NUNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: ADJ | (LO << 2) | (NUNIT << 3) + (internal::packed_triangular_solve_vector::run), + 0, + // array index: NOTR | (UP << 2) | (UNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: TR | (UP << 2) | (UNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: ADJ | (UP << 2) | (UNIT << 3) + (internal::packed_triangular_solve_vector::run), + 0, + // array index: NOTR | (LO << 2) | (UNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: TR | (LO << 2) | (UNIT << 3) + (internal::packed_triangular_solve_vector::run), + // array index: ADJ | (LO << 2) | (UNIT << 3) + (internal::packed_triangular_solve_vector::run), + 0 + }; Scalar* ap = reinterpret_cast(pap); Scalar* x = reinterpret_cast(px); @@ -521,4 +551,3 @@ int EIGEN_BLAS_FUNC(tpsv)(char *uplo, char *opa, char *diag, int *n, RealScalar return 1; } - diff --git a/ground/gcs/src/libs/eigen/blas/level2_real_impl.h b/ground/gcs/src/libs/eigen/blas/level2_real_impl.h index 8d56eaaa14..7620f0a389 100644 --- a/ground/gcs/src/libs/eigen/blas/level2_real_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level2_real_impl.h @@ -10,28 +10,22 @@ #include "common.h" // y = alpha*A*x + beta*y -int EIGEN_BLAS_FUNC(symv) (char *uplo, int *n, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *px, int *incx, RealScalar *pbeta, RealScalar *py, int *incy) +int EIGEN_BLAS_FUNC(symv) (const char *uplo, const int *n, const RealScalar *palpha, const RealScalar *pa, const int *lda, + const RealScalar *px, const int *incx, const RealScalar *pbeta, RealScalar *py, const int *incy) { - typedef void (*functype)(int, const Scalar*, int, const Scalar*, int, Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::selfadjoint_matrix_vector_product::run); - func[LO] = (internal::selfadjoint_matrix_vector_product::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); - Scalar* x = reinterpret_cast(px); + typedef void (*functype)(int, const Scalar*, int, const Scalar*, Scalar*, Scalar); + static const functype func[2] = { + // array index: UP + (internal::selfadjoint_matrix_vector_product::run), + // array index: LO + (internal::selfadjoint_matrix_vector_product::run), + }; + + const Scalar* a = reinterpret_cast(pa); + const Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); // check arguments int info = 0; @@ -46,20 +40,20 @@ int EIGEN_BLAS_FUNC(symv) (char *uplo, int *n, RealScalar *palpha, RealScalar *p if(*n==0) return 0; - Scalar* actual_x = get_compact_vector(x,*n,*incx); + const Scalar* actual_x = get_compact_vector(x,*n,*incx); Scalar* actual_y = get_compact_vector(y,*n,*incy); if(beta!=Scalar(1)) { - if(beta==Scalar(0)) vector(actual_y, *n).setZero(); - else vector(actual_y, *n) *= beta; + if(beta==Scalar(0)) make_vector(actual_y, *n).setZero(); + else make_vector(actual_y, *n) *= beta; } int code = UPLO(*uplo); if(code>=2 || func[code]==0) return 0; - func[code](*n, a, *lda, actual_x, 1, actual_y, alpha); + func[code](*n, a, *lda, actual_x, actual_y, alpha); if(actual_x!=x) delete[] actual_x; if(actual_y!=y) delete[] copy_back(actual_y,y,*n,*incy); @@ -68,41 +62,20 @@ int EIGEN_BLAS_FUNC(symv) (char *uplo, int *n, RealScalar *palpha, RealScalar *p } // C := alpha*x*x' + C -int EIGEN_BLAS_FUNC(syr)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(syr)(const char *uplo, const int *n, const RealScalar *palpha, const RealScalar *px, const int *incx, RealScalar *pc, const int *ldc) { -// typedef void (*functype)(int, const Scalar *, int, Scalar *, int, Scalar); -// static functype func[2]; - -// static bool init = false; -// if(!init) -// { -// for(int k=0; k<2; ++k) -// func[k] = 0; -// -// func[UP] = (internal::selfadjoint_product::run); -// func[LO] = (internal::selfadjoint_product::run); - -// init = true; -// } typedef void (*functype)(int, Scalar*, int, const Scalar*, const Scalar*, const Scalar&); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (selfadjoint_rank1_update::run); - func[LO] = (selfadjoint_rank1_update::run); - - init = true; - } - - Scalar* x = reinterpret_cast(px); + static const functype func[2] = { + // array index: UP + (selfadjoint_rank1_update::run), + // array index: LO + (selfadjoint_rank1_update::run), + }; + + const Scalar* x = reinterpret_cast(px); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); + Scalar alpha = *reinterpret_cast(palpha); int info = 0; if(UPLO(*uplo)==INVALID) info = 1; @@ -115,7 +88,7 @@ int EIGEN_BLAS_FUNC(syr)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, if(*n==0 || alpha==Scalar(0)) return 1; // if the increment is not 1, let's copy it to a temporary vector to enable vectorization - Scalar* x_cpy = get_compact_vector(x,*n,*incx); + const Scalar* x_cpy = get_compact_vector(x,*n,*incx); int code = UPLO(*uplo); if(code>=2 || func[code]==0) @@ -129,41 +102,20 @@ int EIGEN_BLAS_FUNC(syr)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, } // C := alpha*x*y' + alpha*y*x' + C -int EIGEN_BLAS_FUNC(syr2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(syr2)(const char *uplo, const int *n, const RealScalar *palpha, const RealScalar *px, const int *incx, const RealScalar *py, const int *incy, RealScalar *pc, const int *ldc) { -// typedef void (*functype)(int, const Scalar *, int, const Scalar *, int, Scalar *, int, Scalar); -// static functype func[2]; -// -// static bool init = false; -// if(!init) -// { -// for(int k=0; k<2; ++k) -// func[k] = 0; -// -// func[UP] = (internal::selfadjoint_product::run); -// func[LO] = (internal::selfadjoint_product::run); -// -// init = true; -// } typedef void (*functype)(int, Scalar*, int, const Scalar*, const Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::rank2_update_selector::run); - func[LO] = (internal::rank2_update_selector::run); - - init = true; - } - - Scalar* x = reinterpret_cast(px); - Scalar* y = reinterpret_cast(py); + static const functype func[2] = { + // array index: UP + (internal::rank2_update_selector::run), + // array index: LO + (internal::rank2_update_selector::run), + }; + + const Scalar* x = reinterpret_cast(px); + const Scalar* y = reinterpret_cast(py); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); + Scalar alpha = *reinterpret_cast(palpha); int info = 0; if(UPLO(*uplo)==INVALID) info = 1; @@ -177,9 +129,9 @@ int EIGEN_BLAS_FUNC(syr2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px if(alpha==Scalar(0)) return 1; - Scalar* x_cpy = get_compact_vector(x,*n,*incx); - Scalar* y_cpy = get_compact_vector(y,*n,*incy); - + const Scalar* x_cpy = get_compact_vector(x,*n,*incx); + const Scalar* y_cpy = get_compact_vector(y,*n,*incy); + int code = UPLO(*uplo); if(code>=2 || func[code]==0) return 0; @@ -234,19 +186,12 @@ int EIGEN_BLAS_FUNC(syr2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px int EIGEN_BLAS_FUNC(spr)(char *uplo, int *n, Scalar *palpha, Scalar *px, int *incx, Scalar *pap) { typedef void (*functype)(int, Scalar*, const Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::selfadjoint_packed_rank1_update::run); - func[LO] = (internal::selfadjoint_packed_rank1_update::run); - - init = true; - } + static const functype func[2] = { + // array index: UP + (internal::selfadjoint_packed_rank1_update::run), + // array index: LO + (internal::selfadjoint_packed_rank1_update::run), + }; Scalar* x = reinterpret_cast(px); Scalar* ap = reinterpret_cast(pap); @@ -285,19 +230,12 @@ int EIGEN_BLAS_FUNC(spr)(char *uplo, int *n, Scalar *palpha, Scalar *px, int *in int EIGEN_BLAS_FUNC(spr2)(char *uplo, int *n, RealScalar *palpha, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pap) { typedef void (*functype)(int, Scalar*, const Scalar*, const Scalar*, Scalar); - static functype func[2]; - - static bool init = false; - if(!init) - { - for(int k=0; k<2; ++k) - func[k] = 0; - - func[UP] = (internal::packed_rank2_update_selector::run); - func[LO] = (internal::packed_rank2_update_selector::run); - - init = true; - } + static const functype func[2] = { + // array index: UP + (internal::packed_rank2_update_selector::run), + // array index: LO + (internal::packed_rank2_update_selector::run), + }; Scalar* x = reinterpret_cast(px); Scalar* y = reinterpret_cast(py); @@ -366,5 +304,3 @@ int EIGEN_BLAS_FUNC(ger)(int *m, int *n, Scalar *palpha, Scalar *px, int *incx, return 1; } - - diff --git a/ground/gcs/src/libs/eigen/blas/level3_impl.h b/ground/gcs/src/libs/eigen/blas/level3_impl.h index 07dbc22ff2..6c802cd5fd 100644 --- a/ground/gcs/src/libs/eigen/blas/level3_impl.h +++ b/ground/gcs/src/libs/eigen/blas/level3_impl.h @@ -6,37 +6,43 @@ // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - +#include #include "common.h" -int EIGEN_BLAS_FUNC(gemm)(char *opa, char *opb, int *m, int *n, int *k, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *ldb, RealScalar *pbeta, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(gemm)(const char *opa, const char *opb, const int *m, const int *n, const int *k, const RealScalar *palpha, + const RealScalar *pa, const int *lda, const RealScalar *pb, const int *ldb, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { // std::cerr << "in gemm " << *opa << " " << *opb << " " << *m << " " << *n << " " << *k << " " << *lda << " " << *ldb << " " << *ldc << " " << *palpha << " " << *pbeta << "\n"; typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, Scalar, internal::level3_blocking&, Eigen::internal::GemmParallelInfo*); - static functype func[12]; - - static bool init = false; - if(!init) - { - for(int k=0; k<12; ++k) - func[k] = 0; - func[NOTR | (NOTR << 2)] = (internal::general_matrix_matrix_product::run); - func[TR | (NOTR << 2)] = (internal::general_matrix_matrix_product::run); - func[ADJ | (NOTR << 2)] = (internal::general_matrix_matrix_product::run); - func[NOTR | (TR << 2)] = (internal::general_matrix_matrix_product::run); - func[TR | (TR << 2)] = (internal::general_matrix_matrix_product::run); - func[ADJ | (TR << 2)] = (internal::general_matrix_matrix_product::run); - func[NOTR | (ADJ << 2)] = (internal::general_matrix_matrix_product::run); - func[TR | (ADJ << 2)] = (internal::general_matrix_matrix_product::run); - func[ADJ | (ADJ << 2)] = (internal::general_matrix_matrix_product::run); - init = true; - } - - Scalar* a = reinterpret_cast(pa); - Scalar* b = reinterpret_cast(pb); + static const functype func[12] = { + // array index: NOTR | (NOTR << 2) + (internal::general_matrix_matrix_product::run), + // array index: TR | (NOTR << 2) + (internal::general_matrix_matrix_product::run), + // array index: ADJ | (NOTR << 2) + (internal::general_matrix_matrix_product::run), + 0, + // array index: NOTR | (TR << 2) + (internal::general_matrix_matrix_product::run), + // array index: TR | (TR << 2) + (internal::general_matrix_matrix_product::run), + // array index: ADJ | (TR << 2) + (internal::general_matrix_matrix_product::run), + 0, + // array index: NOTR | (ADJ << 2) + (internal::general_matrix_matrix_product::run), + // array index: TR | (ADJ << 2) + (internal::general_matrix_matrix_product::run), + // array index: ADJ | (ADJ << 2) + (internal::general_matrix_matrix_product::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); + const Scalar* b = reinterpret_cast(pb); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); int info = 0; if(OP(*opa)==INVALID) info = 1; @@ -50,70 +56,92 @@ int EIGEN_BLAS_FUNC(gemm)(char *opa, char *opb, int *m, int *n, int *k, RealScal if(info) return xerbla_(SCALAR_SUFFIX_UP"GEMM ",&info,6); + if (*m == 0 || *n == 0) + return 0; + if(beta!=Scalar(1)) { if(beta==Scalar(0)) matrix(c, *m, *n, *ldc).setZero(); else matrix(c, *m, *n, *ldc) *= beta; } - internal::gemm_blocking_space blocking(*m,*n,*k); + if(*k == 0) + return 0; + + internal::gemm_blocking_space blocking(*m,*n,*k,1,true); int code = OP(*opa) | (OP(*opb) << 2); func[code](*m, *n, *k, a, *lda, b, *ldb, c, *ldc, alpha, blocking, 0); return 0; } -int EIGEN_BLAS_FUNC(trsm)(char *side, char *uplo, char *opa, char *diag, int *m, int *n, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *ldb) +int EIGEN_BLAS_FUNC(trsm)(const char *side, const char *uplo, const char *opa, const char *diag, const int *m, const int *n, + const RealScalar *palpha, const RealScalar *pa, const int *lda, RealScalar *pb, const int *ldb) { // std::cerr << "in trsm " << *side << " " << *uplo << " " << *opa << " " << *diag << " " << *m << "," << *n << " " << *palpha << " " << *lda << " " << *ldb<< "\n"; typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, internal::level3_blocking&); - static functype func[32]; - - static bool init = false; - if(!init) - { - for(int k=0; k<32; ++k) - func[k] = 0; - - func[NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - - func[NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - - func[NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - - func[NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::triangular_solve_matrix::run); - - - func[NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (LEFT << 2) | (UP << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - - func[NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - - func[NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (LEFT << 2) | (LO << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - - func[NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - func[ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4)] = (internal::triangular_solve_matrix::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); + static const functype func[32] = { + // array index: NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run),\ + 0, + // array index: NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + 0, + // array index: NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) + (internal::triangular_solve_matrix::run), + 0, + // array index: NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (LEFT << 2) | (UP << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + 0, + // array index: NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (LEFT << 2) | (LO << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + // array index: ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4) + (internal::triangular_solve_matrix::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); Scalar* b = reinterpret_cast(pb); - Scalar alpha = *reinterpret_cast(palpha); + Scalar alpha = *reinterpret_cast(palpha); int info = 0; if(SIDE(*side)==INVALID) info = 1; @@ -127,16 +155,19 @@ int EIGEN_BLAS_FUNC(trsm)(char *side, char *uplo, char *opa, char *diag, int *m, if(info) return xerbla_(SCALAR_SUFFIX_UP"TRSM ",&info,6); + if(*m==0 || *n==0) + return 0; + int code = OP(*opa) | (SIDE(*side) << 2) | (UPLO(*uplo) << 3) | (DIAG(*diag) << 4); - + if(SIDE(*side)==LEFT) { - internal::gemm_blocking_space blocking(*m,*n,*m); + internal::gemm_blocking_space blocking(*m,*n,*m,1,false); func[code](*m, *n, a, *lda, b, *ldb, blocking); } else { - internal::gemm_blocking_space blocking(*m,*n,*n); + internal::gemm_blocking_space blocking(*m,*n,*n,1,false); func[code](*n, *m, a, *lda, b, *ldb, blocking); } @@ -149,55 +180,73 @@ int EIGEN_BLAS_FUNC(trsm)(char *side, char *uplo, char *opa, char *diag, int *m, // b = alpha*op(a)*b for side = 'L'or'l' // b = alpha*b*op(a) for side = 'R'or'r' -int EIGEN_BLAS_FUNC(trmm)(char *side, char *uplo, char *opa, char *diag, int *m, int *n, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *ldb) +int EIGEN_BLAS_FUNC(trmm)(const char *side, const char *uplo, const char *opa, const char *diag, const int *m, const int *n, + const RealScalar *palpha, const RealScalar *pa, const int *lda, RealScalar *pb, const int *ldb) { // std::cerr << "in trmm " << *side << " " << *uplo << " " << *opa << " " << *diag << " " << *m << " " << *n << " " << *lda << " " << *ldb << " " << *palpha << "\n"; typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); - static functype func[32]; - static bool init = false; - if(!init) - { - for(int k=0; k<32; ++k) - func[k] = 0; - - func[NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (LEFT << 2) | (UP << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (LEFT << 2) | (LO << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - func[NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - func[ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4)] = (internal::product_triangular_matrix_matrix::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); + static const functype func[32] = { + // array index: NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (LEFT << 2) | (UP << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (LEFT << 2) | (LO << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0, + // array index: NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + // array index: ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4) + (internal::product_triangular_matrix_matrix::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); Scalar* b = reinterpret_cast(pb); - Scalar alpha = *reinterpret_cast(palpha); + Scalar alpha = *reinterpret_cast(palpha); int info = 0; if(SIDE(*side)==INVALID) info = 1; @@ -222,12 +271,12 @@ int EIGEN_BLAS_FUNC(trmm)(char *side, char *uplo, char *opa, char *diag, int *m, if(SIDE(*side)==LEFT) { - internal::gemm_blocking_space blocking(*m,*n,*m); + internal::gemm_blocking_space blocking(*m,*n,*m,1,false); func[code](*m, *n, *m, a, *lda, tmp.data(), tmp.outerStride(), b, *ldb, alpha, blocking); } else { - internal::gemm_blocking_space blocking(*m,*n,*n); + internal::gemm_blocking_space blocking(*m,*n,*n,1,false); func[code](*m, *n, *n, tmp.data(), tmp.outerStride(), a, *lda, b, *ldb, alpha, blocking); } return 1; @@ -235,14 +284,15 @@ int EIGEN_BLAS_FUNC(trmm)(char *side, char *uplo, char *opa, char *diag, int *m, // c = alpha*a*b + beta*c for side = 'L'or'l' // c = alpha*b*a + beta*c for side = 'R'or'r -int EIGEN_BLAS_FUNC(symm)(char *side, char *uplo, int *m, int *n, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *ldb, RealScalar *pbeta, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(symm)(const char *side, const char *uplo, const int *m, const int *n, const RealScalar *palpha, + const RealScalar *pa, const int *lda, const RealScalar *pb, const int *ldb, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { // std::cerr << "in symm " << *side << " " << *uplo << " " << *m << "x" << *n << " lda:" << *lda << " ldb:" << *ldb << " ldc:" << *ldc << " alpha:" << *palpha << " beta:" << *pbeta << "\n"; - Scalar* a = reinterpret_cast(pa); - Scalar* b = reinterpret_cast(pb); + const Scalar* a = reinterpret_cast(pa); + const Scalar* b = reinterpret_cast(pb); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); int info = 0; if(SIDE(*side)==INVALID) info = 1; @@ -266,9 +316,9 @@ int EIGEN_BLAS_FUNC(symm)(char *side, char *uplo, int *m, int *n, RealScalar *pa return 1; } + int size = (SIDE(*side)==LEFT) ? (*m) : (*n); #if ISCOMPLEX // FIXME add support for symmetric complex matrix - int size = (SIDE(*side)==LEFT) ? (*m) : (*n); Matrix matA(size,size); if(UPLO(*uplo)==UP) { @@ -285,13 +335,15 @@ int EIGEN_BLAS_FUNC(symm)(char *side, char *uplo, int *m, int *n, RealScalar *pa else if(SIDE(*side)==RIGHT) matrix(c, *m, *n, *ldc) += alpha * matrix(b, *m, *n, *ldb) * matA; #else + internal::gemm_blocking_space blocking(*m,*n,size,1,false); + if(SIDE(*side)==LEFT) - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); else return 0; else if(SIDE(*side)==RIGHT) - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); else return 0; else return 0; @@ -302,39 +354,38 @@ int EIGEN_BLAS_FUNC(symm)(char *side, char *uplo, int *m, int *n, RealScalar *pa // c = alpha*a*a' + beta*c for op = 'N'or'n' // c = alpha*a'*a + beta*c for op = 'T'or't','C'or'c' -int EIGEN_BLAS_FUNC(syrk)(char *uplo, char *op, int *n, int *k, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pbeta, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(syrk)(const char *uplo, const char *op, const int *n, const int *k, + const RealScalar *palpha, const RealScalar *pa, const int *lda, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { // std::cerr << "in syrk " << *uplo << " " << *op << " " << *n << " " << *k << " " << *palpha << " " << *lda << " " << *pbeta << " " << *ldc << "\n"; #if !ISCOMPLEX - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&); - static functype func[8]; - - static bool init = false; - if(!init) - { - for(int k=0; k<8; ++k) - func[k] = 0; - - func[NOTR | (UP << 2)] = (internal::general_matrix_matrix_triangular_product::run); - func[TR | (UP << 2)] = (internal::general_matrix_matrix_triangular_product::run); - func[ADJ | (UP << 2)] = (internal::general_matrix_matrix_triangular_product::run); - - func[NOTR | (LO << 2)] = (internal::general_matrix_matrix_triangular_product::run); - func[TR | (LO << 2)] = (internal::general_matrix_matrix_triangular_product::run); - func[ADJ | (LO << 2)] = (internal::general_matrix_matrix_triangular_product::run); - - init = true; - } + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); + static const functype func[8] = { + // array index: NOTR | (UP << 2) + (internal::general_matrix_matrix_triangular_product::run), + // array index: TR | (UP << 2) + (internal::general_matrix_matrix_triangular_product::run), + // array index: ADJ | (UP << 2) + (internal::general_matrix_matrix_triangular_product::run), + 0, + // array index: NOTR | (LO << 2) + (internal::general_matrix_matrix_triangular_product::run), + // array index: TR | (LO << 2) + (internal::general_matrix_matrix_triangular_product::run), + // array index: ADJ | (LO << 2) + (internal::general_matrix_matrix_triangular_product::run), + 0 + }; #endif - Scalar* a = reinterpret_cast(pa); + const Scalar* a = reinterpret_cast(pa); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); int info = 0; if(UPLO(*uplo)==INVALID) info = 1; - else if(OP(*op)==INVALID) info = 2; + else if(OP(*op)==INVALID || (ISCOMPLEX && OP(*op)==ADJ) ) info = 2; else if(*n<0) info = 3; else if(*k<0) info = 4; else if(*lda() *= beta; } + if(*n==0 || *k==0) + return 0; + #if ISCOMPLEX // FIXME add support for symmetric complex matrix if(UPLO(*uplo)==UP) @@ -369,8 +423,10 @@ int EIGEN_BLAS_FUNC(syrk)(char *uplo, char *op, int *n, int *k, RealScalar *palp matrix(c, *n, *n, *ldc).triangularView() += alpha * matrix(a,*k,*n,*lda).transpose() * matrix(a,*k,*n,*lda); } #else + internal::gemm_blocking_space blocking(*n,*n,*k,1,false); + int code = OP(*op) | (UPLO(*uplo) << 2); - func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha); + func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha, blocking); #endif return 0; @@ -378,17 +434,20 @@ int EIGEN_BLAS_FUNC(syrk)(char *uplo, char *op, int *n, int *k, RealScalar *palp // c = alpha*a*b' + alpha*b*a' + beta*c for op = 'N'or'n' // c = alpha*a'*b + alpha*b'*a + beta*c for op = 'T'or't' -int EIGEN_BLAS_FUNC(syr2k)(char *uplo, char *op, int *n, int *k, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *ldb, RealScalar *pbeta, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(syr2k)(const char *uplo, const char *op, const int *n, const int *k, const RealScalar *palpha, + const RealScalar *pa, const int *lda, const RealScalar *pb, const int *ldb, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { - Scalar* a = reinterpret_cast(pa); - Scalar* b = reinterpret_cast(pb); + const Scalar* a = reinterpret_cast(pa); + const Scalar* b = reinterpret_cast(pb); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); + +// std::cerr << "in syr2k " << *uplo << " " << *op << " " << *n << " " << *k << " " << alpha << " " << *lda << " " << *ldb << " " << beta << " " << *ldc << "\n"; int info = 0; if(UPLO(*uplo)==INVALID) info = 1; - else if(OP(*op)==INVALID) info = 2; + else if(OP(*op)==INVALID || (ISCOMPLEX && OP(*op)==ADJ) ) info = 2; else if(*n<0) info = 3; else if(*k<0) info = 4; else if(*lda(pa); - Scalar* b = reinterpret_cast(pb); + const Scalar* a = reinterpret_cast(pa); + const Scalar* b = reinterpret_cast(pb); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); - Scalar beta = *reinterpret_cast(pbeta); + Scalar alpha = *reinterpret_cast(palpha); + Scalar beta = *reinterpret_cast(pbeta); // std::cerr << "in hemm " << *side << " " << *uplo << " " << *m << " " << *n << " " << alpha << " " << *lda << " " << beta << " " << *ldc << "\n"; @@ -472,20 +532,23 @@ int EIGEN_BLAS_FUNC(hemm)(char *side, char *uplo, int *m, int *n, RealScalar *pa return 1; } + int size = (SIDE(*side)==LEFT) ? (*m) : (*n); + internal::gemm_blocking_space blocking(*m,*n,size,1,false); + if(SIDE(*side)==LEFT) { if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix - ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha); + ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix - ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha); + ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); else return 0; } else if(SIDE(*side)==RIGHT) { if(UPLO(*uplo)==UP) matrix(c,*m,*n,*ldc) += alpha * matrix(b,*m,*n,*ldb) * matrix(a,*n,*n,*lda).selfadjointView();/*internal::product_selfadjoint_matrix - ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha);*/ + ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking);*/ else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix - ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha); + ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); else return 0; } else @@ -498,27 +561,28 @@ int EIGEN_BLAS_FUNC(hemm)(char *side, char *uplo, int *m, int *n, RealScalar *pa // c = alpha*a*conj(a') + beta*c for op = 'N'or'n' // c = alpha*conj(a')*a + beta*c for op = 'C'or'c' -int EIGEN_BLAS_FUNC(herk)(char *uplo, char *op, int *n, int *k, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pbeta, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(herk)(const char *uplo, const char *op, const int *n, const int *k, + const RealScalar *palpha, const RealScalar *pa, const int *lda, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&); - static functype func[8]; - - static bool init = false; - if(!init) - { - for(int k=0; k<8; ++k) - func[k] = 0; - - func[NOTR | (UP << 2)] = (internal::general_matrix_matrix_triangular_product::run); - func[ADJ | (UP << 2)] = (internal::general_matrix_matrix_triangular_product::run); - - func[NOTR | (LO << 2)] = (internal::general_matrix_matrix_triangular_product::run); - func[ADJ | (LO << 2)] = (internal::general_matrix_matrix_triangular_product::run); - - init = true; - } - - Scalar* a = reinterpret_cast(pa); +// std::cerr << "in herk " << *uplo << " " << *op << " " << *n << " " << *k << " " << *palpha << " " << *lda << " " << *pbeta << " " << *ldc << "\n"; + + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); + static const functype func[8] = { + // array index: NOTR | (UP << 2) + (internal::general_matrix_matrix_triangular_product::run), + 0, + // array index: ADJ | (UP << 2) + (internal::general_matrix_matrix_triangular_product::run), + 0, + // array index: NOTR | (LO << 2) + (internal::general_matrix_matrix_triangular_product::run), + 0, + // array index: ADJ | (LO << 2) + (internal::general_matrix_matrix_triangular_product::run), + 0 + }; + + const Scalar* a = reinterpret_cast(pa); Scalar* c = reinterpret_cast(pc); RealScalar alpha = *palpha; RealScalar beta = *pbeta; @@ -545,7 +609,7 @@ int EIGEN_BLAS_FUNC(herk)(char *uplo, char *op, int *n, int *k, RealScalar *palp else if(beta==Scalar(0)) matrix(c, *n, *n, *ldc).triangularView().setZero(); else matrix(c, *n, *n, *ldc).triangularView() *= beta; - + if(beta!=Scalar(0)) { matrix(c, *n, *n, *ldc).diagonal().real() *= beta; @@ -555,7 +619,8 @@ int EIGEN_BLAS_FUNC(herk)(char *uplo, char *op, int *n, int *k, RealScalar *palp if(*k>0 && alpha!=RealScalar(0)) { - func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha); + internal::gemm_blocking_space blocking(*n,*n,*k,1,false); + func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha, blocking); matrix(c, *n, *n, *ldc).diagonal().imag().setZero(); } return 0; @@ -563,21 +628,24 @@ int EIGEN_BLAS_FUNC(herk)(char *uplo, char *op, int *n, int *k, RealScalar *palp // c = alpha*a*conj(b') + conj(alpha)*b*conj(a') + beta*c, for op = 'N'or'n' // c = alpha*conj(a')*b + conj(alpha)*conj(b')*a + beta*c, for op = 'C'or'c' -int EIGEN_BLAS_FUNC(her2k)(char *uplo, char *op, int *n, int *k, RealScalar *palpha, RealScalar *pa, int *lda, RealScalar *pb, int *ldb, RealScalar *pbeta, RealScalar *pc, int *ldc) +int EIGEN_BLAS_FUNC(her2k)(const char *uplo, const char *op, const int *n, const int *k, + const RealScalar *palpha, const RealScalar *pa, const int *lda, const RealScalar *pb, const int *ldb, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { - Scalar* a = reinterpret_cast(pa); - Scalar* b = reinterpret_cast(pb); + const Scalar* a = reinterpret_cast(pa); + const Scalar* b = reinterpret_cast(pb); Scalar* c = reinterpret_cast(pc); - Scalar alpha = *reinterpret_cast(palpha); + Scalar alpha = *reinterpret_cast(palpha); RealScalar beta = *pbeta; +// std::cerr << "in her2k " << *uplo << " " << *op << " " << *n << " " << *k << " " << alpha << " " << *lda << " " << *ldb << " " << beta << " " << *ldc << "\n"; + int info = 0; if(UPLO(*uplo)==INVALID) info = 1; else if((OP(*op)==INVALID) || (OP(*op)==TR)) info = 2; else if(*n<0) info = 3; else if(*k<0) info = 4; else if(*lda \brief \b CBLAT1 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM CBLAT1 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the COMPLEX Level 1 BLAS. +*> Based upon the original BLAS test routine together with: +*> +*> F06GAF Example Program Text +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup complex_blas_testing +* +* ===================================================================== PROGRAM CBLAT1 -* Test program for the COMPLEX Level 1 BLAS. -* Based upon the original BLAS test routine together with: -* F06GAF Example Program Text +* +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 +* +* ===================================================================== +* * .. Parameters .. INTEGER NOUT PARAMETER (NOUT=6) @@ -114,8 +156,8 @@ SUBROUTINE CHECK1(SFAC) + (5.0E0,6.0E0), (5.0E0,6.0E0), (0.1E0,0.1E0), + (-0.6E0,0.1E0), (0.1E0,-0.3E0), (7.0E0,8.0E0), + (7.0E0,8.0E0), (7.0E0,8.0E0), (7.0E0,8.0E0), - + (7.0E0,8.0E0), (0.3E0,0.1E0), (0.1E0,0.4E0), - + (0.4E0,0.1E0), (0.1E0,0.2E0), (2.0E0,3.0E0), + + (7.0E0,8.0E0), (0.3E0,0.1E0), (0.5E0,0.0E0), + + (0.0E0,0.5E0), (0.0E0,0.2E0), (2.0E0,3.0E0), + (2.0E0,3.0E0), (2.0E0,3.0E0), (2.0E0,3.0E0)/ DATA ((CV(I,J,2),I=1,8),J=1,5)/(0.1E0,0.1E0), + (4.0E0,5.0E0), (4.0E0,5.0E0), (4.0E0,5.0E0), @@ -129,10 +171,10 @@ SUBROUTINE CHECK1(SFAC) + (3.0E0,6.0E0), (-0.6E0,0.1E0), (4.0E0,7.0E0), + (0.1E0,-0.3E0), (7.0E0,2.0E0), (7.0E0,2.0E0), + (7.0E0,2.0E0), (0.3E0,0.1E0), (5.0E0,8.0E0), - + (0.1E0,0.4E0), (6.0E0,9.0E0), (0.4E0,0.1E0), - + (8.0E0,3.0E0), (0.1E0,0.2E0), (9.0E0,4.0E0)/ - DATA STRUE2/0.0E0, 0.5E0, 0.6E0, 0.7E0, 0.7E0/ - DATA STRUE4/0.0E0, 0.7E0, 1.0E0, 1.3E0, 1.7E0/ + + (0.5E0,0.0E0), (6.0E0,9.0E0), (0.0E0,0.5E0), + + (8.0E0,3.0E0), (0.0E0,0.2E0), (9.0E0,4.0E0)/ + DATA STRUE2/0.0E0, 0.5E0, 0.6E0, 0.7E0, 0.8E0/ + DATA STRUE4/0.0E0, 0.7E0, 1.0E0, 1.3E0, 1.6E0/ DATA ((CTRUE5(I,J,1),I=1,8),J=1,5)/(0.1E0,0.1E0), + (1.0E0,2.0E0), (1.0E0,2.0E0), (1.0E0,2.0E0), + (1.0E0,2.0E0), (1.0E0,2.0E0), (1.0E0,2.0E0), @@ -145,8 +187,8 @@ SUBROUTINE CHECK1(SFAC) + (0.11E0,-0.03E0), (-0.17E0,0.46E0), + (-0.17E0,-0.19E0), (7.0E0,8.0E0), (7.0E0,8.0E0), + (7.0E0,8.0E0), (7.0E0,8.0E0), (7.0E0,8.0E0), - + (0.19E0,-0.17E0), (0.32E0,0.09E0), - + (0.23E0,-0.24E0), (0.18E0,0.01E0), + + (0.19E0,-0.17E0), (0.20E0,-0.35E0), + + (0.35E0,0.20E0), (0.14E0,0.08E0), + (2.0E0,3.0E0), (2.0E0,3.0E0), (2.0E0,3.0E0), + (2.0E0,3.0E0)/ DATA ((CTRUE5(I,J,2),I=1,8),J=1,5)/(0.1E0,0.1E0), @@ -162,9 +204,9 @@ SUBROUTINE CHECK1(SFAC) + (-0.17E0,0.46E0), (4.0E0,7.0E0), + (-0.17E0,-0.19E0), (7.0E0,2.0E0), (7.0E0,2.0E0), + (7.0E0,2.0E0), (0.19E0,-0.17E0), (5.0E0,8.0E0), - + (0.32E0,0.09E0), (6.0E0,9.0E0), - + (0.23E0,-0.24E0), (8.0E0,3.0E0), - + (0.18E0,0.01E0), (9.0E0,4.0E0)/ + + (0.20E0,-0.35E0), (6.0E0,9.0E0), + + (0.35E0,0.20E0), (8.0E0,3.0E0), + + (0.14E0,0.08E0), (9.0E0,4.0E0)/ DATA ((CTRUE6(I,J,1),I=1,8),J=1,5)/(0.1E0,0.1E0), + (1.0E0,2.0E0), (1.0E0,2.0E0), (1.0E0,2.0E0), + (1.0E0,2.0E0), (1.0E0,2.0E0), (1.0E0,2.0E0), @@ -177,8 +219,8 @@ SUBROUTINE CHECK1(SFAC) + (0.03E0,0.03E0), (-0.18E0,0.03E0), + (0.03E0,-0.09E0), (7.0E0,8.0E0), (7.0E0,8.0E0), + (7.0E0,8.0E0), (7.0E0,8.0E0), (7.0E0,8.0E0), - + (0.09E0,0.03E0), (0.03E0,0.12E0), - + (0.12E0,0.03E0), (0.03E0,0.06E0), (2.0E0,3.0E0), + + (0.09E0,0.03E0), (0.15E0,0.00E0), + + (0.00E0,0.15E0), (0.00E0,0.06E0), (2.0E0,3.0E0), + (2.0E0,3.0E0), (2.0E0,3.0E0), (2.0E0,3.0E0)/ DATA ((CTRUE6(I,J,2),I=1,8),J=1,5)/(0.1E0,0.1E0), + (4.0E0,5.0E0), (4.0E0,5.0E0), (4.0E0,5.0E0), @@ -193,8 +235,8 @@ SUBROUTINE CHECK1(SFAC) + (-0.18E0,0.03E0), (4.0E0,7.0E0), + (0.03E0,-0.09E0), (7.0E0,2.0E0), (7.0E0,2.0E0), + (7.0E0,2.0E0), (0.09E0,0.03E0), (5.0E0,8.0E0), - + (0.03E0,0.12E0), (6.0E0,9.0E0), (0.12E0,0.03E0), - + (8.0E0,3.0E0), (0.03E0,0.06E0), (9.0E0,4.0E0)/ + + (0.15E0,0.00E0), (6.0E0,9.0E0), (0.00E0,0.15E0), + + (8.0E0,3.0E0), (0.00E0,0.06E0), (9.0E0,4.0E0)/ DATA ITRUE3/0, 1, 2, 2, 2/ * .. Executable Statements .. DO 60 INCX = 1, 2 @@ -529,7 +571,8 @@ SUBROUTINE STEST(LEN,SCOMP,STRUE,SSIZE,SFAC) * * .. Parameters .. INTEGER NOUT - PARAMETER (NOUT=6) + REAL ZERO + PARAMETER (NOUT=6, ZERO=0.0E0) * .. Scalar Arguments .. REAL SFAC INTEGER LEN @@ -552,7 +595,7 @@ SUBROUTINE STEST(LEN,SCOMP,STRUE,SSIZE,SFAC) * DO 40 I = 1, LEN SD = SCOMP(I) - STRUE(I) - IF (SDIFF(ABS(SSIZE(I))+ABS(SFAC*SD),ABS(SSIZE(I))).EQ.0.0E0) + IF (ABS(SFAC*SD) .LE. ABS(SSIZE(I))*EPSILON(ZERO)) + GO TO 40 * * HERE SCOMP(I) IS NOT CLOSE TO STRUE(I). diff --git a/ground/gcs/src/libs/eigen/blas/testing/cblat2.f b/ground/gcs/src/libs/eigen/blas/testing/cblat2.f index 20f1881005..5833ea81ae 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/cblat2.f +++ b/ground/gcs/src/libs/eigen/blas/testing/cblat2.f @@ -1,68 +1,114 @@ +*> \brief \b CBLAT2 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM CBLAT2 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the COMPLEX Level 2 Blas. +*> +*> The program must be driven by a short data file. The first 18 records +*> of the file are read using list-directed input, the last 17 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 35 lines: +*> 'cblat2.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'CBLA2T.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 4 NUMBER OF VALUES OF K +*> 0 1 2 4 VALUES OF K +*> 4 NUMBER OF VALUES OF INCX AND INCY +*> 1 2 -1 -2 VALUES OF INCX AND INCY +*> 3 NUMBER OF VALUES OF ALPHA +*> (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA +*> CGEMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CGBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CHEMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CHBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CHPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CTRMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CTBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CTPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> CTRSV T PUT F FOR NO TEST. SAME COLUMNS. +*> CTBSV T PUT F FOR NO TEST. SAME COLUMNS. +*> CTPSV T PUT F FOR NO TEST. SAME COLUMNS. +*> CGERC T PUT F FOR NO TEST. SAME COLUMNS. +*> CGERU T PUT F FOR NO TEST. SAME COLUMNS. +*> CHER T PUT F FOR NO TEST. SAME COLUMNS. +*> CHPR T PUT F FOR NO TEST. SAME COLUMNS. +*> CHER2 T PUT F FOR NO TEST. SAME COLUMNS. +*> CHPR2 T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. +*> An extended set of Fortran Basic Linear Algebra Subprograms. +*> +*> Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics +*> and Computer Science Division, Argonne National Laboratory, +*> 9700 South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> Or +*> +*> NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms +*> Group Ltd., NAG Central Office, 256 Banbury Road, Oxford +*> OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st +*> Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +*> +*> +*> -- Written on 10-August-1987. +*> Richard Hanson, Sandia National Labs. +*> Jeremy Du Croz, NAG Central Office. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup complex_blas_testing +* +* ===================================================================== PROGRAM CBLAT2 * -* Test program for the COMPLEX Level 2 Blas. -* -* The program must be driven by a short data file. The first 18 records -* of the file are read using list-directed input, the last 17 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 35 lines: -* 'CBLAT2.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'CBLA2T.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 4 NUMBER OF VALUES OF K -* 0 1 2 4 VALUES OF K -* 4 NUMBER OF VALUES OF INCX AND INCY -* 1 2 -1 -2 VALUES OF INCX AND INCY -* 3 NUMBER OF VALUES OF ALPHA -* (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA -* CGEMV T PUT F FOR NO TEST. SAME COLUMNS. -* CGBMV T PUT F FOR NO TEST. SAME COLUMNS. -* CHEMV T PUT F FOR NO TEST. SAME COLUMNS. -* CHBMV T PUT F FOR NO TEST. SAME COLUMNS. -* CHPMV T PUT F FOR NO TEST. SAME COLUMNS. -* CTRMV T PUT F FOR NO TEST. SAME COLUMNS. -* CTBMV T PUT F FOR NO TEST. SAME COLUMNS. -* CTPMV T PUT F FOR NO TEST. SAME COLUMNS. -* CTRSV T PUT F FOR NO TEST. SAME COLUMNS. -* CTBSV T PUT F FOR NO TEST. SAME COLUMNS. -* CTPSV T PUT F FOR NO TEST. SAME COLUMNS. -* CGERC T PUT F FOR NO TEST. SAME COLUMNS. -* CGERU T PUT F FOR NO TEST. SAME COLUMNS. -* CHER T PUT F FOR NO TEST. SAME COLUMNS. -* CHPR T PUT F FOR NO TEST. SAME COLUMNS. -* CHER2 T PUT F FOR NO TEST. SAME COLUMNS. -* CHPR2 T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. -* An extended set of Fortran Basic Linear Algebra Subprograms. -* -* Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics -* and Computer Science Division, Argonne National Laboratory, -* 9700 South Cass Avenue, Argonne, Illinois 60439, US. -* -* Or -* -* NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms -* Group Ltd., NAG Central Office, 256 Banbury Road, Oxford -* OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st -* Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -* -- Written on 10-August-1987. -* Richard Hanson, Sandia National Labs. -* Jeremy Du Croz, NAG Central Office. +* ===================================================================== * * .. Parameters .. INTEGER NIN @@ -71,8 +117,8 @@ PROGRAM CBLAT2 PARAMETER ( NSUBS = 17 ) COMPLEX ZERO, ONE PARAMETER ( ZERO = ( 0.0, 0.0 ), ONE = ( 1.0, 0.0 ) ) - REAL RZERO, RHALF, RONE - PARAMETER ( RZERO = 0.0, RHALF = 0.5, RONE = 1.0 ) + REAL RZERO + PARAMETER ( RZERO = 0.0 ) INTEGER NMAX, INCMAX PARAMETER ( NMAX = 65, INCMAX = 2 ) INTEGER NINMAX, NIDMAX, NKBMAX, NALMAX, NBEMAX @@ -126,7 +172,7 @@ PROGRAM CBLAT2 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY, STATUS = 'UNKNOWN' ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -135,7 +181,7 @@ PROGRAM CBLAT2 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS, STATUS = 'UNKNOWN' ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -240,14 +286,7 @@ PROGRAM CBLAT2 * * Compute EPS (the machine precision). * - EPS = RONE - 90 CONTINUE - IF( SDIFF( RONE + EPS, RONE ).EQ.RZERO ) - $ GO TO 100 - EPS = RHALF*EPS - GO TO 90 - 100 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(RZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of CMVCH using exact data. @@ -3079,7 +3118,6 @@ LOGICAL FUNCTION LCERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LCERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/cblat3.f b/ground/gcs/src/libs/eigen/blas/testing/cblat3.f index b26be91e6b..09f2cb9c5c 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/cblat3.f +++ b/ground/gcs/src/libs/eigen/blas/testing/cblat3.f @@ -1,50 +1,96 @@ +*> \brief \b CBLAT3 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM CBLAT3 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the COMPLEX Level 3 Blas. +*> +*> The program must be driven by a short data file. The first 14 records +*> of the file are read using list-directed input, the last 9 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 23 lines: +*> 'cblat3.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'CBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 3 NUMBER OF VALUES OF ALPHA +*> (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA +*> CGEMM T PUT F FOR NO TEST. SAME COLUMNS. +*> CHEMM T PUT F FOR NO TEST. SAME COLUMNS. +*> CSYMM T PUT F FOR NO TEST. SAME COLUMNS. +*> CTRMM T PUT F FOR NO TEST. SAME COLUMNS. +*> CTRSM T PUT F FOR NO TEST. SAME COLUMNS. +*> CHERK T PUT F FOR NO TEST. SAME COLUMNS. +*> CSYRK T PUT F FOR NO TEST. SAME COLUMNS. +*> CHER2K T PUT F FOR NO TEST. SAME COLUMNS. +*> CSYR2K T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. +*> A Set of Level 3 Basic Linear Algebra Subprograms. +*> +*> Technical Memorandum No.88 (Revision 1), Mathematics and +*> Computer Science Division, Argonne National Laboratory, 9700 +*> South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> -- Written on 8-February-1989. +*> Jack Dongarra, Argonne National Laboratory. +*> Iain Duff, AERE Harwell. +*> Jeremy Du Croz, Numerical Algorithms Group Ltd. +*> Sven Hammarling, Numerical Algorithms Group Ltd. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup complex_blas_testing +* +* ===================================================================== PROGRAM CBLAT3 * -* Test program for the COMPLEX Level 3 Blas. -* -* The program must be driven by a short data file. The first 14 records -* of the file are read using list-directed input, the last 9 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 23 lines: -* 'CBLAT3.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'CBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 3 NUMBER OF VALUES OF ALPHA -* (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA -* CGEMM T PUT F FOR NO TEST. SAME COLUMNS. -* CHEMM T PUT F FOR NO TEST. SAME COLUMNS. -* CSYMM T PUT F FOR NO TEST. SAME COLUMNS. -* CTRMM T PUT F FOR NO TEST. SAME COLUMNS. -* CTRSM T PUT F FOR NO TEST. SAME COLUMNS. -* CHERK T PUT F FOR NO TEST. SAME COLUMNS. -* CSYRK T PUT F FOR NO TEST. SAME COLUMNS. -* CHER2K T PUT F FOR NO TEST. SAME COLUMNS. -* CSYR2K T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. -* A Set of Level 3 Basic Linear Algebra Subprograms. -* -* Technical Memorandum No.88 (Revision 1), Mathematics and -* Computer Science Division, Argonne National Laboratory, 9700 -* South Cass Avenue, Argonne, Illinois 60439, US. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -- Written on 8-February-1989. -* Jack Dongarra, Argonne National Laboratory. -* Iain Duff, AERE Harwell. -* Jeremy Du Croz, Numerical Algorithms Group Ltd. -* Sven Hammarling, Numerical Algorithms Group Ltd. +* ===================================================================== * * .. Parameters .. INTEGER NIN @@ -53,8 +99,8 @@ PROGRAM CBLAT3 PARAMETER ( NSUBS = 9 ) COMPLEX ZERO, ONE PARAMETER ( ZERO = ( 0.0, 0.0 ), ONE = ( 1.0, 0.0 ) ) - REAL RZERO, RHALF, RONE - PARAMETER ( RZERO = 0.0, RHALF = 0.5, RONE = 1.0 ) + REAL RZERO + PARAMETER ( RZERO = 0.0 ) INTEGER NMAX PARAMETER ( NMAX = 65 ) INTEGER NIDMAX, NALMAX, NBEMAX @@ -103,7 +149,7 @@ PROGRAM CBLAT3 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -112,7 +158,7 @@ PROGRAM CBLAT3 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -189,14 +235,7 @@ PROGRAM CBLAT3 * * Compute EPS (the machine precision). * - EPS = RONE - 70 CONTINUE - IF( SDIFF( RONE + EPS, RONE ).EQ.RZERO ) - $ GO TO 80 - EPS = RHALF*EPS - GO TO 70 - 80 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(RZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of CMMCH using exact data. @@ -1946,7 +1985,7 @@ SUBROUTINE CCHKE( ISNUM, SRNAMT, NOUT ) * * Tests the error exits from the Level 3 Blas. * Requires a special version of the error-handling routine XERBLA. -* ALPHA, RALPHA, BETA, RBETA, A, B and C should not need to be defined. +* A, B and C should not need to be defined. * * Auxiliary routine for test program for Level 3 Blas. * @@ -1956,12 +1995,19 @@ SUBROUTINE CCHKE( ISNUM, SRNAMT, NOUT ) * Jeremy Du Croz, Numerical Algorithms Group Ltd. * Sven Hammarling, Numerical Algorithms Group Ltd. * +* 3-19-92: Initialize ALPHA, BETA, RALPHA, and RBETA (eca) +* 3-19-92: Fix argument 12 in calls to CSYMM and CHEMM +* with INFOT = 9 (eca) +* * .. Scalar Arguments .. INTEGER ISNUM, NOUT CHARACTER*6 SRNAMT * .. Scalars in Common .. INTEGER INFOT, NOUTC LOGICAL LERR, OK +* .. Parameters .. + REAL ONE, TWO + PARAMETER ( ONE = 1.0E0, TWO = 2.0E0 ) * .. Local Scalars .. COMPLEX ALPHA, BETA REAL RALPHA, RBETA @@ -1979,6 +2025,14 @@ SUBROUTINE CCHKE( ISNUM, SRNAMT, NOUT ) * LERR is set to .TRUE. by the special version of XERBLA each time * it is called, and is then tested and re-set by CHKXER. LERR = .FALSE. +* +* Initialize ALPHA, BETA, RALPHA, and RBETA. +* + ALPHA = CMPLX( ONE, -ONE ) + BETA = CMPLX( TWO, -TWO ) + RALPHA = ONE + RBETA = TWO +* GO TO ( 10, 20, 30, 40, 50, 60, 70, 80, $ 90 )ISNUM 10 INFOT = 1 @@ -2205,16 +2259,16 @@ SUBROUTINE CCHKE( ISNUM, SRNAMT, NOUT ) CALL CHEMM( 'R', 'L', 0, 2, ALPHA, A, 1, B, 1, BETA, C, 1 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CHEMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL CHEMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CHEMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL CHEMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CHEMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL CHEMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CHEMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL CHEMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 12 CALL CHEMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 2, BETA, C, 1 ) @@ -2272,16 +2326,16 @@ SUBROUTINE CCHKE( ISNUM, SRNAMT, NOUT ) CALL CSYMM( 'R', 'L', 0, 2, ALPHA, A, 1, B, 1, BETA, C, 1 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL CSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL CSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL CSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL CSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL CSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 12 CALL CSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 2, BETA, C, 1 ) @@ -3268,7 +3322,6 @@ LOGICAL FUNCTION LCERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LCERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/dblat2.f b/ground/gcs/src/libs/eigen/blas/testing/dblat2.f index 4002d43689..0fa80afa4d 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/dblat2.f +++ b/ground/gcs/src/libs/eigen/blas/testing/dblat2.f @@ -1,75 +1,121 @@ +*> \brief \b DBLAT2 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM DBLAT2 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the DOUBLE PRECISION Level 2 Blas. +*> +*> The program must be driven by a short data file. The first 18 records +*> of the file are read using list-directed input, the last 16 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 34 lines: +*> 'dblat2.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'DBLAT2.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 4 NUMBER OF VALUES OF K +*> 0 1 2 4 VALUES OF K +*> 4 NUMBER OF VALUES OF INCX AND INCY +*> 1 2 -1 -2 VALUES OF INCX AND INCY +*> 3 NUMBER OF VALUES OF ALPHA +*> 0.0 1.0 0.7 VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> 0.0 1.0 0.9 VALUES OF BETAC +*> DGEMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DGBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DSYMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DSBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DSPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DTRMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DTBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DTPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> DTRSV T PUT F FOR NO TEST. SAME COLUMNS. +*> DTBSV T PUT F FOR NO TEST. SAME COLUMNS. +*> DTPSV T PUT F FOR NO TEST. SAME COLUMNS. +*> DGER T PUT F FOR NO TEST. SAME COLUMNS. +*> DSYR T PUT F FOR NO TEST. SAME COLUMNS. +*> DSPR T PUT F FOR NO TEST. SAME COLUMNS. +*> DSYR2 T PUT F FOR NO TEST. SAME COLUMNS. +*> DSPR2 T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. +*> An extended set of Fortran Basic Linear Algebra Subprograms. +*> +*> Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics +*> and Computer Science Division, Argonne National Laboratory, +*> 9700 South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> Or +*> +*> NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms +*> Group Ltd., NAG Central Office, 256 Banbury Road, Oxford +*> OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st +*> Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +*> +*> +*> -- Written on 10-August-1987. +*> Richard Hanson, Sandia National Labs. +*> Jeremy Du Croz, NAG Central Office. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup double_blas_testing +* +* ===================================================================== PROGRAM DBLAT2 * -* Test program for the DOUBLE PRECISION Level 2 Blas. -* -* The program must be driven by a short data file. The first 18 records -* of the file are read using list-directed input, the last 16 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 34 lines: -* 'DBLAT2.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'DBLAT2.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 4 NUMBER OF VALUES OF K -* 0 1 2 4 VALUES OF K -* 4 NUMBER OF VALUES OF INCX AND INCY -* 1 2 -1 -2 VALUES OF INCX AND INCY -* 3 NUMBER OF VALUES OF ALPHA -* 0.0 1.0 0.7 VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* 0.0 1.0 0.9 VALUES OF BETA -* DGEMV T PUT F FOR NO TEST. SAME COLUMNS. -* DGBMV T PUT F FOR NO TEST. SAME COLUMNS. -* DSYMV T PUT F FOR NO TEST. SAME COLUMNS. -* DSBMV T PUT F FOR NO TEST. SAME COLUMNS. -* DSPMV T PUT F FOR NO TEST. SAME COLUMNS. -* DTRMV T PUT F FOR NO TEST. SAME COLUMNS. -* DTBMV T PUT F FOR NO TEST. SAME COLUMNS. -* DTPMV T PUT F FOR NO TEST. SAME COLUMNS. -* DTRSV T PUT F FOR NO TEST. SAME COLUMNS. -* DTBSV T PUT F FOR NO TEST. SAME COLUMNS. -* DTPSV T PUT F FOR NO TEST. SAME COLUMNS. -* DGER T PUT F FOR NO TEST. SAME COLUMNS. -* DSYR T PUT F FOR NO TEST. SAME COLUMNS. -* DSPR T PUT F FOR NO TEST. SAME COLUMNS. -* DSYR2 T PUT F FOR NO TEST. SAME COLUMNS. -* DSPR2 T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. -* An extended set of Fortran Basic Linear Algebra Subprograms. -* -* Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics -* and Computer Science Division, Argonne National Laboratory, -* 9700 South Cass Avenue, Argonne, Illinois 60439, US. -* -* Or -* -* NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms -* Group Ltd., NAG Central Office, 256 Banbury Road, Oxford -* OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st -* Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -* -- Written on 10-August-1987. -* Richard Hanson, Sandia National Labs. -* Jeremy Du Croz, NAG Central Office. +* ===================================================================== * * .. Parameters .. INTEGER NIN PARAMETER ( NIN = 5 ) INTEGER NSUBS PARAMETER ( NSUBS = 16 ) - DOUBLE PRECISION ZERO, HALF, ONE - PARAMETER ( ZERO = 0.0D0, HALF = 0.5D0, ONE = 1.0D0 ) + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) INTEGER NMAX, INCMAX PARAMETER ( NMAX = 65, INCMAX = 2 ) INTEGER NINMAX, NIDMAX, NKBMAX, NALMAX, NBEMAX @@ -121,7 +167,7 @@ PROGRAM DBLAT2 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY, STATUS = 'UNKNOWN' ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -130,7 +176,7 @@ PROGRAM DBLAT2 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS, STATUS = 'UNKNOWN' ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -235,14 +281,7 @@ PROGRAM DBLAT2 * * Compute EPS (the machine precision). * - EPS = ONE - 90 CONTINUE - IF( DDIFF( ONE + EPS, ONE ).EQ.ZERO ) - $ GO TO 100 - EPS = HALF*EPS - GO TO 90 - 100 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(ZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of DMVCH using exact data. @@ -2982,7 +3021,6 @@ LOGICAL FUNCTION LDERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LDERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/dblat3.f b/ground/gcs/src/libs/eigen/blas/testing/dblat3.f index 082e03e5e2..8d37c74531 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/dblat3.f +++ b/ground/gcs/src/libs/eigen/blas/testing/dblat3.f @@ -1,55 +1,101 @@ +*> \brief \b DBLAT3 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM DBLAT3 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the DOUBLE PRECISION Level 3 Blas. +*> +*> The program must be driven by a short data file. The first 14 records +*> of the file are read using list-directed input, the last 6 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 20 lines: +*> 'dblat3.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'DBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 3 NUMBER OF VALUES OF ALPHA +*> 0.0 1.0 0.7 VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> 0.0 1.0 1.3 VALUES OF BETA +*> DGEMM T PUT F FOR NO TEST. SAME COLUMNS. +*> DSYMM T PUT F FOR NO TEST. SAME COLUMNS. +*> DTRMM T PUT F FOR NO TEST. SAME COLUMNS. +*> DTRSM T PUT F FOR NO TEST. SAME COLUMNS. +*> DSYRK T PUT F FOR NO TEST. SAME COLUMNS. +*> DSYR2K T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. +*> A Set of Level 3 Basic Linear Algebra Subprograms. +*> +*> Technical Memorandum No.88 (Revision 1), Mathematics and +*> Computer Science Division, Argonne National Laboratory, 9700 +*> South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> -- Written on 8-February-1989. +*> Jack Dongarra, Argonne National Laboratory. +*> Iain Duff, AERE Harwell. +*> Jeremy Du Croz, Numerical Algorithms Group Ltd. +*> Sven Hammarling, Numerical Algorithms Group Ltd. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup double_blas_testing +* +* ===================================================================== PROGRAM DBLAT3 * -* Test program for the DOUBLE PRECISION Level 3 Blas. -* -* The program must be driven by a short data file. The first 14 records -* of the file are read using list-directed input, the last 6 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 20 lines: -* 'DBLAT3.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'DBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 3 NUMBER OF VALUES OF ALPHA -* 0.0 1.0 0.7 VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* 0.0 1.0 1.3 VALUES OF BETA -* DGEMM T PUT F FOR NO TEST. SAME COLUMNS. -* DSYMM T PUT F FOR NO TEST. SAME COLUMNS. -* DTRMM T PUT F FOR NO TEST. SAME COLUMNS. -* DTRSM T PUT F FOR NO TEST. SAME COLUMNS. -* DSYRK T PUT F FOR NO TEST. SAME COLUMNS. -* DSYR2K T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. -* A Set of Level 3 Basic Linear Algebra Subprograms. -* -* Technical Memorandum No.88 (Revision 1), Mathematics and -* Computer Science Division, Argonne National Laboratory, 9700 -* South Cass Avenue, Argonne, Illinois 60439, US. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -- Written on 8-February-1989. -* Jack Dongarra, Argonne National Laboratory. -* Iain Duff, AERE Harwell. -* Jeremy Du Croz, Numerical Algorithms Group Ltd. -* Sven Hammarling, Numerical Algorithms Group Ltd. +* ===================================================================== * * .. Parameters .. INTEGER NIN PARAMETER ( NIN = 5 ) INTEGER NSUBS PARAMETER ( NSUBS = 6 ) - DOUBLE PRECISION ZERO, HALF, ONE - PARAMETER ( ZERO = 0.0D0, HALF = 0.5D0, ONE = 1.0D0 ) + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) INTEGER NMAX PARAMETER ( NMAX = 65 ) INTEGER NIDMAX, NALMAX, NBEMAX @@ -96,7 +142,7 @@ PROGRAM DBLAT3 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY, STATUS = 'UNKNOWN' ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -105,7 +151,7 @@ PROGRAM DBLAT3 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS, STATUS = 'UNKNOWN' ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -182,14 +228,7 @@ PROGRAM DBLAT3 * * Compute EPS (the machine precision). * - EPS = ONE - 70 CONTINUE - IF( DDIFF( ONE + EPS, ONE ).EQ.ZERO ) - $ GO TO 80 - EPS = HALF*EPS - GO TO 70 - 80 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(ZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of DMMCH using exact data. @@ -1802,7 +1841,7 @@ SUBROUTINE DCHKE( ISNUM, SRNAMT, NOUT ) * * Tests the error exits from the Level 3 Blas. * Requires a special version of the error-handling routine XERBLA. -* ALPHA, BETA, A, B and C should not need to be defined. +* A, B and C should not need to be defined. * * Auxiliary routine for test program for Level 3 Blas. * @@ -1812,12 +1851,18 @@ SUBROUTINE DCHKE( ISNUM, SRNAMT, NOUT ) * Jeremy Du Croz, Numerical Algorithms Group Ltd. * Sven Hammarling, Numerical Algorithms Group Ltd. * +* 3-19-92: Initialize ALPHA and BETA (eca) +* 3-19-92: Fix argument 12 in calls to SSYMM with INFOT = 9 (eca) +* * .. Scalar Arguments .. INTEGER ISNUM, NOUT CHARACTER*6 SRNAMT * .. Scalars in Common .. INTEGER INFOT, NOUTC LOGICAL LERR, OK +* .. Parameters .. + DOUBLE PRECISION ONE, TWO + PARAMETER ( ONE = 1.0D0, TWO = 2.0D0 ) * .. Local Scalars .. DOUBLE PRECISION ALPHA, BETA * .. Local Arrays .. @@ -1834,6 +1879,12 @@ SUBROUTINE DCHKE( ISNUM, SRNAMT, NOUT ) * LERR is set to .TRUE. by the special version of XERBLA each time * it is called, and is then tested and re-set by CHKXER. LERR = .FALSE. +* +* Initialize ALPHA and BETA. +* + ALPHA = ONE + BETA = TWO +* GO TO ( 10, 20, 30, 40, 50, 60 )ISNUM 10 INFOT = 1 CALL DGEMM( '/', 'N', 0, 0, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) @@ -1963,16 +2014,16 @@ SUBROUTINE DCHKE( ISNUM, SRNAMT, NOUT ) CALL DSYMM( 'R', 'L', 0, 2, ALPHA, A, 1, B, 1, BETA, C, 1 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL DSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL DSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL DSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL DSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL DSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL DSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL DSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL DSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 12 CALL DSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 2, BETA, C, 1 ) @@ -2660,7 +2711,6 @@ LOGICAL FUNCTION LDERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LDERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/sblat2.f b/ground/gcs/src/libs/eigen/blas/testing/sblat2.f index 057a85429a..71605ed312 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/sblat2.f +++ b/ground/gcs/src/libs/eigen/blas/testing/sblat2.f @@ -1,75 +1,121 @@ +*> \brief \b SBLAT2 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM SBLAT2 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the REAL Level 2 Blas. +*> +*> The program must be driven by a short data file. The first 18 records +*> of the file are read using list-directed input, the last 16 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 34 lines: +*> 'sblat2.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'SBLAT2.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 4 NUMBER OF VALUES OF K +*> 0 1 2 4 VALUES OF K +*> 4 NUMBER OF VALUES OF INCX AND INCY +*> 1 2 -1 -2 VALUES OF INCX AND INCY +*> 3 NUMBER OF VALUES OF ALPHA +*> 0.0 1.0 0.7 VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> 0.0 1.0 0.9 VALUES OF BETA +*> SGEMV T PUT F FOR NO TEST. SAME COLUMNS. +*> SGBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> SSYMV T PUT F FOR NO TEST. SAME COLUMNS. +*> SSBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> SSPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> STRMV T PUT F FOR NO TEST. SAME COLUMNS. +*> STBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> STPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> STRSV T PUT F FOR NO TEST. SAME COLUMNS. +*> STBSV T PUT F FOR NO TEST. SAME COLUMNS. +*> STPSV T PUT F FOR NO TEST. SAME COLUMNS. +*> SGER T PUT F FOR NO TEST. SAME COLUMNS. +*> SSYR T PUT F FOR NO TEST. SAME COLUMNS. +*> SSPR T PUT F FOR NO TEST. SAME COLUMNS. +*> SSYR2 T PUT F FOR NO TEST. SAME COLUMNS. +*> SSPR2 T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. +*> An extended set of Fortran Basic Linear Algebra Subprograms. +*> +*> Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics +*> and Computer Science Division, Argonne National Laboratory, +*> 9700 South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> Or +*> +*> NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms +*> Group Ltd., NAG Central Office, 256 Banbury Road, Oxford +*> OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st +*> Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +*> +*> +*> -- Written on 10-August-1987. +*> Richard Hanson, Sandia National Labs. +*> Jeremy Du Croz, NAG Central Office. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup single_blas_testing +* +* ===================================================================== PROGRAM SBLAT2 * -* Test program for the REAL Level 2 Blas. -* -* The program must be driven by a short data file. The first 18 records -* of the file are read using list-directed input, the last 16 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 34 lines: -* 'SBLAT2.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'SBLAT2.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 4 NUMBER OF VALUES OF K -* 0 1 2 4 VALUES OF K -* 4 NUMBER OF VALUES OF INCX AND INCY -* 1 2 -1 -2 VALUES OF INCX AND INCY -* 3 NUMBER OF VALUES OF ALPHA -* 0.0 1.0 0.7 VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* 0.0 1.0 0.9 VALUES OF BETA -* SGEMV T PUT F FOR NO TEST. SAME COLUMNS. -* SGBMV T PUT F FOR NO TEST. SAME COLUMNS. -* SSYMV T PUT F FOR NO TEST. SAME COLUMNS. -* SSBMV T PUT F FOR NO TEST. SAME COLUMNS. -* SSPMV T PUT F FOR NO TEST. SAME COLUMNS. -* STRMV T PUT F FOR NO TEST. SAME COLUMNS. -* STBMV T PUT F FOR NO TEST. SAME COLUMNS. -* STPMV T PUT F FOR NO TEST. SAME COLUMNS. -* STRSV T PUT F FOR NO TEST. SAME COLUMNS. -* STBSV T PUT F FOR NO TEST. SAME COLUMNS. -* STPSV T PUT F FOR NO TEST. SAME COLUMNS. -* SGER T PUT F FOR NO TEST. SAME COLUMNS. -* SSYR T PUT F FOR NO TEST. SAME COLUMNS. -* SSPR T PUT F FOR NO TEST. SAME COLUMNS. -* SSYR2 T PUT F FOR NO TEST. SAME COLUMNS. -* SSPR2 T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. -* An extended set of Fortran Basic Linear Algebra Subprograms. -* -* Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics -* and Computer Science Division, Argonne National Laboratory, -* 9700 South Cass Avenue, Argonne, Illinois 60439, US. -* -* Or -* -* NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms -* Group Ltd., NAG Central Office, 256 Banbury Road, Oxford -* OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st -* Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -* -- Written on 10-August-1987. -* Richard Hanson, Sandia National Labs. -* Jeremy Du Croz, NAG Central Office. +* ===================================================================== * * .. Parameters .. INTEGER NIN PARAMETER ( NIN = 5 ) INTEGER NSUBS PARAMETER ( NSUBS = 16 ) - REAL ZERO, HALF, ONE - PARAMETER ( ZERO = 0.0, HALF = 0.5, ONE = 1.0 ) + REAL ZERO, ONE + PARAMETER ( ZERO = 0.0, ONE = 1.0 ) INTEGER NMAX, INCMAX PARAMETER ( NMAX = 65, INCMAX = 2 ) INTEGER NINMAX, NIDMAX, NKBMAX, NALMAX, NBEMAX @@ -121,7 +167,7 @@ PROGRAM SBLAT2 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY, STATUS = 'UNKNOWN' ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -130,7 +176,7 @@ PROGRAM SBLAT2 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS, STATUS = 'UNKNOWN' ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -235,14 +281,7 @@ PROGRAM SBLAT2 * * Compute EPS (the machine precision). * - EPS = ONE - 90 CONTINUE - IF( SDIFF( ONE + EPS, ONE ).EQ.ZERO ) - $ GO TO 100 - EPS = HALF*EPS - GO TO 90 - 100 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(ZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of SMVCH using exact data. @@ -2982,7 +3021,6 @@ LOGICAL FUNCTION LSERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LSERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/sblat3.f b/ground/gcs/src/libs/eigen/blas/testing/sblat3.f index 325a9eb927..8792696337 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/sblat3.f +++ b/ground/gcs/src/libs/eigen/blas/testing/sblat3.f @@ -1,55 +1,101 @@ +*> \brief \b SBLAT3 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM SBLAT3 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the REAL Level 3 Blas. +*> +*> The program must be driven by a short data file. The first 14 records +*> of the file are read using list-directed input, the last 6 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 20 lines: +*> 'sblat3.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'SBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 3 NUMBER OF VALUES OF ALPHA +*> 0.0 1.0 0.7 VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> 0.0 1.0 1.3 VALUES OF BETA +*> SGEMM T PUT F FOR NO TEST. SAME COLUMNS. +*> SSYMM T PUT F FOR NO TEST. SAME COLUMNS. +*> STRMM T PUT F FOR NO TEST. SAME COLUMNS. +*> STRSM T PUT F FOR NO TEST. SAME COLUMNS. +*> SSYRK T PUT F FOR NO TEST. SAME COLUMNS. +*> SSYR2K T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. +*> A Set of Level 3 Basic Linear Algebra Subprograms. +*> +*> Technical Memorandum No.88 (Revision 1), Mathematics and +*> Computer Science Division, Argonne National Laboratory, 9700 +*> South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> -- Written on 8-February-1989. +*> Jack Dongarra, Argonne National Laboratory. +*> Iain Duff, AERE Harwell. +*> Jeremy Du Croz, Numerical Algorithms Group Ltd. +*> Sven Hammarling, Numerical Algorithms Group Ltd. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup single_blas_testing +* +* ===================================================================== PROGRAM SBLAT3 * -* Test program for the REAL Level 3 Blas. -* -* The program must be driven by a short data file. The first 14 records -* of the file are read using list-directed input, the last 6 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 20 lines: -* 'SBLAT3.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'SBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 3 NUMBER OF VALUES OF ALPHA -* 0.0 1.0 0.7 VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* 0.0 1.0 1.3 VALUES OF BETA -* SGEMM T PUT F FOR NO TEST. SAME COLUMNS. -* SSYMM T PUT F FOR NO TEST. SAME COLUMNS. -* STRMM T PUT F FOR NO TEST. SAME COLUMNS. -* STRSM T PUT F FOR NO TEST. SAME COLUMNS. -* SSYRK T PUT F FOR NO TEST. SAME COLUMNS. -* SSYR2K T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. -* A Set of Level 3 Basic Linear Algebra Subprograms. -* -* Technical Memorandum No.88 (Revision 1), Mathematics and -* Computer Science Division, Argonne National Laboratory, 9700 -* South Cass Avenue, Argonne, Illinois 60439, US. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -- Written on 8-February-1989. -* Jack Dongarra, Argonne National Laboratory. -* Iain Duff, AERE Harwell. -* Jeremy Du Croz, Numerical Algorithms Group Ltd. -* Sven Hammarling, Numerical Algorithms Group Ltd. +* ===================================================================== * * .. Parameters .. INTEGER NIN PARAMETER ( NIN = 5 ) INTEGER NSUBS PARAMETER ( NSUBS = 6 ) - REAL ZERO, HALF, ONE - PARAMETER ( ZERO = 0.0, HALF = 0.5, ONE = 1.0 ) + REAL ZERO, ONE + PARAMETER ( ZERO = 0.0, ONE = 1.0 ) INTEGER NMAX PARAMETER ( NMAX = 65 ) INTEGER NIDMAX, NALMAX, NBEMAX @@ -96,7 +142,7 @@ PROGRAM SBLAT3 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -105,7 +151,7 @@ PROGRAM SBLAT3 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -182,14 +228,7 @@ PROGRAM SBLAT3 * * Compute EPS (the machine precision). * - EPS = ONE - 70 CONTINUE - IF( SDIFF( ONE + EPS, ONE ).EQ.ZERO ) - $ GO TO 80 - EPS = HALF*EPS - GO TO 70 - 80 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(ZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of SMMCH using exact data. @@ -1802,7 +1841,7 @@ SUBROUTINE SCHKE( ISNUM, SRNAMT, NOUT ) * * Tests the error exits from the Level 3 Blas. * Requires a special version of the error-handling routine XERBLA. -* ALPHA, BETA, A, B and C should not need to be defined. +* A, B and C should not need to be defined. * * Auxiliary routine for test program for Level 3 Blas. * @@ -1812,12 +1851,18 @@ SUBROUTINE SCHKE( ISNUM, SRNAMT, NOUT ) * Jeremy Du Croz, Numerical Algorithms Group Ltd. * Sven Hammarling, Numerical Algorithms Group Ltd. * +* 3-19-92: Initialize ALPHA and BETA (eca) +* 3-19-92: Fix argument 12 in calls to SSYMM with INFOT = 9 (eca) +* * .. Scalar Arguments .. INTEGER ISNUM, NOUT CHARACTER*6 SRNAMT * .. Scalars in Common .. INTEGER INFOT, NOUTC LOGICAL LERR, OK +* .. Parameters .. + REAL ONE, TWO + PARAMETER ( ONE = 1.0E0, TWO = 2.0E0 ) * .. Local Scalars .. REAL ALPHA, BETA * .. Local Arrays .. @@ -1834,6 +1879,12 @@ SUBROUTINE SCHKE( ISNUM, SRNAMT, NOUT ) * LERR is set to .TRUE. by the special version of XERBLA each time * it is called, and is then tested and re-set by CHKXER. LERR = .FALSE. +* +* Initialize ALPHA and BETA. +* + ALPHA = ONE + BETA = TWO +* GO TO ( 10, 20, 30, 40, 50, 60 )ISNUM 10 INFOT = 1 CALL SGEMM( '/', 'N', 0, 0, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) @@ -1963,16 +2014,16 @@ SUBROUTINE SCHKE( ISNUM, SRNAMT, NOUT ) CALL SSYMM( 'R', 'L', 0, 2, ALPHA, A, 1, B, 1, BETA, C, 1 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL SSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL SSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL SSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL SSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL SSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL SSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL SSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL SSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 12 CALL SSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 2, BETA, C, 1 ) @@ -2660,7 +2711,6 @@ LOGICAL FUNCTION LSERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LSERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/zblat1.f b/ground/gcs/src/libs/eigen/blas/testing/zblat1.f index e2415e1c46..d30112c637 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/zblat1.f +++ b/ground/gcs/src/libs/eigen/blas/testing/zblat1.f @@ -1,7 +1,49 @@ +*> \brief \b ZBLAT1 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM ZBLAT1 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the COMPLEX*16 Level 1 BLAS. +*> +*> Based upon the original BLAS test routine together with: +*> F06GAF Example Program Text +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup complex16_blas_testing +* +* ===================================================================== PROGRAM ZBLAT1 -* Test program for the COMPLEX*16 Level 1 BLAS. -* Based upon the original BLAS test routine together with: -* F06GAF Example Program Text +* +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 +* +* ===================================================================== +* * .. Parameters .. INTEGER NOUT PARAMETER (NOUT=6) @@ -114,8 +156,8 @@ SUBROUTINE CHECK1(SFAC) + (5.0D0,6.0D0), (5.0D0,6.0D0), (0.1D0,0.1D0), + (-0.6D0,0.1D0), (0.1D0,-0.3D0), (7.0D0,8.0D0), + (7.0D0,8.0D0), (7.0D0,8.0D0), (7.0D0,8.0D0), - + (7.0D0,8.0D0), (0.3D0,0.1D0), (0.1D0,0.4D0), - + (0.4D0,0.1D0), (0.1D0,0.2D0), (2.0D0,3.0D0), + + (7.0D0,8.0D0), (0.3D0,0.1D0), (0.5D0,0.0D0), + + (0.0D0,0.5D0), (0.0D0,0.2D0), (2.0D0,3.0D0), + (2.0D0,3.0D0), (2.0D0,3.0D0), (2.0D0,3.0D0)/ DATA ((CV(I,J,2),I=1,8),J=1,5)/(0.1D0,0.1D0), + (4.0D0,5.0D0), (4.0D0,5.0D0), (4.0D0,5.0D0), @@ -129,10 +171,10 @@ SUBROUTINE CHECK1(SFAC) + (3.0D0,6.0D0), (-0.6D0,0.1D0), (4.0D0,7.0D0), + (0.1D0,-0.3D0), (7.0D0,2.0D0), (7.0D0,2.0D0), + (7.0D0,2.0D0), (0.3D0,0.1D0), (5.0D0,8.0D0), - + (0.1D0,0.4D0), (6.0D0,9.0D0), (0.4D0,0.1D0), - + (8.0D0,3.0D0), (0.1D0,0.2D0), (9.0D0,4.0D0)/ - DATA STRUE2/0.0D0, 0.5D0, 0.6D0, 0.7D0, 0.7D0/ - DATA STRUE4/0.0D0, 0.7D0, 1.0D0, 1.3D0, 1.7D0/ + + (0.5D0,0.0D0), (6.0D0,9.0D0), (0.0D0,0.5D0), + + (8.0D0,3.0D0), (0.0D0,0.2D0), (9.0D0,4.0D0)/ + DATA STRUE2/0.0D0, 0.5D0, 0.6D0, 0.7D0, 0.8D0/ + DATA STRUE4/0.0D0, 0.7D0, 1.0D0, 1.3D0, 1.6D0/ DATA ((CTRUE5(I,J,1),I=1,8),J=1,5)/(0.1D0,0.1D0), + (1.0D0,2.0D0), (1.0D0,2.0D0), (1.0D0,2.0D0), + (1.0D0,2.0D0), (1.0D0,2.0D0), (1.0D0,2.0D0), @@ -145,8 +187,8 @@ SUBROUTINE CHECK1(SFAC) + (0.11D0,-0.03D0), (-0.17D0,0.46D0), + (-0.17D0,-0.19D0), (7.0D0,8.0D0), (7.0D0,8.0D0), + (7.0D0,8.0D0), (7.0D0,8.0D0), (7.0D0,8.0D0), - + (0.19D0,-0.17D0), (0.32D0,0.09D0), - + (0.23D0,-0.24D0), (0.18D0,0.01D0), + + (0.19D0,-0.17D0), (0.20D0,-0.35D0), + + (0.35D0,0.20D0), (0.14D0,0.08D0), + (2.0D0,3.0D0), (2.0D0,3.0D0), (2.0D0,3.0D0), + (2.0D0,3.0D0)/ DATA ((CTRUE5(I,J,2),I=1,8),J=1,5)/(0.1D0,0.1D0), @@ -162,9 +204,9 @@ SUBROUTINE CHECK1(SFAC) + (-0.17D0,0.46D0), (4.0D0,7.0D0), + (-0.17D0,-0.19D0), (7.0D0,2.0D0), (7.0D0,2.0D0), + (7.0D0,2.0D0), (0.19D0,-0.17D0), (5.0D0,8.0D0), - + (0.32D0,0.09D0), (6.0D0,9.0D0), - + (0.23D0,-0.24D0), (8.0D0,3.0D0), - + (0.18D0,0.01D0), (9.0D0,4.0D0)/ + + (0.20D0,-0.35D0), (6.0D0,9.0D0), + + (0.35D0,0.20D0), (8.0D0,3.0D0), + + (0.14D0,0.08D0), (9.0D0,4.0D0)/ DATA ((CTRUE6(I,J,1),I=1,8),J=1,5)/(0.1D0,0.1D0), + (1.0D0,2.0D0), (1.0D0,2.0D0), (1.0D0,2.0D0), + (1.0D0,2.0D0), (1.0D0,2.0D0), (1.0D0,2.0D0), @@ -177,8 +219,8 @@ SUBROUTINE CHECK1(SFAC) + (0.03D0,0.03D0), (-0.18D0,0.03D0), + (0.03D0,-0.09D0), (7.0D0,8.0D0), (7.0D0,8.0D0), + (7.0D0,8.0D0), (7.0D0,8.0D0), (7.0D0,8.0D0), - + (0.09D0,0.03D0), (0.03D0,0.12D0), - + (0.12D0,0.03D0), (0.03D0,0.06D0), (2.0D0,3.0D0), + + (0.09D0,0.03D0), (0.15D0,0.00D0), + + (0.00D0,0.15D0), (0.00D0,0.06D0), (2.0D0,3.0D0), + (2.0D0,3.0D0), (2.0D0,3.0D0), (2.0D0,3.0D0)/ DATA ((CTRUE6(I,J,2),I=1,8),J=1,5)/(0.1D0,0.1D0), + (4.0D0,5.0D0), (4.0D0,5.0D0), (4.0D0,5.0D0), @@ -193,8 +235,8 @@ SUBROUTINE CHECK1(SFAC) + (-0.18D0,0.03D0), (4.0D0,7.0D0), + (0.03D0,-0.09D0), (7.0D0,2.0D0), (7.0D0,2.0D0), + (7.0D0,2.0D0), (0.09D0,0.03D0), (5.0D0,8.0D0), - + (0.03D0,0.12D0), (6.0D0,9.0D0), (0.12D0,0.03D0), - + (8.0D0,3.0D0), (0.03D0,0.06D0), (9.0D0,4.0D0)/ + + (0.15D0,0.00D0), (6.0D0,9.0D0), (0.00D0,0.15D0), + + (8.0D0,3.0D0), (0.00D0,0.06D0), (9.0D0,4.0D0)/ DATA ITRUE3/0, 1, 2, 2, 2/ * .. Executable Statements .. DO 60 INCX = 1, 2 @@ -529,7 +571,8 @@ SUBROUTINE STEST(LEN,SCOMP,STRUE,SSIZE,SFAC) * * .. Parameters .. INTEGER NOUT - PARAMETER (NOUT=6) + DOUBLE PRECISION ZERO + PARAMETER (NOUT=6, ZERO=0.0D0) * .. Scalar Arguments .. DOUBLE PRECISION SFAC INTEGER LEN @@ -552,7 +595,7 @@ SUBROUTINE STEST(LEN,SCOMP,STRUE,SSIZE,SFAC) * DO 40 I = 1, LEN SD = SCOMP(I) - STRUE(I) - IF (SDIFF(ABS(SSIZE(I))+ABS(SFAC*SD),ABS(SSIZE(I))).EQ.0.0D0) + IF (ABS(SFAC*SD) .LE. ABS(SSIZE(I))*EPSILON(ZERO)) + GO TO 40 * * HERE SCOMP(I) IS NOT CLOSE TO STRUE(I). diff --git a/ground/gcs/src/libs/eigen/blas/testing/zblat2.f b/ground/gcs/src/libs/eigen/blas/testing/zblat2.f index e65cdcc703..53129a11e9 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/zblat2.f +++ b/ground/gcs/src/libs/eigen/blas/testing/zblat2.f @@ -1,68 +1,114 @@ +*> \brief \b ZBLAT2 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM ZBLAT2 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the COMPLEX*16 Level 2 Blas. +*> +*> The program must be driven by a short data file. The first 18 records +*> of the file are read using list-directed input, the last 17 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 35 lines: +*> 'zblat2.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'CBLA2T.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 4 NUMBER OF VALUES OF K +*> 0 1 2 4 VALUES OF K +*> 4 NUMBER OF VALUES OF INCX AND INCY +*> 1 2 -1 -2 VALUES OF INCX AND INCY +*> 3 NUMBER OF VALUES OF ALPHA +*> (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA +*> ZGEMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZGBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHEMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTRMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTBMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTPMV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTRSV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTBSV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTPSV T PUT F FOR NO TEST. SAME COLUMNS. +*> ZGERC T PUT F FOR NO TEST. SAME COLUMNS. +*> ZGERU T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHER T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHPR T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHER2 T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHPR2 T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. +*> An extended set of Fortran Basic Linear Algebra Subprograms. +*> +*> Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics +*> and Computer Science Division, Argonne National Laboratory, +*> 9700 South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> Or +*> +*> NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms +*> Group Ltd., NAG Central Office, 256 Banbury Road, Oxford +*> OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st +*> Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +*> +*> +*> -- Written on 10-August-1987. +*> Richard Hanson, Sandia National Labs. +*> Jeremy Du Croz, NAG Central Office. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup complex16_blas_testing +* +* ===================================================================== PROGRAM ZBLAT2 * -* Test program for the COMPLEX*16 Level 2 Blas. -* -* The program must be driven by a short data file. The first 18 records -* of the file are read using list-directed input, the last 17 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 35 lines: -* 'ZBLAT2.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'CBLA2T.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 4 NUMBER OF VALUES OF K -* 0 1 2 4 VALUES OF K -* 4 NUMBER OF VALUES OF INCX AND INCY -* 1 2 -1 -2 VALUES OF INCX AND INCY -* 3 NUMBER OF VALUES OF ALPHA -* (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA -* ZGEMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZGBMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZHEMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZHBMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZHPMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZTRMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZTBMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZTPMV T PUT F FOR NO TEST. SAME COLUMNS. -* ZTRSV T PUT F FOR NO TEST. SAME COLUMNS. -* ZTBSV T PUT F FOR NO TEST. SAME COLUMNS. -* ZTPSV T PUT F FOR NO TEST. SAME COLUMNS. -* ZGERC T PUT F FOR NO TEST. SAME COLUMNS. -* ZGERU T PUT F FOR NO TEST. SAME COLUMNS. -* ZHER T PUT F FOR NO TEST. SAME COLUMNS. -* ZHPR T PUT F FOR NO TEST. SAME COLUMNS. -* ZHER2 T PUT F FOR NO TEST. SAME COLUMNS. -* ZHPR2 T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Hammarling S. and Hanson R. J.. -* An extended set of Fortran Basic Linear Algebra Subprograms. -* -* Technical Memoranda Nos. 41 (revision 3) and 81, Mathematics -* and Computer Science Division, Argonne National Laboratory, -* 9700 South Cass Avenue, Argonne, Illinois 60439, US. -* -* Or -* -* NAG Technical Reports TR3/87 and TR4/87, Numerical Algorithms -* Group Ltd., NAG Central Office, 256 Banbury Road, Oxford -* OX2 7DE, UK, and Numerical Algorithms Group Inc., 1101 31st -* Street, Suite 100, Downers Grove, Illinois 60515-1263, USA. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -* -- Written on 10-August-1987. -* Richard Hanson, Sandia National Labs. -* Jeremy Du Croz, NAG Central Office. +* ===================================================================== * * .. Parameters .. INTEGER NIN @@ -72,8 +118,8 @@ PROGRAM ZBLAT2 COMPLEX*16 ZERO, ONE PARAMETER ( ZERO = ( 0.0D0, 0.0D0 ), $ ONE = ( 1.0D0, 0.0D0 ) ) - DOUBLE PRECISION RZERO, RHALF, RONE - PARAMETER ( RZERO = 0.0D0, RHALF = 0.5D0, RONE = 1.0D0 ) + DOUBLE PRECISION RZERO + PARAMETER ( RZERO = 0.0D0 ) INTEGER NMAX, INCMAX PARAMETER ( NMAX = 65, INCMAX = 2 ) INTEGER NINMAX, NIDMAX, NKBMAX, NALMAX, NBEMAX @@ -127,7 +173,7 @@ PROGRAM ZBLAT2 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY, STATUS = 'UNKNOWN' ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -136,7 +182,7 @@ PROGRAM ZBLAT2 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS, STATUS = 'UNKNOWN' ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -241,14 +287,7 @@ PROGRAM ZBLAT2 * * Compute EPS (the machine precision). * - EPS = RONE - 90 CONTINUE - IF( DDIFF( RONE + EPS, RONE ).EQ.RZERO ) - $ GO TO 100 - EPS = RHALF*EPS - GO TO 90 - 100 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(RZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of ZMVCH using exact data. @@ -3087,7 +3126,6 @@ LOGICAL FUNCTION LZERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LZERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/testing/zblat3.f b/ground/gcs/src/libs/eigen/blas/testing/zblat3.f index d6a522f2ae..59ca241456 100644 --- a/ground/gcs/src/libs/eigen/blas/testing/zblat3.f +++ b/ground/gcs/src/libs/eigen/blas/testing/zblat3.f @@ -1,50 +1,97 @@ +*> \brief \b ZBLAT3 +* +* =========== DOCUMENTATION =========== +* +* Online html documentation available at +* http://www.netlib.org/lapack/explore-html/ +* +* Definition: +* =========== +* +* PROGRAM ZBLAT3 +* +* +*> \par Purpose: +* ============= +*> +*> \verbatim +*> +*> Test program for the COMPLEX*16 Level 3 Blas. +*> +*> The program must be driven by a short data file. The first 14 records +*> of the file are read using list-directed input, the last 9 records +*> are read using the format ( A6, L2 ). An annotated example of a data +*> file can be obtained by deleting the first 3 characters from the +*> following 23 lines: +*> 'zblat3.out' NAME OF SUMMARY OUTPUT FILE +*> 6 UNIT NUMBER OF SUMMARY FILE +*> 'ZBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE +*> -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) +*> F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. +*> F LOGICAL FLAG, T TO STOP ON FAILURES. +*> T LOGICAL FLAG, T TO TEST ERROR EXITS. +*> 16.0 THRESHOLD VALUE OF TEST RATIO +*> 6 NUMBER OF VALUES OF N +*> 0 1 2 3 5 9 VALUES OF N +*> 3 NUMBER OF VALUES OF ALPHA +*> (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA +*> 3 NUMBER OF VALUES OF BETA +*> (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA +*> ZGEMM T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHEMM T PUT F FOR NO TEST. SAME COLUMNS. +*> ZSYMM T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTRMM T PUT F FOR NO TEST. SAME COLUMNS. +*> ZTRSM T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHERK T PUT F FOR NO TEST. SAME COLUMNS. +*> ZSYRK T PUT F FOR NO TEST. SAME COLUMNS. +*> ZHER2K T PUT F FOR NO TEST. SAME COLUMNS. +*> ZSYR2K T PUT F FOR NO TEST. SAME COLUMNS. +*> +*> +*> Further Details +*> =============== +*> +*> See: +*> +*> Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. +*> A Set of Level 3 Basic Linear Algebra Subprograms. +*> +*> Technical Memorandum No.88 (Revision 1), Mathematics and +*> Computer Science Division, Argonne National Laboratory, 9700 +*> South Cass Avenue, Argonne, Illinois 60439, US. +*> +*> -- Written on 8-February-1989. +*> Jack Dongarra, Argonne National Laboratory. +*> Iain Duff, AERE Harwell. +*> Jeremy Du Croz, Numerical Algorithms Group Ltd. +*> Sven Hammarling, Numerical Algorithms Group Ltd. +*> +*> 10-9-00: Change STATUS='NEW' to 'UNKNOWN' so that the testers +*> can be run multiple times without deleting generated +*> output files (susan) +*> \endverbatim +* +* Authors: +* ======== +* +*> \author Univ. of Tennessee +*> \author Univ. of California Berkeley +*> \author Univ. of Colorado Denver +*> \author NAG Ltd. +* +*> \date April 2012 +* +*> \ingroup complex16_blas_testing +* +* ===================================================================== PROGRAM ZBLAT3 * -* Test program for the COMPLEX*16 Level 3 Blas. -* -* The program must be driven by a short data file. The first 14 records -* of the file are read using list-directed input, the last 9 records -* are read using the format ( A6, L2 ). An annotated example of a data -* file can be obtained by deleting the first 3 characters from the -* following 23 lines: -* 'ZBLAT3.SUMM' NAME OF SUMMARY OUTPUT FILE -* 6 UNIT NUMBER OF SUMMARY FILE -* 'ZBLAT3.SNAP' NAME OF SNAPSHOT OUTPUT FILE -* -1 UNIT NUMBER OF SNAPSHOT FILE (NOT USED IF .LT. 0) -* F LOGICAL FLAG, T TO REWIND SNAPSHOT FILE AFTER EACH RECORD. -* F LOGICAL FLAG, T TO STOP ON FAILURES. -* T LOGICAL FLAG, T TO TEST ERROR EXITS. -* 16.0 THRESHOLD VALUE OF TEST RATIO -* 6 NUMBER OF VALUES OF N -* 0 1 2 3 5 9 VALUES OF N -* 3 NUMBER OF VALUES OF ALPHA -* (0.0,0.0) (1.0,0.0) (0.7,-0.9) VALUES OF ALPHA -* 3 NUMBER OF VALUES OF BETA -* (0.0,0.0) (1.0,0.0) (1.3,-1.1) VALUES OF BETA -* ZGEMM T PUT F FOR NO TEST. SAME COLUMNS. -* ZHEMM T PUT F FOR NO TEST. SAME COLUMNS. -* ZSYMM T PUT F FOR NO TEST. SAME COLUMNS. -* ZTRMM T PUT F FOR NO TEST. SAME COLUMNS. -* ZTRSM T PUT F FOR NO TEST. SAME COLUMNS. -* ZHERK T PUT F FOR NO TEST. SAME COLUMNS. -* ZSYRK T PUT F FOR NO TEST. SAME COLUMNS. -* ZHER2K T PUT F FOR NO TEST. SAME COLUMNS. -* ZSYR2K T PUT F FOR NO TEST. SAME COLUMNS. -* -* See: -* -* Dongarra J. J., Du Croz J. J., Duff I. S. and Hammarling S. -* A Set of Level 3 Basic Linear Algebra Subprograms. -* -* Technical Memorandum No.88 (Revision 1), Mathematics and -* Computer Science Division, Argonne National Laboratory, 9700 -* South Cass Avenue, Argonne, Illinois 60439, US. +* -- Reference BLAS test routine (version 3.4.1) -- +* -- Reference BLAS is a software package provided by Univ. of Tennessee, -- +* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- +* April 2012 * -* -- Written on 8-February-1989. -* Jack Dongarra, Argonne National Laboratory. -* Iain Duff, AERE Harwell. -* Jeremy Du Croz, Numerical Algorithms Group Ltd. -* Sven Hammarling, Numerical Algorithms Group Ltd. +* ===================================================================== * * .. Parameters .. INTEGER NIN @@ -54,8 +101,8 @@ PROGRAM ZBLAT3 COMPLEX*16 ZERO, ONE PARAMETER ( ZERO = ( 0.0D0, 0.0D0 ), $ ONE = ( 1.0D0, 0.0D0 ) ) - DOUBLE PRECISION RZERO, RHALF, RONE - PARAMETER ( RZERO = 0.0D0, RHALF = 0.5D0, RONE = 1.0D0 ) + DOUBLE PRECISION RZERO + PARAMETER ( RZERO = 0.0D0 ) INTEGER NMAX PARAMETER ( NMAX = 65 ) INTEGER NIDMAX, NALMAX, NBEMAX @@ -104,7 +151,7 @@ PROGRAM ZBLAT3 * READ( NIN, FMT = * )SUMMRY READ( NIN, FMT = * )NOUT - OPEN( NOUT, FILE = SUMMRY, STATUS = 'NEW' ) + OPEN( NOUT, FILE = SUMMRY, STATUS = 'UNKNOWN' ) NOUTC = NOUT * * Read name and unit number for snapshot output file and open file. @@ -113,7 +160,7 @@ PROGRAM ZBLAT3 READ( NIN, FMT = * )NTRA TRACE = NTRA.GE.0 IF( TRACE )THEN - OPEN( NTRA, FILE = SNAPS, STATUS = 'NEW' ) + OPEN( NTRA, FILE = SNAPS, STATUS = 'UNKNOWN' ) END IF * Read the flag that directs rewinding of the snapshot file. READ( NIN, FMT = * )REWI @@ -190,14 +237,7 @@ PROGRAM ZBLAT3 * * Compute EPS (the machine precision). * - EPS = RONE - 70 CONTINUE - IF( DDIFF( RONE + EPS, RONE ).EQ.RZERO ) - $ GO TO 80 - EPS = RHALF*EPS - GO TO 70 - 80 CONTINUE - EPS = EPS + EPS + EPS = EPSILON(RZERO) WRITE( NOUT, FMT = 9998 )EPS * * Check the reliability of ZMMCH using exact data. @@ -1949,7 +1989,7 @@ SUBROUTINE ZCHKE( ISNUM, SRNAMT, NOUT ) * * Tests the error exits from the Level 3 Blas. * Requires a special version of the error-handling routine XERBLA. -* ALPHA, RALPHA, BETA, RBETA, A, B and C should not need to be defined. +* A, B and C should not need to be defined. * * Auxiliary routine for test program for Level 3 Blas. * @@ -1959,12 +1999,20 @@ SUBROUTINE ZCHKE( ISNUM, SRNAMT, NOUT ) * Jeremy Du Croz, Numerical Algorithms Group Ltd. * Sven Hammarling, Numerical Algorithms Group Ltd. * +* 3-19-92: Initialize ALPHA, BETA, RALPHA, and RBETA (eca) +* 3-19-92: Fix argument 12 in calls to ZSYMM and ZHEMM +* with INFOT = 9 (eca) +* 10-9-00: Declared INTRINSIC DCMPLX (susan) +* * .. Scalar Arguments .. INTEGER ISNUM, NOUT CHARACTER*6 SRNAMT * .. Scalars in Common .. INTEGER INFOT, NOUTC LOGICAL LERR, OK +* .. Parameters .. + REAL ONE, TWO + PARAMETER ( ONE = 1.0D0, TWO = 2.0D0 ) * .. Local Scalars .. COMPLEX*16 ALPHA, BETA DOUBLE PRECISION RALPHA, RBETA @@ -1973,6 +2021,8 @@ SUBROUTINE ZCHKE( ISNUM, SRNAMT, NOUT ) * .. External Subroutines .. EXTERNAL ZGEMM, ZHEMM, ZHER2K, ZHERK, CHKXER, ZSYMM, $ ZSYR2K, ZSYRK, ZTRMM, ZTRSM +* .. Intrinsic Functions .. + INTRINSIC DCMPLX * .. Common blocks .. COMMON /INFOC/INFOT, NOUTC, OK, LERR * .. Executable Statements .. @@ -1982,6 +2032,14 @@ SUBROUTINE ZCHKE( ISNUM, SRNAMT, NOUT ) * LERR is set to .TRUE. by the special version of XERBLA each time * it is called, and is then tested and re-set by CHKXER. LERR = .FALSE. +* +* Initialize ALPHA, BETA, RALPHA, and RBETA. +* + ALPHA = DCMPLX( ONE, -ONE ) + BETA = DCMPLX( TWO, -TWO ) + RALPHA = ONE + RBETA = TWO +* GO TO ( 10, 20, 30, 40, 50, 60, 70, 80, $ 90 )ISNUM 10 INFOT = 1 @@ -2208,16 +2266,16 @@ SUBROUTINE ZCHKE( ISNUM, SRNAMT, NOUT ) CALL ZHEMM( 'R', 'L', 0, 2, ALPHA, A, 1, B, 1, BETA, C, 1 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZHEMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL ZHEMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZHEMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL ZHEMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZHEMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL ZHEMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZHEMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL ZHEMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 12 CALL ZHEMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 2, BETA, C, 1 ) @@ -2275,16 +2333,16 @@ SUBROUTINE ZCHKE( ISNUM, SRNAMT, NOUT ) CALL ZSYMM( 'R', 'L', 0, 2, ALPHA, A, 1, B, 1, BETA, C, 1 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL ZSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL ZSYMM( 'R', 'U', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 1 ) + CALL ZSYMM( 'L', 'L', 2, 0, ALPHA, A, 2, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 9 - CALL ZSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 1 ) + CALL ZSYMM( 'R', 'L', 2, 0, ALPHA, A, 1, B, 1, BETA, C, 2 ) CALL CHKXER( SRNAMT, INFOT, NOUT, LERR, OK ) INFOT = 12 CALL ZSYMM( 'L', 'U', 2, 0, ALPHA, A, 2, B, 2, BETA, C, 1 ) @@ -3274,7 +3332,6 @@ LOGICAL FUNCTION LZERES( TYPE, UPLO, M, N, AA, AS, LDA ) 50 CONTINUE END IF * - 60 CONTINUE LZERES = .TRUE. GO TO 80 70 CONTINUE diff --git a/ground/gcs/src/libs/eigen/blas/xerbla.cpp b/ground/gcs/src/libs/eigen/blas/xerbla.cpp index dd39a52442..c373e86996 100644 --- a/ground/gcs/src/libs/eigen/blas/xerbla.cpp +++ b/ground/gcs/src/libs/eigen/blas/xerbla.cpp @@ -1,5 +1,5 @@ -#include +#include #if (defined __GNUC__) && (!defined __MINGW32__) && (!defined __CYGWIN__) #define EIGEN_WEAK_LINKING __attribute__ ((weak)) @@ -14,7 +14,7 @@ extern "C" EIGEN_WEAK_LINKING int xerbla_(const char * msg, int *info, int) { - std::cerr << "Eigen BLAS ERROR #" << *info << ": " << msg << "\n"; + printf("Eigen BLAS ERROR #%i: %s\n", *info, msg ); return 0; } diff --git a/ground/gcs/src/libs/eigen/blas/zhbmv.f b/ground/gcs/src/libs/eigen/blas/zhbmv.f deleted file mode 100644 index bca0da5fcd..0000000000 --- a/ground/gcs/src/libs/eigen/blas/zhbmv.f +++ /dev/null @@ -1,310 +0,0 @@ - SUBROUTINE ZHBMV(UPLO,N,K,ALPHA,A,LDA,X,INCX,BETA,Y,INCY) -* .. Scalar Arguments .. - DOUBLE COMPLEX ALPHA,BETA - INTEGER INCX,INCY,K,LDA,N - CHARACTER UPLO -* .. -* .. Array Arguments .. - DOUBLE COMPLEX A(LDA,*),X(*),Y(*) -* .. -* -* Purpose -* ======= -* -* ZHBMV performs the matrix-vector operation -* -* y := alpha*A*x + beta*y, -* -* where alpha and beta are scalars, x and y are n element vectors and -* A is an n by n hermitian band matrix, with k super-diagonals. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the upper or lower -* triangular part of the band matrix A is being supplied as -* follows: -* -* UPLO = 'U' or 'u' The upper triangular part of A is -* being supplied. -* -* UPLO = 'L' or 'l' The lower triangular part of A is -* being supplied. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* K - INTEGER. -* On entry, K specifies the number of super-diagonals of the -* matrix A. K must satisfy 0 .le. K. -* Unchanged on exit. -* -* ALPHA - COMPLEX*16 . -* On entry, ALPHA specifies the scalar alpha. -* Unchanged on exit. -* -* A - COMPLEX*16 array of DIMENSION ( LDA, n ). -* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) -* by n part of the array A must contain the upper triangular -* band part of the hermitian matrix, supplied column by -* column, with the leading diagonal of the matrix in row -* ( k + 1 ) of the array, the first super-diagonal starting at -* position 2 in row k, and so on. The top left k by k triangle -* of the array A is not referenced. -* The following program segment will transfer the upper -* triangular part of a hermitian band matrix from conventional -* full matrix storage to band storage: -* -* DO 20, J = 1, N -* M = K + 1 - J -* DO 10, I = MAX( 1, J - K ), J -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) -* by n part of the array A must contain the lower triangular -* band part of the hermitian matrix, supplied column by -* column, with the leading diagonal of the matrix in row 1 of -* the array, the first sub-diagonal starting at position 1 in -* row 2, and so on. The bottom right k by k triangle of the -* array A is not referenced. -* The following program segment will transfer the lower -* triangular part of a hermitian band matrix from conventional -* full matrix storage to band storage: -* -* DO 20, J = 1, N -* M = 1 - J -* DO 10, I = J, MIN( N, J + K ) -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Note that the imaginary parts of the diagonal elements need -* not be set and are assumed to be zero. -* Unchanged on exit. -* -* LDA - INTEGER. -* On entry, LDA specifies the first dimension of A as declared -* in the calling (sub) program. LDA must be at least -* ( k + 1 ). -* Unchanged on exit. -* -* X - COMPLEX*16 array of DIMENSION at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the -* vector x. -* Unchanged on exit. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* BETA - COMPLEX*16 . -* On entry, BETA specifies the scalar beta. -* Unchanged on exit. -* -* Y - COMPLEX*16 array of DIMENSION at least -* ( 1 + ( n - 1 )*abs( INCY ) ). -* Before entry, the incremented array Y must contain the -* vector y. On exit, Y is overwritten by the updated vector y. -* -* INCY - INTEGER. -* On entry, INCY specifies the increment for the elements of -* Y. INCY must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - DOUBLE COMPLEX ONE - PARAMETER (ONE= (1.0D+0,0.0D+0)) - DOUBLE COMPLEX ZERO - PARAMETER (ZERO= (0.0D+0,0.0D+0)) -* .. -* .. Local Scalars .. - DOUBLE COMPLEX TEMP1,TEMP2 - INTEGER I,INFO,IX,IY,J,JX,JY,KPLUS1,KX,KY,L -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC DBLE,DCONJG,MAX,MIN -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (N.LT.0) THEN - INFO = 2 - ELSE IF (K.LT.0) THEN - INFO = 3 - ELSE IF (LDA.LT. (K+1)) THEN - INFO = 6 - ELSE IF (INCX.EQ.0) THEN - INFO = 8 - ELSE IF (INCY.EQ.0) THEN - INFO = 11 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('ZHBMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF ((N.EQ.0) .OR. ((ALPHA.EQ.ZERO).AND. (BETA.EQ.ONE))) RETURN -* -* Set up the start points in X and Y. -* - IF (INCX.GT.0) THEN - KX = 1 - ELSE - KX = 1 - (N-1)*INCX - END IF - IF (INCY.GT.0) THEN - KY = 1 - ELSE - KY = 1 - (N-1)*INCY - END IF -* -* Start the operations. In this version the elements of the array A -* are accessed sequentially with one pass through A. -* -* First form y := beta*y. -* - IF (BETA.NE.ONE) THEN - IF (INCY.EQ.1) THEN - IF (BETA.EQ.ZERO) THEN - DO 10 I = 1,N - Y(I) = ZERO - 10 CONTINUE - ELSE - DO 20 I = 1,N - Y(I) = BETA*Y(I) - 20 CONTINUE - END IF - ELSE - IY = KY - IF (BETA.EQ.ZERO) THEN - DO 30 I = 1,N - Y(IY) = ZERO - IY = IY + INCY - 30 CONTINUE - ELSE - DO 40 I = 1,N - Y(IY) = BETA*Y(IY) - IY = IY + INCY - 40 CONTINUE - END IF - END IF - END IF - IF (ALPHA.EQ.ZERO) RETURN - IF (LSAME(UPLO,'U')) THEN -* -* Form y when upper triangle of A is stored. -* - KPLUS1 = K + 1 - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 60 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - L = KPLUS1 - J - DO 50 I = MAX(1,J-K),J - 1 - Y(I) = Y(I) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + DCONJG(A(L+I,J))*X(I) - 50 CONTINUE - Y(J) = Y(J) + TEMP1*DBLE(A(KPLUS1,J)) + ALPHA*TEMP2 - 60 CONTINUE - ELSE - JX = KX - JY = KY - DO 80 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - IX = KX - IY = KY - L = KPLUS1 - J - DO 70 I = MAX(1,J-K),J - 1 - Y(IY) = Y(IY) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + DCONJG(A(L+I,J))*X(IX) - IX = IX + INCX - IY = IY + INCY - 70 CONTINUE - Y(JY) = Y(JY) + TEMP1*DBLE(A(KPLUS1,J)) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - IF (J.GT.K) THEN - KX = KX + INCX - KY = KY + INCY - END IF - 80 CONTINUE - END IF - ELSE -* -* Form y when lower triangle of A is stored. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 100 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - Y(J) = Y(J) + TEMP1*DBLE(A(1,J)) - L = 1 - J - DO 90 I = J + 1,MIN(N,J+K) - Y(I) = Y(I) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + DCONJG(A(L+I,J))*X(I) - 90 CONTINUE - Y(J) = Y(J) + ALPHA*TEMP2 - 100 CONTINUE - ELSE - JX = KX - JY = KY - DO 120 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - Y(JY) = Y(JY) + TEMP1*DBLE(A(1,J)) - L = 1 - J - IX = JX - IY = JY - DO 110 I = J + 1,MIN(N,J+K) - IX = IX + INCX - IY = IY + INCY - Y(IY) = Y(IY) + TEMP1*A(L+I,J) - TEMP2 = TEMP2 + DCONJG(A(L+I,J))*X(IX) - 110 CONTINUE - Y(JY) = Y(JY) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - 120 CONTINUE - END IF - END IF -* - RETURN -* -* End of ZHBMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/zhpmv.f b/ground/gcs/src/libs/eigen/blas/zhpmv.f deleted file mode 100644 index b686108b36..0000000000 --- a/ground/gcs/src/libs/eigen/blas/zhpmv.f +++ /dev/null @@ -1,272 +0,0 @@ - SUBROUTINE ZHPMV(UPLO,N,ALPHA,AP,X,INCX,BETA,Y,INCY) -* .. Scalar Arguments .. - DOUBLE COMPLEX ALPHA,BETA - INTEGER INCX,INCY,N - CHARACTER UPLO -* .. -* .. Array Arguments .. - DOUBLE COMPLEX AP(*),X(*),Y(*) -* .. -* -* Purpose -* ======= -* -* ZHPMV performs the matrix-vector operation -* -* y := alpha*A*x + beta*y, -* -* where alpha and beta are scalars, x and y are n element vectors and -* A is an n by n hermitian matrix, supplied in packed form. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the upper or lower -* triangular part of the matrix A is supplied in the packed -* array AP as follows: -* -* UPLO = 'U' or 'u' The upper triangular part of A is -* supplied in AP. -* -* UPLO = 'L' or 'l' The lower triangular part of A is -* supplied in AP. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* ALPHA - COMPLEX*16 . -* On entry, ALPHA specifies the scalar alpha. -* Unchanged on exit. -* -* AP - COMPLEX*16 array of DIMENSION at least -* ( ( n*( n + 1 ) )/2 ). -* Before entry with UPLO = 'U' or 'u', the array AP must -* contain the upper triangular part of the hermitian matrix -* packed sequentially, column by column, so that AP( 1 ) -* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 1, 2 ) -* and a( 2, 2 ) respectively, and so on. -* Before entry with UPLO = 'L' or 'l', the array AP must -* contain the lower triangular part of the hermitian matrix -* packed sequentially, column by column, so that AP( 1 ) -* contains a( 1, 1 ), AP( 2 ) and AP( 3 ) contain a( 2, 1 ) -* and a( 3, 1 ) respectively, and so on. -* Note that the imaginary parts of the diagonal elements need -* not be set and are assumed to be zero. -* Unchanged on exit. -* -* X - COMPLEX*16 array of dimension at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the n -* element vector x. -* Unchanged on exit. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* BETA - COMPLEX*16 . -* On entry, BETA specifies the scalar beta. When BETA is -* supplied as zero then Y need not be set on input. -* Unchanged on exit. -* -* Y - COMPLEX*16 array of dimension at least -* ( 1 + ( n - 1 )*abs( INCY ) ). -* Before entry, the incremented array Y must contain the n -* element vector y. On exit, Y is overwritten by the updated -* vector y. -* -* INCY - INTEGER. -* On entry, INCY specifies the increment for the elements of -* Y. INCY must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - DOUBLE COMPLEX ONE - PARAMETER (ONE= (1.0D+0,0.0D+0)) - DOUBLE COMPLEX ZERO - PARAMETER (ZERO= (0.0D+0,0.0D+0)) -* .. -* .. Local Scalars .. - DOUBLE COMPLEX TEMP1,TEMP2 - INTEGER I,INFO,IX,IY,J,JX,JY,K,KK,KX,KY -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC DBLE,DCONJG -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (N.LT.0) THEN - INFO = 2 - ELSE IF (INCX.EQ.0) THEN - INFO = 6 - ELSE IF (INCY.EQ.0) THEN - INFO = 9 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('ZHPMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF ((N.EQ.0) .OR. ((ALPHA.EQ.ZERO).AND. (BETA.EQ.ONE))) RETURN -* -* Set up the start points in X and Y. -* - IF (INCX.GT.0) THEN - KX = 1 - ELSE - KX = 1 - (N-1)*INCX - END IF - IF (INCY.GT.0) THEN - KY = 1 - ELSE - KY = 1 - (N-1)*INCY - END IF -* -* Start the operations. In this version the elements of the array AP -* are accessed sequentially with one pass through AP. -* -* First form y := beta*y. -* - IF (BETA.NE.ONE) THEN - IF (INCY.EQ.1) THEN - IF (BETA.EQ.ZERO) THEN - DO 10 I = 1,N - Y(I) = ZERO - 10 CONTINUE - ELSE - DO 20 I = 1,N - Y(I) = BETA*Y(I) - 20 CONTINUE - END IF - ELSE - IY = KY - IF (BETA.EQ.ZERO) THEN - DO 30 I = 1,N - Y(IY) = ZERO - IY = IY + INCY - 30 CONTINUE - ELSE - DO 40 I = 1,N - Y(IY) = BETA*Y(IY) - IY = IY + INCY - 40 CONTINUE - END IF - END IF - END IF - IF (ALPHA.EQ.ZERO) RETURN - KK = 1 - IF (LSAME(UPLO,'U')) THEN -* -* Form y when AP contains the upper triangle. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 60 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - K = KK - DO 50 I = 1,J - 1 - Y(I) = Y(I) + TEMP1*AP(K) - TEMP2 = TEMP2 + DCONJG(AP(K))*X(I) - K = K + 1 - 50 CONTINUE - Y(J) = Y(J) + TEMP1*DBLE(AP(KK+J-1)) + ALPHA*TEMP2 - KK = KK + J - 60 CONTINUE - ELSE - JX = KX - JY = KY - DO 80 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - IX = KX - IY = KY - DO 70 K = KK,KK + J - 2 - Y(IY) = Y(IY) + TEMP1*AP(K) - TEMP2 = TEMP2 + DCONJG(AP(K))*X(IX) - IX = IX + INCX - IY = IY + INCY - 70 CONTINUE - Y(JY) = Y(JY) + TEMP1*DBLE(AP(KK+J-1)) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - KK = KK + J - 80 CONTINUE - END IF - ELSE -* -* Form y when AP contains the lower triangle. -* - IF ((INCX.EQ.1) .AND. (INCY.EQ.1)) THEN - DO 100 J = 1,N - TEMP1 = ALPHA*X(J) - TEMP2 = ZERO - Y(J) = Y(J) + TEMP1*DBLE(AP(KK)) - K = KK + 1 - DO 90 I = J + 1,N - Y(I) = Y(I) + TEMP1*AP(K) - TEMP2 = TEMP2 + DCONJG(AP(K))*X(I) - K = K + 1 - 90 CONTINUE - Y(J) = Y(J) + ALPHA*TEMP2 - KK = KK + (N-J+1) - 100 CONTINUE - ELSE - JX = KX - JY = KY - DO 120 J = 1,N - TEMP1 = ALPHA*X(JX) - TEMP2 = ZERO - Y(JY) = Y(JY) + TEMP1*DBLE(AP(KK)) - IX = JX - IY = JY - DO 110 K = KK + 1,KK + N - J - IX = IX + INCX - IY = IY + INCY - Y(IY) = Y(IY) + TEMP1*AP(K) - TEMP2 = TEMP2 + DCONJG(AP(K))*X(IX) - 110 CONTINUE - Y(JY) = Y(JY) + ALPHA*TEMP2 - JX = JX + INCX - JY = JY + INCY - KK = KK + (N-J+1) - 120 CONTINUE - END IF - END IF -* - RETURN -* -* End of ZHPMV . -* - END diff --git a/ground/gcs/src/libs/eigen/blas/ztbmv.f b/ground/gcs/src/libs/eigen/blas/ztbmv.f deleted file mode 100644 index 7c85c1b550..0000000000 --- a/ground/gcs/src/libs/eigen/blas/ztbmv.f +++ /dev/null @@ -1,366 +0,0 @@ - SUBROUTINE ZTBMV(UPLO,TRANS,DIAG,N,K,A,LDA,X,INCX) -* .. Scalar Arguments .. - INTEGER INCX,K,LDA,N - CHARACTER DIAG,TRANS,UPLO -* .. -* .. Array Arguments .. - DOUBLE COMPLEX A(LDA,*),X(*) -* .. -* -* Purpose -* ======= -* -* ZTBMV performs one of the matrix-vector operations -* -* x := A*x, or x := A'*x, or x := conjg( A' )*x, -* -* where x is an n element vector and A is an n by n unit, or non-unit, -* upper or lower triangular band matrix, with ( k + 1 ) diagonals. -* -* Arguments -* ========== -* -* UPLO - CHARACTER*1. -* On entry, UPLO specifies whether the matrix is an upper or -* lower triangular matrix as follows: -* -* UPLO = 'U' or 'u' A is an upper triangular matrix. -* -* UPLO = 'L' or 'l' A is a lower triangular matrix. -* -* Unchanged on exit. -* -* TRANS - CHARACTER*1. -* On entry, TRANS specifies the operation to be performed as -* follows: -* -* TRANS = 'N' or 'n' x := A*x. -* -* TRANS = 'T' or 't' x := A'*x. -* -* TRANS = 'C' or 'c' x := conjg( A' )*x. -* -* Unchanged on exit. -* -* DIAG - CHARACTER*1. -* On entry, DIAG specifies whether or not A is unit -* triangular as follows: -* -* DIAG = 'U' or 'u' A is assumed to be unit triangular. -* -* DIAG = 'N' or 'n' A is not assumed to be unit -* triangular. -* -* Unchanged on exit. -* -* N - INTEGER. -* On entry, N specifies the order of the matrix A. -* N must be at least zero. -* Unchanged on exit. -* -* K - INTEGER. -* On entry with UPLO = 'U' or 'u', K specifies the number of -* super-diagonals of the matrix A. -* On entry with UPLO = 'L' or 'l', K specifies the number of -* sub-diagonals of the matrix A. -* K must satisfy 0 .le. K. -* Unchanged on exit. -* -* A - COMPLEX*16 array of DIMENSION ( LDA, n ). -* Before entry with UPLO = 'U' or 'u', the leading ( k + 1 ) -* by n part of the array A must contain the upper triangular -* band part of the matrix of coefficients, supplied column by -* column, with the leading diagonal of the matrix in row -* ( k + 1 ) of the array, the first super-diagonal starting at -* position 2 in row k, and so on. The top left k by k triangle -* of the array A is not referenced. -* The following program segment will transfer an upper -* triangular band matrix from conventional full matrix storage -* to band storage: -* -* DO 20, J = 1, N -* M = K + 1 - J -* DO 10, I = MAX( 1, J - K ), J -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Before entry with UPLO = 'L' or 'l', the leading ( k + 1 ) -* by n part of the array A must contain the lower triangular -* band part of the matrix of coefficients, supplied column by -* column, with the leading diagonal of the matrix in row 1 of -* the array, the first sub-diagonal starting at position 1 in -* row 2, and so on. The bottom right k by k triangle of the -* array A is not referenced. -* The following program segment will transfer a lower -* triangular band matrix from conventional full matrix storage -* to band storage: -* -* DO 20, J = 1, N -* M = 1 - J -* DO 10, I = J, MIN( N, J + K ) -* A( M + I, J ) = matrix( I, J ) -* 10 CONTINUE -* 20 CONTINUE -* -* Note that when DIAG = 'U' or 'u' the elements of the array A -* corresponding to the diagonal elements of the matrix are not -* referenced, but are assumed to be unity. -* Unchanged on exit. -* -* LDA - INTEGER. -* On entry, LDA specifies the first dimension of A as declared -* in the calling (sub) program. LDA must be at least -* ( k + 1 ). -* Unchanged on exit. -* -* X - COMPLEX*16 array of dimension at least -* ( 1 + ( n - 1 )*abs( INCX ) ). -* Before entry, the incremented array X must contain the n -* element vector x. On exit, X is overwritten with the -* tranformed vector x. -* -* INCX - INTEGER. -* On entry, INCX specifies the increment for the elements of -* X. INCX must not be zero. -* Unchanged on exit. -* -* Further Details -* =============== -* -* Level 2 Blas routine. -* -* -- Written on 22-October-1986. -* Jack Dongarra, Argonne National Lab. -* Jeremy Du Croz, Nag Central Office. -* Sven Hammarling, Nag Central Office. -* Richard Hanson, Sandia National Labs. -* -* ===================================================================== -* -* .. Parameters .. - DOUBLE COMPLEX ZERO - PARAMETER (ZERO= (0.0D+0,0.0D+0)) -* .. -* .. Local Scalars .. - DOUBLE COMPLEX TEMP - INTEGER I,INFO,IX,J,JX,KPLUS1,KX,L - LOGICAL NOCONJ,NOUNIT -* .. -* .. External Functions .. - LOGICAL LSAME - EXTERNAL LSAME -* .. -* .. External Subroutines .. - EXTERNAL XERBLA -* .. -* .. Intrinsic Functions .. - INTRINSIC DCONJG,MAX,MIN -* .. -* -* Test the input parameters. -* - INFO = 0 - IF (.NOT.LSAME(UPLO,'U') .AND. .NOT.LSAME(UPLO,'L')) THEN - INFO = 1 - ELSE IF (.NOT.LSAME(TRANS,'N') .AND. .NOT.LSAME(TRANS,'T') .AND. - + .NOT.LSAME(TRANS,'C')) THEN - INFO = 2 - ELSE IF (.NOT.LSAME(DIAG,'U') .AND. .NOT.LSAME(DIAG,'N')) THEN - INFO = 3 - ELSE IF (N.LT.0) THEN - INFO = 4 - ELSE IF (K.LT.0) THEN - INFO = 5 - ELSE IF (LDA.LT. (K+1)) THEN - INFO = 7 - ELSE IF (INCX.EQ.0) THEN - INFO = 9 - END IF - IF (INFO.NE.0) THEN - CALL XERBLA('ZTBMV ',INFO) - RETURN - END IF -* -* Quick return if possible. -* - IF (N.EQ.0) RETURN -* - NOCONJ = LSAME(TRANS,'T') - NOUNIT = LSAME(DIAG,'N') -* -* Set up the start point in X if the increment is not unity. This -* will be ( N - 1 )*INCX too small for descending loops. -* - IF (INCX.LE.0) THEN - KX = 1 - (N-1)*INCX - ELSE IF (INCX.NE.1) THEN - KX = 1 - END IF -* -* Start the operations. In this version the elements of A are -* accessed sequentially with one pass through A. -* - IF (LSAME(TRANS,'N')) THEN -* -* Form x := A*x. -* - IF (LSAME(UPLO,'U')) THEN - KPLUS1 = K + 1 - IF (INCX.EQ.1) THEN - DO 20 J = 1,N - IF (X(J).NE.ZERO) THEN - TEMP = X(J) - L = KPLUS1 - J - DO 10 I = MAX(1,J-K),J - 1 - X(I) = X(I) + TEMP*A(L+I,J) - 10 CONTINUE - IF (NOUNIT) X(J) = X(J)*A(KPLUS1,J) - END IF - 20 CONTINUE - ELSE - JX = KX - DO 40 J = 1,N - IF (X(JX).NE.ZERO) THEN - TEMP = X(JX) - IX = KX - L = KPLUS1 - J - DO 30 I = MAX(1,J-K),J - 1 - X(IX) = X(IX) + TEMP*A(L+I,J) - IX = IX + INCX - 30 CONTINUE - IF (NOUNIT) X(JX) = X(JX)*A(KPLUS1,J) - END IF - JX = JX + INCX - IF (J.GT.K) KX = KX + INCX - 40 CONTINUE - END IF - ELSE - IF (INCX.EQ.1) THEN - DO 60 J = N,1,-1 - IF (X(J).NE.ZERO) THEN - TEMP = X(J) - L = 1 - J - DO 50 I = MIN(N,J+K),J + 1,-1 - X(I) = X(I) + TEMP*A(L+I,J) - 50 CONTINUE - IF (NOUNIT) X(J) = X(J)*A(1,J) - END IF - 60 CONTINUE - ELSE - KX = KX + (N-1)*INCX - JX = KX - DO 80 J = N,1,-1 - IF (X(JX).NE.ZERO) THEN - TEMP = X(JX) - IX = KX - L = 1 - J - DO 70 I = MIN(N,J+K),J + 1,-1 - X(IX) = X(IX) + TEMP*A(L+I,J) - IX = IX - INCX - 70 CONTINUE - IF (NOUNIT) X(JX) = X(JX)*A(1,J) - END IF - JX = JX - INCX - IF ((N-J).GE.K) KX = KX - INCX - 80 CONTINUE - END IF - END IF - ELSE -* -* Form x := A'*x or x := conjg( A' )*x. -* - IF (LSAME(UPLO,'U')) THEN - KPLUS1 = K + 1 - IF (INCX.EQ.1) THEN - DO 110 J = N,1,-1 - TEMP = X(J) - L = KPLUS1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(KPLUS1,J) - DO 90 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + A(L+I,J)*X(I) - 90 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*DCONJG(A(KPLUS1,J)) - DO 100 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + DCONJG(A(L+I,J))*X(I) - 100 CONTINUE - END IF - X(J) = TEMP - 110 CONTINUE - ELSE - KX = KX + (N-1)*INCX - JX = KX - DO 140 J = N,1,-1 - TEMP = X(JX) - KX = KX - INCX - IX = KX - L = KPLUS1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(KPLUS1,J) - DO 120 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + A(L+I,J)*X(IX) - IX = IX - INCX - 120 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*DCONJG(A(KPLUS1,J)) - DO 130 I = J - 1,MAX(1,J-K),-1 - TEMP = TEMP + DCONJG(A(L+I,J))*X(IX) - IX = IX - INCX - 130 CONTINUE - END IF - X(JX) = TEMP - JX = JX - INCX - 140 CONTINUE - END IF - ELSE - IF (INCX.EQ.1) THEN - DO 170 J = 1,N - TEMP = X(J) - L = 1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(1,J) - DO 150 I = J + 1,MIN(N,J+K) - TEMP = TEMP + A(L+I,J)*X(I) - 150 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*DCONJG(A(1,J)) - DO 160 I = J + 1,MIN(N,J+K) - TEMP = TEMP + DCONJG(A(L+I,J))*X(I) - 160 CONTINUE - END IF - X(J) = TEMP - 170 CONTINUE - ELSE - JX = KX - DO 200 J = 1,N - TEMP = X(JX) - KX = KX + INCX - IX = KX - L = 1 - J - IF (NOCONJ) THEN - IF (NOUNIT) TEMP = TEMP*A(1,J) - DO 180 I = J + 1,MIN(N,J+K) - TEMP = TEMP + A(L+I,J)*X(IX) - IX = IX + INCX - 180 CONTINUE - ELSE - IF (NOUNIT) TEMP = TEMP*DCONJG(A(1,J)) - DO 190 I = J + 1,MIN(N,J+K) - TEMP = TEMP + DCONJG(A(L+I,J))*X(IX) - IX = IX + INCX - 190 CONTINUE - END IF - X(JX) = TEMP - JX = JX + INCX - 200 CONTINUE - END IF - END IF - END IF -* - RETURN -* -* End of ZTBMV . -* - END diff --git a/ground/gcs/src/libs/eigen/cmake/Eigen3Config.cmake.in b/ground/gcs/src/libs/eigen/cmake/Eigen3Config.cmake.in new file mode 100644 index 0000000000..c5c5468877 --- /dev/null +++ b/ground/gcs/src/libs/eigen/cmake/Eigen3Config.cmake.in @@ -0,0 +1,21 @@ +# This file exports the Eigen3::Eigen CMake target which should be passed to the +# target_link_libraries command. + +@PACKAGE_INIT@ + +include ("${CMAKE_CURRENT_LIST_DIR}/Eigen3Targets.cmake") + +# Legacy variables, do *not* use. May be removed in the future. + +set (EIGEN3_FOUND 1) +set (EIGEN3_USE_FILE "${CMAKE_CURRENT_LIST_DIR}/UseEigen3.cmake") + +set (EIGEN3_DEFINITIONS "@EIGEN_DEFINITIONS@") +set (EIGEN3_INCLUDE_DIR "@PACKAGE_EIGEN_INCLUDE_DIR@") +set (EIGEN3_INCLUDE_DIRS "@PACKAGE_EIGEN_INCLUDE_DIR@") +set (EIGEN3_ROOT_DIR "@PACKAGE_EIGEN_ROOT_DIR@") + +set (EIGEN3_VERSION_STRING "@EIGEN_VERSION_STRING@") +set (EIGEN3_VERSION_MAJOR "@EIGEN_VERSION_MAJOR@") +set (EIGEN3_VERSION_MINOR "@EIGEN_VERSION_MINOR@") +set (EIGEN3_VERSION_PATCH "@EIGEN_VERSION_PATCH@") diff --git a/ground/gcs/src/libs/eigen/cmake/Eigen3ConfigLegacy.cmake.in b/ground/gcs/src/libs/eigen/cmake/Eigen3ConfigLegacy.cmake.in new file mode 100644 index 0000000000..62d722469b --- /dev/null +++ b/ground/gcs/src/libs/eigen/cmake/Eigen3ConfigLegacy.cmake.in @@ -0,0 +1,30 @@ +# -*- cmake -*- +# +# Eigen3Config.cmake(.in) + +# Use the following variables to compile and link against Eigen: +# EIGEN3_FOUND - True if Eigen was found on your system +# EIGEN3_USE_FILE - The file making Eigen usable +# EIGEN3_DEFINITIONS - Definitions needed to build with Eigen +# EIGEN3_INCLUDE_DIR - Directory where signature_of_eigen3_matrix_library can be found +# EIGEN3_INCLUDE_DIRS - List of directories of Eigen and it's dependencies +# EIGEN3_ROOT_DIR - The base directory of Eigen +# EIGEN3_VERSION_STRING - A human-readable string containing the version +# EIGEN3_VERSION_MAJOR - The major version of Eigen +# EIGEN3_VERSION_MINOR - The minor version of Eigen +# EIGEN3_VERSION_PATCH - The patch version of Eigen + +@PACKAGE_INIT@ + +set ( EIGEN3_FOUND 1 ) +set ( EIGEN3_USE_FILE "${CMAKE_CURRENT_LIST_DIR}/UseEigen3.cmake" ) + +set ( EIGEN3_DEFINITIONS "@EIGEN_DEFINITIONS@" ) +set ( EIGEN3_INCLUDE_DIR "@PACKAGE_EIGEN_INCLUDE_DIR@" ) +set ( EIGEN3_INCLUDE_DIRS "@PACKAGE_EIGEN_INCLUDE_DIR@" ) +set ( EIGEN3_ROOT_DIR "@PACKAGE_EIGEN_ROOT_DIR@" ) + +set ( EIGEN3_VERSION_STRING "@EIGEN_VERSION_STRING@" ) +set ( EIGEN3_VERSION_MAJOR "@EIGEN_VERSION_MAJOR@" ) +set ( EIGEN3_VERSION_MINOR "@EIGEN_VERSION_MINOR@" ) +set ( EIGEN3_VERSION_PATCH "@EIGEN_VERSION_PATCH@" ) diff --git a/ground/gcs/src/libs/eigen/cmake/EigenConfigureTesting.cmake b/ground/gcs/src/libs/eigen/cmake/EigenConfigureTesting.cmake index 2b11d8360d..afc24b5e99 100644 --- a/ground/gcs/src/libs/eigen/cmake/EigenConfigureTesting.cmake +++ b/ground/gcs/src/libs/eigen/cmake/EigenConfigureTesting.cmake @@ -14,17 +14,10 @@ add_dependencies(check buildtests) # check whether /bin/bash exists find_file(EIGEN_BIN_BASH_EXISTS "/bin/bash" PATHS "/" NO_DEFAULT_PATH) -# CMake/Ctest does not allow us to change the build command, -# so we have to workaround by directly editing the generated DartConfiguration.tcl file -# save CMAKE_MAKE_PROGRAM -set(CMAKE_MAKE_PROGRAM_SAVE ${CMAKE_MAKE_PROGRAM}) -# and set a fake one -set(CMAKE_MAKE_PROGRAM "@EIGEN_MAKECOMMAND_PLACEHOLDER@") - # This call activates testing and generates the DartConfiguration.tcl include(CTest) -set(EIGEN_TEST_BUILD_FLAGS " " CACHE STRING "Options passed to the build command of unit tests") +set(EIGEN_TEST_BUILD_FLAGS "" CACHE STRING "Options passed to the build command of unit tests") # Overwrite default DartConfiguration.tcl such that ctest can build our unit tests. # Recall that our unit tests are not in the "all" target, so we have to explicitely ask ctest to build our custom 'buildtests' target. @@ -53,18 +46,16 @@ if(CMAKE_COMPILER_IS_GNUCXX) if(EIGEN_COVERAGE_TESTING) set(COVERAGE_FLAGS "-fprofile-arcs -ftest-coverage") set(CTEST_CUSTOM_COVERAGE_EXCLUDE "/test/") - else(EIGEN_COVERAGE_TESTING) - set(COVERAGE_FLAGS "") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${COVERAGE_FLAGS}") endif(EIGEN_COVERAGE_TESTING) - if(EIGEN_TEST_C++0x) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++0x") - endif(EIGEN_TEST_C++0x) - if(CMAKE_SYSTEM_NAME MATCHES Linux) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${COVERAGE_FLAGS} -g2") - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${COVERAGE_FLAGS} -O2 -g2") - set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${COVERAGE_FLAGS} -fno-inline-functions") - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${COVERAGE_FLAGS} -O0 -g3") - endif(CMAKE_SYSTEM_NAME MATCHES Linux) + elseif(MSVC) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /D_CRT_SECURE_NO_WARNINGS /D_SCL_SECURE_NO_WARNINGS") endif(CMAKE_COMPILER_IS_GNUCXX) + + +check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CXX11) + +if(EIGEN_TEST_CXX11 AND EIGEN_COMPILER_SUPPORT_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +endif() diff --git a/ground/gcs/src/libs/eigen/cmake/EigenDetermineOSVersion.cmake b/ground/gcs/src/libs/eigen/cmake/EigenDetermineOSVersion.cmake index 3c48d4c374..9246fa67c8 100644 --- a/ground/gcs/src/libs/eigen/cmake/EigenDetermineOSVersion.cmake +++ b/ground/gcs/src/libs/eigen/cmake/EigenDetermineOSVersion.cmake @@ -26,7 +26,7 @@ function(DetermineShortWindowsName WIN_VERSION win_num_version) endfunction() function(DetermineOSVersion OS_VERSION) - if (WIN32) + if (WIN32 AND CMAKE_HOST_SYSTEM_NAME MATCHES Windows) file (TO_NATIVE_PATH "$ENV{COMSPEC}" SHELL) exec_program( ${SHELL} ARGS "/c" "ver" OUTPUT_VARIABLE ver_output) diff --git a/ground/gcs/src/libs/eigen/cmake/EigenDetermineVSServicePack.cmake b/ground/gcs/src/libs/eigen/cmake/EigenDetermineVSServicePack.cmake index 8e5546a851..fed78194d2 100644 --- a/ground/gcs/src/libs/eigen/cmake/EigenDetermineVSServicePack.cmake +++ b/ground/gcs/src/libs/eigen/cmake/EigenDetermineVSServicePack.cmake @@ -4,7 +4,6 @@ include(CMakeDetermineVSServicePack) # _DetermineVSServicePack_FastCheckVersionWithCompiler which lead to errors on some systems. function(EigenDetermineVSServicePack _pack) if(NOT DETERMINED_VS_SERVICE_PACK OR NOT ${_pack}) - if(NOT DETERMINED_VS_SERVICE_PACK) _DetermineVSServicePack_CheckVersionWithTryCompile(DETERMINED_VS_SERVICE_PACK _cl_version) if(NOT DETERMINED_VS_SERVICE_PACK) @@ -13,10 +12,25 @@ function(EigenDetermineVSServicePack _pack) endif() if(DETERMINED_VS_SERVICE_PACK) - if(_cl_version) # Call helper function to determine VS version _DetermineVSServicePackFromCompiler(_sp "${_cl_version}") + + # temporary fix, until CMake catches up + if (NOT _sp) + if(${_cl_version} VERSION_EQUAL "17.00.50727.1") + set(_sp "vc110") + elseif(${_cl_version} VERSION_EQUAL "17.00.51106.1") + set(_sp "vc110sp1") + elseif(${_cl_version} VERSION_EQUAL "17.00.60315.1") + set(_sp "vc110sp2") + elseif(${_cl_version} VERSION_EQUAL "17.00.60610.1") + set(_sp "vc110sp3") + else() + set(_sp ${CMAKE_CXX_COMPILER_VERSION}) + endif() + endif() + if(_sp) set(${_pack} ${_sp} CACHE INTERNAL "The Visual Studio Release with Service Pack") diff --git a/ground/gcs/src/libs/eigen/cmake/EigenTesting.cmake b/ground/gcs/src/libs/eigen/cmake/EigenTesting.cmake index cbe12d51b8..a92a2978b0 100644 --- a/ground/gcs/src/libs/eigen/cmake/EigenTesting.cmake +++ b/ground/gcs/src/libs/eigen/cmake/EigenTesting.cmake @@ -1,19 +1,48 @@ macro(ei_add_property prop value) - get_property(previous GLOBAL PROPERTY ${prop}) + get_property(previous GLOBAL PROPERTY ${prop}) if ((NOT previous) OR (previous STREQUAL "")) set_property(GLOBAL PROPERTY ${prop} "${value}") else() set_property(GLOBAL PROPERTY ${prop} "${previous} ${value}") - endif() + endif() endmacro(ei_add_property) #internal. See documentation of ei_add_test for details. macro(ei_add_test_internal testname testname_with_suffix) set(targetname ${testname_with_suffix}) - set(filename ${testname}.cpp) - add_executable(${targetname} ${filename}) + if(EIGEN_ADD_TEST_FILENAME_EXTENSION) + set(filename ${testname}.${EIGEN_ADD_TEST_FILENAME_EXTENSION}) + else() + set(filename ${testname}.cpp) + endif() + + if(EIGEN_ADD_TEST_FILENAME_EXTENSION STREQUAL cu) + if(EIGEN_TEST_CUDA_CLANG) + set_source_files_properties(${filename} PROPERTIES LANGUAGE CXX) + if(CUDA_64_BIT_DEVICE_CODE) + link_directories("${CUDA_TOOLKIT_ROOT_DIR}/lib64") + else() + link_directories("${CUDA_TOOLKIT_ROOT_DIR}/lib") + endif() + if (${ARGC} GREATER 2) + add_executable(${targetname} ${filename}) + else() + add_executable(${targetname} ${filename} OPTIONS ${ARGV2}) + endif() + target_link_libraries(${targetname} "cudart_static" "cuda" "dl" "rt" "pthread") + else() + if (${ARGC} GREATER 2) + cuda_add_executable(${targetname} ${filename} OPTIONS ${ARGV2}) + else() + cuda_add_executable(${targetname} ${filename}) + endif() + endif() + else() + add_executable(${targetname} ${filename}) + endif() + if (targetname MATCHES "^eigen2_") add_dependencies(eigen2_buildtests ${targetname}) else() @@ -27,20 +56,20 @@ macro(ei_add_test_internal testname testname_with_suffix) ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_DEBUG_ASSERTS=1") endif(EIGEN_DEBUG_ASSERTS) endif(EIGEN_NO_ASSERTION_CHECKING) - + ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_TEST_MAX_SIZE=${EIGEN_TEST_MAX_SIZE}") ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_TEST_FUNC=${testname}") - - if(MSVC AND NOT EIGEN_SPLIT_LARGE_TESTS) + + if(MSVC) ei_add_target_property(${targetname} COMPILE_FLAGS "/bigobj") - endif() + endif() # let the user pass flags. if(${ARGC} GREATER 2) ei_add_target_property(${targetname} COMPILE_FLAGS "${ARGV2}") endif(${ARGC} GREATER 2) - + if(EIGEN_TEST_CUSTOM_CXX_FLAGS) ei_add_target_property(${targetname} COMPILE_FLAGS "${EIGEN_TEST_CUSTOM_CXX_FLAGS}") endif() @@ -66,16 +95,12 @@ macro(ei_add_test_internal testname testname_with_suffix) # notice: no double quotes around ${libs_to_link} here. It may be a list. target_link_libraries(${targetname} ${libs_to_link}) endif() - endif() - - if(EIGEN_BIN_BASH_EXISTS) - add_test(${testname_with_suffix} "${Eigen_SOURCE_DIR}/test/runtest.sh" "${testname_with_suffix}") - else() - add_test(${testname_with_suffix} "${targetname}") endif() - + + add_test(${testname_with_suffix} "${targetname}") + # Specify target and test labels accoirding to EIGEN_CURRENT_SUBPROJECT - get_property(current_subproject GLOBAL PROPERTY EIGEN_CURRENT_SUBPROJECT) + get_property(current_subproject GLOBAL PROPERTY EIGEN_CURRENT_SUBPROJECT) if ((current_subproject) AND (NOT (current_subproject STREQUAL ""))) set_property(TARGET ${targetname} PROPERTY LABELS "Build${current_subproject}") add_dependencies("Build${current_subproject}" ${targetname}) @@ -84,6 +109,103 @@ macro(ei_add_test_internal testname testname_with_suffix) endmacro(ei_add_test_internal) +# SYCL +macro(ei_add_test_internal_sycl testname testname_with_suffix) + include_directories( SYSTEM ${COMPUTECPP_PACKAGE_ROOT_DIR}/include) + set(targetname ${testname_with_suffix}) + + if(EIGEN_ADD_TEST_FILENAME_EXTENSION) + set(filename ${testname}.${EIGEN_ADD_TEST_FILENAME_EXTENSION}) + else() + set(filename ${testname}.cpp) + endif() + + set( include_file ${CMAKE_CURRENT_BINARY_DIR}/inc_${filename}) + set( bc_file ${CMAKE_CURRENT_BINARY_DIR}/${filename}) + set( host_file ${CMAKE_CURRENT_SOURCE_DIR}/${filename}) + + ADD_CUSTOM_COMMAND( + OUTPUT ${include_file} + COMMAND ${CMAKE_COMMAND} -E echo "\\#include \\\"${host_file}\\\"" > ${include_file} + COMMAND ${CMAKE_COMMAND} -E echo "\\#include \\\"${bc_file}.sycl\\\"" >> ${include_file} + DEPENDS ${filename} ${bc_file}.sycl + COMMENT "Building ComputeCpp integration header file ${include_file}" + ) + # Add a custom target for the generated integration header + add_custom_target(${testname}_integration_header_sycl DEPENDS ${include_file}) + + add_executable(${targetname} ${include_file}) + add_dependencies(${targetname} ${testname}_integration_header_sycl) + add_sycl_to_target(${targetname} ${filename} ${CMAKE_CURRENT_BINARY_DIR}) + + if (targetname MATCHES "^eigen2_") + add_dependencies(eigen2_buildtests ${targetname}) + else() + add_dependencies(buildtests ${targetname}) + endif() + + if(EIGEN_NO_ASSERTION_CHECKING) + ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_NO_ASSERTION_CHECKING=1") + else(EIGEN_NO_ASSERTION_CHECKING) + if(EIGEN_DEBUG_ASSERTS) + ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_DEBUG_ASSERTS=1") + endif(EIGEN_DEBUG_ASSERTS) + endif(EIGEN_NO_ASSERTION_CHECKING) + + ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_TEST_MAX_SIZE=${EIGEN_TEST_MAX_SIZE}") + + ei_add_target_property(${targetname} COMPILE_FLAGS "-DEIGEN_TEST_FUNC=${testname}") + + if(MSVC AND NOT EIGEN_SPLIT_LARGE_TESTS) + ei_add_target_property(${targetname} COMPILE_FLAGS "/bigobj") + endif() + + # let the user pass flags. + if(${ARGC} GREATER 2) + ei_add_target_property(${targetname} COMPILE_FLAGS "${ARGV2}") + endif(${ARGC} GREATER 2) + + if(EIGEN_TEST_CUSTOM_CXX_FLAGS) + ei_add_target_property(${targetname} COMPILE_FLAGS "${EIGEN_TEST_CUSTOM_CXX_FLAGS}") + endif() + + if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) + target_link_libraries(${targetname} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) + endif() + if(EXTERNAL_LIBS) + target_link_libraries(${targetname} ${EXTERNAL_LIBS}) + endif() + if(EIGEN_TEST_CUSTOM_LINKER_FLAGS) + target_link_libraries(${targetname} ${EIGEN_TEST_CUSTOM_LINKER_FLAGS}) + endif() + + if(${ARGC} GREATER 3) + set(libs_to_link ${ARGV3}) + # it could be that some cmake module provides a bad library string " " (just spaces), + # and that severely breaks target_link_libraries ("can't link to -l-lstdc++" errors). + # so we check for strings containing only spaces. + string(STRIP "${libs_to_link}" libs_to_link_stripped) + string(LENGTH "${libs_to_link_stripped}" libs_to_link_stripped_length) + if(${libs_to_link_stripped_length} GREATER 0) + # notice: no double quotes around ${libs_to_link} here. It may be a list. + target_link_libraries(${targetname} ${libs_to_link}) + endif() + endif() + + add_test(${testname_with_suffix} "${targetname}") + + # Specify target and test labels according to EIGEN_CURRENT_SUBPROJECT + get_property(current_subproject GLOBAL PROPERTY EIGEN_CURRENT_SUBPROJECT) + if ((current_subproject) AND (NOT (current_subproject STREQUAL ""))) + set_property(TARGET ${targetname} PROPERTY LABELS "Build${current_subproject}") + add_dependencies("Build${current_subproject}" ${targetname}) + set_property(TEST ${testname_with_suffix} PROPERTY LABELS "${current_subproject}") + endif() + + +endmacro(ei_add_test_internal_sycl) + + # Macro to add a test # # the unique mandatory parameter testname must correspond to a file @@ -131,7 +253,13 @@ macro(ei_add_test testname) set(EIGEN_TESTS_LIST "${EIGEN_TESTS_LIST}${testname}\n") set_property(GLOBAL PROPERTY EIGEN_TESTS_LIST "${EIGEN_TESTS_LIST}") - file(READ "${testname}.cpp" test_source) + if(EIGEN_ADD_TEST_FILENAME_EXTENSION) + set(filename ${testname}.${EIGEN_ADD_TEST_FILENAME_EXTENSION}) + else() + set(filename ${testname}.cpp) + endif() + + file(READ "${filename}" test_source) set(parts 0) string(REGEX MATCHALL "CALL_SUBTEST_[0-9]+|EIGEN_TEST_PART_[0-9]+|EIGEN_SUFFIXES(;[0-9]+)+" occurences "${test_source}") @@ -154,6 +282,39 @@ macro(ei_add_test testname) endif(EIGEN_SPLIT_LARGE_TESTS AND suffixes) endmacro(ei_add_test) +macro(ei_add_test_sycl testname) + get_property(EIGEN_TESTS_LIST GLOBAL PROPERTY EIGEN_TESTS_LIST) + set(EIGEN_TESTS_LIST "${EIGEN_TESTS_LIST}${testname}\n") + set_property(GLOBAL PROPERTY EIGEN_TESTS_LIST "${EIGEN_TESTS_LIST}") + + if(EIGEN_ADD_TEST_FILENAME_EXTENSION) + set(filename ${testname}.${EIGEN_ADD_TEST_FILENAME_EXTENSION}) + else() + set(filename ${testname}.cpp) + endif() + + file(READ "${filename}" test_source) + set(parts 0) + string(REGEX MATCHALL "CALL_SUBTEST_[0-9]+|EIGEN_TEST_PART_[0-9]+|EIGEN_SUFFIXES(;[0-9]+)+" + occurences "${test_source}") + string(REGEX REPLACE "CALL_SUBTEST_|EIGEN_TEST_PART_|EIGEN_SUFFIXES" "" suffixes "${occurences}") + list(REMOVE_DUPLICATES suffixes) + if(EIGEN_SPLIT_LARGE_TESTS AND suffixes) + add_custom_target(${testname}) + foreach(suffix ${suffixes}) + ei_add_test_internal_sycl(${testname} ${testname}_${suffix} + "${ARGV1} -DEIGEN_TEST_PART_${suffix}=1" "${ARGV2}") + add_dependencies(${testname} ${testname}_${suffix}) + endforeach(suffix) + else(EIGEN_SPLIT_LARGE_TESTS AND suffixes) + set(symbols_to_enable_all_parts "") + foreach(suffix ${suffixes}) + set(symbols_to_enable_all_parts + "${symbols_to_enable_all_parts} -DEIGEN_TEST_PART_${suffix}=1") + endforeach(suffix) + ei_add_test_internal_sycl(${testname} ${testname} "${ARGV1} ${symbols_to_enable_all_parts}" "${ARGV2}") + endif(EIGEN_SPLIT_LARGE_TESTS AND suffixes) +endmacro(ei_add_test_sycl) # adds a failtest, i.e. a test that succeed if the program fails to compile # note that the test runner for these is CMake itself, when passed -DEIGEN_FAILTEST=ON @@ -218,7 +379,7 @@ macro(ei_testing_print_summary) elseif(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION) message(STATUS "Explicit vectorization disabled (alignment kept enabled)") else() - + message(STATUS "Maximal matrix/vector size: ${EIGEN_TEST_MAX_SIZE}") if(EIGEN_TEST_SSE2) @@ -251,18 +412,75 @@ macro(ei_testing_print_summary) message(STATUS "SSE4.2: Using architecture defaults") endif() + if(EIGEN_TEST_AVX) + message(STATUS "AVX: ON") + else() + message(STATUS "AVX: Using architecture defaults") + endif() + + if(EIGEN_TEST_FMA) + message(STATUS "FMA: ON") + else() + message(STATUS "FMA: Using architecture defaults") + endif() + + if(EIGEN_TEST_AVX512) + message(STATUS "AVX512: ON") + else() + message(STATUS "AVX512: Using architecture defaults") + endif() + if(EIGEN_TEST_ALTIVEC) message(STATUS "Altivec: ON") else() message(STATUS "Altivec: Using architecture defaults") endif() + if(EIGEN_TEST_VSX) + message(STATUS "VSX: ON") + else() + message(STATUS "VSX: Using architecture defaults") + endif() + if(EIGEN_TEST_NEON) message(STATUS "ARM NEON: ON") else() message(STATUS "ARM NEON: Using architecture defaults") endif() + if(EIGEN_TEST_NEON64) + message(STATUS "ARMv8 NEON: ON") + else() + message(STATUS "ARMv8 NEON: Using architecture defaults") + endif() + + if(EIGEN_TEST_ZVECTOR) + message(STATUS "S390X ZVECTOR: ON") + else() + message(STATUS "S390X ZVECTOR: Using architecture defaults") + endif() + + if(EIGEN_TEST_CXX11) + message(STATUS "C++11: ON") + else() + message(STATUS "C++11: OFF") + endif() + + if(EIGEN_TEST_SYCL) + message(STATUS "SYCL: ON") + else() + message(STATUS "SYCL: OFF") + endif() + if(EIGEN_TEST_CUDA) + if(EIGEN_TEST_CUDA_CLANG) + message(STATUS "CUDA: ON (using clang)") + else() + message(STATUS "CUDA: ON (using nvcc)") + endif() + else() + message(STATUS "CUDA: OFF") + endif() + endif() # vectorization / alignment options message(STATUS "\n${EIGEN_TESTING_SUMMARY}") @@ -287,7 +505,7 @@ macro(ei_init_testing) set_property(GLOBAL PROPERTY EIGEN_FAILTEST_FAILURE_COUNT "0") set_property(GLOBAL PROPERTY EIGEN_FAILTEST_COUNT "0") - + # uncomment anytime you change the ei_get_compilerver_from_cxx_version_string macro # ei_test_get_compilerver_from_cxx_version_string() endmacro(ei_init_testing) @@ -296,47 +514,47 @@ macro(ei_set_sitename) # if the sitename is not yet set, try to set it if(NOT ${SITE} OR ${SITE} STREQUAL "") set(eigen_computername $ENV{COMPUTERNAME}) - set(eigen_hostname $ENV{HOSTNAME}) + set(eigen_hostname $ENV{HOSTNAME}) if(eigen_hostname) set(SITE ${eigen_hostname}) - elseif(eigen_computername) - set(SITE ${eigen_computername}) + elseif(eigen_computername) + set(SITE ${eigen_computername}) endif() endif() # in case it is already set, enforce lower case if(SITE) string(TOLOWER ${SITE} SITE) - endif() + endif() endmacro(ei_set_sitename) macro(ei_get_compilerver VAR) - if(MSVC) - # on windows system, we use a modified CMake script - include(EigenDetermineVSServicePack) - EigenDetermineVSServicePack( my_service_pack ) + if(MSVC) + # on windows system, we use a modified CMake script + include(EigenDetermineVSServicePack) + EigenDetermineVSServicePack( my_service_pack ) - if( my_service_pack ) - set(${VAR} ${my_service_pack}) + if( my_service_pack ) + set(${VAR} ${my_service_pack}) + else() + set(${VAR} "na") + endif() else() - set(${VAR} "na") - endif() - else() # on all other system we rely on ${CMAKE_CXX_COMPILER} # supporting a "--version" or "/version" flag - + if(WIN32 AND ${CMAKE_CXX_COMPILER_ID} EQUAL "Intel") set(EIGEN_CXX_FLAG_VERSION "/version") else() set(EIGEN_CXX_FLAG_VERSION "--version") endif() - - execute_process(COMMAND ${CMAKE_CXX_COMPILER} ${EIGEN_CXX_FLAG_VERSION} + + execute_process(COMMAND ${CMAKE_CXX_COMPILER} ${EIGEN_CXX_FLAG_VERSION} OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) string(REGEX REPLACE "[\n\r].*" "" eigen_cxx_compiler_version_string ${eigen_cxx_compiler_version_string}) - + ei_get_compilerver_from_cxx_version_string("${eigen_cxx_compiler_version_string}" CNAME CVER) set(${VAR} "${CNAME}-${CVER}") - + endif() endmacro(ei_get_compilerver) @@ -345,18 +563,20 @@ endmacro(ei_get_compilerver) # the testing macro call in ei_init_testing() of the EigenTesting.cmake file. # See also the ei_test_get_compilerver_from_cxx_version_string macro at the end of the file macro(ei_get_compilerver_from_cxx_version_string VERSTRING CNAME CVER) - # extract possible compiler names + # extract possible compiler names string(REGEX MATCH "g\\+\\+" ei_has_gpp ${VERSTRING}) string(REGEX MATCH "llvm|LLVM" ei_has_llvm ${VERSTRING}) string(REGEX MATCH "gcc|GCC" ei_has_gcc ${VERSTRING}) string(REGEX MATCH "icpc|ICC" ei_has_icpc ${VERSTRING}) string(REGEX MATCH "clang|CLANG" ei_has_clang ${VERSTRING}) - + # combine them if((ei_has_llvm) AND (ei_has_gpp OR ei_has_gcc)) set(${CNAME} "llvm-g++") elseif((ei_has_llvm) AND (ei_has_clang)) set(${CNAME} "llvm-clang++") + elseif(ei_has_clang) + set(${CNAME} "clang++") elseif(ei_has_icpc) set(${CNAME} "icpc") elseif(ei_has_gpp OR ei_has_gcc) @@ -364,7 +584,7 @@ macro(ei_get_compilerver_from_cxx_version_string VERSTRING CNAME CVER) else() set(${CNAME} "_") endif() - + # extract possible version numbers # first try to extract 3 isolated numbers: string(REGEX MATCH " [0-9]+\\.[0-9]+\\.[0-9]+" eicver ${VERSTRING}) @@ -382,9 +602,9 @@ macro(ei_get_compilerver_from_cxx_version_string VERSTRING CNAME CVER) endif() endif() endif() - + string(REGEX REPLACE ".(.*)" "\\1" ${CVER} ${eicver}) - + endmacro(ei_get_compilerver_from_cxx_version_string) macro(ei_get_cxxflags VAR) @@ -392,8 +612,18 @@ macro(ei_get_cxxflags VAR) ei_is_64bit_env(IS_64BIT_ENV) if(EIGEN_TEST_NEON) set(${VAR} NEON) + elseif(EIGEN_TEST_NEON64) + set(${VAR} NEON) + elseif(EIGEN_TEST_ZVECTOR) + set(${VAR} ZVECTOR) + elseif(EIGEN_TEST_VSX) + set(${VAR} VSX) elseif(EIGEN_TEST_ALTIVEC) set(${VAR} ALVEC) + elseif(EIGEN_TEST_FMA) + set(${VAR} FMA) + elseif(EIGEN_TEST_AVX) + set(${VAR} AVX) elseif(EIGEN_TEST_SSE4_2) set(${VAR} SSE42) elseif(EIGEN_TEST_SSE4_1) @@ -403,30 +633,30 @@ macro(ei_get_cxxflags VAR) elseif(EIGEN_TEST_SSE3) set(${VAR} SSE3) elseif(EIGEN_TEST_SSE2 OR IS_64BIT_ENV) - set(${VAR} SSE2) + set(${VAR} SSE2) endif() if(EIGEN_TEST_OPENMP) if (${VAR} STREQUAL "") - set(${VAR} OMP) - else() - set(${VAR} ${${VAR}}-OMP) - endif() + set(${VAR} OMP) + else() + set(${VAR} ${${VAR}}-OMP) + endif() endif() - + if(EIGEN_DEFAULT_TO_ROW_MAJOR) if (${VAR} STREQUAL "") - set(${VAR} ROW) - else() - set(${VAR} ${${VAR}}-ROWMAJ) - endif() + set(${VAR} ROW) + else() + set(${VAR} ${${VAR}}-ROWMAJ) + endif() endif() endmacro(ei_get_cxxflags) macro(ei_set_build_string) ei_get_compilerver(LOCAL_COMPILER_VERSION) ei_get_cxxflags(LOCAL_COMPILER_FLAGS) - + include(EigenDetermineOSVersion) DetermineOSVersion(OS_VERSION) @@ -442,7 +672,11 @@ macro(ei_set_build_string) else() set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-64bit) endif() - + + if(EIGEN_TEST_CXX11) + set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-cxx11) + endif() + if(EIGEN_BUILD_STRING_SUFFIX) set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-${EIGEN_BUILD_STRING_SUFFIX}) endif() diff --git a/ground/gcs/src/libs/eigen/cmake/EigenUninstall.cmake b/ground/gcs/src/libs/eigen/cmake/EigenUninstall.cmake new file mode 100644 index 0000000000..4dae8c85c8 --- /dev/null +++ b/ground/gcs/src/libs/eigen/cmake/EigenUninstall.cmake @@ -0,0 +1,40 @@ +################ CMake Uninstall Template ####################### +# CMake Template file for uninstallation of files +# mentioned in 'install_manifest.txt' +# +# Used by uinstall target +################################################################# + +set(MANIFEST "${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt") + +if(EXISTS ${MANIFEST}) + message(STATUS "============== Uninstalling Eigen ===================") + + file(STRINGS ${MANIFEST} files) + foreach(file ${files}) + if(EXISTS ${file}) + message(STATUS "Removing file: '${file}'") + + execute_process( + COMMAND ${CMAKE_COMMAND} -E remove ${file} + OUTPUT_VARIABLE rm_out + RESULT_VARIABLE rm_retval + ) + + if(NOT "${rm_retval}" STREQUAL 0) + message(FATAL_ERROR "Failed to remove file: '${file}'.") + endif() + else() + message(STATUS "File '${file}' does not exist.") + endif() + endforeach(file) + + message(STATUS "========== Finished Uninstalling Eigen ==============") +else() + message(STATUS "Cannot find install manifest: '${MANIFEST}'") + message(STATUS "Probably make install has not been performed") + message(STATUS " or install_manifest.txt has been deleted.") +endif() + + + diff --git a/ground/gcs/src/libs/eigen/cmake/FindAdolc.cmake b/ground/gcs/src/libs/eigen/cmake/FindAdolc.cmake index 1a7ff3628a..937e549904 100644 --- a/ground/gcs/src/libs/eigen/cmake/FindAdolc.cmake +++ b/ground/gcs/src/libs/eigen/cmake/FindAdolc.cmake @@ -5,7 +5,7 @@ endif (ADOLC_INCLUDES AND ADOLC_LIBRARIES) find_path(ADOLC_INCLUDES NAMES - adolc/adouble.h + adolc/adtl.h PATHS $ENV{ADOLCDIR} ${INCLUDE_INSTALL_DIR} diff --git a/ground/gcs/src/libs/eigen/cmake/FindComputeCpp.cmake b/ground/gcs/src/libs/eigen/cmake/FindComputeCpp.cmake new file mode 100644 index 0000000000..07ebed61b9 --- /dev/null +++ b/ground/gcs/src/libs/eigen/cmake/FindComputeCpp.cmake @@ -0,0 +1,245 @@ +#.rst: +# FindComputeCpp +#--------------- +# +# Copyright 2016 Codeplay Software Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use these files except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +######################### +# FindComputeCpp.cmake +######################### +# +# Tools for finding and building with ComputeCpp. +# +# User must define COMPUTECPP_PACKAGE_ROOT_DIR pointing to the ComputeCpp +# installation. +# +# Latest version of this file can be found at: +# https://github.com/codeplaysoftware/computecpp-sdk + +# Require CMake version 3.2.2 or higher +cmake_minimum_required(VERSION 3.2.2) + +# Check that a supported host compiler can be found +if(CMAKE_COMPILER_IS_GNUCXX) + # Require at least gcc 4.8 + if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) + message(FATAL_ERROR + "host compiler - Not found! (gcc version must be at least 4.8)") + # Require the GCC dual ABI to be disabled for 5.1 or higher + elseif (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.1) + set(COMPUTECPP_DISABLE_GCC_DUAL_ABI "True") + message(STATUS + "host compiler - gcc ${CMAKE_CXX_COMPILER_VERSION} (note pre 5.1 gcc ABI enabled)") + else() + message(STATUS "host compiler - gcc ${CMAKE_CXX_COMPILER_VERSION}") + endif() +elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + # Require at least clang 3.6 + if (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 3.6) + message(FATAL_ERROR + "host compiler - Not found! (clang version must be at least 3.6)") + else() + message(STATUS "host compiler - clang ${CMAKE_CXX_COMPILER_VERSION}") + endif() +else() + message(WARNING + "host compiler - Not found! (ComputeCpp supports GCC and Clang, see readme)") +endif() + +set(COMPUTECPP_64_BIT_DEFAULT ON) +option(COMPUTECPP_64_BIT_CODE "Compile device code in 64 bit mode" + ${COMPUTECPP_64_BIT_DEFAULT}) +mark_as_advanced(COMPUTECPP_64_BIT_CODE) + +# Find OpenCL package +find_package(OpenCL REQUIRED) + +# Find ComputeCpp packagee +if(NOT COMPUTECPP_PACKAGE_ROOT_DIR) + message(FATAL_ERROR + "ComputeCpp package - Not found! (please set COMPUTECPP_PACKAGE_ROOT_DIR") +else() + message(STATUS "ComputeCpp package - Found") +endif() +option(COMPUTECPP_PACKAGE_ROOT_DIR "Path to the ComputeCpp Package") + +# Obtain the path to compute++ +find_program(COMPUTECPP_DEVICE_COMPILER compute++ PATHS + ${COMPUTECPP_PACKAGE_ROOT_DIR} PATH_SUFFIXES bin) +if (EXISTS ${COMPUTECPP_DEVICE_COMPILER}) + mark_as_advanced(COMPUTECPP_DEVICE_COMPILER) + message(STATUS "compute++ - Found") +else() + message(FATAL_ERROR "compute++ - Not found! (${COMPUTECPP_DEVICE_COMPILER})") +endif() + +# Obtain the path to computecpp_info +find_program(COMPUTECPP_INFO_TOOL computecpp_info PATHS + ${COMPUTECPP_PACKAGE_ROOT_DIR} PATH_SUFFIXES bin) +if (EXISTS ${COMPUTECPP_INFO_TOOL}) + mark_as_advanced(${COMPUTECPP_INFO_TOOL}) + message(STATUS "computecpp_info - Found") +else() + message(FATAL_ERROR "computecpp_info - Not found! (${COMPUTECPP_INFO_TOOL})") +endif() + +# Obtain the path to the ComputeCpp runtime library +find_library(COMPUTECPP_RUNTIME_LIBRARY ComputeCpp PATHS ${COMPUTECPP_PACKAGE_ROOT_DIR} + HINTS ${COMPUTECPP_PACKAGE_ROOT_DIR}/lib PATH_SUFFIXES lib + DOC "ComputeCpp Runtime Library" NO_DEFAULT_PATH) + +if (EXISTS ${COMPUTECPP_RUNTIME_LIBRARY}) + mark_as_advanced(COMPUTECPP_RUNTIME_LIBRARY) + message(STATUS "libComputeCpp.so - Found") +else() + message(FATAL_ERROR "libComputeCpp.so - Not found!") +endif() + +# Obtain the ComputeCpp include directory +set(COMPUTECPP_INCLUDE_DIRECTORY ${COMPUTECPP_PACKAGE_ROOT_DIR}/include/) +if (NOT EXISTS ${COMPUTECPP_INCLUDE_DIRECTORY}) + message(FATAL_ERROR "ComputeCpp includes - Not found!") +else() + message(STATUS "ComputeCpp includes - Found") +endif() + +# Obtain the package version +execute_process(COMMAND ${COMPUTECPP_INFO_TOOL} "--dump-version" + OUTPUT_VARIABLE COMPUTECPP_PACKAGE_VERSION + RESULT_VARIABLE COMPUTECPP_INFO_TOOL_RESULT OUTPUT_STRIP_TRAILING_WHITESPACE) +if(NOT COMPUTECPP_INFO_TOOL_RESULT EQUAL "0") + message(FATAL_ERROR "Package version - Error obtaining version!") +else() + mark_as_advanced(COMPUTECPP_PACKAGE_VERSION) + message(STATUS "Package version - ${COMPUTECPP_PACKAGE_VERSION}") +endif() + +# Obtain the device compiler flags +execute_process(COMMAND ${COMPUTECPP_INFO_TOOL} "--dump-device-compiler-flags" + OUTPUT_VARIABLE COMPUTECPP_DEVICE_COMPILER_FLAGS + RESULT_VARIABLE COMPUTECPP_INFO_TOOL_RESULT OUTPUT_STRIP_TRAILING_WHITESPACE) +if(NOT COMPUTECPP_INFO_TOOL_RESULT EQUAL "0") + message(FATAL_ERROR "compute++ flags - Error obtaining compute++ flags!") +else() + mark_as_advanced(COMPUTECPP_COMPILER_FLAGS) + message(STATUS "compute++ flags - ${COMPUTECPP_DEVICE_COMPILER_FLAGS}") +endif() + +set(COMPUTECPP_DEVICE_COMPILER_FLAGS ${COMPUTECPP_DEVICE_COMPILER_FLAGS} -sycl-compress-name -no-serial-memop -DEIGEN_NO_ASSERTION_CHECKING=1) + +# Check if the platform is supported +execute_process(COMMAND ${COMPUTECPP_INFO_TOOL} "--dump-is-supported" + OUTPUT_VARIABLE COMPUTECPP_PLATFORM_IS_SUPPORTED + RESULT_VARIABLE COMPUTECPP_INFO_TOOL_RESULT OUTPUT_STRIP_TRAILING_WHITESPACE) +if(NOT COMPUTECPP_INFO_TOOL_RESULT EQUAL "0") + message(FATAL_ERROR "platform - Error checking platform support!") +else() + mark_as_advanced(COMPUTECPP_PLATFORM_IS_SUPPORTED) + if (COMPUTECPP_PLATFORM_IS_SUPPORTED) + message(STATUS "platform - your system can support ComputeCpp") + else() + message(STATUS "platform - your system CANNOT support ComputeCpp") + endif() +endif() + +#################### +# __build_sycl +#################### +# +# Adds a custom target for running compute++ and adding a dependency for the +# resulting integration header. +# +# targetName : Name of the target. +# sourceFile : Source file to be compiled. +# binaryDir : Intermediate directory to output the integration header. +# +function(__build_spir targetName sourceFile binaryDir) + + # Retrieve source file name. + get_filename_component(sourceFileName ${sourceFile} NAME) + + # Set the path to the Sycl file. + set(outputSyclFile ${binaryDir}/${sourceFileName}.sycl) + + # Add any user-defined include to the device compiler + get_property(includeDirectories DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY + INCLUDE_DIRECTORIES) + set(device_compiler_includes "") + foreach(directory ${includeDirectories}) + set(device_compiler_includes "-I${directory}" ${device_compiler_includes}) + endforeach() + if (CMAKE_INCLUDE_PATH) + foreach(directory ${CMAKE_INCLUDE_PATH}) + set(device_compiler_includes "-I${directory}" + ${device_compiler_includes}) + endforeach() + endif() + + # Convert argument list format + separate_arguments(COMPUTECPP_DEVICE_COMPILER_FLAGS) + + # Add custom command for running compute++ + add_custom_command( + OUTPUT ${outputSyclFile} + COMMAND ${COMPUTECPP_DEVICE_COMPILER} + ${COMPUTECPP_DEVICE_COMPILER_FLAGS} + -isystem ${COMPUTECPP_INCLUDE_DIRECTORY} + ${COMPUTECPP_PLATFORM_SPECIFIC_ARGS} + ${device_compiler_includes} + -o ${outputSyclFile} + -c ${CMAKE_CURRENT_SOURCE_DIR}/${sourceFile} + DEPENDS ${sourceFile} + WORKING_DIRECTORY ${binaryDir} + COMMENT "Building ComputeCpp integration header file ${outputSyclFile}") + + # Add a custom target for the generated integration header + add_custom_target(${targetName}_integration_header DEPENDS ${outputSyclFile}) + + # Add a dependency on the integration header + add_dependencies(${targetName} ${targetName}_integration_header) + + # Set the host compiler C++ standard to C++11 + set_property(TARGET ${targetName} PROPERTY CXX_STANDARD 11) + + # Disable GCC dual ABI on GCC 5.1 and higher + if(COMPUTECPP_DISABLE_GCC_DUAL_ABI) + set_property(TARGET ${targetName} APPEND PROPERTY COMPILE_DEFINITIONS + "_GLIBCXX_USE_CXX11_ABI=0") + endif() + +endfunction() + +####################### +# add_sycl_to_target +####################### +# +# Adds a SYCL compilation custom command associated with an existing +# target and sets a dependancy on that new command. +# +# targetName : Name of the target to add a SYCL to. +# sourceFile : Source file to be compiled for SYCL. +# binaryDir : Intermediate directory to output the integration header. +# +function(add_sycl_to_target targetName sourceFile binaryDir) + + # Add custom target to run compute++ and generate the integration header + __build_spir(${targetName} ${sourceFile} ${binaryDir}) + + # Link with the ComputeCpp runtime library + target_link_libraries(${targetName} PUBLIC ${COMPUTECPP_RUNTIME_LIBRARY} + PUBLIC ${OpenCL_LIBRARIES}) + +endfunction(add_sycl_to_target) diff --git a/ground/gcs/src/libs/eigen/cmake/FindEigen3.cmake b/ground/gcs/src/libs/eigen/cmake/FindEigen3.cmake index 9c546a05d8..9e96978608 100644 --- a/ground/gcs/src/libs/eigen/cmake/FindEigen3.cmake +++ b/ground/gcs/src/libs/eigen/cmake/FindEigen3.cmake @@ -9,6 +9,12 @@ # EIGEN3_FOUND - system has eigen lib with correct version # EIGEN3_INCLUDE_DIR - the eigen include directory # EIGEN3_VERSION - eigen version +# +# This module reads hints about search locations from +# the following enviroment variables: +# +# EIGEN3_ROOT +# EIGEN3_ROOT_DIR # Copyright (c) 2006, 2007 Montel Laurent, # Copyright (c) 2008, 2009 Gael Guennebaud, @@ -60,13 +66,23 @@ if (EIGEN3_INCLUDE_DIR) set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) + + # search first if an Eigen3Config.cmake is available in the system, + # if successful this would set EIGEN3_INCLUDE_DIR and the rest of + # the script will work as usual + find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) + + if(NOT EIGEN3_INCLUDE_DIR) + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + HINTS + ENV EIGEN3_ROOT + ENV EIGEN3_ROOT_DIR + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + endif(NOT EIGEN3_INCLUDE_DIR) if(EIGEN3_INCLUDE_DIR) _eigen3_check_version() diff --git a/ground/gcs/src/libs/eigen/cmake/FindSuperLU.cmake b/ground/gcs/src/libs/eigen/cmake/FindSuperLU.cmake index 8a3df36668..f38146e06c 100644 --- a/ground/gcs/src/libs/eigen/cmake/FindSuperLU.cmake +++ b/ground/gcs/src/libs/eigen/cmake/FindSuperLU.cmake @@ -17,10 +17,81 @@ find_path(SUPERLU_INCLUDES SRC ) -find_library(SUPERLU_LIBRARIES superlu PATHS $ENV{SUPERLUDIR} ${LIB_INSTALL_DIR} PATH_SUFFIXES lib) - +find_library(SUPERLU_LIBRARIES + NAMES "superlu_5.2.1" "superlu_5.2" "superlu_5.1.1" "superlu_5.1" "superlu_5.0" "superlu_4.3" "superlu_4.2" "superlu_4.1" "superlu_4.0" "superlu_3.1" "superlu_3.0" "superlu" + PATHS $ENV{SUPERLUDIR} ${LIB_INSTALL_DIR} + PATH_SUFFIXES lib) + +if(SUPERLU_INCLUDES AND SUPERLU_LIBRARIES) + +include(CheckCXXSourceCompiles) +include(CMakePushCheckState) +cmake_push_check_state() + +set(CMAKE_REQUIRED_INCLUDES ${CMAKE_REQUIRED_INCLUDES} ${SUPERLU_INCLUDES}) + +# check whether struct mem_usage_t is globally defined +check_cxx_source_compiles(" +typedef int int_t; +#include +#include +int main() { + mem_usage_t mem; + return 0; +}" +SUPERLU_HAS_GLOBAL_MEM_USAGE_T) + + +check_cxx_source_compiles(" +typedef int int_t; +#include +#include +int main() { + return SLU_SINGLE; +}" +SUPERLU_HAS_CLEAN_ENUMS) + +check_cxx_source_compiles(" +typedef int int_t; +#include +#include +int main(void) +{ + GlobalLU_t glu; + return 0; +}" +SUPERLU_HAS_GLOBALLU_T) + +if(SUPERLU_HAS_GLOBALLU_T) + # at least 5.0 + set(SUPERLU_VERSION_VAR "5.0") +elseif(SUPERLU_HAS_CLEAN_ENUMS) + # at least 4.3 + set(SUPERLU_VERSION_VAR "4.3") +elseif(SUPERLU_HAS_GLOBAL_MEM_USAGE_T) + # at least 4.0 + set(SUPERLU_VERSION_VAR "4.0") +else() + set(SUPERLU_VERSION_VAR "3.0") +endif() + +cmake_pop_check_state() + +if(SuperLU_FIND_VERSION) + if(${SUPERLU_VERSION_VAR} VERSION_LESS ${SuperLU_FIND_VERSION}) + set(SUPERLU_VERSION_OK FALSE) + else() + set(SUPERLU_VERSION_OK TRUE) + endif() +else() + set(SUPERLU_VERSION_OK TRUE) +endif() + +endif() + include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(SUPERLU DEFAULT_MSG - SUPERLU_INCLUDES SUPERLU_LIBRARIES) +find_package_handle_standard_args(SUPERLU + REQUIRED_VARS SUPERLU_INCLUDES SUPERLU_LIBRARIES SUPERLU_VERSION_OK + VERSION_VAR SUPERLU_VERSION_VAR) mark_as_advanced(SUPERLU_INCLUDES SUPERLU_LIBRARIES) diff --git a/ground/gcs/src/libs/eigen/cmake/UseEigen3.cmake b/ground/gcs/src/libs/eigen/cmake/UseEigen3.cmake new file mode 100644 index 0000000000..a38bac82d5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/cmake/UseEigen3.cmake @@ -0,0 +1,6 @@ +# -*- cmake -*- +# +# UseEigen3.cmake + +add_definitions ( ${EIGEN3_DEFINITIONS} ) +include_directories ( ${EIGEN3_INCLUDE_DIRS} ) diff --git a/ground/gcs/src/libs/eigen/cmake/language_support.cmake b/ground/gcs/src/libs/eigen/cmake/language_support.cmake index 231f7ff703..2f14f30b84 100644 --- a/ground/gcs/src/libs/eigen/cmake/language_support.cmake +++ b/ground/gcs/src/libs/eigen/cmake/language_support.cmake @@ -43,7 +43,7 @@ function(workaround_9220 language language_works) if(return_code EQUAL 0) # Second run execute_process ( - COMMAND ${CMAKE_COMMAND} . + COMMAND ${CMAKE_COMMAND} . -G "${CMAKE_GENERATOR}" WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language} RESULT_VARIABLE return_code OUTPUT_QUIET @@ -64,3 +64,4 @@ endfunction(workaround_9220) #message("CXX_language_works = ${CXX_language_works}") #workaround_9220(CXXp CXXp_language_works) #message("CXXp_language_works = ${CXXp_language_works}") + diff --git a/ground/gcs/src/libs/eigen/debug/gdb/printers.py b/ground/gcs/src/libs/eigen/debug/gdb/printers.py index 86996a4f9f..0d67a5f998 100644 --- a/ground/gcs/src/libs/eigen/debug/gdb/printers.py +++ b/ground/gcs/src/libs/eigen/debug/gdb/printers.py @@ -49,7 +49,7 @@ def __init__(self, variety, val): regex = re.compile('\<.*\>') m = regex.findall(tag)[0][1:-1] template_params = m.split(',') - template_params = map(lambda x:x.replace(" ", ""), template_params) + template_params = [x.replace(" ", "") for x in template_params] if template_params[1] == '-0x00000000000000001' or template_params[1] == '-0x000000001' or template_params[1] == '-1': self.rows = val['m_storage']['m_rows'] @@ -88,8 +88,11 @@ def __init__ (self, rows, cols, dataPtr, rowMajor): def __iter__ (self): return self - + def next(self): + return self.__next__() # Python 2.x compatibility + + def __next__(self): row = self.currentRow col = self.currentCol @@ -151,8 +154,11 @@ def __init__ (self, dataPtr): def __iter__ (self): return self - + def next(self): + return self.__next__() # Python 2.x compatibility + + def __next__(self): element = self.currentElement if self.currentElement >= 4: #there are 4 elements in a quanternion diff --git a/ground/gcs/src/libs/eigen/demos/opengl/quaternion_demo.cpp b/ground/gcs/src/libs/eigen/demos/opengl/quaternion_demo.cpp index 04165619b0..dd323a4c96 100644 --- a/ground/gcs/src/libs/eigen/demos/opengl/quaternion_demo.cpp +++ b/ground/gcs/src/libs/eigen/demos/opengl/quaternion_demo.cpp @@ -234,7 +234,7 @@ void RenderingWidget::drawScene() gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitZ(), Color(0,0,1,1)); // draw the fractal object - float sqrt3 = internal::sqrt(3.); + float sqrt3 = std::sqrt(3.); glLightfv(GL_LIGHT0, GL_AMBIENT, Vector4f(0.5,0.5,0.5,1).data()); glLightfv(GL_LIGHT0, GL_DIFFUSE, Vector4f(0.5,1,0.5,1).data()); glLightfv(GL_LIGHT0, GL_SPECULAR, Vector4f(1,1,1,1).data()); diff --git a/ground/gcs/src/libs/eigen/demos/opengl/trackball.cpp b/ground/gcs/src/libs/eigen/demos/opengl/trackball.cpp index 77ac790c84..7c2da8e965 100644 --- a/ground/gcs/src/libs/eigen/demos/opengl/trackball.cpp +++ b/ground/gcs/src/libs/eigen/demos/opengl/trackball.cpp @@ -23,7 +23,7 @@ void Trackball::track(const Vector2i& point2D) { Vector3f axis = mLastPoint3D.cross(newPoint3D).normalized(); float cos_angle = mLastPoint3D.dot(newPoint3D); - if ( internal::abs(cos_angle) < 1.0 ) + if ( std::abs(cos_angle) < 1.0 ) { float angle = 2. * acos(cos_angle); if (mMode==Around) diff --git a/ground/gcs/src/libs/eigen/doc/A05_PortingFrom2To3.dox b/ground/gcs/src/libs/eigen/doc/A05_PortingFrom2To3.dox index 4d5f3ae1ff..51555f9967 100644 --- a/ground/gcs/src/libs/eigen/doc/A05_PortingFrom2To3.dox +++ b/ground/gcs/src/libs/eigen/doc/A05_PortingFrom2To3.dox @@ -2,8 +2,6 @@ namespace Eigen { /** \page Eigen2ToEigen3 Porting from Eigen2 to Eigen3 -
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
- This page lists the most important API changes between Eigen2 and Eigen3, and gives tips to help porting your application from Eigen2 to Eigen3. @@ -11,11 +9,8 @@ and gives tips to help porting your application from Eigen2 to Eigen3. \section CompatibilitySupport Eigen2 compatibility support -In order to ease the switch from Eigen2 to Eigen3, Eigen3 features \subpage Eigen2SupportModes "Eigen2 support modes". - -The quick way to enable this is to define the \c EIGEN2_SUPPORT preprocessor token \b before including any Eigen header (typically it should be set in your project options). - -A more powerful, \em staged migration path is also provided, which may be useful to migrate larger projects from Eigen2 to Eigen3. This is explained in the \ref Eigen2SupportModes "Eigen 2 support modes" page. +Up to version 3.2 %Eigen provides Eigen2 support modes. These are removed now, because they were barely used anymore and became hard to maintain after internal re-designs. +You can still use them by first porting your code to Eigen 3.2. \section Using The USING_PART_OF_NAMESPACE_EIGEN macro @@ -228,7 +223,7 @@ triangular part to work on \section GeometryModule Changes in the Geometry module -The Geometry module is the one that changed the most. If you rely heavily on it, it's probably a good idea to use the \ref Eigen2SupportModes "Eigen 2 support modes" to perform your migration. +The Geometry module is the one that changed the most. If you rely heavily on it, it's probably a good idea to use the "Eigen 2 support modes" to perform your migration. \section Transform The Transform class @@ -266,7 +261,7 @@ use it unless you are sure of what you are doing, i.e., you have rigourosly meas The EIGEN_ALIGN_128 macro has been renamed to EIGEN_ALIGN16. Don't be surprised, it's just that we switched to counting in bytes ;-) -The EIGEN_DONT_ALIGN option still exists in Eigen 3, but it has a new cousin: EIGEN_DONT_ALIGN_STATICALLY. It allows to get rid of all static alignment issues while keeping alignment of dynamic-size heap-allocated arrays, thus keeping vectorization for dynamic-size objects. +The \link TopicPreprocessorDirectivesPerformance EIGEN_DONT_ALIGN \endlink option still exists in Eigen 3, but it has a new cousin: \link TopicPreprocessorDirectivesPerformance EIGEN_DONT_ALIGN_STATICALLY.\endlink It allows to get rid of all static alignment issues while keeping alignment of dynamic-size heap-allocated arrays. Vectorization of statically allocated arrays is still preserved (unless you define \link TopicPreprocessorDirectivesPerformance EIGEN_UNALIGNED_VECTORIZE \endlink =0), at the cost of unaligned memory stores. \section AlignedMap Aligned Map objects @@ -283,7 +278,7 @@ result = Vector4f::MapAligned(some_aligned_array); \section StdContainers STL Containers -In Eigen2, #include tweaked std::vector to automatically align elements. The problem was that that was quite invasive. In Eigen3, we only override standard behavior if you use Eigen::aligned_allocator as your allocator type. So for example, if you use std::vector, you need to do the following change (note that aligned_allocator is under namespace Eigen): +In Eigen2, \#include\ tweaked std::vector to automatically align elements. The problem was that that was quite invasive. In Eigen3, we only override standard behavior if you use Eigen::aligned_allocator as your allocator type. So for example, if you use std::vector, you need to do the following change (note that aligned_allocator is under namespace Eigen): diff --git a/ground/gcs/src/libs/eigen/doc/A10_Eigen2SupportModes.dox b/ground/gcs/src/libs/eigen/doc/A10_Eigen2SupportModes.dox deleted file mode 100644 index f3df91515a..0000000000 --- a/ground/gcs/src/libs/eigen/doc/A10_Eigen2SupportModes.dox +++ /dev/null @@ -1,95 +0,0 @@ -namespace Eigen { - -/** \page Eigen2SupportModes Eigen 2 support modes - -
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
- -This page documents the Eigen2 support modes, a powerful tool to help migrating your project from Eigen 2 to Eigen 3. -Don't miss our page on \ref Eigen2ToEigen3 "API changes" between Eigen 2 and Eigen 3. - -\eigenAutoToc - -\section EIGEN2_SUPPORT_Macro The quick way: define EIGEN2_SUPPORT - -By defining EIGEN2_SUPPORT before including any Eigen 3 header, you get back a large part of the Eigen 2 API, while keeping the Eigen 3 API and ABI unchanged. - -This defaults to the \ref Stage30 "stage 30" described below. - -The rest of this page describes an optional, more powerful \em staged migration path. - -\section StagedMigrationPathOverview Overview of the staged migration path - -The primary reason why EIGEN2_SUPPORT alone may not be enough to migrate a large project from Eigen 2 to Eigen 3 is that some of the Eigen 2 API is inherently incompatible with the Eigen 3 API. This happens when the same identifier is used in Eigen 2 and in Eigen 3 with different meanings. To help migrate projects that rely on such API, we provide a staged migration path allowing to perform the migration \em incrementally. - -It goes as follows: -\li Step 0: start with a project using Eigen 2. -\li Step 1: build your project against Eigen 3 with \ref Stage10 "Eigen 2 support stage 10". This mode enables maximum compatibility with the Eigen 2 API, with just a few exceptions. -\li Step 2: build your project against Eigen 3 with \ref Stage20 "Eigen 2 support stage 20". This mode forces you to add eigen2_ prefixes to the Eigen2 identifiers that conflict with Eigen 3 API. -\li Step 3: build your project against Eigen 3 with \ref Stage30 "Eigen 2 support stage 30". This mode enables the full Eigen 3 API. -\li Step 4: build your project against Eigen 3 with \ref Stage40 "Eigen 2 support stage 40". This mode enables the full Eigen 3 strictness on matters, such as const-correctness, where Eigen 2 was looser. -\li Step 5: build your project against Eigen 3 without any Eigen 2 support mode. - -\section Stage10 Stage 10: define EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API - -Enable this mode by defining the EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API preprocessor macro before including any Eigen 3 header. - -This mode maximizes support for the Eigen 2 API. As a result, it does not offer the full Eigen 3 API. Also, it doesn't offer quite 100% of the Eigen 2 API. - -The part of the Eigen 3 API that is not present in this mode, is Eigen 3's Geometry module. Indeed, this mode completely replaces it by a copy of Eigen 2's Geometry module. - -The parts of the API that are still not 100% Eigen 2 compatible in this mode are: -\li Dot products over complex numbers. Eigen 2's dot product was linear in the first variable. Eigen 3's dot product is linear in the second variable. In other words, the Eigen 2 code \code x.dot(y) \endcode is equivalent to the Eigen 3 code \code y.dot(x) \endcode In yet other words, dot products are complex-conjugated in Eigen 3 compared to Eigen 2. The switch to the new convention was commanded by common usage, especially with the notation \f$ x^Ty \f$ for dot products of column-vectors. -\li The Sparse module. -\li Certain fine details of linear algebraic decompositions. For example, LDLT decomposition is now pivoting in Eigen 3 whereas it wasn't in Eigen 2, so code that was relying on its underlying matrix structure will break. -\li Usage of Eigen types in STL containers, \ref Eigen2ToEigen3 "as explained on this page". - -\section Stage20 Stage 20: define EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS - -Enable this mode by defining the EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API preprocessor macro before including any Eigen 3 header. - -This mode removes the Eigen 2 API that is directly conflicting with Eigen 3 API. Instead, these bits of Eigen 2 API remain available with eigen2_ prefixes. The main examples of such API are: -\li the whole Geometry module. For example, replace \c Quaternion by \c eigen2_Quaternion, replace \c Transform3f by \c eigen2_Transform3f, etc. -\li the lu() method to obtain a LU decomposition. Replace by eigen2_lu(). - -There is also one more eigen2_-prefixed identifier that you should know about, even though its use is not checked at compile time by this mode: the dot() method. As was discussed above, over complex numbers, its meaning is different between Eigen 2 and Eigen 3. You can use eigen2_dot() to get the Eigen 2 behavior. - -\section Stage30 Stage 30: define EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API - -Enable this mode by defining the EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API preprocessor macro before including any Eigen 3 header. Also, this mode is what you get by default when you just define EIGEN2_SUPPORT. - -This mode gives you the full unaltered Eigen 3 API, while still keeping as much support as possible for the Eigen 2 API. - -The eigen2_-prefixed identifiers are still available, but at this stage you should now replace them by Eigen 3 identifiers. Have a look at our page on \ref Eigen2ToEigen3 "API changes" between Eigen 2 and Eigen 3. - -\section Stage40 Stage 40: define EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS - -Enable this mode by defining the EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS preprocessor macro before including any Eigen 3 header. - -This mode tightens the last bits of strictness, especially const-correctness, that had to be loosened to support what Eigen 2 allowed. For example, this code compiled in Eigen 2: -\code -const float array[4]; -x = Map(array); -\endcode -That allowed to circumvent constness. This is no longer allowed in Eigen 3. If you have to map const data in Eigen 3, map it as a const-qualified type. However, rather than explictly constructing Map objects, we strongly encourage you to use the static Map methods instead, as they take care of all of this for you: -\code -const float array[4]; -x = Vector4f::Map(array); -\endcode -This lets Eigen do the right thing for you and works equally well in Eigen 2 and in Eigen 3. - -\section FinallyDropAllEigen2Support Finally drop all Eigen 2 support - -Stage 40 is the first where it's "comfortable" to stay for a little longer period, since it preserves 100% Eigen 3 compatibility. However, we still encourage you to complete your migration as quickly as possible. While we do run the Eigen 2 test suite against Eigen 3's stage 10 support mode, we can't guarantee the same level of support and quality assurance for Eigen 2 support as we do for Eigen 3 itself, especially not in the long term. \ref Eigen2ToEigen3 "This page" describes a large part of the changes that you may need to perform. - -\section ABICompatibility What about ABI compatibility? - -It goes as follows: -\li Stage 10 already is ABI compatible with Eigen 3 for the basic (Matrix, Array, SparseMatrix...) types. However, since this stage uses a copy of Eigen 2's Geometry module instead of Eigen 3's own Geometry module, the ABI in the Geometry module is not Eigen 3 compatible. -\li Stage 20 removes the Eigen 3-incompatible Eigen 2 Geometry module (it remains available with eigen2_ prefix). So at this stage, all the identifiers that exist in Eigen 3 have the Eigen 3 ABI (and API). -\li Stage 30 introduces the remaining Eigen 3 identifiers. So at this stage, you have the full Eigen 3 ABI. -\li Stage 40 is no different than Stage 30 in these matters. - - -*/ - -} diff --git a/ground/gcs/src/libs/eigen/doc/AsciiQuickReference.txt b/ground/gcs/src/libs/eigen/doc/AsciiQuickReference.txt index b9f497f87e..8409f88506 100644 --- a/ground/gcs/src/libs/eigen/doc/AsciiQuickReference.txt +++ b/ground/gcs/src/libs/eigen/doc/AsciiQuickReference.txt @@ -32,17 +32,19 @@ A << 1, 2, 3, // Initialize A. The elements can also be B << A, A, A; // B is three horizontally stacked A's. A.fill(10); // Fill A with all 10's. -// Eigen // Matlab -MatrixXd::Identity(rows,cols) // eye(rows,cols) -C.setIdentity(rows,cols) // C = eye(rows,cols) -MatrixXd::Zero(rows,cols) // zeros(rows,cols) -C.setZero(rows,cols) // C = ones(rows,cols) -MatrixXd::Ones(rows,cols) // ones(rows,cols) -C.setOnes(rows,cols) // C = ones(rows,cols) -MatrixXd::Random(rows,cols) // rand(rows,cols)*2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1). -C.setRandom(rows,cols) // C = rand(rows,cols)*2-1 -VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)' -v.setLinSpaced(size,low,high) // v = linspace(low,high,size)' +// Eigen // Matlab +MatrixXd::Identity(rows,cols) // eye(rows,cols) +C.setIdentity(rows,cols) // C = eye(rows,cols) +MatrixXd::Zero(rows,cols) // zeros(rows,cols) +C.setZero(rows,cols) // C = zeros(rows,cols) +MatrixXd::Ones(rows,cols) // ones(rows,cols) +C.setOnes(rows,cols) // C = ones(rows,cols) +MatrixXd::Random(rows,cols) // rand(rows,cols)*2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1). +C.setRandom(rows,cols) // C = rand(rows,cols)*2-1 +VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)' +v.setLinSpaced(size,low,high) // v = linspace(low,high,size)' +VectorXi::LinSpaced(((hi-low)/step)+1, // low:step:hi + low,low+step*(size-1)) // // Matrix slicing and blocks. All expressions listed here are read/write. @@ -82,17 +84,20 @@ P.bottomRightCorner() // P(end-rows+1:end, end-cols+1:end) // Of particular note is Eigen's swap function which is highly optimized. // Eigen // Matlab -R.row(i) = P.col(j); // R(i, :) = P(:, i) +R.row(i) = P.col(j); // R(i, :) = P(:, j) R.col(j1).swap(mat1.col(j2)); // R(:, [j1 j2]) = R(:, [j2, j1]) -// Views, transpose, etc; all read-write except for .adjoint(). +// Views, transpose, etc; // Eigen // Matlab R.adjoint() // R' -R.transpose() // R.' or conj(R') -R.diagonal() // diag(R) +R.transpose() // R.' or conj(R') // Read-write +R.diagonal() // diag(R) // Read-write x.asDiagonal() // diag(x) -R.transpose().colwise().reverse(); // rot90(R) -R.conjugate() // conj(R) +R.transpose().colwise().reverse() // rot90(R) // Read-write +R.rowwise().reverse() // fliplr(R) +R.colwise().reverse() // flipud(R) +R.replicate(i,j) // repmat(P,i,j) + // All the same as Matlab, but matlab doesn't have *= style operators. // Matrix-vector. Matrix-matrix. Matrix-scalar. @@ -104,37 +109,40 @@ a *= M; R = P + Q; R = P/s; R -= Q; R /= s; // Vectorized operations on each element independently -// Eigen // Matlab -R = P.cwiseProduct(Q); // R = P .* Q -R = P.array() * s.array();// R = P .* s -R = P.cwiseQuotient(Q); // R = P ./ Q -R = P.array() / Q.array();// R = P ./ Q -R = P.array() + s.array();// R = P + s -R = P.array() - s.array();// R = P - s -R.array() += s; // R = R + s -R.array() -= s; // R = R - s -R.array() < Q.array(); // R < Q -R.array() <= Q.array(); // R <= Q -R.cwiseInverse(); // 1 ./ P -R.array().inverse(); // 1 ./ P -R.array().sin() // sin(P) -R.array().cos() // cos(P) -R.array().pow(s) // P .^ s -R.array().square() // P .^ 2 -R.array().cube() // P .^ 3 -R.cwiseSqrt() // sqrt(P) -R.array().sqrt() // sqrt(P) -R.array().exp() // exp(P) -R.array().log() // log(P) -R.cwiseMax(P) // max(R, P) -R.array().max(P.array()) // max(R, P) -R.cwiseMin(P) // min(R, P) -R.array().min(P.array()) // min(R, P) -R.cwiseAbs() // abs(P) -R.array().abs() // abs(P) -R.cwiseAbs2() // abs(P.^2) -R.array().abs2() // abs(P.^2) -(R.array() < s).select(P,Q); // (R < s ? P : Q) +// Eigen // Matlab +R = P.cwiseProduct(Q); // R = P .* Q +R = P.array() * s.array(); // R = P .* s +R = P.cwiseQuotient(Q); // R = P ./ Q +R = P.array() / Q.array(); // R = P ./ Q +R = P.array() + s.array(); // R = P + s +R = P.array() - s.array(); // R = P - s +R.array() += s; // R = R + s +R.array() -= s; // R = R - s +R.array() < Q.array(); // R < Q +R.array() <= Q.array(); // R <= Q +R.cwiseInverse(); // 1 ./ P +R.array().inverse(); // 1 ./ P +R.array().sin() // sin(P) +R.array().cos() // cos(P) +R.array().pow(s) // P .^ s +R.array().square() // P .^ 2 +R.array().cube() // P .^ 3 +R.cwiseSqrt() // sqrt(P) +R.array().sqrt() // sqrt(P) +R.array().exp() // exp(P) +R.array().log() // log(P) +R.cwiseMax(P) // max(R, P) +R.array().max(P.array()) // max(R, P) +R.cwiseMin(P) // min(R, P) +R.array().min(P.array()) // min(R, P) +R.cwiseAbs() // abs(P) +R.array().abs() // abs(P) +R.cwiseAbs2() // abs(P.^2) +R.array().abs2() // abs(P.^2) +(R.array() < s).select(P,Q ); // (R < s ? P : Q) +R = (Q.array()==0).select(P,A) // R(Q==0) = P(Q==0) +R = P.unaryExpr(ptr_fun(func)) // R = arrayfun(func, P) // with: scalar func(const scalar &x); + // Reductions. int r, c; @@ -165,12 +173,12 @@ x.dot(y) // dot(x, y) x.cross(y) // cross(x, y) Requires #include //// Type conversion -// Eigen // Matlab -A.cast(); // double(A) -A.cast(); // single(A) -A.cast(); // int32(A) -A.real(); // real(A) -A.imag(); // imag(A) +// Eigen // Matlab +A.cast(); // double(A) +A.cast(); // single(A) +A.cast(); // int32(A) +A.real(); // real(A) +A.imag(); // imag(A) // if the original type equals destination type, no work is done // Note that for most operations Eigen requires all operands to have the same type: diff --git a/ground/gcs/src/libs/eigen/doc/B01_Experimental.dox b/ground/gcs/src/libs/eigen/doc/B01_Experimental.dox index 5fc0ccd609..e1f031db84 100644 --- a/ground/gcs/src/libs/eigen/doc/B01_Experimental.dox +++ b/ground/gcs/src/libs/eigen/doc/B01_Experimental.dox @@ -4,7 +4,7 @@ namespace Eigen { \eigenAutoToc -\section summary Summary +\section Experimental_summary Summary With the 2.0 release, Eigen's API is, to a large extent, stable. However, we wish to retain the freedom to make API incompatible changes. To that effect, we call many parts of Eigen "experimental" which means that they are not subject to API stability guarantee. @@ -17,7 +17,7 @@ Experimental features may at any time: \li be subject to an API incompatible change; \li introduce API or ABI incompatible changes in your own code if you let them affect your API or ABI. -\section modules Experimental modules +\section Experimental_modules Experimental modules The following modules are considered entirely experimental, and we make no firm API stability guarantee about them for the time being: \li SVD @@ -26,7 +26,7 @@ The following modules are considered entirely experimental, and we make no firm \li Sparse \li Geometry (this one should be mostly stable, but it's a little too early to make a formal guarantee) -\section core Experimental parts of the Core module +\section Experimental_core Experimental parts of the Core module In the Core module, the only classes subject to ABI stability guarantee (meaning that you can use it for data members in your public ABI) is: \li Matrix diff --git a/ground/gcs/src/libs/eigen/doc/CMakeLists.txt b/ground/gcs/src/libs/eigen/doc/CMakeLists.txt index 02790ee43d..db413bc656 100644 --- a/ground/gcs/src/libs/eigen/doc/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/doc/CMakeLists.txt @@ -10,12 +10,20 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif(CMAKE_SYSTEM_NAME MATCHES Linux) endif(CMAKE_COMPILER_IS_GNUCXX) +option(EIGEN_INTERNAL_DOCUMENTATION "Build internal documentation" OFF) + + # Set some Doxygen flags set(EIGEN_DOXY_PROJECT_NAME "Eigen") set(EIGEN_DOXY_OUTPUT_DIRECTORY_SUFFIX "") set(EIGEN_DOXY_INPUT "\"${Eigen_SOURCE_DIR}/Eigen\" \"${Eigen_SOURCE_DIR}/doc\"") set(EIGEN_DOXY_HTML_COLORSTYLE_HUE "220") set(EIGEN_DOXY_TAGFILES "") +if(EIGEN_INTERNAL_DOCUMENTATION) + set(EIGEN_DOXY_INTERNAL "YES") +else(EIGEN_INTERNAL_DOCUMENTATION) + set(EIGEN_DOXY_INTERNAL "NO") +endif(EIGEN_INTERNAL_DOCUMENTATION) configure_file( ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in @@ -70,6 +78,8 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}/html/ COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/eigen_navtree_hacks.js ${CMAKE_CURRENT_BINARY_DIR}/html/ COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/Eigen_Silly_Professor_64x64.png ${CMAKE_CURRENT_BINARY_DIR}/html/ + COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/ftv2pnode.png ${CMAKE_CURRENT_BINARY_DIR}/html/ + COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/ftv2node.png ${CMAKE_CURRENT_BINARY_DIR}/html/ COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/AsciiQuickReference.txt ${CMAKE_CURRENT_BINARY_DIR}/html/ WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} ) @@ -80,6 +90,8 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -E make_directory ${Eigen_BINARY_DIR}/doc/html/unsupported COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/eigen_navtree_hacks.js ${CMAKE_CURRENT_BINARY_DIR}/html/unsupported/ COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/Eigen_Silly_Professor_64x64.png ${CMAKE_CURRENT_BINARY_DIR}/html/unsupported/ + COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/ftv2pnode.png ${CMAKE_CURRENT_BINARY_DIR}/html/unsupported/ + COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_SOURCE_DIR}/ftv2node.png ${CMAKE_CURRENT_BINARY_DIR}/html/unsupported/ WORKING_DIRECTORY ${Eigen_BINARY_DIR}/doc ) diff --git a/ground/gcs/src/libs/eigen/doc/CoeffwiseMathFunctionsTable.dox b/ground/gcs/src/libs/eigen/doc/CoeffwiseMathFunctionsTable.dox new file mode 100644 index 0000000000..3ae9420dc6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/CoeffwiseMathFunctionsTable.dox @@ -0,0 +1,525 @@ +namespace Eigen { + +/** \eigenManualPage CoeffwiseMathFunctions Catalog of coefficient-wise math functions + + + + +This table presents a catalog of the coefficient-wise math functions supported by %Eigen. +In this table, \c a, \c b, refer to Array objects or expressions, and \c m refers to a linear algebra Matrix/Vector object. Standard scalar types are abbreviated as follows: + - \c int: \c i32 + - \c float: \c f + - \c double: \c d + - \c std::complex: \c cf + - \c std::complex: \c cd + +For each row, the first column list the equivalent calls for arrays, and matrices when supported. Of course, all functions are available for matrices by first casting it as an array: \c m.array(). + +The third column gives some hints in the underlying scalar implementation. In most cases, %Eigen does not implement itself the math function but relies on the STL for standard scalar types, or user-provided functions for custom scalar types. +For instance, some simply calls the respective function of the STL while preserving argument-dependent lookup for custom types. +The following: +\code +using std::foo; +foo(a[i]); +\endcode +means that the STL's function \c std::foo will be potentially called if it is compatible with the underlying scalar type. If not, then the user must ensure that an overload of the function foo is available for the given scalar type (usually defined in the same namespace as the given scalar type). +This also means that, unless specified, if the function \c std::foo is available only in some recent c++ versions (e.g., c++11), then the respective %Eigen's function/method will be usable on standard types only if the compiler support the required c++ version. + +
Eigen 2Eigen 3
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
APIDescriptionDefault scalar implementationSIMD
Basic operations
+ \anchor cwisetable_abs + a.\link ArrayBase::abs abs\endlink(); \n + \link Eigen::abs abs\endlink(a); \n + m.\link MatrixBase::cwiseAbs cwiseAbs\endlink(); + absolute value (\f$ |a_i| \f$) + using std::abs; \n + abs(a[i]); + SSE2, AVX (i32,f,d)
+ \anchor cwisetable_inverse + a.\link ArrayBase::inverse inverse\endlink(); \n + \link Eigen::inverse inverse\endlink(a); \n + m.\link MatrixBase::cwiseInverse cwiseInverse\endlink(); + inverse value (\f$ 1/a_i \f$) + 1/a[i]; + All engines (f,d,fc,fd)
+ \anchor cwisetable_conj + a.\link ArrayBase::conjugate conjugate\endlink(); \n + \link Eigen::conj conj\endlink(a); \n + m.\link MatrixBase::conjugate conjugate(); + complex conjugate (\f$ \bar{a_i} \f$),\n + no-op for real + using std::conj; \n + conj(a[i]); + All engines (fc,fd)
Exponential functions
+ \anchor cwisetable_exp + a.\link ArrayBase::exp exp\endlink(); \n + \link Eigen::exp exp\endlink(a); + \f$ e \f$ raised to the given power (\f$ e^{a_i} \f$) + using std::exp; \n + exp(a[i]); + SSE2, AVX (f,d)
+ \anchor cwisetable_log + a.\link ArrayBase::log log\endlink(); \n + \link Eigen::log log\endlink(a); + natural (base \f$ e \f$) logarithm (\f$ \ln({a_i}) \f$) + using std::log; \n + log(a[i]); + SSE2, AVX (f)
+ \anchor cwisetable_log1p + a.\link ArrayBase::log1p log1p\endlink(); \n + \link Eigen::log1p log1p\endlink(a); + natural (base \f$ e \f$) logarithm of 1 plus \n the given number (\f$ \ln({1+a_i}) \f$)built-in generic implementation based on \c log,\n + plus \c using \c std::log1p ; \cpp11
+ \anchor cwisetable_log10 + a.\link ArrayBase::log10 log10\endlink(); \n + \link Eigen::log10 log10\endlink(a); + base 10 logarithm (\f$ \log_{10}({a_i}) \f$) + using std::log10; \n + log10(a[i]); +
Power functions
+ \anchor cwisetable_pow + a.\link ArrayBase::pow pow\endlink(b); \n + \link Eigen::pow pow\endlink(a,b); + raises a number to the given power (\f$ a_i ^ {b_i} \f$) \n \c a and \c b can be either an array or scalar. + using std::pow; \n + pow(a[i],b[i]);\n + (plus builtin for integer types)
+ \anchor cwisetable_sqrt + a.\link ArrayBase::sqrt sqrt\endlink(); \n + \link Eigen::sqrt sqrt\endlink(a);\n + m.\link MatrixBase::cwiseSqrt cwiseSqrt\endlink(); + computes square root (\f$ \sqrt a_i \f$) + using std::sqrt; \n + sqrt(a[i]);SSE2, AVX (f,d)
+ \anchor cwisetable_rsqrt + a.\link ArrayBase::rsqrt rsqrt\endlink(); \n + \link Eigen::rsqrt rsqrt\endlink(a); + reciprocal square root (\f$ 1/{\sqrt a_i} \f$) + using std::sqrt; \n + 1/sqrt(a[i]); \n + SSE2, AVX, AltiVec, ZVector (f,d)\n + (approx + 1 Newton iteration)
+ \anchor cwisetable_square + a.\link ArrayBase::square square\endlink(); \n + \link Eigen::square square\endlink(a); + computes square power (\f$ a_i^2 \f$) + a[i]*a[i]All (i32,f,d,cf,cd)
+ \anchor cwisetable_cube + a.\link ArrayBase::cube cube\endlink(); \n + \link Eigen::cube cube\endlink(a); + computes cubic power (\f$ a_i^3 \f$) + a[i]*a[i]*a[i]All (i32,f,d,cf,cd)
+ \anchor cwisetable_abs2 + a.\link ArrayBase::abs2 abs2\endlink(); \n + \link Eigen::abs2 abs2\endlink(a);\n + m.\link MatrixBase::cwiseAbs2 cwiseAbs2\endlink(); + computes the squared absolute value (\f$ |a_i|^2 \f$) + real: a[i]*a[i] \n + complex: real(a[i])*real(a[i]) \n +        + imag(a[i])*imag(a[i])All (i32,f,d)
Trigonometric functions
+ \anchor cwisetable_sin + a.\link ArrayBase::sin sin\endlink(); \n + \link Eigen::sin sin\endlink(a); + computes sine + using std::sin; \n + sin(a[i]);SSE2, AVX (f)
+ \anchor cwisetable_cos + a.\link ArrayBase::cos cos\endlink(); \n + \link Eigen::cos cos\endlink(a); + computes cosine + using std::cos; \n + cos(a[i]);SSE2, AVX (f)
+ \anchor cwisetable_tan + a.\link ArrayBase::tan tan\endlink(); \n + \link Eigen::tan tan\endlink(a); + computes tangent + using std::tan; \n + tan(a[i]);
+ \anchor cwisetable_asin + a.\link ArrayBase::asin asin\endlink(); \n + \link Eigen::asin asin\endlink(a); + computes arc sine (\f$ \sin^{-1} a_i \f$) + using std::asin; \n + asin(a[i]);
+ \anchor cwisetable_acos + a.\link ArrayBase::acos acos\endlink(); \n + \link Eigen::acos acos\endlink(a); + computes arc cosine (\f$ \cos^{-1} a_i \f$) + using std::acos; \n + acos(a[i]);
+ \anchor cwisetable_atan + a.\link ArrayBase::atan tan\endlink(); \n + \link Eigen::atan atan\endlink(a); + computes arc tangent (\f$ \tan^{-1} a_i \f$) + using std::atan; \n + atan(a[i]);
Hyperbolic functions
+ \anchor cwisetable_sinh + a.\link ArrayBase::sinh sinh\endlink(); \n + \link Eigen::sinh sinh\endlink(a); + computes hyperbolic sine + using std::sinh; \n + sinh(a[i]);
+ \anchor cwisetable_cosh + a.\link ArrayBase::cosh cohs\endlink(); \n + \link Eigen::cosh cosh\endlink(a); + computes hyperbolic cosine + using std::cosh; \n + cosh(a[i]);
+ \anchor cwisetable_tanh + a.\link ArrayBase::tanh tanh\endlink(); \n + \link Eigen::tanh tanh\endlink(a); + computes hyperbolic tangent + using std::tanh; \n + tanh(a[i]);
Nearest integer floating point operations
+ \anchor cwisetable_ceil + a.\link ArrayBase::ceil ceil\endlink(); \n + \link Eigen::ceil ceil\endlink(a); + nearest integer not less than the given value + using std::ceil; \n + ceil(a[i]);SSE4,AVX,ZVector (f,d)
+ \anchor cwisetable_floor + a.\link ArrayBase::floor floor\endlink(); \n + \link Eigen::floor floor\endlink(a); + nearest integer not greater than the given value + using std::floor; \n + floor(a[i]);SSE4,AVX,ZVector (f,d)
+ \anchor cwisetable_round + a.\link ArrayBase::round round\endlink(); \n + \link Eigen::round round\endlink(a); + nearest integer, \n rounding away from zero in halfway casesbuilt-in generic implementation \n based on \c floor and \c ceil,\n + plus \c using \c std::round ; \cpp11SSE4,AVX,ZVector (f,d)
Floating point manipulation functions
Classification and comparison
+ \anchor cwisetable_isfinite + a.\link ArrayBase::isFinite isFinite\endlink(); \n + \link Eigen::isfinite isfinite\endlink(a); + checks if the given number has finite valuebuilt-in generic implementation,\n + plus \c using \c std::isfinite ; \cpp11
+ \anchor cwisetable_isinf + a.\link ArrayBase::isInf isInf\endlink(); \n + \link Eigen::isinf isinf\endlink(a); + checks if the given number is infinitebuilt-in generic implementation,\n + plus \c using \c std::isinf ; \cpp11
+ \anchor cwisetable_isnan + a.\link ArrayBase::isNaN isNaN\endlink(); \n + \link Eigen::isnan isnan\endlink(a); + checks if the given number is not a numberbuilt-in generic implementation,\n + plus \c using \c std::isnan ; \cpp11
Error and gamma functions
Require \c \#include \c
+ \anchor cwisetable_erf + a.\link ArrayBase::erf erf\endlink(); \n + \link Eigen::erf erf\endlink(a); + error function + using std::erf; \cpp11 \n + erf(a[i]); +
+ \anchor cwisetable_erfc + a.\link ArrayBase::erfc erfc\endlink(); \n + \link Eigen::erfc erfc\endlink(a); + complementary error function + using std::erfc; \cpp11 \n + erfc(a[i]); +
+ \anchor cwisetable_lgamma + a.\link ArrayBase::lgamma lgamma\endlink(); \n + \link Eigen::lgamma lgamma\endlink(a); + natural logarithm of the gamma function + using std::lgamma; \cpp11 \n + lgamma(a[i]); +
+ \anchor cwisetable_digamma + a.\link ArrayBase::digamma digamma\endlink(); \n + \link Eigen::digamma digamma\endlink(a); + logarithmic derivative of the gamma function + built-in for float and double +
+ \anchor cwisetable_igamma + \link Eigen::igamma igamma\endlink(a,x); + lower incomplete gamma integral + \n \f$ \gamma(a_i,x_i)= \frac{1}{|a_i|} \int_{0}^{x_i}e^{\text{-}t} t^{a_i-1} \mathrm{d} t \f$ + built-in for float and double,\n but requires \cpp11 +
+ \anchor cwisetable_igammac + \link Eigen::igammac igammac\endlink(a,x); + upper incomplete gamma integral + \n \f$ \Gamma(a_i,x_i) = \frac{1}{|a_i|} \int_{x_i}^{\infty}e^{\text{-}t} t^{a_i-1} \mathrm{d} t \f$ + built-in for float and double,\n but requires \cpp11 +
Special functions
Require \c \#include \c
+ \anchor cwisetable_polygamma + \link Eigen::polygamma polygamma\endlink(n,x); + n-th derivative of digamma at x + built-in generic based on\n \c lgamma , + \c digamma + and \c zeta . +
+ \anchor cwisetable_betainc + \link Eigen::betainc betainc\endlink(a,b,x); + Incomplete beta function + built-in for float and double,\n but requires \cpp11 +
+ \anchor cwisetable_zeta + \link Eigen::zeta zeta\endlink(a,b); + Hurwitz zeta function + \n \f$ \zeta(a_i,b_i)=\sum_{k=0}^{\infty}(b_i+k)^{\text{-}a_i} \f$ + built-in for float and double +
+ +\n + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/CustomizingEigen.dox b/ground/gcs/src/libs/eigen/doc/CustomizingEigen.dox deleted file mode 100644 index 5a0890ea9d..0000000000 --- a/ground/gcs/src/libs/eigen/doc/CustomizingEigen.dox +++ /dev/null @@ -1,188 +0,0 @@ -namespace Eigen { - -/** \page TopicCustomizingEigen Customizing/Extending Eigen - -Eigen can be extended in several ways, for instance, by defining global methods, \ref ExtendingMatrixBase "by adding custom methods to MatrixBase", adding support to \ref CustomScalarType "custom types" etc. - -\eigenAutoToc - -\section ExtendingMatrixBase Extending MatrixBase (and other classes) - -In this section we will see how to add custom methods to MatrixBase. Since all expressions and matrix types inherit MatrixBase, adding a method to MatrixBase make it immediately available to all expressions ! A typical use case is, for instance, to make Eigen compatible with another API. - -You certainly know that in C++ it is not possible to add methods to an existing class. So how that's possible ? Here the trick is to include in the declaration of MatrixBase a file defined by the preprocessor token \c EIGEN_MATRIXBASE_PLUGIN: -\code -class MatrixBase { - // ... - #ifdef EIGEN_MATRIXBASE_PLUGIN - #include EIGEN_MATRIXBASE_PLUGIN - #endif -}; -\endcode -Therefore to extend MatrixBase with your own methods you just have to create a file with your method declaration and define EIGEN_MATRIXBASE_PLUGIN before you include any Eigen's header file. - -You can extend many of the other classes used in Eigen by defining similarly named preprocessor symbols. For instance, define \c EIGEN_ARRAYBASE_PLUGIN if you want to extend the ArrayBase class. A full list of classes that can be extended in this way and the corresponding preprocessor symbols can be found on our page \ref TopicPreprocessorDirectives. - -Here is an example of an extension file for adding methods to MatrixBase: \n -\b MatrixBaseAddons.h -\code -inline Scalar at(uint i, uint j) const { return this->operator()(i,j); } -inline Scalar& at(uint i, uint j) { return this->operator()(i,j); } -inline Scalar at(uint i) const { return this->operator[](i); } -inline Scalar& at(uint i) { return this->operator[](i); } - -inline RealScalar squaredLength() const { return squaredNorm(); } -inline RealScalar length() const { return norm(); } -inline RealScalar invLength(void) const { return fast_inv_sqrt(squaredNorm()); } - -template -inline Scalar squaredDistanceTo(const MatrixBase& other) const -{ return (derived() - other.derived()).squaredNorm(); } - -template -inline RealScalar distanceTo(const MatrixBase& other) const -{ return internal::sqrt(derived().squaredDistanceTo(other)); } - -inline void scaleTo(RealScalar l) { RealScalar vl = norm(); if (vl>1e-9) derived() *= (l/vl); } - -inline Transpose transposed() {return this->transpose();} -inline const Transpose transposed() const {return this->transpose();} - -inline uint minComponentId(void) const { int i; this->minCoeff(&i); return i; } -inline uint maxComponentId(void) const { int i; this->maxCoeff(&i); return i; } - -template -void makeFloor(const MatrixBase& other) { derived() = derived().cwiseMin(other.derived()); } -template -void makeCeil(const MatrixBase& other) { derived() = derived().cwiseMax(other.derived()); } - -const CwiseUnaryOp, Derived> -operator+(const Scalar& scalar) const -{ return CwiseUnaryOp, Derived>(derived(), internal::scalar_add_op(scalar)); } - -friend const CwiseUnaryOp, Derived> -operator+(const Scalar& scalar, const MatrixBase& mat) -{ return CwiseUnaryOp, Derived>(mat.derived(), internal::scalar_add_op(scalar)); } -\endcode - -Then one can the following declaration in the config.h or whatever prerequisites header file of his project: -\code -#define EIGEN_MATRIXBASE_PLUGIN "MatrixBaseAddons.h" -\endcode - -\section InheritingFromMatrix Inheriting from Matrix - -Before inheriting from Matrix, be really, i mean REALLY sure that using -EIGEN_MATRIX_PLUGIN is not what you really want (see previous section). -If you just need to add few members to Matrix, this is the way to go. - -An example of when you actually need to inherit Matrix, is when you have -several layers of heritage such as MyVerySpecificVector1,MyVerySpecificVector1 -> MyVector1 -> Matrix and. -MyVerySpecificVector3,MyVerySpecificVector4 -> MyVector2 -> Matrix. - -In order for your object to work within the %Eigen framework, you need to -define a few members in your inherited class. - -Here is a minimalistic example:\n -\code -class MyVectorType : public Eigen::VectorXd -{ -public: - MyVectorType(void):Eigen::VectorXd() {} - - typedef Eigen::VectorXd Base; - - // This constructor allows you to construct MyVectorType from Eigen expressions - template - MyVectorType(const Eigen::MatrixBase& other) - : Eigen::Vector3d(other) - { } - - // This method allows you to assign Eigen expressions to MyVectorType - template - MyVectorType & operator= (const Eigen::MatrixBase & other) - { - this->Base::operator=(other); - return *this; - } -}; -\endcode - -This is the kind of error you can get if you don't provide those methods -\code -error: no match for ‘operator=’ in ‘delta = -(((Eigen::MatrixBase, 10000, 1, 2, 10000, -1> >*)(& delta)) + 8u)->Eigen::MatrixBase::cwise [with Derived = -Eigen::Matrix, 10000, 1, 2, 10000, -1>]().Eigen::Cwise::operator* [with OtherDerived = -Eigen::Matrix, 10000, 1, 2, 10000, 1>, ExpressionType = -Eigen::Matrix, 10000, 1, 2, 10000, 1>](((const -Eigen::MatrixBase, 10000, 1, 2, 10000, 1> ->&)(((const Eigen::MatrixBase, 10000, 1, ->2, 10000, 1> >*)((const spectral1d*)where)) + 8u)))’ -\endcode - -\anchor user_defined_scalars \section CustomScalarType Using custom scalar types - -By default, Eigen currently supports standard floating-point types (\c float, \c double, \c std::complex, \c std::complex, \c long \c double), as well as all native integer types (e.g., \c int, \c unsigned \c int, \c short, etc.), and \c bool. -On x86-64 systems, \c long \c double permits to locally enforces the use of x87 registers with extended accuracy (in comparison to SSE). - -In order to add support for a custom type \c T you need: --# make sure the common operator (+,-,*,/,etc.) are supported by the type \c T --# add a specialization of struct Eigen::NumTraits (see \ref NumTraits) --# define the math functions that makes sense for your type. This includes standard ones like sqrt, pow, sin, tan, conj, real, imag, etc, as well as abs2 which is Eigen specific. - (see the file Eigen/src/Core/MathFunctions.h) - -The math function should be defined in the same namespace than \c T, or in the \c std namespace though that second approach is not recommended. - -Here is a concrete example adding support for the Adolc's \c adouble type. Adolc is an automatic differentiation library. The type \c adouble is basically a real value tracking the values of any number of partial derivatives. - -\code -#ifndef ADOLCSUPPORT_H -#define ADOLCSUPPORT_H - -#define ADOLC_TAPELESS -#include -#include - -namespace Eigen { - -template<> struct NumTraits - : NumTraits // permits to get the epsilon, dummy_precision, lowest, highest functions -{ - typedef adtl::adouble Real; - typedef adtl::adouble NonInteger; - typedef adtl::adouble Nested; - - enum { - IsComplex = 0, - IsInteger = 0, - IsSigned = 1, - RequireInitialization = 1, - ReadCost = 1, - AddCost = 3, - MulCost = 3 - }; -}; - -} - -namespace adtl { - -inline const adouble& conj(const adouble& x) { return x; } -inline const adouble& real(const adouble& x) { return x; } -inline adouble imag(const adouble&) { return 0.; } -inline adouble abs(const adouble& x) { return fabs(x); } -inline adouble abs2(const adouble& x) { return x*x; } - -} - -#endif // ADOLCSUPPORT_H -\endcode - - -\sa \ref TopicPreprocessorDirectives - -*/ - -} diff --git a/ground/gcs/src/libs/eigen/doc/CustomizingEigen_CustomScalar.dox b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_CustomScalar.dox new file mode 100644 index 0000000000..1ee78cbe5f --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_CustomScalar.dox @@ -0,0 +1,120 @@ +namespace Eigen { + +/** \page TopicCustomizing_CustomScalar Using custom scalar types +\anchor user_defined_scalars + +By default, Eigen currently supports standard floating-point types (\c float, \c double, \c std::complex, \c std::complex, \c long \c double), as well as all native integer types (e.g., \c int, \c unsigned \c int, \c short, etc.), and \c bool. +On x86-64 systems, \c long \c double permits to locally enforces the use of x87 registers with extended accuracy (in comparison to SSE). + +In order to add support for a custom type \c T you need: +-# make sure the common operator (+,-,*,/,etc.) are supported by the type \c T +-# add a specialization of struct Eigen::NumTraits (see \ref NumTraits) +-# define the math functions that makes sense for your type. This includes standard ones like sqrt, pow, sin, tan, conj, real, imag, etc, as well as abs2 which is Eigen specific. + (see the file Eigen/src/Core/MathFunctions.h) + +The math function should be defined in the same namespace than \c T, or in the \c std namespace though that second approach is not recommended. + +Here is a concrete example adding support for the Adolc's \c adouble type. Adolc is an automatic differentiation library. The type \c adouble is basically a real value tracking the values of any number of partial derivatives. + +\code +#ifndef ADOLCSUPPORT_H +#define ADOLCSUPPORT_H + +#define ADOLC_TAPELESS +#include +#include + +namespace Eigen { + +template<> struct NumTraits + : NumTraits // permits to get the epsilon, dummy_precision, lowest, highest functions +{ + typedef adtl::adouble Real; + typedef adtl::adouble NonInteger; + typedef adtl::adouble Nested; + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 3, + MulCost = 3 + }; +}; + +} + +namespace adtl { + +inline const adouble& conj(const adouble& x) { return x; } +inline const adouble& real(const adouble& x) { return x; } +inline adouble imag(const adouble&) { return 0.; } +inline adouble abs(const adouble& x) { return fabs(x); } +inline adouble abs2(const adouble& x) { return x*x; } + +} + +#endif // ADOLCSUPPORT_H +\endcode + +This other example adds support for the \c mpq_class type from GMP. It shows in particular how to change the way Eigen picks the best pivot during LU factorization. It selects the coefficient with the highest score, where the score is by default the absolute value of a number, but we can define a different score, for instance to prefer pivots with a more compact representation (this is an example, not a recommendation). Note that the scores should always be non-negative and only zero is allowed to have a score of zero. Also, this can interact badly with thresholds for inexact scalar types. + +\code +#include +#include +#include + +namespace Eigen { + template<> struct NumTraits : GenericNumTraits + { + typedef mpq_class Real; + typedef mpq_class NonInteger; + typedef mpq_class Nested; + + static inline Real epsilon() { return 0; } + static inline Real dummy_precision() { return 0; } + static inline Real digits10() { return 0; } + + enum { + IsInteger = 0, + IsSigned = 1, + IsComplex = 0, + RequireInitialization = 1, + ReadCost = 6, + AddCost = 150, + MulCost = 100 + }; + }; + + namespace internal { + + template<> struct scalar_score_coeff_op { + struct result_type : boost::totally_ordered1 { + std::size_t len; + result_type(int i = 0) : len(i) {} // Eigen uses Score(0) and Score() + result_type(mpq_class const& q) : + len(mpz_size(q.get_num_mpz_t())+ + mpz_size(q.get_den_mpz_t())-1) {} + friend bool operator<(result_type x, result_type y) { + // 0 is the worst possible pivot + if (x.len == 0) return y.len > 0; + if (y.len == 0) return false; + // Prefer a pivot with a small representation + return x.len > y.len; + } + friend bool operator==(result_type x, result_type y) { + // Only used to test if the score is 0 + return x.len == y.len; + } + }; + result_type operator()(mpq_class const& x) const { return x; } + }; + } +} +\endcode + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/CustomizingEigen_InheritingMatrix.dox b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_InheritingMatrix.dox new file mode 100644 index 0000000000..b21e554337 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_InheritingMatrix.dox @@ -0,0 +1,34 @@ +namespace Eigen { + +/** \page TopicCustomizing_InheritingMatrix Inheriting from Matrix + +Before inheriting from Matrix, be really, I mean REALLY, sure that using +EIGEN_MATRIX_PLUGIN is not what you really want (see previous section). +If you just need to add few members to Matrix, this is the way to go. + +An example of when you actually need to inherit Matrix, is when you +have several layers of heritage such as +MyVerySpecificVector1, MyVerySpecificVector2 -> MyVector1 -> Matrix and +MyVerySpecificVector3, MyVerySpecificVector4 -> MyVector2 -> Matrix. + +In order for your object to work within the %Eigen framework, you need to +define a few members in your inherited class. + +Here is a minimalistic example: + +\include CustomizingEigen_Inheritance.cpp + +Output: \verbinclude CustomizingEigen_Inheritance.out + +This is the kind of error you can get if you don't provide those methods +\verbatim +error: no match for ‘operator=’ in ‘v = Eigen::operator*( +const Eigen::MatrixBase >::Scalar&, +const Eigen::MatrixBase >::StorageBaseType&) +(((const Eigen::MatrixBase >::StorageBaseType&) +((const Eigen::MatrixBase >::StorageBaseType*)(& v))))’ +\endverbatim + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/CustomizingEigen_NullaryExpr.dox b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_NullaryExpr.dox new file mode 100644 index 0000000000..37c8dcd896 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_NullaryExpr.dox @@ -0,0 +1,86 @@ +namespace Eigen { + +/** \page TopicCustomizing_NullaryExpr Matrix manipulation via nullary-expressions + + +The main purpose of the class CwiseNullaryOp is to define \em procedural matrices such as constant or random matrices as returned by the Ones(), Zero(), Constant(), Identity() and Random() methods. +Nevertheless, with some imagination it is possible to accomplish very sophisticated matrix manipulation with minimal efforts such that \ref TopicNewExpressionType "implementing new expression" is rarely needed. + +\section NullaryExpr_Circulant Example 1: circulant matrix + +To explore these possibilities let us start with the \em circulant example of the \ref TopicNewExpressionType "implementing new expression" topic. +Let us recall that a circulant matrix is a matrix where each column is the same as the +column to the left, except that it is cyclically shifted downwards. +For example, here is a 4-by-4 circulant matrix: +\f[ \begin{bmatrix} + 1 & 8 & 4 & 2 \\ + 2 & 1 & 8 & 4 \\ + 4 & 2 & 1 & 8 \\ + 8 & 4 & 2 & 1 +\end{bmatrix} \f] +A circulant matrix is uniquely determined by its first column. We wish +to write a function \c makeCirculant which, given the first column, +returns an expression representing the circulant matrix. + +For this exercise, the return type of \c makeCirculant will be a CwiseNullaryOp that we need to instantiate with: +1 - a proper \c circulant_functor storing the input vector and implementing the adequate coefficient accessor \c operator(i,j) +2 - a template instantiation of class Matrix conveying compile-time information such as the scalar type, sizes, and preferred storage layout. + +Calling \c ArgType the type of the input vector, we can construct the equivalent squared Matrix type as follows: + +\snippet make_circulant2.cpp square + +This little helper structure will help us to implement our \c makeCirculant function as follows: + +\snippet make_circulant2.cpp makeCirculant + +As usual, our function takes as argument a \c MatrixBase (see this \ref TopicFunctionTakingEigenTypes "page" for more details). +Then, the CwiseNullaryOp object is constructed through the DenseBase::NullaryExpr static method with the adequate runtime sizes. + +Then, we need to implement our \c circulant_functor, which is a straightforward exercise: + +\snippet make_circulant2.cpp circulant_func + +We are now all set to try our new feature: + +\snippet make_circulant2.cpp main + + +If all the fragments are combined, the following output is produced, +showing that the program works as expected: + +\include make_circulant2.out + +This implementation of \c makeCirculant is much simpler than \ref TopicNewExpressionType "defining a new expression" from scratch. + + +\section NullaryExpr_Indexing Example 2: indexing rows and columns + +The goal here is to mimic MatLab's ability to index a matrix through two vectors of indices referencing the rows and columns to be picked respectively, like this: + +\snippet nullary_indexing.out main1 + +To this end, let us first write a nullary-functor storing references to the input matrix and to the two arrays of indices, and implementing the required \c operator()(i,j): + +\snippet nullary_indexing.cpp functor + +Then, let's create an \c indexing(A,rows,cols) function creating the nullary expression: + +\snippet nullary_indexing.cpp function + +Finally, here is an example of how this function can be used: + +\snippet nullary_indexing.cpp main1 + +This straightforward implementation is already quite powerful as the row or column index arrays can also be expressions to perform offsetting, modulo, striding, reverse, etc. + +\snippet nullary_indexing.cpp main2 + +and the output is: + +\snippet nullary_indexing.out main2 + +*/ + +} + diff --git a/ground/gcs/src/libs/eigen/doc/CustomizingEigen_Plugins.dox b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_Plugins.dox new file mode 100644 index 0000000000..d88f2409b9 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/CustomizingEigen_Plugins.dox @@ -0,0 +1,69 @@ +namespace Eigen { + +/** \page TopicCustomizing_Plugins Extending MatrixBase (and other classes) + +In this section we will see how to add custom methods to MatrixBase. Since all expressions and matrix types inherit MatrixBase, adding a method to MatrixBase make it immediately available to all expressions ! A typical use case is, for instance, to make Eigen compatible with another API. + +You certainly know that in C++ it is not possible to add methods to an existing class. So how that's possible ? Here the trick is to include in the declaration of MatrixBase a file defined by the preprocessor token \c EIGEN_MATRIXBASE_PLUGIN: +\code +class MatrixBase { + // ... + #ifdef EIGEN_MATRIXBASE_PLUGIN + #include EIGEN_MATRIXBASE_PLUGIN + #endif +}; +\endcode +Therefore to extend MatrixBase with your own methods you just have to create a file with your method declaration and define EIGEN_MATRIXBASE_PLUGIN before you include any Eigen's header file. + +You can extend many of the other classes used in Eigen by defining similarly named preprocessor symbols. For instance, define \c EIGEN_ARRAYBASE_PLUGIN if you want to extend the ArrayBase class. A full list of classes that can be extended in this way and the corresponding preprocessor symbols can be found on our page \ref TopicPreprocessorDirectives. + +Here is an example of an extension file for adding methods to MatrixBase: \n +\b MatrixBaseAddons.h +\code +inline Scalar at(uint i, uint j) const { return this->operator()(i,j); } +inline Scalar& at(uint i, uint j) { return this->operator()(i,j); } +inline Scalar at(uint i) const { return this->operator[](i); } +inline Scalar& at(uint i) { return this->operator[](i); } + +inline RealScalar squaredLength() const { return squaredNorm(); } +inline RealScalar length() const { return norm(); } +inline RealScalar invLength(void) const { return fast_inv_sqrt(squaredNorm()); } + +template +inline Scalar squaredDistanceTo(const MatrixBase& other) const +{ return (derived() - other.derived()).squaredNorm(); } + +template +inline RealScalar distanceTo(const MatrixBase& other) const +{ return internal::sqrt(derived().squaredDistanceTo(other)); } + +inline void scaleTo(RealScalar l) { RealScalar vl = norm(); if (vl>1e-9) derived() *= (l/vl); } + +inline Transpose transposed() {return this->transpose();} +inline const Transpose transposed() const {return this->transpose();} + +inline uint minComponentId(void) const { int i; this->minCoeff(&i); return i; } +inline uint maxComponentId(void) const { int i; this->maxCoeff(&i); return i; } + +template +void makeFloor(const MatrixBase& other) { derived() = derived().cwiseMin(other.derived()); } +template +void makeCeil(const MatrixBase& other) { derived() = derived().cwiseMax(other.derived()); } + +const CwiseBinaryOp, const Derived, const ConstantReturnType> +operator+(const Scalar& scalar) const +{ return CwiseBinaryOp, const Derived, const ConstantReturnType>(derived(), Constant(rows(),cols(),scalar)); } + +friend const CwiseBinaryOp, const ConstantReturnType, Derived> +operator+(const Scalar& scalar, const MatrixBase& mat) +{ return CwiseBinaryOp, const ConstantReturnType, Derived>(Constant(rows(),cols(),scalar), mat.derived()); } +\endcode + +Then one can the following declaration in the config.h or whatever prerequisites header file of his project: +\code +#define EIGEN_MATRIXBASE_PLUGIN "MatrixBaseAddons.h" +\endcode + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/DenseDecompositionBenchmark.dox b/ground/gcs/src/libs/eigen/doc/DenseDecompositionBenchmark.dox new file mode 100644 index 0000000000..7be9c70cd5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/DenseDecompositionBenchmark.dox @@ -0,0 +1,42 @@ +namespace Eigen { + +/** \eigenManualPage DenseDecompositionBenchmark Benchmark of dense decompositions + +This page presents a speed comparison of the dense matrix decompositions offered by %Eigen for a wide range of square matrices and overconstrained problems. + +For a more general overview on the features and numerical robustness of linear solvers and decompositions, check this \link TopicLinearAlgebraDecompositions table \endlink. + +This benchmark has been run on a laptop equipped with an Intel core i7 \@ 2,6 GHz, and compiled with clang with \b AVX and \b FMA instruction sets enabled but without multi-threading. +It uses \b single \b precision \b float numbers. For double, you can get a good estimate by multiplying the timings by a factor 2. + +The square matrices are symmetric, and for the overconstrained matrices, the reported timmings include the cost to compute the symmetric covariance matrix \f$ A^T A \f$ for the first four solvers based on Cholesky and LU, as denoted by the \b * symbol (top-right corner part of the table). +Timings are in \b milliseconds, and factors are relative to the LLT decomposition which is the fastest but also the least general and robust. + + + + + + + + + + + + + + +
solver/size8x8 100x100 1000x1000 4000x4000 10000x8 10000x100 10000x1000 10000x4000
LLT0.050.425.83374.556.79 *30.15 *236.34 *3847.17 *
LDLT0.07 (x1.3)0.65 (x1.5)26.86 (x4.6)2361.18 (x6.3)6.81 (x1) *31.91 (x1.1) *252.61 (x1.1) *5807.66 (x1.5) *
PartialPivLU0.08 (x1.5)0.69 (x1.6)15.63 (x2.7)709.32 (x1.9)6.81 (x1) *31.32 (x1) *241.68 (x1) *4270.48 (x1.1) *
FullPivLU0.1 (x1.9)4.48 (x10.6)281.33 (x48.2)-6.83 (x1) *32.67 (x1.1) *498.25 (x2.1) *-
HouseholderQR0.19 (x3.5)2.18 (x5.2)23.42 (x4)1337.52 (x3.6)34.26 (x5)129.01 (x4.3)377.37 (x1.6)4839.1 (x1.3)
ColPivHouseholderQR0.23 (x4.3)2.23 (x5.3)103.34 (x17.7)9987.16 (x26.7)36.05 (x5.3)163.18 (x5.4)2354.08 (x10)37860.5 (x9.8)
CompleteOrthogonalDecomposition0.23 (x4.3)2.22 (x5.2)99.44 (x17.1)10555.3 (x28.2)35.75 (x5.3)169.39 (x5.6)2150.56 (x9.1)36981.8 (x9.6)
FullPivHouseholderQR0.23 (x4.3)4.64 (x11)289.1 (x49.6)-69.38 (x10.2)446.73 (x14.8)4852.12 (x20.5)-
JacobiSVD1.01 (x18.6)71.43 (x168.4)--113.81 (x16.7)1179.66 (x39.1)--
BDCSVD1.07 (x19.7)21.83 (x51.5)331.77 (x56.9)18587.9 (x49.6)110.53 (x16.3)397.67 (x13.2)2975 (x12.6)48593.2 (x12.6)
+ +\b *: This decomposition do not support direct least-square solving for over-constrained problems, and the reported timing include the cost to form the symmetric covariance matrix \f$ A^T A \f$. + +\b Observations: + + LLT is always the fastest solvers. + + For largely over-constrained problems, the cost of Cholesky/LU decompositions is dominated by the computation of the symmetric covariance matrix. + + For large problem sizes, only the decomposition implementing a cache-friendly blocking strategy scale well. Those include LLT, PartialPivLU, HouseholderQR, and BDCSVD. This explain why for a 4k x 4k matrix, HouseholderQR is faster than LDLT. In the future, LDLT and ColPivHouseholderQR will also implement blocking strategies. + + CompleteOrthogonalDecomposition is based on ColPivHouseholderQR and they thus achieve the same level of performance. + +The above table has been generated by the bench/dense_solvers.cpp file, feel-free to hack it to generate a table matching your hardware, compiler, and favorite problem sizes. + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/Doxyfile.in b/ground/gcs/src/libs/eigen/doc/Doxyfile.in index 696dd2af1c..48bb0a8ec2 100644 --- a/ground/gcs/src/libs/eigen/doc/Doxyfile.in +++ b/ground/gcs/src/libs/eigen/doc/Doxyfile.in @@ -125,7 +125,7 @@ ALWAYS_DETAILED_SEC = NO # members were ordinary class members. Constructors, destructors and assignment # operators of the base classes will not be shown. -INLINE_INHERITED_MEMB = YES +INLINE_INHERITED_MEMB = NO # If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full # path before files name in the file list and in the header files. If set @@ -206,6 +206,7 @@ TAB_SIZE = 8 # You can put \n's in the value part of an alias to insert newlines. ALIASES = "only_for_vectors=This is only for vectors (either row-vectors or column-vectors), i.e. matrices which are known at compile-time to have either one row or one column." \ + "not_reentrant=\warning This function is not re-entrant." \ "array_module=This is defined in the %Array module. \code #include \endcode" \ "cholesky_module=This is defined in the %Cholesky module. \code #include \endcode" \ "eigenvalues_module=This is defined in the %Eigenvalues module. \code #include \endcode" \ @@ -215,6 +216,7 @@ ALIASES = "only_for_vectors=This is only for vectors (either row- "lu_module=This is defined in the %LU module. \code #include \endcode" \ "qr_module=This is defined in the %QR module. \code #include \endcode" \ "svd_module=This is defined in the %SVD module. \code #include \endcode" \ + "specialfunctions_module=This is defined in the \b unsupported SpecialFunctions module. \code #include \endcode" \ "label=\bug" \ "matrixworld=*" \ "arrayworld=*" \ @@ -222,7 +224,13 @@ ALIASES = "only_for_vectors=This is only for vectors (either row- "note_about_using_kernel_to_study_multiple_solutions=If you need a complete analysis of the space of solutions, take the one solution obtained by this method and add to it elements of the kernel, as determined by kernel()." \ "note_about_checking_solutions=This method just tries to find as good a solution as possible. If you want to check whether a solution exists or if it is accurate, just call this function to get a result and then compute the error of this result, or use MatrixBase::isApprox() directly, for instance like this: \code bool a_solution_exists = (A*result).isApprox(b, precision); \endcode This method avoids dividing by zero, so that the non-existence of a solution doesn't by itself mean that you'll get \c inf or \c nan values." \ "note_try_to_help_rvo=This function returns the result by value. In order to make that efficient, it is implemented as just a return statement using a special constructor, hopefully allowing the compiler to perform a RVO (return value optimization)." \ - "nonstableyet=\warning This is not considered to be part of the stable public API yet. Changes may happen in future releases. See \ref Experimental \"Experimental parts of Eigen\"" + "nonstableyet=\warning This is not considered to be part of the stable public API yet. Changes may happen in future releases. See \ref Experimental \"Experimental parts of Eigen\"" \ + "implsparsesolverconcept=This class follows the \link TutorialSparseSolverConcept sparse solver concept \endlink." \ + "blank= " \ + "cpp11=[c++11]" \ + "cpp14=[c++14]" \ + "cpp17=[c++17]" + ALIASES += "eigenAutoToc= " ALIASES += "eigenManualPage=\defgroup" @@ -270,7 +278,7 @@ OPTIMIZE_OUTPUT_VHDL = NO # (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions # you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. -EXTENSION_MAPPING = +EXTENSION_MAPPING = .h=C++ no_extension=C++ # If MARKDOWN_SUPPORT is enabled (the default) then doxygen pre-processes all # comments according to the Markdown format, which allows for more readable @@ -458,7 +466,7 @@ HIDE_IN_BODY_DOCS = NO # to NO (the default) then the documentation will be excluded. # Set it to YES to include the internal documentation. -INTERNAL_DOCS = NO +INTERNAL_DOCS = ${EIGEN_DOXY_INTERNAL} # If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate # file names in lower-case letters. If set to YES upper-case letters are also @@ -472,13 +480,13 @@ CASE_SENSE_NAMES = YES # will show members with their full class and namespace scopes in the # documentation. If set to YES the scope will be hidden. -HIDE_SCOPE_NAMES = YES +HIDE_SCOPE_NAMES = NO # If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen # will put a list of the files that are included by a file in the documentation # of that file. -SHOW_INCLUDE_FILES = NO +SHOW_INCLUDE_FILES = ${EIGEN_DOXY_INTERNAL} # If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen # will list include files with double quotes in the documentation @@ -544,7 +552,7 @@ STRICT_PROTO_MATCHING = NO # disable (NO) the todo list. This list is created by putting \todo # commands in the documentation. -GENERATE_TODOLIST = NO +GENERATE_TODOLIST = ${EIGEN_DOXY_INTERNAL} # The GENERATE_TESTLIST tag can be used to enable (YES) or # disable (NO) the test list. This list is created by putting \test @@ -556,7 +564,7 @@ GENERATE_TESTLIST = NO # disable (NO) the bug list. This list is created by putting \bug # commands in the documentation. -GENERATE_BUGLIST = NO +GENERATE_BUGLIST = ${EIGEN_DOXY_INTERNAL} # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or # disable (NO) the deprecated list. This list is created by putting @@ -719,7 +727,8 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = "${Eigen_SOURCE_DIR}/Eigen/Eigen2Support" \ +EXCLUDE = "${Eigen_SOURCE_DIR}/Eigen/src/Core/products" \ + "${Eigen_SOURCE_DIR}/Eigen/Eigen2Support" \ "${Eigen_SOURCE_DIR}/Eigen/src/Eigen2Support" \ "${Eigen_SOURCE_DIR}/doc/examples" \ "${Eigen_SOURCE_DIR}/doc/special_examples" \ @@ -800,7 +809,7 @@ EXAMPLE_RECURSIVE = NO # directories that contain image that are included in the documentation (see # the \image command). -IMAGE_PATH = +IMAGE_PATH = ${Eigen_BINARY_DIR}/doc/html # The INPUT_FILTER tag can be used to specify a program that doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program @@ -864,13 +873,13 @@ STRIP_CODE_COMMENTS = YES # then for each documented function all documented # functions referencing it will be listed. -REFERENCED_BY_RELATION = YES +REFERENCED_BY_RELATION = NO # If the REFERENCES_RELATION tag is set to YES # then for each documented function all documented entities # called/used by that function will be listed. -REFERENCES_RELATION = YES +REFERENCES_RELATION = NO # If the REFERENCES_LINK_SOURCE tag is set to YES (the default) # and SOURCE_BROWSER tag is set to YES, then the hyperlinks from @@ -1581,9 +1590,14 @@ PREDEFINED = EIGEN_EMPTY_STRUCT \ EIGEN_VECTORIZE \ EIGEN_QT_SUPPORT \ EIGEN_STRONG_INLINE=inline \ - "EIGEN2_SUPPORT_STAGE=99" \ + EIGEN_DEVICE_FUNC= \ "EIGEN_MAKE_CWISE_BINARY_OP(METHOD,FUNCTOR)=template const CwiseBinaryOp, const Derived, const OtherDerived> METHOD(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const;" \ - "EIGEN_CWISE_PRODUCT_RETURN_TYPE(LHS,RHS)=CwiseBinaryOp, const LHS, const RHS>" + "EIGEN_CWISE_PRODUCT_RETURN_TYPE(LHS,RHS)=CwiseBinaryOp, const LHS, const RHS>"\ + "EIGEN_CAT2(a,b)= a ## b"\ + "EIGEN_CAT(a,b)=EIGEN_CAT2(a,b)"\ + "EIGEN_CWISE_BINARY_RETURN_TYPE(LHS,RHS,OPNAME)=CwiseBinaryOp, const LHS, const RHS>"\ + DOXCOMMA=, + # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then # this tag can be used to specify a list of macro names that should be expanded. @@ -1599,7 +1613,15 @@ EXPAND_AS_DEFINED = EIGEN_MAKE_TYPEDEFS \ EIGEN_CURRENT_STORAGE_BASE_CLASS \ EIGEN_MATHFUNC_IMPL \ _EIGEN_GENERIC_PUBLIC_INTERFACE \ - EIGEN2_SUPPORT + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY \ + EIGEN_EMPTY \ + EIGEN_EULER_ANGLES_TYPEDEFS \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF \ + EIGEN_EULER_SYSTEM_TYPEDEF \ + EIGEN_DOC_UNARY_ADDONS \ + EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL \ + EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF + # If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then # doxygen's preprocessor will remove all references to function-like macros diff --git a/ground/gcs/src/libs/eigen/doc/FixedSizeVectorizable.dox b/ground/gcs/src/libs/eigen/doc/FixedSizeVectorizable.dox index 8ae1351738..49e38af768 100644 --- a/ground/gcs/src/libs/eigen/doc/FixedSizeVectorizable.dox +++ b/ground/gcs/src/libs/eigen/doc/FixedSizeVectorizable.dox @@ -4,7 +4,7 @@ namespace Eigen { The goal of this page is to explain what we mean by "fixed-size vectorizable". -\section summary Executive Summary +\section FixedSizeVectorizable_summary Executive Summary An Eigen object is called "fixed-size vectorizable" if it has fixed size and that size is a multiple of 16 bytes. @@ -21,7 +21,7 @@ Examples include: \li Eigen::Quaterniond \li Eigen::Quaternionf -\section explanation Explanation +\section FixedSizeVectorizable_explanation Explanation First, "fixed-size" should be clear: an Eigen object has fixed size if its number of rows and its number of columns are fixed at compile-time. So for example Matrix3f has fixed size, but MatrixXf doesn't (the opposite of fixed-size is dynamic-size). diff --git a/ground/gcs/src/libs/eigen/doc/InplaceDecomposition.dox b/ground/gcs/src/libs/eigen/doc/InplaceDecomposition.dox new file mode 100644 index 0000000000..cb1c6d4138 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/InplaceDecomposition.dox @@ -0,0 +1,115 @@ +namespace Eigen { + +/** \eigenManualPage InplaceDecomposition Inplace matrix decompositions + +Starting from %Eigen 3.3, the LU, Cholesky, and QR decompositions can operate \em inplace, that is, directly within the given input matrix. +This feature is especially useful when dealing with huge matrices, and or when the available memory is very limited (embedded systems). + +To this end, the respective decomposition class must be instantiated with a Ref<> matrix type, and the decomposition object must be constructed with the input matrix as argument. As an example, let us consider an inplace LU decomposition with partial pivoting. + +Let's start with the basic inclusions, and declaration of a 2x2 matrix \c A: + + + + + + + +
codeoutput
\snippet TutorialInplaceLU.cpp init + \snippet TutorialInplaceLU.out init +
+ +No surprise here! Then, let's declare our inplace LU object \c lu, and check the content of the matrix \c A: + + + + + + +
\snippet TutorialInplaceLU.cpp declaration + \snippet TutorialInplaceLU.out declaration +
+ +Here, the \c lu object computes and stores the \c L and \c U factors within the memory held by the matrix \c A. +The coefficients of \c A have thus been destroyed during the factorization, and replaced by the L and U factors as one can verify: + + + + + + +
\snippet TutorialInplaceLU.cpp matrixLU + \snippet TutorialInplaceLU.out matrixLU +
+ +Then, one can use the \c lu object as usual, for instance to solve the Ax=b problem: + + + + + +
\snippet TutorialInplaceLU.cpp solve + \snippet TutorialInplaceLU.out solve +
+ +Here, since the content of the original matrix \c A has been lost, we had to declared a new matrix \c A0 to verify the result. + +Since the memory is shared between \c A and \c lu, modifying the matrix \c A will make \c lu invalid. +This can easily be verified by modifying the content of \c A and trying to solve the initial problem again: + + + + + + +
\snippet TutorialInplaceLU.cpp modifyA + \snippet TutorialInplaceLU.out modifyA +
+ +Note that there is no shared pointer under the hood, it is the \b responsibility \b of \b the \b user to keep the input matrix \c A in life as long as \c lu is living. + +If one wants to update the factorization with the modified A, one has to call the compute method as usual: + + + + + +
\snippet TutorialInplaceLU.cpp recompute + \snippet TutorialInplaceLU.out recompute +
+ +Note that calling compute does not change the memory which is referenced by the \c lu object. Therefore, if the compute method is called with another matrix \c A1 different than \c A, then the content of \c A1 won't be modified. This is still the content of \c A that will be used to store the L and U factors of the matrix \c A1. +This can easily be verified as follows: + + + + + +
\snippet TutorialInplaceLU.cpp recompute_bis0 + \snippet TutorialInplaceLU.out recompute_bis0 +
+The matrix \c A1 is unchanged, and one can thus solve A1*x=b, and directly check the residual without any copy of \c A1: + + + + + +
\snippet TutorialInplaceLU.cpp recompute_bis1 + \snippet TutorialInplaceLU.out recompute_bis1 +
+ + +Here is the list of matrix decompositions supporting this inplace mechanism: + +- class LLT +- class LDLT +- class PartialPivLU +- class FullPivLU +- class HouseholderQR +- class ColPivHouseholderQR +- class FullPivHouseholderQR +- class CompleteOrthogonalDecomposition + +*/ + +} \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/LeastSquares.dox b/ground/gcs/src/libs/eigen/doc/LeastSquares.dox new file mode 100644 index 0000000000..e2191a22f7 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/LeastSquares.dox @@ -0,0 +1,70 @@ +namespace Eigen { + +/** \eigenManualPage LeastSquares Solving linear least squares systems + +This page describes how to solve linear least squares systems using %Eigen. An overdetermined system +of equations, say \a Ax = \a b, has no solutions. In this case, it makes sense to search for the +vector \a x which is closest to being a solution, in the sense that the difference \a Ax - \a b is +as small as possible. This \a x is called the least square solution (if the Euclidean norm is used). + +The three methods discussed on this page are the SVD decomposition, the QR decomposition and normal +equations. Of these, the SVD decomposition is generally the most accurate but the slowest, normal +equations is the fastest but least accurate, and the QR decomposition is in between. + +\eigenAutoToc + + +\section LeastSquaresSVD Using the SVD decomposition + +The \link JacobiSVD::solve() solve() \endlink method in the JacobiSVD class can be directly used to +solve linear squares systems. It is not enough to compute only the singular values (the default for +this class); you also need the singular vectors but the thin SVD decomposition suffices for +computing least squares solutions: + + + + + + + +
Example:Output:
\include TutorialLinAlgSVDSolve.cpp \verbinclude TutorialLinAlgSVDSolve.out
+ +This is example from the page \link TutorialLinearAlgebra Linear algebra and decompositions \endlink. + + +\section LeastSquaresQR Using the QR decomposition + +The solve() method in QR decomposition classes also computes the least squares solution. There are +three QR decomposition classes: HouseholderQR (no pivoting, so fast but unstable), +ColPivHouseholderQR (column pivoting, thus a bit slower but more accurate) and FullPivHouseholderQR +(full pivoting, so slowest and most stable). Here is an example with column pivoting: + + + + + + + +
Example:Output:
\include LeastSquaresQR.cpp \verbinclude LeastSquaresQR.out
+ + +\section LeastSquaresNormalEquations Using normal equations + +Finding the least squares solution of \a Ax = \a b is equivalent to solving the normal equation +ATAx = ATb. This leads to the following code + + + + + + + +
Example:Output:
\include LeastSquaresNormalEquations.cpp \verbinclude LeastSquaresNormalEquations.out
+ +If the matrix \a A is ill-conditioned, then this is not a good method, because the condition number +of ATA is the square of the condition number of \a A. This means that you +lose twice as many digits using normal equation than if you use the other methods. + +*/ + +} \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/Manual.dox b/ground/gcs/src/libs/eigen/doc/Manual.dox index 52427f0668..342b145fde 100644 --- a/ground/gcs/src/libs/eigen/doc/Manual.dox +++ b/ground/gcs/src/libs/eigen/doc/Manual.dox @@ -3,19 +3,32 @@ namespace Eigen { +/** \page UserManual_CustomizingEigen Extending/Customizing Eigen + %Eigen can be extended in several ways, for instance, by defining global methods, by inserting custom methods within main %Eigen's classes through the \ref TopicCustomizing_Plugins "plugin" mechanism, by adding support to \ref TopicCustomizing_CustomScalar "custom scalar types" etc. See below for the respective sub-topics. + - \subpage TopicCustomizing_Plugins + - \subpage TopicCustomizing_InheritingMatrix + - \subpage TopicCustomizing_CustomScalar + - \subpage TopicCustomizing_NullaryExpr + - \subpage TopicNewExpressionType + \sa \ref TopicPreprocessorDirectives +*/ + + /** \page UserManual_Generalities General topics - \subpage Eigen2ToEigen3 - \subpage TopicFunctionTakingEigenTypes - \subpage TopicPreprocessorDirectives - \subpage TopicAssertions - - \subpage TopicCustomizingEigen - \subpage TopicMultiThreading + - \subpage TopicUsingBlasLapack - \subpage TopicUsingIntelMKL + - \subpage TopicCUDA - \subpage TopicPitfalls - \subpage TopicTemplateKeyword - \subpage UserManual_UnderstandingEigen + - \subpage TopicCMakeGuide */ - + /** \page UserManual_UnderstandingEigen Understanding Eigen - \subpage TopicInsideEigenExample - \subpage TopicClassHierarchy @@ -57,6 +70,8 @@ namespace Eigen { \ingroup DenseMatrixManipulation_chapter */ /** \addtogroup TutorialMapClass \ingroup DenseMatrixManipulation_chapter */ +/** \addtogroup TutorialReshapeSlicing + \ingroup DenseMatrixManipulation_chapter */ /** \addtogroup TopicAliasing \ingroup DenseMatrixManipulation_chapter */ /** \addtogroup TopicStorageOrders @@ -86,6 +101,9 @@ namespace Eigen { /** \addtogroup Householder_Module \ingroup DenseMatrixManipulation_Reference */ +/** \addtogroup CoeffwiseMathFunctions + \ingroup DenseMatrixManipulation_chapter */ + /** \addtogroup QuickRefPage \ingroup DenseMatrixManipulation_chapter */ @@ -97,6 +115,12 @@ namespace Eigen { \ingroup DenseLinearSolvers_chapter */ /** \addtogroup TopicLinearAlgebraDecompositions \ingroup DenseLinearSolvers_chapter */ +/** \addtogroup LeastSquares + \ingroup DenseLinearSolvers_chapter */ +/** \addtogroup InplaceDecomposition + \ingroup DenseLinearSolvers_chapter */ +/** \addtogroup DenseDecompositionBenchmark + \ingroup DenseLinearSolvers_chapter */ /** \addtogroup DenseLinearSolvers_Reference \ingroup DenseLinearSolvers_chapter */ @@ -159,4 +183,7 @@ namespace Eigen { \ingroup Geometry_Reference */ /** \addtogroup Splines_Module \ingroup Geometry_Reference */ + +/** \internal \brief Namespace containing low-level routines from the %Eigen library. */ +namespace internal {} } diff --git a/ground/gcs/src/libs/eigen/doc/MatrixfreeSolverExample.dox b/ground/gcs/src/libs/eigen/doc/MatrixfreeSolverExample.dox index 9921c72614..3efa292b56 100644 --- a/ground/gcs/src/libs/eigen/doc/MatrixfreeSolverExample.dox +++ b/ground/gcs/src/libs/eigen/doc/MatrixfreeSolverExample.dox @@ -6,13 +6,12 @@ namespace Eigen { \eigenManualPage MatrixfreeSolverExample Matrix-free solvers Iterative solvers such as ConjugateGradient and BiCGSTAB can be used in a matrix free context. To this end, user must provide a wrapper class inheriting EigenBase<> and implementing the following methods: - - Index rows() and Index cols(): returns number of rows and columns respectively - - operator* with and Eigen dense column vector - - resize(rows,cols): needed for source compatibility (can stay empty) + - \c Index \c rows() and \c Index \c cols(): returns number of rows and columns respectively + - \c operator* with your type and an %Eigen dense column vector (its actual implementation goes in a specialization of the internal::generic_product_impl class) -Eigen::internal::traits<> must also be specialized for the wrapper type. +\c Eigen::internal::traits<> must also be specialized for the wrapper type. -For efficiency purpose, one might also want to provide a custom preconditioner. Here is an example using ConjugateGradient and implementing also a custom Jacobi preconditioner: +Here is a complete example wrapping an Eigen::SparseMatrix: \include matrixfree_cg.cpp Output: \verbinclude matrixfree_cg.out diff --git a/ground/gcs/src/libs/eigen/doc/NewExpressionType.dox b/ground/gcs/src/libs/eigen/doc/NewExpressionType.dox new file mode 100644 index 0000000000..c2f2433128 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/NewExpressionType.dox @@ -0,0 +1,143 @@ +namespace Eigen { + +/** \page TopicNewExpressionType Adding a new expression type + + +\warning +Disclaimer: this page is tailored to very advanced users who are not afraid of dealing with some %Eigen's internal aspects. +In most cases, a custom expression can be avoided by either using custom \ref MatrixBase::unaryExpr "unary" or \ref MatrixBase::binaryExpr "binary" functors, +while extremely complex matrix manipulations can be achieved by a nullary functors as described in the \ref TopicCustomizing_NullaryExpr "previous page". + +This page describes with the help of an example how to implement a new +light-weight expression type in %Eigen. This consists of three parts: +the expression type itself, a traits class containing compile-time +information about the expression, and the evaluator class which is +used to evaluate the expression to a matrix. + +\b TO \b DO: Write a page explaining the design, with details on +vectorization etc., and refer to that page here. + + +\eigenAutoToc + +\section TopicSetting The setting + +A circulant matrix is a matrix where each column is the same as the +column to the left, except that it is cyclically shifted downwards. +For example, here is a 4-by-4 circulant matrix: +\f[ \begin{bmatrix} + 1 & 8 & 4 & 2 \\ + 2 & 1 & 8 & 4 \\ + 4 & 2 & 1 & 8 \\ + 8 & 4 & 2 & 1 +\end{bmatrix} \f] +A circulant matrix is uniquely determined by its first column. We wish +to write a function \c makeCirculant which, given the first column, +returns an expression representing the circulant matrix. + +For simplicity, we restrict the \c makeCirculant function to dense +matrices. It may make sense to also allow arrays, or sparse matrices, +but we will not do so here. We also do not want to support +vectorization. + + +\section TopicPreamble Getting started + +We will present the file implementing the \c makeCirculant function +part by part. We start by including the appropriate header files and +forward declaring the expression class, which we will call +\c Circulant. The \c makeCirculant function will return an object of +this type. The class \c Circulant is in fact a class template; the +template argument \c ArgType refers to the type of the vector passed +to the \c makeCirculant function. + +\include make_circulant.cpp.preamble + + +\section TopicTraits The traits class + +For every expression class \c X, there should be a traits class +\c Traits in the \c Eigen::internal namespace containing +information about \c X known as compile time. + +As explained in \ref TopicSetting, we designed the \c Circulant +expression class to refer to dense matrices. The entries of the +circulant matrix have the same type as the entries of the vector +passed to the \c makeCirculant function. The type used to index the +entries is also the same. Again for simplicity, we will only return +column-major matrices. Finally, the circulant matrix is a square +matrix (number of rows equals number of columns), and the number of +rows equals the number of rows of the column vector passed to the +\c makeCirculant function. If this is a dynamic-size vector, then the +size of the circulant matrix is not known at compile-time. + +This leads to the following code: + +\include make_circulant.cpp.traits + + +\section TopicExpression The expression class + +The next step is to define the expression class itself. In our case, +we want to inherit from \c MatrixBase in order to expose the interface +for dense matrices. In the constructor, we check that we are passed a +column vector (see \ref TopicAssertions) and we store the vector from +which we are going to build the circulant matrix in the member +variable \c m_arg. Finally, the expression class should compute the +size of the corresponding circulant matrix. As explained above, this +is a square matrix with as many columns as the vector used to +construct the matrix. + +\b TO \b DO: What about the \c Nested typedef? It seems to be +necessary; is this only temporary? + +\include make_circulant.cpp.expression + + +\section TopicEvaluator The evaluator + +The last big fragment implements the evaluator for the \c Circulant +expression. The evaluator computes the entries of the circulant +matrix; this is done in the \c .coeff() member function. The entries +are computed by finding the corresponding entry of the vector from +which the circulant matrix is constructed. Getting this entry may +actually be non-trivial when the circulant matrix is constructed from +a vector which is given by a complicated expression, so we use the +evaluator which corresponds to the vector. + +The \c CoeffReadCost constant records the cost of computing an entry +of the circulant matrix; we ignore the index computation and say that +this is the same as the cost of computing an entry of the vector from +which the circulant matrix is constructed. + +In the constructor, we save the evaluator for the column vector which +defined the circulant matrix. We also save the size of that vector; +remember that we can query an expression object to find the size but +not the evaluator. + +\include make_circulant.cpp.evaluator + + +\section TopicEntry The entry point + +After all this, the \c makeCirculant function is very simple. It +simply creates an expression object and returns it. + +\include make_circulant.cpp.entry + + +\section TopicMain A simple main function for testing + +Finally, a short \c main function that shows how the \c makeCirculant +function can be called. + +\include make_circulant.cpp.main + +If all the fragments are combined, the following output is produced, +showing that the program works as expected: + +\include make_circulant.out + +*/ +} + diff --git a/ground/gcs/src/libs/eigen/doc/Overview.dox b/ground/gcs/src/libs/eigen/doc/Overview.dox index 9ab96233a7..dbb49bd218 100644 --- a/ground/gcs/src/libs/eigen/doc/Overview.dox +++ b/ground/gcs/src/libs/eigen/doc/Overview.dox @@ -17,7 +17,9 @@ You're a MatLab user? There is also a short AS The \b main \b documentation is organized into \em chapters covering different domains of features. They are themselves composed of \em user \em manual pages describing the different features in a comprehensive way, and \em reference pages that gives you access to the API documentation through the related Eigen's \em modules and \em classes. -Under the \subpage UserManual_Generalities section, you will find documentation on more general topics such as preprocessor directives, controlling assertions, multi-threading, MKL support, some Eigen's internal insights, and much more... +Under the \subpage UserManual_CustomizingEigen section, you will find discussions and examples on extending %Eigen's features and supporting custom scalar types. + +Under the \subpage UserManual_Generalities section, you will find documentation on more general topics such as preprocessor directives, controlling assertions, multi-threading, MKL support, some Eigen's internal insights, and much more... Finally, do not miss the search engine, useful to quickly get to the documentation of a given class or function. diff --git a/ground/gcs/src/libs/eigen/doc/PreprocessorDirectives.dox b/ground/gcs/src/libs/eigen/doc/PreprocessorDirectives.dox index cfaba35d84..f01b39aec5 100644 --- a/ground/gcs/src/libs/eigen/doc/PreprocessorDirectives.dox +++ b/ground/gcs/src/libs/eigen/doc/PreprocessorDirectives.dox @@ -5,7 +5,7 @@ namespace Eigen { You can control some aspects of %Eigen by defining the preprocessor tokens using \c \#define. These macros should be defined before any %Eigen headers are included. Often they are best set in the project options. -This page lists the preprocesor tokens recognised by %Eigen. +This page lists the preprocessor tokens recognized by %Eigen. \eigenAutoToc @@ -18,25 +18,67 @@ one option, and other parts (or libraries that you use) are compiled with anothe fail to link or exhibit subtle bugs. Nevertheless, these options can be useful for people who know what they are doing. - - \b EIGEN2_SUPPORT - if defined, enables the Eigen2 compatibility mode. This is meant to ease the transition - of Eigen2 to Eigen3 (see \ref Eigen2ToEigen3). Not defined by default. - - \b EIGEN2_SUPPORT_STAGEnn_xxx (for various values of nn and xxx) - staged migration path from Eigen2 to - Eigen3; see \ref Eigen2SupportModes. + - \b EIGEN2_SUPPORT and \b EIGEN2_SUPPORT_STAGEnn_xxx are disabled starting from the 3.3 release. + Defining one of these will raise a compile-error. If you need to compile Eigen2 code, + check this site. - \b EIGEN_DEFAULT_DENSE_INDEX_TYPE - the type for column and row indices in matrices, vectors and array (DenseBase::Index). Set to \c std::ptrdiff_t by default. - \b EIGEN_DEFAULT_IO_FORMAT - the IOFormat to use when printing a matrix if no %IOFormat is specified. Defaults to the %IOFormat constructed by the default constructor IOFormat::IOFormat(). - \b EIGEN_INITIALIZE_MATRICES_BY_ZERO - if defined, all entries of newly constructed matrices and arrays are initialized to zero, as are new entries in matrices and arrays after resizing. Not defined by default. + \warning The unary (resp. binary) constructor of \c 1x1 (resp. \c 2x1 or \c 1x2) fixed size matrices is + always interpreted as an initialization constructor where the argument(s) are the coefficient values + and not the sizes. For instance, \code Vector2d v(2,1); \endcode will create a vector with coeficients [2,1], + and \b not a \c 2x1 vector initialized with zeros (i.e., [0,0]). If such cases might occur, then it is + recommended to use the default constructor with a explicit call to resize: + \code + Matrix v; + v.resize(size); + Matrix m; + m.resize(rows,cols); + \endcode - \b EIGEN_INITIALIZE_MATRICES_BY_NAN - if defined, all entries of newly constructed matrices and arrays are initialized to NaN, as are new entries in matrices and arrays after resizing. This option is especially useful for debugging purpose, though a memory tool like valgrind is preferable. Not defined by default. + \warning See the documentation of \c EIGEN_INITIALIZE_MATRICES_BY_ZERO for a discussion on a limitations + of these macros when applied to \c 1x1, \c 1x2, and \c 2x1 fixed-size matrices. - \b EIGEN_NO_AUTOMATIC_RESIZING - if defined, the matrices (or arrays) on both sides of an assignment a = b have to be of the same size; otherwise, %Eigen automatically resizes \c a so that it is of the correct size. Not defined by default. +\section TopicPreprocessorDirectivesCppVersion C++ standard features + +By default, %Eigen strive to automatically detect and enable langage features at compile-time based on +the information provided by the compiler. + + - \b EIGEN_MAX_CPP_VER - disables usage of C++ features requiring a version greater than EIGEN_MAX_CPP_VER. + Possible values are: 03, 11, 14, 17, etc. If not defined (the default), %Eigen enables all features supported + by the compiler. + +Individual features can be explicitly enabled or disabled by defining the following token to 0 or 1 respectively. +For instance, one might limit the C++ version to C++03 by defining EIGEN_MAX_CPP_VER=03, but still enable C99 math +functions by defining EIGEN_HAS_C99_MATH=1. + + - \b EIGEN_HAS_C99_MATH - controls the usage of C99 math functions such as erf, erfc, lgamma, etc. + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + - \b EIGEN_HAS_CXX11_MATH - controls the implementation of some functions such as round, logp1, isinf, isnan, etc. + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + - \b EIGEN_HAS_RVALUE_REFERENCES - defines whetehr rvalue references are supported + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + - \b EIGEN_HAS_STD_RESULT_OF - defines whether std::result_of is supported + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + - \b EIGEN_HAS_VARIADIC_TEMPLATES - defines whether variadic templates are supported + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + - \b EIGEN_HAS_CONSTEXPR - defines whether relaxed const expression are supported + Automatic detection disabled if EIGEN_MAX_CPP_VER<14. + - \b EIGEN_HAS_CXX11_CONTAINERS - defines whether STL's containers follows C++11 specifications + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + - \b EIGEN_HAS_CXX11_NOEXCEPT - defines whether noexcept is supported + Automatic detection disabled if EIGEN_MAX_CPP_VER<11. + \section TopicPreprocessorDirectivesAssertions Assertions The %Eigen library contains many assertions to guard against programming errors, both at compile time and at @@ -55,32 +97,39 @@ run time. However, these assertions do cost time and can thus be turned off. \section TopicPreprocessorDirectivesPerformance Alignment, vectorization and performance tweaking - - \b EIGEN_MALLOC_ALREADY_ALIGNED - Can be set to 0 or 1 to tell whether default system malloc already + - \b \c EIGEN_MALLOC_ALREADY_ALIGNED - Can be set to 0 or 1 to tell whether default system \c malloc already returns aligned buffers. In not defined, then this information is automatically deduced from the compiler and system preprocessor tokens. - - \b EIGEN_DONT_ALIGN - disables alignment completely. %Eigen will not try to align its objects and does not - expect that any objects passed to it are aligned. This will turn off vectorization. Not defined by default. - - \b EIGEN_DONT_ALIGN_STATICALLY - disables alignment of arrays on the stack. Not defined by default, unless - \c EIGEN_DONT_ALIGN is defined. - - \b EIGEN_DONT_PARALLELIZE - if defined, this disables multi-threading. This is only relevant if you enabled OpenMP. + - \b \c EIGEN_MAX_ALIGN_BYTES - Must be a power of two, or 0. Defines an upper bound on the memory boundary in bytes on which dynamically and statically allocated data may be aligned by %Eigen. If not defined, a default value is automatically computed based on architecture, compiler, and OS. + This option is typically used to enforce binary compatibility between code/libraries compiled with different SIMD options. For instance, one may compile AVX code and enforce ABI compatibility with existing SSE code by defining \c EIGEN_MAX_ALIGN_BYTES=16. In the other way round, since by default AVX implies 32 bytes alignment for best performance, one can compile SSE code to be ABI compatible with AVX code by defining \c EIGEN_MAX_ALIGN_BYTES=32. + - \b \c EIGEN_MAX_STATIC_ALIGN_BYTES - Same as \c EIGEN_MAX_ALIGN_BYTES but for statically allocated data only. By default, if only \c EIGEN_MAX_ALIGN_BYTES is defined, then \c EIGEN_MAX_STATIC_ALIGN_BYTES == \c EIGEN_MAX_ALIGN_BYTES, otherwise a default value is automatically computed based on architecture, compiler, and OS (can be smaller than the default value of EIGEN_MAX_ALIGN_BYTES on architectures that do not support stack alignment). + Let us emphasize that \c EIGEN_MAX_*_ALIGN_BYTES define only a diserable upper bound. In practice data is aligned to largest power-of-two common divisor of \c EIGEN_MAX_STATIC_ALIGN_BYTES and the size of the data, such that memory is not wasted. + - \b \c EIGEN_DONT_PARALLELIZE - if defined, this disables multi-threading. This is only relevant if you enabled OpenMP. See \ref TopicMultiThreading for details. - \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN. - - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently + - \b \c EIGEN_UNALIGNED_VECTORIZE - disables/enables vectorization with unaligned stores. Default is 1 (enabled). + If set to 0 (disabled), then expression for which the destination cannot be aligned are not vectorized (e.g., unaligned + small fixed size vectors or matrices) + - \b \c EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently enables the SSE vectorization of sin() and cos(), and speedups sqrt() for single precision. Defined to 1 by default. Define it to 0 to disable. - - \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable + - \b \c EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not correspond to the number of iterations or the number of instructions. The default is value 100. - - \b EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. For internal + - \b \c EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. For internal temporary buffers, dynamic memory allocation is employed as a fall back. For fixed-size matrices or arrays, exceeding this threshold raises a compile time assertion. Use 0 to set no limit. Default is 128 KB. + - \c EIGEN_DONT_ALIGN - Deprecated, it is a synonym for \c EIGEN_MAX_ALIGN_BYTES=0. It disables alignment completely. %Eigen will not try to align its objects and does not expect that any objects passed to it are aligned. This will turn off vectorization if \b EIGEN_UNALIGNED_VECTORIZE=1. Not defined by default. + - \c EIGEN_DONT_ALIGN_STATICALLY - Deprecated, it is a synonym for \c EIGEN_MAX_STATIC_ALIGN_BYTES=0. It disables alignment of arrays on the stack. Not defined by default, unless \c EIGEN_DONT_ALIGN is defined. + + \section TopicPreprocessorDirectivesPlugins Plugins It is possible to add new methods to many fundamental classes in %Eigen by writing a plugin. As explained in -the section \ref ExtendingMatrixBase, the plugin is specified by defining a \c EIGEN_xxx_PLUGIN macro. The +the section \ref TopicCustomizing_Plugins, the plugin is specified by defining a \c EIGEN_xxx_PLUGIN macro. The following macros are supported; none of them are defined by default. - \b EIGEN_ARRAY_PLUGIN - filename of plugin for extending the Array class. @@ -92,6 +141,7 @@ following macros are supported; none of them are defined by default. - \b EIGEN_MATRIXBASE_PLUGIN - filename of plugin for extending the MatrixBase class. - \b EIGEN_PLAINOBJECTBASE_PLUGIN - filename of plugin for extending the PlainObjectBase class. - \b EIGEN_MAPBASE_PLUGIN - filename of plugin for extending the MapBase class. + - \b EIGEN_QUATERNION_PLUGIN - filename of plugin for extending the Quaternion class. - \b EIGEN_QUATERNIONBASE_PLUGIN - filename of plugin for extending the QuaternionBase class. - \b EIGEN_SPARSEMATRIX_PLUGIN - filename of plugin for extending the SparseMatrix class. - \b EIGEN_SPARSEMATRIXBASE_PLUGIN - filename of plugin for extending the SparseMatrixBase class. diff --git a/ground/gcs/src/libs/eigen/doc/QuickReference.dox b/ground/gcs/src/libs/eigen/doc/QuickReference.dox index a4be0f68a1..44f5410db7 100644 --- a/ground/gcs/src/libs/eigen/doc/QuickReference.dox +++ b/ground/gcs/src/libs/eigen/doc/QuickReference.dox @@ -13,17 +13,17 @@ The Eigen library is divided in a Core module and several additional modules. Ea - + - - - - - - - - - + + + + + + + + +
ModuleHeader fileContents
\link Core_Module Core \endlink\code#include \endcodeMatrix and Array classes, basic linear algebra (including triangular and selfadjoint products), array manipulation
\link Core_Module Core \endlink\code#include \endcodeMatrix and Array classes, basic linear algebra (including triangular and selfadjoint products), array manipulation
\link Geometry_Module Geometry \endlink\code#include \endcodeTransform, Translation, Scaling, Rotation2D and 3D rotations (Quaternion, AngleAxis)
\link LU_Module LU \endlink\code#include \endcodeInverse, determinant, LU decompositions with solver (FullPivLU, PartialPivLU)
\link Cholesky_Module Cholesky \endlink\code#include \endcodeLLT and LDLT Cholesky factorization with solver
\link Householder_Module Householder \endlink\code#include \endcodeHouseholder transformations; this module is used by several linear algebra modules
\link SVD_Module SVD \endlink\code#include \endcodeSVD decomposition with least-squares solver (JacobiSVD)
\link QR_Module QR \endlink\code#include \endcodeQR decomposition with solver (HouseholderQR, ColPivHouseholderQR, FullPivHouseholderQR)
\link Eigenvalues_Module Eigenvalues \endlink\code#include \endcodeEigenvalue, eigenvector decompositions (EigenSolver, SelfAdjointEigenSolver, ComplexEigenSolver)
\link Sparse_modules Sparse \endlink\code#include \endcode%Sparse matrix storage and related basic linear algebra (SparseMatrix, DynamicSparseMatrix, SparseVector)
\code#include \endcodeIncludes Core, Geometry, LU, Cholesky, SVD, QR, and Eigenvalues header files
\code#include \endcodeIncludes %Dense and %Sparse header files (the whole Eigen library)
\link LU_Module LU \endlink\code#include \endcodeInverse, determinant, LU decompositions with solver (FullPivLU, PartialPivLU)
\link Cholesky_Module Cholesky \endlink\code#include \endcodeLLT and LDLT Cholesky factorization with solver
\link Householder_Module Householder \endlink\code#include \endcodeHouseholder transformations; this module is used by several linear algebra modules
\link SVD_Module SVD \endlink\code#include \endcodeSVD decompositions with least-squares solver (JacobiSVD, BDCSVD)
\link QR_Module QR \endlink\code#include \endcodeQR decomposition with solver (HouseholderQR, ColPivHouseholderQR, FullPivHouseholderQR)
\link Eigenvalues_Module Eigenvalues \endlink\code#include \endcodeEigenvalue, eigenvector decompositions (EigenSolver, SelfAdjointEigenSolver, ComplexEigenSolver)
\link Sparse_Module Sparse \endlink\code#include \endcode%Sparse matrix storage and related basic linear algebra (SparseMatrix, SparseVector) \n (see \ref SparseQuickRefPage for details on sparse modules)
\code#include \endcodeIncludes Core, Geometry, LU, Cholesky, SVD, QR, and Eigenvalues header files
\code#include \endcodeIncludes %Dense and %Sparse header files (the whole Eigen library)
top @@ -340,7 +340,7 @@ mat1 = mat2.adjoint(); mat1.adjointInPlace(); \endcode -\link MatrixBase::dot() dot \endlink product \n inner product \matrixworld\code +\link MatrixBase::dot dot \endlink product \n inner product \matrixworld\code scalar = vec1.dot(vec2); scalar = col1.adjoint() * col2; scalar = (col1.adjoint() * col2).value();\endcode @@ -364,32 +364,10 @@ vec3 = vec1.cross(vec2);\endcode top \section QuickRef_Coeffwise Coefficient-wise \& Array operators -Coefficient-wise operators for matrices and vectors: - - - -
Matrix API \matrixworldVia Array conversions
\code -mat1.cwiseMin(mat2) -mat1.cwiseMax(mat2) -mat1.cwiseAbs2() -mat1.cwiseAbs() -mat1.cwiseSqrt() -mat1.cwiseProduct(mat2) -mat1.cwiseQuotient(mat2)\endcode -\code -mat1.array().min(mat2.array()) -mat1.array().max(mat2.array()) -mat1.array().abs2() -mat1.array().abs() -mat1.array().sqrt() -mat1.array() * mat2.array() -mat1.array() / mat2.array() -\endcode
- -It is also very simple to apply any user defined function \c foo using DenseBase::unaryExpr together with std::ptr_fun: -\code mat1.unaryExpr(std::ptr_fun(foo))\endcode -Array operators:\arrayworld +In addition to the aforementioned operators, Eigen supports numerous coefficient-wise operator and functions. +Most of them unambiguously makes sense in array-world\arrayworld. The following operators are readily available for arrays, +or available through .array() for vectors and matrices: - +
Arithmetic operators\code @@ -400,28 +378,108 @@ array1 + scalar array1 - scalar array1 += scalar array1 -= scalar array1 < array2 array1 > array2 array1 < scalar array1 > scalar array1 <= array2 array1 >= array2 array1 <= scalar array1 >= scalar array1 == array2 array1 != array2 array1 == scalar array1 != scalar +array1.min(array2) array1.max(array2) array1.min(scalar) array1.max(scalar) \endcode
Trigo, power, and \n misc functions \n and the STL variants\code -array1.min(array2) -array1.max(array2) +
Trigo, power, and \n misc functions \n and the STL-like variants\code array1.abs2() array1.abs() abs(array1) array1.sqrt() sqrt(array1) array1.log() log(array1) +array1.log10() log10(array1) array1.exp() exp(array1) -array1.pow(exponent) pow(array1,exponent) +array1.pow(array2) pow(array1,array2) +array1.pow(scalar) pow(array1,scalar) + pow(scalar,array2) array1.square() array1.cube() array1.inverse() + array1.sin() sin(array1) array1.cos() cos(array1) array1.tan() tan(array1) array1.asin() asin(array1) array1.acos() acos(array1) +array1.atan() atan(array1) +array1.sinh() sinh(array1) +array1.cosh() cosh(array1) +array1.tanh() tanh(array1) +array1.arg() arg(array1) + +array1.floor() floor(array1) +array1.ceil() ceil(array1) +array1.round() round(aray1) + +array1.isFinite() isfinite(array1) +array1.isInf() isinf(array1) +array1.isNaN() isnan(array1) +\endcode +
+ + +The following coefficient-wise operators are available for all kind of expressions (matrices, vectors, and arrays), and for both real or complex scalar types: + + + +
Eigen's APISTL-like APIs\arrayworld Comments
\code +mat1.real() +mat1.imag() +mat1.conjugate() +\endcode +\code +real(array1) +imag(array1) +conj(array1) +\endcode + +\code + // read-write, no-op for real expressions + // read-only for real, read-write for complexes + // no-op for real expressions \endcode
+Some coefficient-wise operators are readily available for for matrices and vectors through the following cwise* methods: + + + +
Matrix API \matrixworldVia Array conversions
\code +mat1.cwiseMin(mat2) mat1.cwiseMin(scalar) +mat1.cwiseMax(mat2) mat1.cwiseMax(scalar) +mat1.cwiseAbs2() +mat1.cwiseAbs() +mat1.cwiseSqrt() +mat1.cwiseInverse() +mat1.cwiseProduct(mat2) +mat1.cwiseQuotient(mat2) +mat1.cwiseEqual(mat2) mat1.cwiseEqual(scalar) +mat1.cwiseNotEqual(mat2) +\endcode +\code +mat1.array().min(mat2.array()) mat1.array().min(scalar) +mat1.array().max(mat2.array()) mat1.array().max(scalar) +mat1.array().abs2() +mat1.array().abs() +mat1.array().sqrt() +mat1.array().inverse() +mat1.array() * mat2.array() +mat1.array() / mat2.array() +mat1.array() == mat2.array() mat1.array() == scalar +mat1.array() != mat2.array() +\endcode
+The main difference between the two API is that the one based on cwise* methods returns an expression in the matrix world, +while the second one (based on .array()) returns an array expression. +Recall that .array() has no cost, it only changes the available API and interpretation of the data. + +It is also very simple to apply any user defined function \c foo using DenseBase::unaryExpr together with std::ptr_fun (c++03), std::ref (c++11), or lambdas (c++11): +\code +mat1.unaryExpr(std::ptr_fun(foo)); +mat1.unaryExpr(std::ref(foo)); +mat1.unaryExpr([](double x) { return foo(x); }); +\endcode + + top \section QuickRef_Reductions Reductions diff --git a/ground/gcs/src/libs/eigen/doc/SparseLinearSystems.dox b/ground/gcs/src/libs/eigen/doc/SparseLinearSystems.dox index 051a02ed96..fc33b93e7e 100644 --- a/ground/gcs/src/libs/eigen/doc/SparseLinearSystems.dox +++ b/ground/gcs/src/libs/eigen/doc/SparseLinearSystems.dox @@ -4,33 +4,63 @@ In Eigen, there are several methods available to solve linear systems when the c \eigenAutoToc -\section TutorialSparseDirectSolvers Sparse solvers +\section TutorialSparseSolverList List of sparse solvers -%Eigen currently provides a limited set of built-in solvers, as well as wrappers to external solver libraries. -They are summarized in the following table: +%Eigen currently provides a wide set of built-in solvers, as well as wrappers to external solver libraries. +They are summarized in the following tables: + +\subsection TutorialSparseSolverList_Direct Built-in direct solvers - - - - + + + + + - - + + + - - - - - - - + + - - + + + + - - + + +
ClassModuleSolver kindMatrix kindFeatures related to performanceDependencies,License

Notes

SimplicialLLT \link SparseCholesky_Module SparseCholesky \endlinkDirect LLt factorizationSPDFill-in reducingbuilt-in, LGPL
ClassSolver kindMatrix kindFeatures related to performanceLicense

Notes

SimplicialLLT \n \#includeDirect LLt factorizationSPDFill-in reducingLGPL SimplicialLDLT is often preferable
SimplicialLDLT \link SparseCholesky_Module SparseCholesky \endlinkDirect LDLt factorizationSPDFill-in reducingbuilt-in, LGPL
SimplicialLDLT \n \#includeDirect LDLt factorizationSPDFill-in reducingLGPL Recommended for very sparse and not too large problems (e.g., 2D Poisson eq.)
ConjugateGradient\link IterativeLinearSolvers_Module IterativeLinearSolvers \endlinkClassic iterative CGSPDPreconditionningbuilt-in, MPL2Recommended for large symmetric problems (e.g., 3D Poisson eq.)
BiCGSTAB\link IterativeLinearSolvers_Module IterativeLinearSolvers \endlinkIterative stabilized bi-conjugate gradientSquarePreconditionningbuilt-in, MPL2To speedup the convergence, try it with the \ref IncompleteLUT preconditioner.
SparseLU \link SparseLU_Module SparseLU \endlink LU factorization
SparseLU \n \#include LU factorization Square Fill-in reducing, Leverage fast dense algebra built-in, MPL2 optimized for small and large problems with irregular patterns
SparseQR \link SparseQR_Module SparseQR \endlink QR factorizationMPL2optimized for small and large problems with irregular patterns
SparseQR \n \#include QR factorization Any, rectangular Fill-in reducingbuilt-in, MPL2recommended for least-square problems, has a basic rank-revealing feature
Wrappers to external solvers
MPL2recommended for least-square problems, has a basic rank-revealing feature
+ +\subsection TutorialSparseSolverList_Iterative Built-in iterative solvers + + + + + + + + + + + + + + + + + + + +
ClassSolver kindMatrix kindSupported preconditioners, [default]License

Notes

ConjugateGradient \n \#include Classic iterative CGSPDIdentityPreconditioner, [DiagonalPreconditioner], IncompleteCholeskyMPL2Recommended for large symmetric problems (e.g., 3D Poisson eq.)
LeastSquaresConjugateGradient \n \#includeCG for rectangular least-square problemRectangularIdentityPreconditioner, [LeastSquareDiagonalPreconditioner]MPL2Solve for min |A'Ax-b|^2 without forming A'A
BiCGSTAB \n \#includeIterative stabilized bi-conjugate gradientSquareIdentityPreconditioner, [DiagonalPreconditioner], IncompleteLUTMPL2To speedup the convergence, try it with the \ref IncompleteLUT preconditioner.
+ +\subsection TutorialSparseSolverList_Wrapper Wrappers to external solvers + + + + @@ -46,10 +76,15 @@ They are summarized in the following table: + + +
ClassModuleSolver kindMatrix kindFeatures related to performanceDependencies,License

Notes

PastixLLT \n PastixLDLT \n PastixLU\link PaStiXSupport_Module PaStiXSupport \endlinkDirect LLt, LDLt, LU factorizationsSPD \n SPD \n SquareFill-in reducing, Leverage fast dense algebra, Multithreading Requires the PaStiX package, \b CeCILL-C optimized for tough problems and symmetric patterns
SPQR\link SPQRSupport_Module SPQRSupport \endlink QR factorization Any, rectangularfill-in reducing, multithreaded, fast dense algebra requires the SuiteSparse package, \b GPL recommended for linear least-squares problems, has a rank-revealing feature
PardisoLLT \n PardisoLDLT \n PardisoLU\link PardisoSupport_Module PardisoSupport \endlinkDirect LLt, LDLt, LU factorizationsSPD \n SPD \n SquareFill-in reducing, Leverage fast dense algebra, MultithreadingRequires the Intel MKL package, \b Proprietary optimized for tough problems patterns, see also \link TopicUsingIntelMKL using MKL with Eigen \endlink
Here \c SPD means symmetric positive definite. +\section TutorialSparseSolverConcept Sparse solver concept + All these solvers follow the same general concept. Here is a typical and general example: \code @@ -101,8 +136,10 @@ x2 = solver.solve(b2); \endcode The compute() method is equivalent to calling both analyzePattern() and factorize(). -Finally, each solver provides some specific features, such as determinant, access to the factors, controls of the iterations, and so on. -More details are availble in the documentations of the respective classes. +Each solver provides some specific features, such as determinant, access to the factors, controls of the iterations, and so on. +More details are available in the documentations of the respective classes. + +Finally, most of the iterative solvers, can also be used in a \b matrix-free context, see the following \link MatrixfreeSolverExample example \endlink. \section TheSparseCompute The Compute Step In the compute() function, the matrix is generally factorized: LLT for self-adjoint matrices, LDLT for general hermitian matrices, LU for non hermitian matrices and QR for rectangular matrices. These are the results of using direct solvers. For this class of solvers precisely, the compute step is further subdivided into analyzePattern() and factorize(). @@ -140,7 +177,16 @@ x2 = solver.solve(b2); For direct methods, the solution are computed at the machine precision. Sometimes, the solution need not be too accurate. In this case, the iterative methods are more suitable and the desired accuracy can be set before the solve step using \b setTolerance(). For all the available functions, please, refer to the documentation of the \link IterativeLinearSolvers_Module Iterative solvers module \endlink. \section BenchmarkRoutine -Most of the time, all you need is to know how much time it will take to qolve your system, and hopefully, what is the most suitable solver. In Eigen, we provide a benchmark routine that can be used for this purpose. It is very easy to use. In the build directory, navigate to bench/spbench and compile the routine by typing \b make \e spbenchsolver. Run it with --help option to get the list of all available options. Basically, the matrices to test should be in MatrixMarket Coordinate format, and the routine returns the statistics from all available solvers in Eigen. +Most of the time, all you need is to know how much time it will take to solve your system, and hopefully, what is the most suitable solver. In Eigen, we provide a benchmark routine that can be used for this purpose. It is very easy to use. In the build directory, navigate to bench/spbench and compile the routine by typing \b make \e spbenchsolver. Run it with --help option to get the list of all available options. Basically, the matrices to test should be in MatrixMarket Coordinate format, and the routine returns the statistics from all available solvers in Eigen. + +To export your matrices and right-hand-side vectors in the matrix-market format, you can the the unsupported SparseExtra module: +\code +#include +... +Eigen::saveMarket(A, "filename.mtx"); +Eigen::saveMarket(A, "filename_SPD.mtx", Eigen::Symmetric); // if A is symmetric-positive-definite +Eigen::saveMarketVector(B, "filename_b.mtx"); +\endcode The following table gives an example of XML statistics from several Eigen built-in and external solvers. diff --git a/ground/gcs/src/libs/eigen/doc/SparseQuickReference.dox b/ground/gcs/src/libs/eigen/doc/SparseQuickReference.dox index e0a30edccb..a25622e800 100644 --- a/ground/gcs/src/libs/eigen/doc/SparseQuickReference.dox +++ b/ground/gcs/src/libs/eigen/doc/SparseQuickReference.dox @@ -206,7 +206,7 @@ See \ref TutorialSparse_SubMatrices and below for read-write sub-matrices. sm1.innerVectors(start, size); // RW sm1.leftCols(size); // RW sm2.rightCols(size); // RO because sm2 is row-major - sm1.middleRows(start, numRows); // RO becasue sm1 is column-major + sm1.middleRows(start, numRows); // RO because sm1 is column-major sm1.middleCols(start, numCols); // RW sm1.col(j); // RW \endcode @@ -253,6 +253,20 @@ If the matrix is not in compressed form, makeCompressed() should be called befor Note that these functions are mostly provided for interoperability purposes with external libraries.\n A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section + + + + +
Mapping external buffers
+\code +int outerIndexPtr[cols+1]; +int innerIndices[nnz]; +double values[nnz]; +Map > sm1(rows,cols,nnz,outerIndexPtr, // read-write + innerIndices,values); +Map > sm2(...); // read-only +\endcode +As for dense matrices, class Map can be used to see external buffers as an %Eigen's SparseMatrix object.
*/ } diff --git a/ground/gcs/src/libs/eigen/doc/StlContainers.dox b/ground/gcs/src/libs/eigen/doc/StlContainers.dox index d8d0d529c5..e0f8714a95 100644 --- a/ground/gcs/src/libs/eigen/doc/StlContainers.dox +++ b/ground/gcs/src/libs/eigen/doc/StlContainers.dox @@ -4,7 +4,7 @@ namespace Eigen { \eigenAutoToc -\section summary Executive summary +\section StlContainers_summary Executive summary Using STL containers on \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types", or classes having members of such types, requires taking the following two steps: @@ -28,7 +28,7 @@ std::map, \endcode Note that the third parameter "std::less" is just the default value, but we have to include it because we want to specify the fourth parameter, which is the allocator type. -\section vector The case of std::vector +\section StlContainers_vector The case of std::vector The situation with std::vector was even worse (explanation below) so we had to specialize it for the Eigen::aligned_allocator type. In practice you \b must use the Eigen::aligned_allocator (not another aligned allocator), \b and \#include . diff --git a/ground/gcs/src/libs/eigen/doc/StructHavingEigenMembers.dox b/ground/gcs/src/libs/eigen/doc/StructHavingEigenMembers.dox index 74a8d52173..7fbed0eb01 100644 --- a/ground/gcs/src/libs/eigen/doc/StructHavingEigenMembers.dox +++ b/ground/gcs/src/libs/eigen/doc/StructHavingEigenMembers.dox @@ -4,11 +4,11 @@ namespace Eigen { \eigenAutoToc -\section summary Executive Summary +\section StructHavingEigenMembers_summary Executive Summary -If you define a structure having members of \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types", you must overload its "operator new" so that it generates 16-bytes-aligned pointers. Fortunately, Eigen provides you with a macro EIGEN_MAKE_ALIGNED_OPERATOR_NEW that does that for you. +If you define a structure having members of \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types", you must overload its "operator new" so that it generates 16-bytes-aligned pointers. Fortunately, %Eigen provides you with a macro EIGEN_MAKE_ALIGNED_OPERATOR_NEW that does that for you. -\section what What kind of code needs to be changed? +\section StructHavingEigenMembers_what What kind of code needs to be changed? The kind of code that needs to be changed is this: @@ -27,7 +27,7 @@ Foo *foo = new Foo; In other words: you have a class that has as a member a \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen object", and then you dynamically create an object of that class. -\section how How should such code be modified? +\section StructHavingEigenMembers_how How should such code be modified? Very easy, you just need to put a EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro in a public part of your class, like this: @@ -48,9 +48,9 @@ Foo *foo = new Foo; This macro makes "new Foo" always return an aligned pointer. -If this approach is too intrusive, see also the \ref othersolutions. +If this approach is too intrusive, see also the \ref StructHavingEigenMembers_othersolutions "other solutions". -\section why Why is this needed? +\section StructHavingEigenMembers_why Why is this needed? OK let's say that your code looks like this: @@ -67,7 +67,7 @@ class Foo Foo *foo = new Foo; \endcode -A Eigen::Vector2d consists of 2 doubles, which is 128 bits. Which is exactly the size of a SSE packet, which makes it possible to use SSE for all sorts of operations on this vector. But SSE instructions (at least the ones that Eigen uses, which are the fast ones) require 128-bit alignment. Otherwise you get a segmentation fault. +A Eigen::Vector2d consists of 2 doubles, which is 128 bits. Which is exactly the size of a SSE packet, which makes it possible to use SSE for all sorts of operations on this vector. But SSE instructions (at least the ones that %Eigen uses, which are the fast ones) require 128-bit alignment. Otherwise you get a segmentation fault. For this reason, Eigen takes care by itself to require 128-bit alignment for Eigen::Vector2d, by doing two things: \li Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). With GCC, this is done with a __attribute__ ((aligned(16))). @@ -81,7 +81,7 @@ The alignment attribute of the member v is then relative to the start of the cla The solution is to let class Foo have an aligned "operator new", as we showed in the previous section. -\section movetotop Should I then put all the members of Eigen types at the beginning of my class? +\section StructHavingEigenMembers_movetotop Should I then put all the members of Eigen types at the beginning of my class? That's not required. Since Eigen takes care of declaring 128-bit alignment, all members that need it are automatically 128-bit aligned relatively to the class. So code like this works fine: @@ -95,15 +95,15 @@ public: }; \endcode -\section dynamicsize What about dynamic-size matrices and vectors? +\section StructHavingEigenMembers_dynamicsize What about dynamic-size matrices and vectors? Dynamic-size matrices and vectors, such as Eigen::VectorXd, allocate dynamically their own array of coefficients, so they take care of requiring absolute alignment automatically. So they don't cause this issue. The issue discussed here is only with \ref TopicFixedSizeVectorizable "fixed-size vectorizable matrices and vectors". -\section bugineigen So is this a bug in Eigen? +\section StructHavingEigenMembers_bugineigen So is this a bug in Eigen? No, it's not our bug. It's more like an inherent problem of the C++98 language specification, and seems to be taken care of in the upcoming language revision: see this document. -\section conditional What if I want to do this conditionnally (depending on template parameters) ? +\section StructHavingEigenMembers_conditional What if I want to do this conditionnally (depending on template parameters) ? For this situation, we offer the macro EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign). It will generate aligned operators like EIGEN_MAKE_ALIGNED_OPERATOR_NEW if NeedsToAlign is true. It will generate operators with the default alignment if NeedsToAlign is false. @@ -128,7 +128,7 @@ Foo<3> *foo3 = new Foo<3>; // foo3 has only the system default alignment guarant \endcode -\section othersolutions Other solutions +\section StructHavingEigenMembers_othersolutions Other solutions In case putting the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro everywhere is too intrusive, there exists at least two other solutions. diff --git a/ground/gcs/src/libs/eigen/doc/TemplateKeyword.dox b/ground/gcs/src/libs/eigen/doc/TemplateKeyword.dox index f4e4f237e5..b84cfdae96 100644 --- a/ground/gcs/src/libs/eigen/doc/TemplateKeyword.dox +++ b/ground/gcs/src/libs/eigen/doc/TemplateKeyword.dox @@ -5,7 +5,8 @@ namespace Eigen { There are two uses for the \c template and \c typename keywords in C++. One of them is fairly well known amongst programmers: to define templates. The other use is more obscure: to specify that an expression refers to a template function or a type. This regularly trips up programmers that use the %Eigen library, often -leading to error messages from the compiler that are difficult to understand. +leading to error messages from the compiler that are difficult to understand, such as "expected expression" or +"no match for operator<". \eigenAutoToc @@ -72,23 +73,23 @@ for operator<". The reason that the \c template keyword is necessary in the last example has to do with the rules for how templates are supposed to be compiled in C++. The compiler has to check the code for correct syntax at the point where the template is defined, without knowing the actual value of the template arguments (\c Derived1 -and \c Derived2 in the example). That means that the compiler cannot know that dst.triangularPart is +and \c Derived2 in the example). That means that the compiler cannot know that dst.triangularView is a member template and that the following < symbol is part of the delimiter for the template -parameter. Another possibility would be that dst.triangularPart is a member variable with the < +parameter. Another possibility would be that dst.triangularView is a member variable with the < symbol refering to the operator<() function. In fact, the compiler should choose the second -possibility, according to the standard. If dst.triangularPart is a member template (as in our case), +possibility, according to the standard. If dst.triangularView is a member template (as in our case), the programmer should specify this explicitly with the \c template keyword and write dst.template -triangularPart. +triangularView. The precise rules are rather complicated, but ignoring some subtleties we can summarize them as follows: - A dependent name is name that depends (directly or indirectly) on a template parameter. In the example, \c dst is a dependent name because it is of type MatrixBase<Derived1> which depends on the template parameter \c Derived1. -- If the code contains either one of the contructions xxx.yyy or xxx->yyy and \c xxx is a +- If the code contains either one of the constructs xxx.yyy or xxx->yyy and \c xxx is a dependent name and \c yyy refers to a member template, then the \c template keyword must be used before \c yyy, leading to xxx.template yyy or xxx->template yyy. -- If the code contains the contruction xxx::yyy and \c xxx is a dependent name and \c yyy refers to a - member typedef, then the \c typename keyword must be used before the whole construction, leading to +- If the code contains the construct xxx::yyy and \c xxx is a dependent name and \c yyy refers to a + member typedef, then the \c typename keyword must be used before the whole construct, leading to typename xxx::yyy. As an example where the \c typename keyword is required, consider the following code in \ref TutorialSparse diff --git a/ground/gcs/src/libs/eigen/doc/TopicAliasing.dox b/ground/gcs/src/libs/eigen/doc/TopicAliasing.dox index c2654aed26..a8f1644284 100644 --- a/ground/gcs/src/libs/eigen/doc/TopicAliasing.dox +++ b/ground/gcs/src/libs/eigen/doc/TopicAliasing.dox @@ -153,10 +153,11 @@ not necessary to evaluate the right-hand side explicitly. \section TopicAliasingMatrixMult Aliasing and matrix multiplication -Matrix multiplication is the only operation in %Eigen that assumes aliasing by default. Thus, if \c matA is a -matrix, then the statement matA = matA * matA; is safe. All other operations in %Eigen assume that -there are no aliasing problems, either because the result is assigned to a different matrix or because it is a -component-wise operation. +Matrix multiplication is the only operation in %Eigen that assumes aliasing by default, under the +condition that the destination matrix is not resized. +Thus, if \c matA is a \b squared matrix, then the statement matA = matA * matA; is safe. +All other operations in %Eigen assume that there are no aliasing problems, +either because the result is assigned to a different matrix or because it is a component-wise operation. @@ -198,6 +199,27 @@ may get wrong results: \verbinclude TopicAliasing_mult3.out
ExampleOutput
+Moreover, starting in Eigen 3.3, aliasing is \b not assumed if the destination matrix is resized and the product is not directly assigned to the destination. +Therefore, the following example is also wrong: + + + + +
ExampleOutput
+\include TopicAliasing_mult4.cpp + +\verbinclude TopicAliasing_mult4.out +
+ +As for any aliasing issue, you can resolve it by explicitly evaluating the expression prior to assignment: + + + +
ExampleOutput
+\include TopicAliasing_mult5.cpp + +\verbinclude TopicAliasing_mult5.out +
\section TopicAliasingSummary Summary diff --git a/ground/gcs/src/libs/eigen/doc/TopicAssertions.dox b/ground/gcs/src/libs/eigen/doc/TopicAssertions.dox index 4ead40174b..c8b4d84f2f 100644 --- a/ground/gcs/src/libs/eigen/doc/TopicAssertions.dox +++ b/ground/gcs/src/libs/eigen/doc/TopicAssertions.dox @@ -16,7 +16,7 @@ Both eigen_assert and eigen_plain_assert are defined in Macros.h. Defining eigen #include #undef eigen_assert #define eigen_assert(x) \ - if (!x) { throw (std::runtime_error("Put your message here")); } + if (!(x)) { throw (std::runtime_error("Put your message here")); } \endcode \subsection DisableAssert Disabling assertions diff --git a/ground/gcs/src/libs/eigen/doc/TopicCMakeGuide.dox b/ground/gcs/src/libs/eigen/doc/TopicCMakeGuide.dox new file mode 100644 index 0000000000..896cfa831d --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/TopicCMakeGuide.dox @@ -0,0 +1,52 @@ +namespace Eigen { + +/** + +\page TopicCMakeGuide Using %Eigen in CMake Projects + +%Eigen provides native CMake support which allows the library to be easily +used in CMake projects. + +\note %CMake 3.0 (or later) is required to enable this functionality. + +%Eigen exports a CMake target called `Eigen3::Eigen` which can be imported +using the `find_package` CMake command and used by calling +`target_link_libraries` as in the following example: +\code{.cmake} +cmake_minimum_required (VERSION 3.0) +project (myproject) + +find_package (Eigen3 3.3 REQUIRED NO_MODULE) + +add_executable (example example.cpp) +target_link_libraries (example Eigen3::Eigen) +\endcode + +The above code snippet must be placed in a file called `CMakeLists.txt` alongside +`example.cpp`. After running +\code{.sh} +$ cmake path-to-example-directory +\endcode +CMake will produce project files that generate an executable called `example` +which requires at least version 3.3 of %Eigen. Here, `path-to-example-directory` +is the path to the directory that contains both `CMakeLists.txt` and +`example.cpp`. + +If you have multiple installed version of %Eigen, you can pick your favorite one by setting the \c Eigen3_DIR cmake's variable to the respective path containing the \c Eigen3*.cmake files. For instance: +\code +cmake path-to-example-directory -DEigen3_DIR=$HOME/mypackages/share/eigen3/cmake/ +\endcode + +If the `REQUIRED` option is omitted when locating %Eigen using +`find_package`, one can check whether the package was found as follows: +\code{.cmake} +find_package (Eigen3 3.3 NO_MODULE) + +if (TARGET Eigen3::Eigen) + # Use the imported target +endif (TARGET Eigen3::Eigen) +\endcode + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/TopicLazyEvaluation.dox b/ground/gcs/src/libs/eigen/doc/TopicLazyEvaluation.dox index 393bc41d87..101ef8c72c 100644 --- a/ground/gcs/src/libs/eigen/doc/TopicLazyEvaluation.dox +++ b/ground/gcs/src/libs/eigen/doc/TopicLazyEvaluation.dox @@ -36,7 +36,7 @@ Here is now a more involved example: Eigen chooses lazy evaluation at every stage in that example, which is clearly the correct choice. In fact, lazy evaluation is the "default choice" and Eigen will choose it except in a few circumstances. -The first circumstance in which Eigen chooses immediate evaluation, is when it sees an assignment a = b; and the expression \c b has the evaluate-before-assigning \link flags flag\endlink. The most important example of such an expression is the \link GeneralProduct matrix product expression\endlink. For example, when you do +The first circumstance in which Eigen chooses immediate evaluation, is when it sees an assignment a = b; and the expression \c b has the evaluate-before-assigning \link flags flag\endlink. The most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do \code matrix = matrix * matrix; \endcode @@ -48,7 +48,7 @@ What if you know that the result does no alias the operand of the product and wa Here, since we know that matrix2 is not the same matrix as matrix1, we know that lazy evaluation is not dangerous, so we may force lazy evaluation. Concretely, the effect of noalias() here is to bypass the evaluate-before-assigning \link flags flag\endlink. -The second circumstance in which Eigen chooses immediate evaluation, is when it sees a nested expression such as a + b where \c b is already an expression having the evaluate-before-nesting \link flags flag\endlink. Again, the most important example of such an expression is the \link GeneralProduct matrix product expression\endlink. For example, when you do +The second circumstance in which Eigen chooses immediate evaluation, is when it sees a nested expression such as a + b where \c b is already an expression having the evaluate-before-nesting \link flags flag\endlink. Again, the most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do \code matrix1 = matrix2 + matrix3 * matrix4; \endcode diff --git a/ground/gcs/src/libs/eigen/doc/TopicLinearAlgebraDecompositions.dox b/ground/gcs/src/libs/eigen/doc/TopicLinearAlgebraDecompositions.dox index 8649cc27b6..4914706279 100644 --- a/ground/gcs/src/libs/eigen/doc/TopicLinearAlgebraDecompositions.dox +++ b/ground/gcs/src/libs/eigen/doc/TopicLinearAlgebraDecompositions.dox @@ -4,6 +4,7 @@ namespace Eigen { This page presents a catalogue of the dense matrix decompositions offered by Eigen. For an introduction on linear solvers and decompositions, check this \link TutorialLinearAlgebra page \endlink. +To get an overview of the true relative speed of the different decomposition, check this \link DenseDecompositionBenchmark benchmark \endlink. \section TopicLinAlgBigTable Catalogue of decompositions offered by Eigen @@ -116,7 +117,7 @@ For an introduction on linear solvers and decompositions, check this \link Tutor JacobiSVD (two-sided) - Slow (but fast for small matrices) - Excellent-Proven3 + Proven3 Yes Singular values/vectors, least squares Yes (and does least squares) @@ -132,7 +133,7 @@ For an introduction on linear solvers and decompositions, check this \link Tutor Yes Eigenvalues/vectors - - Good + Excellent Closed forms for 2x2 and 3x3 @@ -249,13 +250,14 @@ For an introduction on linear solvers and decompositions, check this \link Tutor
Implicit Multi Threading (MT)
Means the algorithm can take advantage of multicore processors via OpenMP. "Implicit" means the algortihm itself is not parallelized, but that it relies on parallelized matrix-matrix product rountines.
Explicit Multi Threading (MT)
-
Means the algorithm is explicitely parallelized to take advantage of multicore processors via OpenMP.
+
Means the algorithm is explicitly parallelized to take advantage of multicore processors via OpenMP.
Meta-unroller
Means the algorithm is automatically and explicitly unrolled for very small fixed size matrices.
+ */ } diff --git a/ground/gcs/src/libs/eigen/doc/TopicMultithreading.dox b/ground/gcs/src/libs/eigen/doc/TopicMultithreading.dox index 7db2b0e070..47c9b261f8 100644 --- a/ground/gcs/src/libs/eigen/doc/TopicMultithreading.dox +++ b/ground/gcs/src/libs/eigen/doc/TopicMultithreading.dox @@ -8,13 +8,13 @@ Some Eigen's algorithms can exploit the multiple cores present in your hardware. * GCC: \c -fopenmp * ICC: \c -openmp * MSVC: check the respective option in the build properties. -You can control the number of thread that will be used using either the OpenMP API or Eiegn's API using the following priority: +You can control the number of thread that will be used using either the OpenMP API or Eigen's API using the following priority: \code OMP_NUM_THREADS=n ./my_program omp_set_num_threads(n); Eigen::setNbThreads(n); \endcode -Unless setNbThreads has been called, Eigen uses the number of threads specified by OpenMP. You can restore this bahavior by calling \code setNbThreads(0); \endcode +Unless setNbThreads has been called, Eigen uses the number of threads specified by OpenMP. You can restore this behavior by calling \code setNbThreads(0); \endcode You can query the number of threads that will be used with: \code n = Eigen::nbThreads( ); @@ -22,8 +22,12 @@ n = Eigen::nbThreads( ); You can disable Eigen's multi threading at compile time by defining the EIGEN_DONT_PARALLELIZE preprocessor token. Currently, the following algorithms can make use of multi-threading: - * general matrix - matrix products - * PartialPivLU + - general dense matrix - matrix products + - PartialPivLU + - row-major-sparse * dense vector/matrix products + - ConjugateGradient with \c Lower|Upper as the \c UpLo template parameter. + - BiCGSTAB with a row-major sparse matrix format. + - LeastSquaresConjugateGradient \section TopicMultiThreading_UsingEigenWithMT Using Eigen in a multi-threaded application @@ -39,6 +43,10 @@ int main(int argc, char** argv) } \endcode +\note With Eigen 3.3, and a fully C++11 compliant compiler (i.e., thread-safe static local variable initialization), then calling \c initParallel() is optional. + +\warning note that all functions generating random matrices are \b not re-entrant nor thread-safe. Those include DenseBase::Random(), and DenseBase::setRandom() despite a call to Eigen::initParallel(). This is because these functions are based on std::rand which is not re-entrant. For thread-safe random generator, we recommend the use of boost::random or c++11 random feature. + In the case your application is parallelized with OpenMP, you might want to disable Eigen's own parallization as detailed in the previous section. */ diff --git a/ground/gcs/src/libs/eigen/doc/TutorialArrayClass.dox b/ground/gcs/src/libs/eigen/doc/TutorialArrayClass.dox index 6432684aa5..f6f351091a 100644 --- a/ground/gcs/src/libs/eigen/doc/TutorialArrayClass.dox +++ b/ground/gcs/src/libs/eigen/doc/TutorialArrayClass.dox @@ -157,7 +157,7 @@ The following example shows how to use array operations on a Matrix object by em * to multiply them coefficient-wise and assigns the result to the matrix variable \c result (this is legal because Eigen allows assigning array expressions to matrix variables). -As a matter of fact, this usage case is so common that Eigen provides a \link MatrixBase::cwiseProduct() const +As a matter of fact, this usage case is so common that Eigen provides a \link MatrixBase::cwiseProduct const .cwiseProduct(.) \endlink method for matrices to compute the coefficient-wise product. This is also shown in the example program. diff --git a/ground/gcs/src/libs/eigen/doc/TutorialGeometry.dox b/ground/gcs/src/libs/eigen/doc/TutorialGeometry.dox index 372a275de4..2e1420f98c 100644 --- a/ground/gcs/src/libs/eigen/doc/TutorialGeometry.dox +++ b/ground/gcs/src/libs/eigen/doc/TutorialGeometry.dox @@ -126,11 +126,12 @@ Apply the transformation to a \b vector \code VectorNf vec1, vec2; vec2 = t.linear() * vec1;\endcode -Apply a \em general transformation \n to a \b normal \b vector -(explanations)\code +Apply a \em general transformation \n to a \b normal \b vector \n +\code VectorNf n1, n2; MatrixNf normalMatrix = t.linear().inverse().transpose(); n2 = (normalMatrix * n1).normalized();\endcode +(See subject 5.27 of this faq for the explanations) Apply a transformation with \em pure \em rotation \n to a \b normal \b vector (no scaling, no shear)\code @@ -231,8 +232,8 @@ On the other hand, since there exist 24 different conventions, they are pretty c to create a rotation matrix according to the 2-1-2 convention.\code Matrix3f m; m = AngleAxisf(angle1, Vector3f::UnitZ()) -* * AngleAxisf(angle2, Vector3f::UnitY()) -* * AngleAxisf(angle3, Vector3f::UnitZ()); + * AngleAxisf(angle2, Vector3f::UnitY()) + * AngleAxisf(angle3, Vector3f::UnitZ()); \endcode diff --git a/ground/gcs/src/libs/eigen/doc/TutorialLinearAlgebra.dox b/ground/gcs/src/libs/eigen/doc/TutorialLinearAlgebra.dox index b09f3543e9..cb92ceeae1 100644 --- a/ground/gcs/src/libs/eigen/doc/TutorialLinearAlgebra.dox +++ b/ground/gcs/src/libs/eigen/doc/TutorialLinearAlgebra.dox @@ -40,8 +40,9 @@ depending on your matrix and the trade-off you want to make: Decomposition Method - Requirements on the matrix - Speed + Requirements
on the matrix + Speed
(small-to-medium) + Speed
(large) Accuracy @@ -49,6 +50,7 @@ depending on your matrix and the trade-off you want to make: partialPivLu() Invertible ++ + ++ + @@ -56,6 +58,7 @@ depending on your matrix and the trade-off you want to make: fullPivLu() None - + - - +++ @@ -63,20 +66,23 @@ depending on your matrix and the trade-off you want to make: householderQr() None ++ + ++ + ColPivHouseholderQR colPivHouseholderQr() None - + ++ + - + +++ FullPivHouseholderQR fullPivHouseholderQr() None - + - - +++ @@ -84,21 +90,31 @@ depending on your matrix and the trade-off you want to make: llt() Positive definite +++ + +++ + LDLT ldlt() - Positive or negative semidefinite + Positive or negative
semidefinite +++ + + ++ + + JacobiSVD + jacobiSvd() + None + - - + - - - + +++ + All of these decompositions offer a solve() method that works as in the above example. For example, if your matrix is positive definite, the above table says that a very good -choice is then the LDLT decomposition. Here's an example, also demonstrating that using a general +choice is then the LLT or LDLT decomposition. Here's an example, also demonstrating that using a general matrix (not a vector) as right hand side is possible. @@ -167,8 +183,8 @@ Here is an example: \section TutorialLinAlgLeastsquares Least squares solving -The best way to do least squares solving is with a SVD decomposition. Eigen provides one as the JacobiSVD class, and its solve() -is doing least-squares solving. +The most accurate method to do least squares solving is with a SVD decomposition. Eigen provides one +as the JacobiSVD class, and its solve() is doing least-squares solving. Here is an example:
@@ -179,9 +195,10 @@ Here is an example:
-Another way, potentially faster but less reliable, is to use a LDLT decomposition -of the normal matrix. In any case, just read any reference text on least squares, and it will be very easy for you -to implement any linear least squares computation on top of Eigen. +Another methods, potentially faster but less reliable, are to use a Cholesky decomposition of the +normal matrix or a QR decomposition. Our page on \link LeastSquares least squares solving \endlink +has more details. + \section TutorialLinAlgSeparateComputation Separating the computation from the construction diff --git a/ground/gcs/src/libs/eigen/doc/TutorialReductionsVisitorsBroadcasting.dox b/ground/gcs/src/libs/eigen/doc/TutorialReductionsVisitorsBroadcasting.dox index 992cf6f345..f5322b4a6f 100644 --- a/ground/gcs/src/libs/eigen/doc/TutorialReductionsVisitorsBroadcasting.dox +++ b/ground/gcs/src/libs/eigen/doc/TutorialReductionsVisitorsBroadcasting.dox @@ -32,7 +32,7 @@ Eigen also provides the \link MatrixBase::norm() norm() \endlink method, which r These operations can also operate on matrices; in that case, a n-by-p matrix is seen as a vector of size (n*p), so for example the \link MatrixBase::norm() norm() \endlink method returns the "Frobenius" or "Hilbert-Schmidt" norm. We refrain from speaking of the \f$\ell^2\f$ norm of a matrix because that can mean different things. -If you want other \f$\ell^p\f$ norms, use the \link MatrixBase::lpNorm() lpNnorm

() \endlink method. The template parameter \a p can take the special value \a Infinity if you want the \f$\ell^\infty\f$ norm, which is the maximum of the absolute values of the coefficients. +If you want other coefficient-wise \f$\ell^p\f$ norms, use the \link MatrixBase::lpNorm lpNorm

() \endlink method. The template parameter \a p can take the special value \a Infinity if you want the \f$\ell^\infty\f$ norm, which is the maximum of the absolute values of the coefficients. The following example demonstrates these methods. @@ -45,6 +45,17 @@ The following example demonstrates these methods. \verbinclude Tutorial_ReductionsVisitorsBroadcasting_reductions_norm.out +\b Operator \b norm: The 1-norm and \f$\infty\f$-norm matrix operator norms can easily be computed as follows: + + + +
Example:Output:
+\include Tutorial_ReductionsVisitorsBroadcasting_reductions_operatornorm.cpp + +\verbinclude Tutorial_ReductionsVisitorsBroadcasting_reductions_operatornorm.out +
+See below for more explanations on the syntax of these expressions. + \subsection TutorialReductionsVisitorsBroadcastingReductionsBool Boolean reductions The following reductions operate on boolean values: @@ -79,7 +90,7 @@ Array. The arguments passed to a visitor are pointers to the variables where the row and column position are to be stored. These variables should be of type -\link DenseBase::Index Index \endlink, as shown below: +\link Eigen::Index Index \endlink, as shown below: @@ -90,17 +101,16 @@ row and column position are to be stored. These variables should be of type \verbinclude Tutorial_ReductionsVisitorsBroadcasting_visitors.out
Example:Output:
-Note that both functions also return the value of the minimum or maximum coefficient if needed, -as if it was a typical reduction operation. +Both functions also return the value of the minimum or maximum coefficient. \section TutorialReductionsVisitorsBroadcastingPartialReductions Partial reductions Partial reductions are reductions that can operate column- or row-wise on a Matrix or Array, applying the reduction operation on each column or row and -returning a column or row-vector with the corresponding values. Partial reductions are applied +returning a column or row vector with the corresponding values. Partial reductions are applied with \link DenseBase::colwise() colwise() \endlink or \link DenseBase::rowwise() rowwise() \endlink. A simple example is obtaining the maximum of the elements -in each column in a given matrix, storing the result in a row-vector: +in each column in a given matrix, storing the result in a row vector: @@ -122,8 +132,7 @@ The same operation can be performed row-wise: \verbinclude Tutorial_ReductionsVisitorsBroadcasting_rowwise.out
Example:Output:
-Note that column-wise operations return a 'row-vector' while row-wise operations -return a 'column-vector' +Note that column-wise operations return a row vector, while row-wise operations return a column vector. \subsection TutorialReductionsVisitorsBroadcastingPartialReductionsCombined Combining partial reductions with other operations It is also possible to use the result of a partial reduction to do further processing. @@ -165,7 +174,7 @@ The concept behind broadcasting is similar to partial reductions, with the diffe constructs an expression where a vector (column or row) is interpreted as a matrix by replicating it in one direction. -A simple example is to add a certain column-vector to each column in a matrix. +A simple example is to add a certain column vector to each column in a matrix. This can be accomplished with: @@ -242,7 +251,7 @@ is a new matrix whose size is the same as matrix m: \f[ \f] - (m.colwise() - v).colwise().squaredNorm() is a partial reduction, computing the squared norm column-wise. The result of -this operation is a row-vector where each coefficient is the squared Euclidean distance between each column in m and v: \f[ +this operation is a row vector where each coefficient is the squared Euclidean distance between each column in m and v: \f[ \mbox{(m.colwise() - v).colwise().squaredNorm()} = \begin{bmatrix} 1 & 505 & 32 & 50 diff --git a/ground/gcs/src/libs/eigen/doc/TutorialReshapeSlicing.dox b/ground/gcs/src/libs/eigen/doc/TutorialReshapeSlicing.dox new file mode 100644 index 0000000000..3730a5de6e --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/TutorialReshapeSlicing.dox @@ -0,0 +1,65 @@ +namespace Eigen { + +/** \eigenManualPage TutorialReshapeSlicing Reshape and Slicing + +%Eigen does not expose convenient methods to take slices or to reshape a matrix yet. +Nonetheless, such features can easily be emulated using the Map class. + +\eigenAutoToc + +\section TutorialReshape Reshape + +A reshape operation consists in modifying the sizes of a matrix while keeping the same coefficients. +Instead of modifying the input matrix itself, which is not possible for compile-time sizes, the approach consist in creating a different \em view on the storage using class Map. +Here is a typical example creating a 1D linear view of a matrix: + +
+ + +
Example:Output:
+\include Tutorial_ReshapeMat2Vec.cpp + +\verbinclude Tutorial_ReshapeMat2Vec.out +
+ +Remark how the storage order of the input matrix modifies the order of the coefficients in the linear view. +Here is another example reshaping a 2x6 matrix to a 6x2 one: + + + +
Example:Output:
+\include Tutorial_ReshapeMat2Mat.cpp + +\verbinclude Tutorial_ReshapeMat2Mat.out +
+ + + +\section TutorialSlicing Slicing + +Slicing consists in taking a set of rows, columns, or elements, uniformly spaced within a matrix. +Again, the class Map allows to easily mimic this feature. + +For instance, one can skip every P elements in a vector: + + + +
Example:Output:
+\include Tutorial_SlicingVec.cpp + +\verbinclude Tutorial_SlicingVec.out +
+ +One can olso take one column over three using an adequate outer-stride or inner-stride depending on the actual storage order: + + + +
Example:Output:
+\include Tutorial_SlicingCol.cpp + +\verbinclude Tutorial_SlicingCol.out +
+ +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/TutorialSparse.dox b/ground/gcs/src/libs/eigen/doc/TutorialSparse.dox index ee206cc421..3529074088 100644 --- a/ground/gcs/src/libs/eigen/doc/TutorialSparse.dox +++ b/ground/gcs/src/libs/eigen/doc/TutorialSparse.dox @@ -83,7 +83,7 @@ There is no notion of compressed/uncompressed mode for a SparseVector. \section TutorialSparseExample First example -Before describing each individual class, let's start with the following typical example: solving the Laplace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. +Before describing each individual class, let's start with the following typical example: solving the Laplace equation \f$ \Delta u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. Such problem can be mathematically expressed as a linear problem of the form \f$ Ax=b \f$ where \f$ x \f$ is the vector of \c m unknowns (in our case, the values of the pixels), \f$ b \f$ is the right hand side vector resulting from the boundary conditions, and \f$ A \f$ is an \f$ m \times m \f$ matrix containing only a few non-zero elements resulting from the discretization of the Laplacian operator. @@ -253,15 +253,19 @@ SparseMatrix A, B; B = SparseMatrix(A.transpose()) + A; \endcode -Some binary coefficient-wise operators can also mix sparse and dense expressions: +Binary coefficient wise operators can also mix sparse and dense expressions: \code sm2 = sm1.cwiseProduct(dm1); -dm1 += sm1; +dm2 = sm1 + dm1; +dm2 = dm1 - sm1; \endcode +Performance-wise, the adding/subtracting sparse and dense matrices is better performed in two steps. For instance, instead of doing dm2 = sm1 + dm1, better write: +\code +dm2 = dm1; +dm2 += sm1; +\endcode +This version has the advantage to fully exploit the higher performance of dense storage (no indirection, SIMD, etc.), and to pay the cost of slow sparse evaluation on the few non-zeros of the sparse matrix only. -However, it is not yet possible to add a sparse and a dense matrix as in dm2 = sm1 + dm1. -Please write this as the equivalent dm2 = dm1; dm2 += sm1 (we plan to lift this restriction -in the next release of %Eigen). %Sparse expressions also support transposition: \code diff --git a/ground/gcs/src/libs/eigen/doc/UnalignedArrayAssert.dox b/ground/gcs/src/libs/eigen/doc/UnalignedArrayAssert.dox index b0d6e18f29..95d95a2d51 100644 --- a/ground/gcs/src/libs/eigen/doc/UnalignedArrayAssert.dox +++ b/ground/gcs/src/libs/eigen/doc/UnalignedArrayAssert.dox @@ -8,7 +8,7 @@ my_program: path/to/eigen/Eigen/src/Core/DenseStorage.h:44: Eigen::internal::matrix_array::internal::matrix_array() [with T = double, int Size = 2, int MatrixOptions = 2, bool Align = true]: Assertion `(reinterpret_cast(array) & (sizemask)) == 0 && "this assertion -is explained here: http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html +is explained here: http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html **** READ THIS WEB PAGE !!! ****"' failed. @@ -92,27 +92,28 @@ Note that here, Eigen::Quaternionf is only used as an example, more generally th \section explanation General explanation of this assertion -\ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen objects" must absolutely be created at 16-byte-aligned locations, otherwise SIMD instructions adressing them will crash. +\ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen objects" must absolutely be created at 16-byte-aligned locations, otherwise SIMD instructions addressing them will crash. Eigen normally takes care of these alignment issues for you, by setting an alignment attribute on them and by overloading their "operator new". However there are a few corner cases where these alignment settings get overridden: they are the possible causes for this assertion. -\section getrid I don't care about vectorization, how do I get rid of that stuff? +\section getrid I don't care about optimal vectorization, how do I get rid of that stuff? -Two possibilities: +Three possibilities:
    -
  • Define EIGEN_DONT_ALIGN_STATICALLY. That disables all 128-bit static alignment code, while keeping 128-bit heap alignment. This has the effect of - disabling vectorization for fixed-size objects (like Matrix4d) while keeping vectorization of dynamic-size objects - (like MatrixXd). But do note that this breaks ABI compatibility with the default behavior of 128-bit static alignment.
    • -
  • Or define both EIGEN_DONT_VECTORIZE and EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT. This keeps the - 128-bit alignment code and thus preserves ABI compatibility, but completely disables vectorization.
    • +
  • Use the \c DontAlign option to Matrix, Array, Quaternion, etc. objects that gives you trouble. This way Eigen won't try to align them, and thus won"t assume any special alignment. On the down side, you will pay the cost of unaligned loads/stores for them, but on modern CPUs, the overhead is either null or marginal. See \link StructHavingEigenMembers_othersolutions here \endlink for an example.
    • +
  • Define \link TopicPreprocessorDirectivesPerformance EIGEN_DONT_ALIGN_STATICALLY \endlink. That disables all 16-byte (and above) static alignment code, while keeping 16-byte (or above) heap alignment. This has the effect of + vectorizing fixed-size objects (like Matrix4d) through unaligned stores (as controlled by \link TopicPreprocessorDirectivesPerformance EIGEN_UNALIGNED_VECTORIZE \endlink), while keeping unchanged the vectorization of dynamic-size objects + (like MatrixXd). But do note that this breaks ABI compatibility with the default behavior of static alignment.
    • +
  • Or define both \link TopicPreprocessorDirectivesPerformance EIGEN_DONT_VECTORIZE \endlink and EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT. This keeps the + 16-byte alignment code and thus preserves ABI compatibility, but completely disables vectorization.
-If you want to know why defining EIGEN_DONT_VECTORIZE does not by itself disable 128-bit alignment and the assertion, here's the explanation: +If you want to know why defining EIGEN_DONT_VECTORIZE does not by itself disable 16-byte alignment and the assertion, here's the explanation: It doesn't disable the assertion, because otherwise code that runs fine without vectorization would suddenly crash when enabling vectorization. -It doesn't disable 128bit alignment, because that would mean that vectorized and non-vectorized code are not mutually ABI-compatible. This ABI compatibility is very important, even for people who develop only an in-house application, as for instance one may want to have in the same application a vectorized path and a non-vectorized path. +It doesn't disable 16-byte alignment, because that would mean that vectorized and non-vectorized code are not mutually ABI-compatible. This ABI compatibility is very important, even for people who develop only an in-house application, as for instance one may want to have in the same application a vectorized path and a non-vectorized path. */ diff --git a/ground/gcs/src/libs/eigen/doc/UsingBlasLapackBackends.dox b/ground/gcs/src/libs/eigen/doc/UsingBlasLapackBackends.dox new file mode 100644 index 0000000000..caa597122a --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/UsingBlasLapackBackends.dox @@ -0,0 +1,133 @@ +/* + Copyright (c) 2011, Intel Corporation. All rights reserved. + Copyright (C) 2011-2016 Gael Guennebaud + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of Intel Corporation nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + ******************************************************************************** + * Content : Documentation on the use of BLAS/LAPACK libraries through Eigen + ******************************************************************************** +*/ + +namespace Eigen { + +/** \page TopicUsingBlasLapack Using BLAS/LAPACK from %Eigen + + +Since %Eigen version 3.3 and later, any F77 compatible BLAS or LAPACK libraries can be used as backends for dense matrix products and dense matrix decompositions. +For instance, one can use Intel® MKL, Apple's Accelerate framework on OSX, OpenBLAS, Netlib LAPACK, etc. + +Do not miss this \link TopicUsingIntelMKL page \endlink for further discussions on the specific use of Intel® MKL (also includes VML, PARDISO, etc.) + +In order to use an external BLAS and/or LAPACK library, you must link you own application to the respective libraries and their dependencies. +For LAPACK, you must also link to the standard Lapacke library, which is used as a convenient think layer between %Eigen's C++ code and LAPACK F77 interface. Then you must activate their usage by defining one or multiple of the following macros (\b before including any %Eigen's header): + +\note For Mac users, in order to use the lapack version shipped with the Accelerate framework, you also need the lapacke library. +Using MacPorts, this is as easy as: +\code +sudo port install lapack +\endcode +and then use the following link flags: \c -framework \c Accelerate \c /opt/local/lib/lapack/liblapacke.dylib + +
+ + + +
\c EIGEN_USE_BLAS Enables the use of external BLAS level 2 and 3 routines (compatible with any F77 BLAS interface)
\c EIGEN_USE_LAPACKE Enables the use of external Lapack routines via the Lapacke C interface to Lapack (compatible with any F77 LAPACK interface)
\c EIGEN_USE_LAPACKE_STRICT Same as \c EIGEN_USE_LAPACKE but algorithms of lower numerical robustness are disabled. \n This currently concerns only JacobiSVD which otherwise would be replaced by \c gesvd that is less robust than Jacobi rotations.
+ +When doing so, a number of %Eigen's algorithms are silently substituted with calls to BLAS or LAPACK routines. +These substitutions apply only for \b Dynamic \b or \b large enough objects with one of the following four standard scalar types: \c float, \c double, \c complex, and \c complex. +Operations on other scalar types or mixing reals and complexes will continue to use the built-in algorithms. + +The breadth of %Eigen functionality that can be substituted is listed in the table below. + + + + + + + + + + +
Functional domainCode exampleBLAS/LAPACK routines
Matrix-matrix operations \n \c EIGEN_USE_BLAS \code +m1*m2.transpose(); +m1.selfadjointView()*m2; +m1*m2.triangularView(); +m1.selfadjointView().rankUpdate(m2,1.0); +\endcode\code +?gemm +?symm/?hemm +?trmm +dsyrk/ssyrk +\endcode
Matrix-vector operations \n \c EIGEN_USE_BLAS \code +m1.adjoint()*b; +m1.selfadjointView()*b; +m1.triangularView()*b; +\endcode\code +?gemv +?symv/?hemv +?trmv +\endcode
LU decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code +v1 = m1.lu().solve(v2); +\endcode\code +?getrf +\endcode
Cholesky decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code +v1 = m2.selfadjointView().llt().solve(v2); +\endcode\code +?potrf +\endcode
QR decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code +m1.householderQr(); +m1.colPivHouseholderQr(); +\endcode\code +?geqrf +?geqp3 +\endcode
Singular value decomposition \n \c EIGEN_USE_LAPACKE \code +JacobiSVD svd; +svd.compute(m1, ComputeThinV); +\endcode\code +?gesvd +\endcode
Eigen-value decompositions \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code +EigenSolver es(m1); +ComplexEigenSolver ces(m1); +SelfAdjointEigenSolver saes(m1+m1.transpose()); +GeneralizedSelfAdjointEigenSolver + gsaes(m1+m1.transpose(),m2+m2.transpose()); +\endcode\code +?gees +?gees +?syev/?heev +?syev/?heev, +?potrf +\endcode
Schur decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code +RealSchur schurR(m1); +ComplexSchur schurC(m1); +\endcode\code +?gees +\endcode
+In the examples, m1 and m2 are dense matrices and v1 and v2 are dense vectors. + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/UsingIntelMKL.dox b/ground/gcs/src/libs/eigen/doc/UsingIntelMKL.dox index 84db992b63..a1a3a18f26 100644 --- a/ground/gcs/src/libs/eigen/doc/UsingIntelMKL.dox +++ b/ground/gcs/src/libs/eigen/doc/UsingIntelMKL.dox @@ -32,107 +32,45 @@ namespace Eigen { -/** \page TopicUsingIntelMKL Using Intel® Math Kernel Library from Eigen +/** \page TopicUsingIntelMKL Using Intel® MKL from %Eigen -\section TopicUsingIntelMKL_Intro Eigen and Intel® Math Kernel Library (Intel® MKL) + + +Since %Eigen version 3.1 and later, users can benefit from built-in Intel® Math Kernel Library (MKL) optimizations with an installed copy of Intel MKL 10.3 (or later). -Since Eigen version 3.1 and later, users can benefit from built-in Intel MKL optimizations with an installed copy of Intel MKL 10.3 (or later). Intel MKL provides highly optimized multi-threaded mathematical routines for x86-compatible architectures. Intel MKL is available on Linux, Mac and Windows for both Intel64 and IA32 architectures. \note Intel® MKL is a proprietary software and it is the responsibility of users to buy or register for community (free) Intel MKL licenses for their products. Moreover, the license of the user product has to allow linking to proprietary software that excludes any unmodified versions of the GPL. -Using Intel MKL through Eigen is easy: --# define the \c EIGEN_USE_MKL_ALL macro before including any Eigen's header +Using Intel MKL through %Eigen is easy: +-# define the \c EIGEN_USE_MKL_ALL macro before including any %Eigen's header -# link your program to MKL libraries (see the MKL linking advisor) -# on a 64bits system, you must use the LP64 interface (not the ILP64 one) -When doing so, a number of Eigen's algorithms are silently substituted with calls to Intel MKL routines. +When doing so, a number of %Eigen's algorithms are silently substituted with calls to Intel MKL routines. These substitutions apply only for \b Dynamic \b or \b large enough objects with one of the following four standard scalar types: \c float, \c double, \c complex, and \c complex. Operations on other scalar types or mixing reals and complexes will continue to use the built-in algorithms. -In addition you can coarsely select choose which parts will be substituted by defining one or multiple of the following macros: +In addition you can choose which parts will be substituted by defining one or multiple of the following macros: - - - + + +
\c EIGEN_USE_BLAS Enables the use of external BLAS level 2 and 3 routines (currently works with Intel MKL only)
\c EIGEN_USE_LAPACKE Enables the use of external Lapack routines via the Intel Lapacke C interface to Lapack (currently works with Intel MKL only)
\c EIGEN_USE_LAPACKE_STRICT Same as \c EIGEN_USE_LAPACKE but algorithm of lower robustness are disabled. This currently concerns only JacobiSVD which otherwise would be replaced by \c gesvd that is less robust than Jacobi rotations.
\c EIGEN_USE_BLAS Enables the use of external BLAS level 2 and 3 routines
\c EIGEN_USE_LAPACKE Enables the use of external Lapack routines via the Lapacke C interface to Lapack
\c EIGEN_USE_LAPACKE_STRICT Same as \c EIGEN_USE_LAPACKE but algorithm of lower robustness are disabled. \n This currently concerns only JacobiSVD which otherwise would be replaced by \c gesvd that is less robust than Jacobi rotations.
\c EIGEN_USE_MKL_VML Enables the use of Intel VML (vector operations)
\c EIGEN_USE_MKL_ALL Defines \c EIGEN_USE_BLAS, \c EIGEN_USE_LAPACKE, and \c EIGEN_USE_MKL_VML
-Finally, the PARDISO sparse solver shipped with Intel MKL can be used through the \ref PardisoLU, \ref PardisoLLT and \ref PardisoLDLT classes of the \ref PardisoSupport_Module. - +Note that the BLAS and LAPACKE backends can be enabled for any F77 compatible BLAS and LAPACK libraries. See this \link TopicUsingBlasLapack page \endlink for the details. -\section TopicUsingIntelMKL_SupportedFeatures List of supported features +Finally, the PARDISO sparse solver shipped with Intel MKL can be used through the \ref PardisoLU, \ref PardisoLLT and \ref PardisoLDLT classes of the \ref PardisoSupport_Module. -The breadth of Eigen functionality covered by Intel MKL is listed in the table below. +The following table summarizes the list of functions covered by \c EIGEN_USE_MKL_VML: - - - - - - - - - - +
Functional domainCode exampleMKL routines
Matrix-matrix operations \n \c EIGEN_USE_BLAS \code -m1*m2.transpose(); -m1.selfadjointView()*m2; -m1*m2.triangularView(); -m1.selfadjointView().rankUpdate(m2,1.0); -\endcode\code -?gemm -?symm/?hemm -?trmm -dsyrk/ssyrk -\endcode
Matrix-vector operations \n \c EIGEN_USE_BLAS \code -m1.adjoint()*b; -m1.selfadjointView()*b; -m1.triangularView()*b; -\endcode\code -?gemv -?symv/?hemv -?trmv -\endcode
LU decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code -v1 = m1.lu().solve(v2); -\endcode\code -?getrf -\endcode
Cholesky decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code -v1 = m2.selfadjointView().llt().solve(v2); -\endcode\code -?potrf -\endcode
QR decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code -m1.householderQr(); -m1.colPivHouseholderQr(); -\endcode\code -?geqrf -?geqp3 -\endcode
Singular value decomposition \n \c EIGEN_USE_LAPACKE \code -JacobiSVD svd; -svd.compute(m1, ComputeThinV); -\endcode\code -?gesvd -\endcode
Eigen-value decompositions \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code -EigenSolver es(m1); -ComplexEigenSolver ces(m1); -SelfAdjointEigenSolver saes(m1+m1.transpose()); -GeneralizedSelfAdjointEigenSolver - gsaes(m1+m1.transpose(),m2+m2.transpose()); -\endcode\code -?gees -?gees -?syev/?heev -?syev/?heev, -?potrf -\endcode
Schur decomposition \n \c EIGEN_USE_LAPACKE \n \c EIGEN_USE_LAPACKE_STRICT \code -RealSchur schurR(m1); -ComplexSchur schurC(m1); -\endcode\code -?gees -\endcode
Vector Math \n \c EIGEN_USE_MKL_VML \code +
Code exampleMKL routines
\code v2=v1.array().sin(); v2=v1.array().asin(); v2=v1.array().cos(); @@ -156,7 +94,7 @@ v?Sqr v?Powx \endcode
-In the examples, m1 and m2 are dense matrices and v1 and v2 are dense vectors. +In the examples, v1 and v2 are dense vectors. \section TopicUsingIntelMKL_Links Links diff --git a/ground/gcs/src/libs/eigen/doc/UsingNVCC.dox b/ground/gcs/src/libs/eigen/doc/UsingNVCC.dox new file mode 100644 index 0000000000..f8e755b79d --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/UsingNVCC.dox @@ -0,0 +1,32 @@ + +namespace Eigen { + +/** \page TopicCUDA Using Eigen in CUDA kernels + +\b Disclaimer: this page is about an \b experimental feature in %Eigen. + +Staring from CUDA 5.0, the CUDA compiler, \c nvcc, is able to properly parse %Eigen's code (almost). +A few adaptations of the %Eigen's code already allows to use some parts of %Eigen in your own CUDA kernels. +To this end you need the devel branch of %Eigen, CUDA 5.0 or greater with GCC. + +Known issues: + + - \c nvcc with MS Visual Studio does not work (patch welcome) + + - \c nvcc with \c clang does not work (patch welcome) + + - \c nvcc 5.5 with gcc-4.7 (or greater) has issues with the standard \c \ header file. To workaround this, you can add the following before including any other files: + \code + // workaround issue between gcc >= 4.7 and cuda 5.5 + #if (defined __GNUC__) && (__GNUC__>4 || __GNUC_MINOR__>=7) + #undef _GLIBCXX_ATOMIC_BUILTINS + #undef _GLIBCXX_USE_INT128 + #endif + \endcode + + - On 64bits system Eigen uses \c long \c int as the default type for indexes and sizes. On CUDA device, it would make sense to default to 32 bits \c int. + However, to keep host and CUDA code compatible, this cannot be done automatically by %Eigen, and the user is thus required to define \c EIGEN_DEFAULT_DENSE_INDEX_TYPE to \c int throughout his code (or only for CUDA code if there is no interaction between host and CUDA code through %Eigen's object). + +*/ + +} diff --git a/ground/gcs/src/libs/eigen/doc/eigendoxy.css b/ground/gcs/src/libs/eigen/doc/eigendoxy.css index efaeb46dc2..6274e6c704 100644 --- a/ground/gcs/src/libs/eigen/doc/eigendoxy.css +++ b/ground/gcs/src/libs/eigen/doc/eigendoxy.css @@ -45,7 +45,7 @@ pre.fragment { /* Common style for all Eigen's tables */ -table.example, table.manual, table.manual-vl { +table.example, table.manual, table.manual-vl, table.manual-hl { max-width:100%; border-collapse: collapse; border-style: solid; @@ -58,7 +58,7 @@ table.example, table.manual, table.manual-vl { -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); } -table.example th, table.manual th, table.manual-vl th { +table.example th, table.manual th, table.manual-vl th, table.manual-hl th { padding: 0.5em 0.5em 0.5em 0.5em; text-align: left; padding-right: 1em; @@ -70,7 +70,7 @@ table.example th, table.manual th, table.manual-vl th { filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#FFFFFF', endColorstr='#F4F4E5'); } -table.example td, table.manual td, table.manual-vl td { +table.example td, table.manual td, table.manual-vl td, table.manual-hl td { vertical-align:top; border-width: 1px; border-color: #cccccc; @@ -108,15 +108,15 @@ table.example td { /* standard class for the manual */ -table.manual, table.manual-vl { +table.manual, table.manual-vl, table.manual-hl { padding: 0.2em 0em 0.5em 0em; } -table.manual th, table.manual-vl th { +table.manual th, table.manual-vl th, table.manual-hl th { margin: 0em 0em 0.3em 0em; } -table.manual td, table.manual-vl td { +table.manual td, table.manual-vl td, table.manual-hl td { padding: 0.3em 0.5em 0.3em 0.5em; vertical-align:top; border-width: 1px; @@ -136,6 +136,16 @@ table.manual-vl th.inter { border-style: solid solid solid solid; } +table.manual-hl td { + border-color: #cccccc; + border-width: 1px; + border-style: solid none solid none; +} + +table td.code { + font-family: monospace; +} + h2 { margin-top:2em; border-style: none none solid none; @@ -166,6 +176,11 @@ div.toc ul { margin: 0.2em 0 0.4em 0.5em; } +span.cpp11,span.cpp14,span.cpp17 { + color: #119911; + font-weight: bold; +} + /**** old Eigen's styles ****/ @@ -177,8 +192,8 @@ table.tutorial_code td { /* Whenever doxygen meets a '\n' or a '
', it will put - * the text containing the characted into a

. - * This little hack togehter with table.tutorial_code td.note + * the text containing the character into a

. + * This little hack together with table.tutorial_code td.note * aims at fixing this issue. */ table.tutorial_code td.note p.starttd { margin: 0px; @@ -199,13 +214,3 @@ h3.version { td.width20em p.endtd { width: 20em; } - -.bigwarning { - font-size:2em; - font-weight:bold; - margin:1em; - padding:1em; - color:red; - border:solid; -} - diff --git a/ground/gcs/src/libs/eigen/doc/examples/CMakeLists.txt b/ground/gcs/src/libs/eigen/doc/examples/CMakeLists.txt index 08cf8efd7c..f7a19055fc 100644 --- a/ground/gcs/src/libs/eigen/doc/examples/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/doc/examples/CMakeLists.txt @@ -14,3 +14,8 @@ foreach(example_src ${examples_SRCS}) ) add_dependencies(all_examples ${example}) endforeach(example_src) + +check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11) +if(EIGEN_COMPILER_SUPPORT_CPP11) +ei_add_target_property(nullary_indexing COMPILE_FLAGS "-std=c++11") +endif() \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/examples/CustomizingEigen_Inheritance.cpp b/ground/gcs/src/libs/eigen/doc/examples/CustomizingEigen_Inheritance.cpp new file mode 100644 index 0000000000..48df64ee36 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/CustomizingEigen_Inheritance.cpp @@ -0,0 +1,30 @@ +#include +#include + +class MyVectorType : public Eigen::VectorXd +{ +public: + MyVectorType(void):Eigen::VectorXd() {} + + // This constructor allows you to construct MyVectorType from Eigen expressions + template + MyVectorType(const Eigen::MatrixBase& other) + : Eigen::VectorXd(other) + { } + + // This method allows you to assign Eigen expressions to MyVectorType + template + MyVectorType& operator=(const Eigen::MatrixBase & other) + { + this->Eigen::VectorXd::operator=(other); + return *this; + } +}; + +int main() +{ + MyVectorType v = MyVectorType::Ones(4); + v(2) += 10; + v = 2 * v; + std::cout << v.transpose() << std::endl; +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/Cwise_erf.cpp b/ground/gcs/src/libs/eigen/doc/examples/Cwise_erf.cpp new file mode 100644 index 0000000000..e7cd2c1c0f --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/Cwise_erf.cpp @@ -0,0 +1,9 @@ +#include +#include +#include +using namespace Eigen; +int main() +{ + Array4d v(-0.5,2,0,-7); + std::cout << v.erf() << std::endl; +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/Cwise_erfc.cpp b/ground/gcs/src/libs/eigen/doc/examples/Cwise_erfc.cpp new file mode 100644 index 0000000000..d8bb04c307 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/Cwise_erfc.cpp @@ -0,0 +1,9 @@ +#include +#include +#include +using namespace Eigen; +int main() +{ + Array4d v(-0.5,2,0,-7); + std::cout << v.erfc() << std::endl; +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/Cwise_lgamma.cpp b/ground/gcs/src/libs/eigen/doc/examples/Cwise_lgamma.cpp new file mode 100644 index 0000000000..f1c4f503e1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/Cwise_lgamma.cpp @@ -0,0 +1,9 @@ +#include +#include +#include +using namespace Eigen; +int main() +{ + Array4d v(0.5,10,0,-1); + std::cout << v.lgamma() << std::endl; +} \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/examples/MatrixBase_cwise_const.cpp b/ground/gcs/src/libs/eigen/doc/examples/MatrixBase_cwise_const.cpp deleted file mode 100644 index 23700e0b6c..0000000000 --- a/ground/gcs/src/libs/eigen/doc/examples/MatrixBase_cwise_const.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#define EIGEN2_SUPPORT -#include -#include - -using namespace Eigen; -using namespace std; - -int main() -{ - Matrix3i m = Matrix3i::Random(); - cout << "Here is the matrix m:" << endl << m << endl; - Matrix3i n = Matrix3i::Random(); - cout << "And here is the matrix n:" << endl << n << endl; - cout << "The coefficient-wise product of m and n is:" << endl; - cout << m.cwise() * n << endl; - cout << "Taking the cube of the coefficients of m yields:" << endl; - cout << m.cwise().pow(3) << endl; -} diff --git a/ground/gcs/src/libs/eigen/doc/examples/TutorialInplaceLU.cpp b/ground/gcs/src/libs/eigen/doc/examples/TutorialInplaceLU.cpp new file mode 100644 index 0000000000..cb9c59b607 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/TutorialInplaceLU.cpp @@ -0,0 +1,61 @@ +#include +struct init { + init() { std::cout << "[" << "init" << "]" << std::endl; } +}; +init init_obj; +// [init] +#include +#include + +using namespace std; +using namespace Eigen; + +int main() +{ + MatrixXd A(2,2); + A << 2, -1, 1, 3; + cout << "Here is the input matrix A before decomposition:\n" << A << endl; +cout << "[init]" << endl; + +cout << "[declaration]" << endl; + PartialPivLU > lu(A); + cout << "Here is the input matrix A after decomposition:\n" << A << endl; +cout << "[declaration]" << endl; + +cout << "[matrixLU]" << endl; + cout << "Here is the matrix storing the L and U factors:\n" << lu.matrixLU() << endl; +cout << "[matrixLU]" << endl; + +cout << "[solve]" << endl; + MatrixXd A0(2,2); A0 << 2, -1, 1, 3; + VectorXd b(2); b << 1, 2; + VectorXd x = lu.solve(b); + cout << "Residual: " << (A0 * x - b).norm() << endl; +cout << "[solve]" << endl; + +cout << "[modifyA]" << endl; + A << 3, 4, -2, 1; + x = lu.solve(b); + cout << "Residual: " << (A0 * x - b).norm() << endl; +cout << "[modifyA]" << endl; + +cout << "[recompute]" << endl; + A0 = A; // save A + lu.compute(A); + x = lu.solve(b); + cout << "Residual: " << (A0 * x - b).norm() << endl; +cout << "[recompute]" << endl; + +cout << "[recompute_bis0]" << endl; + MatrixXd A1(2,2); + A1 << 5,-2,3,4; + lu.compute(A1); + cout << "Here is the input matrix A1 after decomposition:\n" << A1 << endl; +cout << "[recompute_bis0]" << endl; + +cout << "[recompute_bis1]" << endl; + x = lu.solve(b); + cout << "Residual: " << (A1 * x - b).norm() << endl; +cout << "[recompute_bis1]" << endl; + +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/TutorialLinAlgInverseDeterminant.cpp b/ground/gcs/src/libs/eigen/doc/examples/TutorialLinAlgInverseDeterminant.cpp index 43970ff053..14dde5b357 100644 --- a/ground/gcs/src/libs/eigen/doc/examples/TutorialLinAlgInverseDeterminant.cpp +++ b/ground/gcs/src/libs/eigen/doc/examples/TutorialLinAlgInverseDeterminant.cpp @@ -13,4 +13,4 @@ int main() cout << "Here is the matrix A:\n" << A << endl; cout << "The determinant of A is " << A.determinant() << endl; cout << "The inverse of A is:\n" << A.inverse() << endl; -} \ No newline at end of file +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_reductions_operatornorm.cpp b/ground/gcs/src/libs/eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_reductions_operatornorm.cpp new file mode 100644 index 0000000000..62e28fc316 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/Tutorial_ReductionsVisitorsBroadcasting_reductions_operatornorm.cpp @@ -0,0 +1,18 @@ +#include +#include + +using namespace Eigen; +using namespace std; + +int main() +{ + MatrixXf m(2,2); + m << 1,-2, + -3,4; + + cout << "1-norm(m) = " << m.cwiseAbs().colwise().sum().maxCoeff() + << " == " << m.colwise().lpNorm<1>().maxCoeff() << endl; + + cout << "infty-norm(m) = " << m.cwiseAbs().rowwise().sum().maxCoeff() + << " == " << m.rowwise().lpNorm<1>().maxCoeff() << endl; +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp new file mode 100644 index 0000000000..92e6aaa2b2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp @@ -0,0 +1,11 @@ +/* +This program is presented in several fragments in the doc page. +Every fragment is in its own file; this file simply combines them. +*/ + +#include "make_circulant.cpp.preamble" +#include "make_circulant.cpp.traits" +#include "make_circulant.cpp.expression" +#include "make_circulant.cpp.evaluator" +#include "make_circulant.cpp.entry" +#include "make_circulant.cpp.main" diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.entry b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.entry new file mode 100644 index 0000000000..f9d2eb8a95 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.entry @@ -0,0 +1,5 @@ +template +Circulant makeCirculant(const Eigen::MatrixBase& arg) +{ + return Circulant(arg.derived()); +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.evaluator b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.evaluator new file mode 100644 index 0000000000..2ba79e7836 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.evaluator @@ -0,0 +1,32 @@ +namespace Eigen { + namespace internal { + template + struct evaluator > + : evaluator_base > + { + typedef Circulant XprType; + typedef typename nested_eval::type ArgTypeNested; + typedef typename remove_all::type ArgTypeNestedCleaned; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + Flags = Eigen::ColMajor + }; + + evaluator(const XprType& xpr) + : m_argImpl(xpr.m_arg), m_rows(xpr.rows()) + { } + + CoeffReturnType coeff(Index row, Index col) const + { + Index index = row - col; + if (index < 0) index += m_rows; + return m_argImpl.coeff(index); + } + + evaluator m_argImpl; + const Index m_rows; + }; + } +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.expression b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.expression new file mode 100644 index 0000000000..380cd44504 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.expression @@ -0,0 +1,20 @@ +template +class Circulant : public Eigen::MatrixBase > +{ +public: + Circulant(const ArgType& arg) + : m_arg(arg) + { + EIGEN_STATIC_ASSERT(ArgType::ColsAtCompileTime == 1, + YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX); + } + + typedef typename Eigen::internal::ref_selector::type Nested; + + typedef Eigen::Index Index; + Index rows() const { return m_arg.rows(); } + Index cols() const { return m_arg.rows(); } + + typedef typename Eigen::internal::ref_selector::type ArgTypeNested; + ArgTypeNested m_arg; +}; diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.main b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.main new file mode 100644 index 0000000000..877f97f62a --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.main @@ -0,0 +1,8 @@ +int main() +{ + Eigen::VectorXd vec(4); + vec << 1, 2, 4, 8; + Eigen::MatrixXd mat; + mat = makeCirculant(vec); + std::cout << mat << std::endl; +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.preamble b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.preamble new file mode 100644 index 0000000000..e575cce146 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.preamble @@ -0,0 +1,4 @@ +#include +#include + +template class Circulant; diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.traits b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.traits new file mode 100644 index 0000000000..4e04535d31 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant.cpp.traits @@ -0,0 +1,19 @@ +namespace Eigen { + namespace internal { + template + struct traits > + { + typedef Eigen::Dense StorageKind; + typedef Eigen::MatrixXpr XprKind; + typedef typename ArgType::StorageIndex StorageIndex; + typedef typename ArgType::Scalar Scalar; + enum { + Flags = Eigen::ColMajor, + RowsAtCompileTime = ArgType::RowsAtCompileTime, + ColsAtCompileTime = ArgType::RowsAtCompileTime, + MaxRowsAtCompileTime = ArgType::MaxRowsAtCompileTime, + MaxColsAtCompileTime = ArgType::MaxRowsAtCompileTime + }; + }; + } +} diff --git a/ground/gcs/src/libs/eigen/doc/examples/make_circulant2.cpp b/ground/gcs/src/libs/eigen/doc/examples/make_circulant2.cpp new file mode 100644 index 0000000000..95d3dd31a1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/make_circulant2.cpp @@ -0,0 +1,52 @@ +#include +#include + +using namespace Eigen; + +// [circulant_func] +template +class circulant_functor { + const ArgType &m_vec; +public: + circulant_functor(const ArgType& arg) : m_vec(arg) {} + + const typename ArgType::Scalar& operator() (Index row, Index col) const { + Index index = row - col; + if (index < 0) index += m_vec.size(); + return m_vec(index); + } +}; +// [circulant_func] + +// [square] +template +struct circulant_helper { + typedef Matrix MatrixType; +}; +// [square] + +// [makeCirculant] +template +CwiseNullaryOp, typename circulant_helper::MatrixType> +makeCirculant(const Eigen::MatrixBase& arg) +{ + typedef typename circulant_helper::MatrixType MatrixType; + return MatrixType::NullaryExpr(arg.size(), arg.size(), circulant_functor(arg.derived())); +} +// [makeCirculant] + +// [main] +int main() +{ + Eigen::VectorXd vec(4); + vec << 1, 2, 4, 8; + Eigen::MatrixXd mat; + mat = makeCirculant(vec); + std::cout << mat << std::endl; +} +// [main] diff --git a/ground/gcs/src/libs/eigen/doc/examples/matrixfree_cg.cpp b/ground/gcs/src/libs/eigen/doc/examples/matrixfree_cg.cpp index f0631c3a31..6a205aea3e 100644 --- a/ground/gcs/src/libs/eigen/doc/examples/matrixfree_cg.cpp +++ b/ground/gcs/src/libs/eigen/doc/examples/matrixfree_cg.cpp @@ -2,179 +2,127 @@ #include #include #include +#include class MatrixReplacement; -template class MatrixReplacement_ProductReturnType; +using Eigen::SparseMatrix; namespace Eigen { namespace internal { + // MatrixReplacement looks-like a SparseMatrix, so let's inherits its traits: template<> - struct traits : Eigen::internal::traits > + struct traits : public Eigen::internal::traits > {}; - - template - struct traits > { - // The equivalent plain objet type of the product. This type is used if the product needs to be evaluated into a temporary. - typedef Eigen::Matrix ReturnType; - }; } } -// Inheriting EigenBase should not be needed in the future. +// Example of a matrix-free wrapper from a user type to Eigen's compatible type +// For the sake of simplicity, this example simply wrap a Eigen::SparseMatrix. class MatrixReplacement : public Eigen::EigenBase { public: - // Expose some compile-time information to Eigen: + // Required typedefs, constants, and method: typedef double Scalar; typedef double RealScalar; + typedef int StorageIndex; enum { ColsAtCompileTime = Eigen::Dynamic, - RowsAtCompileTime = Eigen::Dynamic, MaxColsAtCompileTime = Eigen::Dynamic, - MaxRowsAtCompileTime = Eigen::Dynamic + IsRowMajor = false }; - Index rows() const { return 4; } - Index cols() const { return 4; } + Index rows() const { return mp_mat->rows(); } + Index cols() const { return mp_mat->cols(); } - void resize(Index a_rows, Index a_cols) - { - // This method should not be needed in the future. - assert(a_rows==0 && a_cols==0 || a_rows==rows() && a_cols==cols()); - } - - // In the future, the return type should be Eigen::Product template - MatrixReplacement_ProductReturnType operator*(const Eigen::MatrixBase& x) const { - return MatrixReplacement_ProductReturnType(*this, x.derived()); + Eigen::Product operator*(const Eigen::MatrixBase& x) const { + return Eigen::Product(*this, x.derived()); } -}; + // Custom API: + MatrixReplacement() : mp_mat(0) {} -// The proxy class representing the product of a MatrixReplacement with a MatrixBase<> -template -class MatrixReplacement_ProductReturnType : public Eigen::ReturnByValue > { -public: - typedef MatrixReplacement::Index Index; - - // The ctor store references to the matrix and right-hand-side object (usually a vector). - MatrixReplacement_ProductReturnType(const MatrixReplacement& matrix, const Rhs& rhs) - : m_matrix(matrix), m_rhs(rhs) - {} - - Index rows() const { return m_matrix.rows(); } - Index cols() const { return m_rhs.cols(); } - - // This function is automatically called by Eigen. It must evaluate the product of matrix * rhs into y. - template - void evalTo(Dest& y) const - { - y.setZero(4); - - y(0) += 2 * m_rhs(0); y(1) += 1 * m_rhs(0); - y(0) += 1 * m_rhs(1); y(1) += 2 * m_rhs(1); y(2) += 1 * m_rhs(1); - y(1) += 1 * m_rhs(2); y(2) += 2 * m_rhs(2); y(3) += 1 * m_rhs(2); - y(2) += 1 * m_rhs(3); y(3) += 2 * m_rhs(3); + void attachMyMatrix(const SparseMatrix &mat) { + mp_mat = &mat; } + const SparseMatrix my_matrix() const { return *mp_mat; } -protected: - const MatrixReplacement& m_matrix; - typename Rhs::Nested m_rhs; +private: + const SparseMatrix *mp_mat; }; -/*****/ - -// This class simply warp a diagonal matrix as a Jacobi preconditioner. -// In the future such simple and generic wrapper should be shipped within Eigen itsel. -template -class MyJacobiPreconditioner -{ - typedef _Scalar Scalar; - typedef Eigen::Matrix Vector; - typedef typename Vector::Index Index; - - public: - // this typedef is only to export the scalar type and compile-time dimensions to solve_retval - typedef Eigen::Matrix MatrixType; - - MyJacobiPreconditioner() : m_isInitialized(false) {} - - void setInvDiag(const Eigen::VectorXd &invdiag) { - m_invdiag=invdiag; - m_isInitialized=true; - } - - Index rows() const { return m_invdiag.size(); } - Index cols() const { return m_invdiag.size(); } - - template - MyJacobiPreconditioner& analyzePattern(const MatType& ) { return *this; } - - template - MyJacobiPreconditioner& factorize(const MatType& mat) { return *this; } - - template - MyJacobiPreconditioner& compute(const MatType& mat) { return *this; } - - template - void _solve(const Rhs& b, Dest& x) const - { - x = m_invdiag.array() * b.array() ; - } - - template inline const Eigen::internal::solve_retval - solve(const Eigen::MatrixBase& b) const - { - eigen_assert(m_isInitialized && "MyJacobiPreconditioner is not initialized."); - eigen_assert(m_invdiag.size()==b.rows() - && "MyJacobiPreconditioner::solve(): invalid number of rows of the right hand side matrix b"); - return Eigen::internal::solve_retval(*this, b.derived()); - } - - protected: - Vector m_invdiag; - bool m_isInitialized; -}; - +// Implementation of MatrixReplacement * Eigen::DenseVector though a specialization of internal::generic_product_impl: namespace Eigen { namespace internal { -template -struct solve_retval, Rhs> - : solve_retval_base, Rhs> -{ - typedef MyJacobiPreconditioner<_MatrixType> Dec; - EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs) - - template void evalTo(Dest& dst) const + template + struct generic_product_impl // GEMV stands for matrix-vector + : generic_product_impl_base > { - dec()._solve(rhs(),dst); - } -}; + typedef typename Product::Scalar Scalar; + + template + static void scaleAndAddTo(Dest& dst, const MatrixReplacement& lhs, const Rhs& rhs, const Scalar& alpha) + { + // This method should implement "dst += alpha * lhs * rhs" inplace, + // however, for iterative solvers, alpha is always equal to 1, so let's not bother about it. + assert(alpha==Scalar(1) && "scaling is not implemented"); + + // Here we could simply call dst.noalias() += lhs.my_matrix() * rhs, + // but let's do something fancier (and less efficient): + for(Index i=0; i S = Eigen::MatrixXd::Random(n,n).sparseView(0.5,1); + S = S.transpose()*S; + MatrixReplacement A; - Eigen::VectorXd b(4), x; - b << 1, 1, 1, 1; + A.attachMyMatrix(S); - // solve Ax = b using CG with matrix-free version: - Eigen::ConjugateGradient < MatrixReplacement, Eigen::Lower|Eigen::Upper, MyJacobiPreconditioner > cg; + Eigen::VectorXd b(n), x; + b.setRandom(); - Eigen::VectorXd invdiag(4); - invdiag << 1./3., 1./4., 1./4., 1./3.; + // Solve Ax = b using various iterative solver with matrix-free version: + { + Eigen::ConjugateGradient cg; + cg.compute(A); + x = cg.solve(b); + std::cout << "CG: #iterations: " << cg.iterations() << ", estimated error: " << cg.error() << std::endl; + } + + { + Eigen::BiCGSTAB bicg; + bicg.compute(A); + x = bicg.solve(b); + std::cout << "BiCGSTAB: #iterations: " << bicg.iterations() << ", estimated error: " << bicg.error() << std::endl; + } + + { + Eigen::GMRES gmres; + gmres.compute(A); + x = gmres.solve(b); + std::cout << "GMRES: #iterations: " << gmres.iterations() << ", estimated error: " << gmres.error() << std::endl; + } - cg.preconditioner().setInvDiag(invdiag); - cg.compute(A); - x = cg.solve(b); + { + Eigen::DGMRES gmres; + gmres.compute(A); + x = gmres.solve(b); + std::cout << "DGMRES: #iterations: " << gmres.iterations() << ", estimated error: " << gmres.error() << std::endl; + } - std::cout << "#iterations: " << cg.iterations() << std::endl; - std::cout << "estimated error: " << cg.error() << std::endl; + { + Eigen::MINRES minres; + minres.compute(A); + x = minres.solve(b); + std::cout << "MINRES: #iterations: " << minres.iterations() << ", estimated error: " << minres.error() << std::endl; + } } diff --git a/ground/gcs/src/libs/eigen/doc/examples/nullary_indexing.cpp b/ground/gcs/src/libs/eigen/doc/examples/nullary_indexing.cpp new file mode 100644 index 0000000000..e27c3585ab --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/examples/nullary_indexing.cpp @@ -0,0 +1,66 @@ +#include +#include + +using namespace Eigen; + +// [functor] +template +class indexing_functor { + const ArgType &m_arg; + const RowIndexType &m_rowIndices; + const ColIndexType &m_colIndices; +public: + typedef Matrix MatrixType; + + indexing_functor(const ArgType& arg, const RowIndexType& row_indices, const ColIndexType& col_indices) + : m_arg(arg), m_rowIndices(row_indices), m_colIndices(col_indices) + {} + + const typename ArgType::Scalar& operator() (Index row, Index col) const { + return m_arg(m_rowIndices[row], m_colIndices[col]); + } +}; +// [functor] + +// [function] +template +CwiseNullaryOp, typename indexing_functor::MatrixType> +indexing(const Eigen::MatrixBase& arg, const RowIndexType& row_indices, const ColIndexType& col_indices) +{ + typedef indexing_functor Func; + typedef typename Func::MatrixType MatrixType; + return MatrixType::NullaryExpr(row_indices.size(), col_indices.size(), Func(arg.derived(), row_indices, col_indices)); +} +// [function] + + +int main() +{ + std::cout << "[main1]\n"; + Eigen::MatrixXi A = Eigen::MatrixXi::Random(4,4); + Array3i ri(1,2,1); + ArrayXi ci(6); ci << 3,2,1,0,0,2; + Eigen::MatrixXi B = indexing(A, ri, ci); + std::cout << "A =" << std::endl; + std::cout << A << std::endl << std::endl; + std::cout << "A([" << ri.transpose() << "], [" << ci.transpose() << "]) =" << std::endl; + std::cout << B << std::endl; + std::cout << "[main1]\n"; + + std::cout << "[main2]\n"; + B = indexing(A, ri+1, ci); + std::cout << "A(ri+1,ci) =" << std::endl; + std::cout << B << std::endl << std::endl; +#if __cplusplus >= 201103L + B = indexing(A, ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3)); + std::cout << "A(ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3)) =" << std::endl; + std::cout << B << std::endl << std::endl; +#endif + std::cout << "[main2]\n"; +} + diff --git a/ground/gcs/src/libs/eigen/doc/ftv2node.png b/ground/gcs/src/libs/eigen/doc/ftv2node.png new file mode 100644 index 0000000000..63c605bb4c Binary files /dev/null and b/ground/gcs/src/libs/eigen/doc/ftv2node.png differ diff --git a/ground/gcs/src/libs/eigen/doc/ftv2pnode.png b/ground/gcs/src/libs/eigen/doc/ftv2pnode.png new file mode 100644 index 0000000000..c6ee22f937 Binary files /dev/null and b/ground/gcs/src/libs/eigen/doc/ftv2pnode.png differ diff --git a/ground/gcs/src/libs/eigen/doc/snippets/BiCGSTAB_simple.cpp b/ground/gcs/src/libs/eigen/doc/snippets/BiCGSTAB_simple.cpp new file mode 100644 index 0000000000..5520f4f1f0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/BiCGSTAB_simple.cpp @@ -0,0 +1,11 @@ + int n = 10000; + VectorXd x(n), b(n); + SparseMatrix A(n,n); + /* ... fill A and b ... */ + BiCGSTAB > solver; + solver.compute(A); + x = solver.solve(b); + std::cout << "#iterations: " << solver.iterations() << std::endl; + std::cout << "estimated error: " << solver.error() << std::endl; + /* ... update b ... */ + x = solver.solve(b); // solve again \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/BiCGSTAB_step_by_step.cpp b/ground/gcs/src/libs/eigen/doc/snippets/BiCGSTAB_step_by_step.cpp new file mode 100644 index 0000000000..06147bb81e --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/BiCGSTAB_step_by_step.cpp @@ -0,0 +1,14 @@ + int n = 10000; + VectorXd x(n), b(n); + SparseMatrix A(n,n); + /* ... fill A and b ... */ + BiCGSTAB > solver(A); + // start from a random solution + x = VectorXd::Random(n); + solver.setMaxIterations(1); + int i = 0; + do { + x = solver.solveWithGuess(b,x); + std::cout << i << " : " << solver.error() << std::endl; + ++i; + } while (solver.info()!=Success && i<100); \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/CMakeLists.txt b/ground/gcs/src/libs/eigen/doc/snippets/CMakeLists.txt index 1135900cf4..1baf32fbac 100644 --- a/ground/gcs/src/libs/eigen/doc/snippets/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/doc/snippets/CMakeLists.txt @@ -24,5 +24,3 @@ foreach(snippet_src ${snippets_SRCS}) set_source_files_properties(${CMAKE_CURRENT_BINARY_DIR}/${compile_snippet_src} PROPERTIES OBJECT_DEPENDS ${snippet_src}) endforeach(snippet_src) - -ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_arg.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_arg.cpp new file mode 100644 index 0000000000..3f45133b62 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_arg.cpp @@ -0,0 +1,3 @@ +ArrayXcf v = ArrayXcf::Random(3); +cout << v << endl << endl; +cout << arg(v) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_array_power_array.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_array_power_array.cpp new file mode 100644 index 0000000000..432a76ee59 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_array_power_array.cpp @@ -0,0 +1,4 @@ +Array x(8,25,3), + e(1./3.,0.5,2.); +cout << "[" << x << "]^[" << e << "] = " << x.pow(e) << endl; // using ArrayBase::pow +cout << "[" << x << "]^[" << e << "] = " << pow(x,e) << endl; // using Eigen::pow diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_atan.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_atan.cpp new file mode 100644 index 0000000000..4468447266 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_atan.cpp @@ -0,0 +1,2 @@ +ArrayXd v = ArrayXd::LinSpaced(5,0,1); +cout << v.atan() << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_boolean_not.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_boolean_not.cpp new file mode 100644 index 0000000000..40009f15ae --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_boolean_not.cpp @@ -0,0 +1,5 @@ +Array3d v(1,2,3); +v(1) *= 0.0/0.0; +v(2) /= 0.0; +cout << v << endl << endl; +cout << !isfinite(v) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_boolean_xor.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_boolean_xor.cpp new file mode 100644 index 0000000000..fafbec8064 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_boolean_xor.cpp @@ -0,0 +1,2 @@ +Array3d v(-1,2,1), w(-3,2,3); +cout << ((v e(2,-3,1./3.); +cout << "10^[" << e << "] = " << pow(10,e) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_sign.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_sign.cpp new file mode 100644 index 0000000000..49920e4f12 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_sign.cpp @@ -0,0 +1,2 @@ +Array3d v(-3,5,0); +cout << v.sign() << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_sinh.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_sinh.cpp new file mode 100644 index 0000000000..fac9b19a85 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_sinh.cpp @@ -0,0 +1,2 @@ +ArrayXd v = ArrayXd::LinSpaced(5,0,1); +cout << sinh(v) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Cwise_tanh.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_tanh.cpp new file mode 100644 index 0000000000..30cd0450d9 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Cwise_tanh.cpp @@ -0,0 +1,2 @@ +ArrayXd v = ArrayXd::LinSpaced(5,0,1); +cout << tanh(v) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/DenseBase_LinSpacedInt.cpp b/ground/gcs/src/libs/eigen/doc/snippets/DenseBase_LinSpacedInt.cpp new file mode 100644 index 0000000000..0d7ae068e4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/DenseBase_LinSpacedInt.cpp @@ -0,0 +1,8 @@ +cout << "Even spacing inputs:" << endl; +cout << VectorXi::LinSpaced(8,1,4).transpose() << endl; +cout << VectorXi::LinSpaced(8,1,8).transpose() << endl; +cout << VectorXi::LinSpaced(8,1,15).transpose() << endl; +cout << "Uneven spacing inputs:" << endl; +cout << VectorXi::LinSpaced(8,1,7).transpose() << endl; +cout << VectorXi::LinSpaced(8,1,9).transpose() << endl; +cout << VectorXi::LinSpaced(8,1,16).transpose() << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/DirectionWise_hnormalized.cpp b/ground/gcs/src/libs/eigen/doc/snippets/DirectionWise_hnormalized.cpp new file mode 100644 index 0000000000..3410790a87 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/DirectionWise_hnormalized.cpp @@ -0,0 +1,7 @@ +typedef Matrix Matrix4Xd; +Matrix4Xd M = Matrix4Xd::Random(4,5); +Projective3d P(Matrix4d::Random()); +cout << "The matrix M is:" << endl << M << endl << endl; +cout << "M.colwise().hnormalized():" << endl << M.colwise().hnormalized() << endl << endl; +cout << "P*M:" << endl << P*M << endl << endl; +cout << "(P*M).colwise().hnormalized():" << endl << (P*M).colwise().hnormalized() << endl << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/EigenSolver_eigenvectors.cpp b/ground/gcs/src/libs/eigen/doc/snippets/EigenSolver_eigenvectors.cpp index 0fad4dadb4..8355f76c94 100644 --- a/ground/gcs/src/libs/eigen/doc/snippets/EigenSolver_eigenvectors.cpp +++ b/ground/gcs/src/libs/eigen/doc/snippets/EigenSolver_eigenvectors.cpp @@ -1,4 +1,4 @@ MatrixXd ones = MatrixXd::Ones(3,3); EigenSolver es(ones); -cout << "The first eigenvector of the 3x3 matrix of ones is:" - << endl << es.eigenvectors().col(1) << endl; +cout << "The first eigenvector of the 3x3 matrix of ones is:" + << endl << es.eigenvectors().col(0) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/LeastSquaresNormalEquations.cpp b/ground/gcs/src/libs/eigen/doc/snippets/LeastSquaresNormalEquations.cpp new file mode 100644 index 0000000000..997cf1715b --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/LeastSquaresNormalEquations.cpp @@ -0,0 +1,4 @@ +MatrixXf A = MatrixXf::Random(3, 2); +VectorXf b = VectorXf::Random(3); +cout << "The solution using normal equations is:\n" + << (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/LeastSquaresQR.cpp b/ground/gcs/src/libs/eigen/doc/snippets/LeastSquaresQR.cpp new file mode 100644 index 0000000000..6c97045479 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/LeastSquaresQR.cpp @@ -0,0 +1,4 @@ +MatrixXf A = MatrixXf::Random(3, 2); +VectorXf b = VectorXf::Random(3); +cout << "The solution using the QR decomposition is:\n" + << A.colPivHouseholderQr().solve(b) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_cwiseSign.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_cwiseSign.cpp new file mode 100644 index 0000000000..efd717955f --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_cwiseSign.cpp @@ -0,0 +1,4 @@ +MatrixXd m(2,3); +m << 2, -4, 6, + -5, 1, 0; +cout << m.cwiseSign() << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_extract.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_extract.cpp deleted file mode 100644 index c96220f72c..0000000000 --- a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_extract.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef _MSC_VER - #warning deprecated -#endif -/* deprecated -Matrix3i m = Matrix3i::Random(); -cout << "Here is the matrix m:" << endl << m << endl; -cout << "Here is the upper-triangular matrix extracted from m:" << endl - << m.part() << endl; -cout << "Here is the strictly-upper-triangular matrix extracted from m:" << endl - << m.part() << endl; -cout << "Here is the unit-lower-triangular matrix extracted from m:" << endl - << m.part() << endl; -*/ \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_hnormalized.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_hnormalized.cpp new file mode 100644 index 0000000000..652cd77c09 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_hnormalized.cpp @@ -0,0 +1,6 @@ +Vector4d v = Vector4d::Random(); +Projective3d P(Matrix4d::Random()); +cout << "v = " << v.transpose() << "]^T" << endl; +cout << "v.hnormalized() = " << v.hnormalized().transpose() << "]^T" << endl; +cout << "P*v = " << (P*v).transpose() << "]^T" << endl; +cout << "(P*v).hnormalized() = " << (P*v).hnormalized().transpose() << "]^T" << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_homogeneous.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_homogeneous.cpp new file mode 100644 index 0000000000..457c28f91a --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_homogeneous.cpp @@ -0,0 +1,6 @@ +Vector3d v = Vector3d::Random(), w; +Projective3d P(Matrix4d::Random()); +cout << "v = [" << v.transpose() << "]^T" << endl; +cout << "h.homogeneous() = [" << v.homogeneous().transpose() << "]^T" << endl; +cout << "(P * v.homogeneous()) = [" << (P * v.homogeneous()).transpose() << "]^T" << endl; +cout << "(P * v.homogeneous()).hnormalized() = [" << (P * v.homogeneous()).eval().hnormalized().transpose() << "]^T" << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_marked.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_marked.cpp deleted file mode 100644 index f60712178a..0000000000 --- a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_marked.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef _MSC_VER - #warning deprecated -#endif -/* -Matrix3d m = Matrix3d::Zero(); -m.part().setOnes(); -cout << "Here is the matrix m:" << endl << m << endl; -Matrix3d n = Matrix3d::Ones(); -n.part() *= 2; -cout << "Here is the matrix n:" << endl << n << endl; -cout << "And now here is m.inverse()*n, taking advantage of the fact that" - " m is upper-triangular:" << endl - << m.marked().solveTriangular(n); -*/ \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_part.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_part.cpp deleted file mode 100644 index d3e7f482e7..0000000000 --- a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_part.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef _MSC_VER - #warning deprecated -#endif -/* -Matrix3d m = Matrix3d::Zero(); -m.part().setOnes(); -cout << "Here is the matrix m:" << endl << m << endl; -cout << "And let us now compute m*m.adjoint() in a very optimized way" << endl - << "taking advantage of the symmetry." << endl; -Matrix3d n; -n.part() = (m*m.adjoint()).lazy(); -cout << "The result is:" << endl << n << endl; -*/ \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_selfadjointView.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_selfadjointView.cpp new file mode 100644 index 0000000000..4bd3c7eeb2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_selfadjointView.cpp @@ -0,0 +1,6 @@ +Matrix3i m = Matrix3i::Random(); +cout << "Here is the matrix m:" << endl << m << endl; +cout << "Here is the symmetric matrix extracted from the upper part of m:" << endl + << Matrix3i(m.selfadjointView()) << endl; +cout << "Here is the symmetric matrix extracted from the lower part of m:" << endl + << Matrix3i(m.selfadjointView()) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_triangularView.cpp b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_triangularView.cpp new file mode 100644 index 0000000000..03aa303f0d --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/MatrixBase_triangularView.cpp @@ -0,0 +1,9 @@ +Matrix3i m = Matrix3i::Random(); +cout << "Here is the matrix m:" << endl << m << endl; +cout << "Here is the upper-triangular matrix extracted from m:" << endl + << Matrix3i(m.triangularView()) << endl; +cout << "Here is the strictly-upper-triangular matrix extracted from m:" << endl + << Matrix3i(m.triangularView()) << endl; +cout << "Here is the unit-lower-triangular matrix extracted from m:" << endl + << Matrix3i(m.triangularView()) << endl; +// FIXME need to implement output for triangularViews (Bug 885) diff --git a/ground/gcs/src/libs/eigen/doc/snippets/PartialRedux_count.cpp b/ground/gcs/src/libs/eigen/doc/snippets/PartialRedux_count.cpp index c7b3097e4d..1c3b3a28f9 100644 --- a/ground/gcs/src/libs/eigen/doc/snippets/PartialRedux_count.cpp +++ b/ground/gcs/src/libs/eigen/doc/snippets/PartialRedux_count.cpp @@ -1,3 +1,5 @@ Matrix3d m = Matrix3d::Random(); cout << "Here is the matrix m:" << endl << m << endl; -cout << "Here is the count of elements larger or equal than 0.5 of each row:" << endl << (m.array() >= 0.5).rowwise().count() << endl; +Matrix res = (m.array() >= 0.5).rowwise().count(); +cout << "Here is the count of elements larger or equal than 0.5 of each row:" << endl; +cout << res << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/SparseMatrix_coeffs.cpp b/ground/gcs/src/libs/eigen/doc/snippets/SparseMatrix_coeffs.cpp new file mode 100644 index 0000000000..f71a69b078 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/SparseMatrix_coeffs.cpp @@ -0,0 +1,9 @@ +SparseMatrix A(3,3); +A.insert(1,2) = 0; +A.insert(0,1) = 1; +A.insert(2,0) = 2; +A.makeCompressed(); +cout << "The matrix A is:" << endl << MatrixXd(A) << endl; +cout << "it has " << A.nonZeros() << " stored non zero coefficients that are: " << A.coeffs().transpose() << endl; +A.coeffs() += 10; +cout << "After adding 10 to every stored non zero coefficient, the matrix A is:" << endl << MatrixXd(A) << endl; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/TopicAliasing_mult4.cpp b/ground/gcs/src/libs/eigen/doc/snippets/TopicAliasing_mult4.cpp new file mode 100644 index 0000000000..8a8992f6ca --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/TopicAliasing_mult4.cpp @@ -0,0 +1,5 @@ +MatrixXf A(2,2), B(3,2); +B << 2, 0, 0, 3, 1, 1; +A << 2, 0, 0, -2; +A = (B * A).cwiseAbs(); +cout << A; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/TopicAliasing_mult5.cpp b/ground/gcs/src/libs/eigen/doc/snippets/TopicAliasing_mult5.cpp new file mode 100644 index 0000000000..1a36defde3 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/TopicAliasing_mult5.cpp @@ -0,0 +1,5 @@ +MatrixXf A(2,2), B(3,2); +B << 2, 0, 0, 3, 1, 1; +A << 2, 0, 0, -2; +A = (B * A).eval().cwiseAbs(); +cout << A; diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Triangular_solve.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Triangular_solve.cpp new file mode 100644 index 0000000000..5484424673 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Triangular_solve.cpp @@ -0,0 +1,11 @@ +Matrix3d m = Matrix3d::Zero(); +m.triangularView().setOnes(); +cout << "Here is the matrix m:\n" << m << endl; +Matrix3d n = Matrix3d::Ones(); +n.triangularView() *= 2; +cout << "Here is the matrix n:\n" << n << endl; +cout << "And now here is m.inverse()*n, taking advantage of the fact that" + " m is upper-triangular:\n" + << m.triangularView().solve(n) << endl; +cout << "And this is n*m.inverse():\n" + << m.triangularView().solve(n); diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_AdvancedInitialization_Join.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_AdvancedInitialization_Join.cpp index 84e8715cbe..55a21539d6 100644 --- a/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_AdvancedInitialization_Join.cpp +++ b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_AdvancedInitialization_Join.cpp @@ -3,7 +3,7 @@ vec1 << 1, 2, 3; std::cout << "vec1 = " << vec1 << std::endl; RowVectorXd vec2(4); -vec2 << 1, 4, 9, 16;; +vec2 << 1, 4, 9, 16; std::cout << "vec2 = " << vec2 << std::endl; RowVectorXd joined(7); diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_ReshapeMat2Mat.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_ReshapeMat2Mat.cpp new file mode 100644 index 0000000000..f84d6e76d0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_ReshapeMat2Mat.cpp @@ -0,0 +1,6 @@ +MatrixXf M1(2,6); // Column-major storage +M1 << 1, 2, 3, 4, 5, 6, + 7, 8, 9, 10, 11, 12; + +Map M2(M1.data(), 6,2); +cout << "M2:" << endl << M2 << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_ReshapeMat2Vec.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_ReshapeMat2Vec.cpp new file mode 100644 index 0000000000..95bd4e0e6b --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_ReshapeMat2Vec.cpp @@ -0,0 +1,11 @@ +MatrixXf M1(3,3); // Column-major storage +M1 << 1, 2, 3, + 4, 5, 6, + 7, 8, 9; + +Map v1(M1.data(), M1.size()); +cout << "v1:" << endl << v1 << endl; + +Matrix M2(M1); +Map v2(M2.data(), M2.size()); +cout << "v2:" << endl << v2 << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_SlicingCol.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_SlicingCol.cpp new file mode 100644 index 0000000000..f667ff6894 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_SlicingCol.cpp @@ -0,0 +1,11 @@ +MatrixXf M1 = MatrixXf::Random(3,8); +cout << "Column major input:" << endl << M1 << "\n"; +Map > M2(M1.data(), M1.rows(), (M1.cols()+2)/3, OuterStride<>(M1.outerStride()*3)); +cout << "1 column over 3:" << endl << M2 << "\n"; + +typedef Matrix RowMajorMatrixXf; +RowMajorMatrixXf M3(M1); +cout << "Row major input:" << endl << M3 << "\n"; +Map > M4(M3.data(), M3.rows(), (M3.cols()+2)/3, + Stride(M3.outerStride(),3)); +cout << "1 column over 3:" << endl << M4 << "\n"; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_SlicingVec.cpp b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_SlicingVec.cpp new file mode 100644 index 0000000000..07e10bf695 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/Tutorial_SlicingVec.cpp @@ -0,0 +1,4 @@ +RowVectorXf v = RowVectorXf::LinSpaced(20,0,19); +cout << "Input:" << endl << v << endl; +Map > v2(v.data(), v.size()/2); +cout << "Even:" << v2 << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/VectorwiseOp_homogeneous.cpp b/ground/gcs/src/libs/eigen/doc/snippets/VectorwiseOp_homogeneous.cpp new file mode 100644 index 0000000000..aba4fed0eb --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/snippets/VectorwiseOp_homogeneous.cpp @@ -0,0 +1,7 @@ +typedef Matrix Matrix3Xd; +Matrix3Xd M = Matrix3Xd::Random(3,5); +Projective3d P(Matrix4d::Random()); +cout << "The matrix M is:" << endl << M << endl << endl; +cout << "M.colwise().homogeneous():" << endl << M.colwise().homogeneous() << endl << endl; +cout << "P * M.colwise().homogeneous():" << endl << P * M.colwise().homogeneous() << endl << endl; +cout << "P * M.colwise().homogeneous().hnormalized(): " << endl << (P * M.colwise().homogeneous()).colwise().hnormalized() << endl << endl; \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/doc/snippets/compile_snippet.cpp.in b/ground/gcs/src/libs/eigen/doc/snippets/compile_snippet.cpp.in index 894cd526c2..d63f371a30 100644 --- a/ground/gcs/src/libs/eigen/doc/snippets/compile_snippet.cpp.in +++ b/ground/gcs/src/libs/eigen/doc/snippets/compile_snippet.cpp.in @@ -1,5 +1,13 @@ -#include +static bool eigen_did_assert = false; +#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;} + #include +#include + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + using namespace Eigen; using namespace std; diff --git a/ground/gcs/src/libs/eigen/doc/special_examples/CMakeLists.txt b/ground/gcs/src/libs/eigen/doc/special_examples/CMakeLists.txt index 3ab75dbfe9..101fbc5f9c 100644 --- a/ground/gcs/src/libs/eigen/doc/special_examples/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/doc/special_examples/CMakeLists.txt @@ -19,3 +19,17 @@ if(QT4_FOUND) add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) +check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11) +if(EIGEN_COMPILER_SUPPORT_CPP11) + add_executable(random_cpp11 random_cpp11.cpp) + target_link_libraries(random_cpp11 ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) + add_dependencies(all_examples random_cpp11) + ei_add_target_property(random_cpp11 COMPILE_FLAGS "-std=c++11") + + add_custom_command( + TARGET random_cpp11 + POST_BUILD + COMMAND random_cpp11 + ARGS >${CMAKE_CURRENT_BINARY_DIR}/random_cpp11.out + ) +endif() diff --git a/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example.cpp b/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example.cpp index 002f19f017..830e196eaa 100644 --- a/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example.cpp +++ b/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example.cpp @@ -9,6 +9,8 @@ void saveAsBitmap(const Eigen::VectorXd& x, int n, const char* filename); int main(int argc, char** argv) { + assert(argc==2); + int n = 300; // size of the image int m = n*n; // number of unknows (=number of pixels) diff --git a/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example_details.cpp b/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example_details.cpp index 7d820b44a2..bc18b01884 100644 --- a/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example_details.cpp +++ b/ground/gcs/src/libs/eigen/doc/special_examples/Tutorial_sparse_example_details.cpp @@ -8,7 +8,7 @@ typedef Eigen::Triplet T; void insertCoefficient(int id, int i, int j, double w, std::vector& coeffs, Eigen::VectorXd& b, const Eigen::VectorXd& boundary) { - int n = boundary.size(); + int n = int(boundary.size()); int id1 = i+j*n; if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coefficient diff --git a/ground/gcs/src/libs/eigen/doc/special_examples/random_cpp11.cpp b/ground/gcs/src/libs/eigen/doc/special_examples/random_cpp11.cpp new file mode 100644 index 0000000000..33744c0516 --- /dev/null +++ b/ground/gcs/src/libs/eigen/doc/special_examples/random_cpp11.cpp @@ -0,0 +1,14 @@ +#include +#include +#include + +using namespace Eigen; + +int main() { + std::default_random_engine generator; + std::poisson_distribution distribution(4.1); + auto poisson = [&] () {return distribution(generator);}; + + RowVectorXi v = RowVectorXi::NullaryExpr(10, poisson ); + std::cout << v << "\n"; +} diff --git a/ground/gcs/src/libs/eigen/failtest/CMakeLists.txt b/ground/gcs/src/libs/eigen/failtest/CMakeLists.txt index cadc6a2556..1a73f05e62 100644 --- a/ground/gcs/src/libs/eigen/failtest/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/failtest/CMakeLists.txt @@ -7,6 +7,9 @@ ei_add_failtest("block_nonconst_ctor_on_const_xpr_1") ei_add_failtest("block_nonconst_ctor_on_const_xpr_2") ei_add_failtest("transpose_nonconst_ctor_on_const_xpr") ei_add_failtest("diagonal_nonconst_ctor_on_const_xpr") +ei_add_failtest("cwiseunaryview_nonconst_ctor_on_const_xpr") +ei_add_failtest("triangularview_nonconst_ctor_on_const_xpr") +ei_add_failtest("selfadjointview_nonconst_ctor_on_const_xpr") ei_add_failtest("const_qualified_block_method_retval_0") ei_add_failtest("const_qualified_block_method_retval_1") @@ -25,6 +28,9 @@ ei_add_failtest("block_on_const_type_actually_const_0") ei_add_failtest("block_on_const_type_actually_const_1") ei_add_failtest("transpose_on_const_type_actually_const") ei_add_failtest("diagonal_on_const_type_actually_const") +ei_add_failtest("cwiseunaryview_on_const_type_actually_const") +ei_add_failtest("triangularview_on_const_type_actually_const") +ei_add_failtest("selfadjointview_on_const_type_actually_const") ei_add_failtest("ref_1") ei_add_failtest("ref_2") @@ -32,6 +38,20 @@ ei_add_failtest("ref_3") ei_add_failtest("ref_4") ei_add_failtest("ref_5") +ei_add_failtest("swap_1") +ei_add_failtest("swap_2") + +ei_add_failtest("ternary_1") +ei_add_failtest("ternary_2") + +ei_add_failtest("sparse_ref_1") +ei_add_failtest("sparse_ref_2") +ei_add_failtest("sparse_ref_3") +ei_add_failtest("sparse_ref_4") +ei_add_failtest("sparse_ref_5") + +ei_add_failtest("sparse_storage_mismatch") + ei_add_failtest("partialpivlu_int") ei_add_failtest("fullpivlu_int") ei_add_failtest("llt_int") @@ -40,6 +60,7 @@ ei_add_failtest("qr_int") ei_add_failtest("colpivqr_int") ei_add_failtest("fullpivqr_int") ei_add_failtest("jacobisvd_int") +ei_add_failtest("bdcsvd_int") ei_add_failtest("eigensolver_int") ei_add_failtest("eigensolver_cplx") diff --git a/ground/gcs/src/libs/eigen/failtest/bdcsvd_int.cpp b/ground/gcs/src/libs/eigen/failtest/bdcsvd_int.cpp new file mode 100644 index 0000000000..670752cf59 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/bdcsvd_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/SVD" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + BDCSVD > qr(Matrix::Random(10,10)); +} diff --git a/ground/gcs/src/libs/eigen/failtest/cwiseunaryview_nonconst_ctor_on_const_xpr.cpp b/ground/gcs/src/libs/eigen/failtest/cwiseunaryview_nonconst_ctor_on_const_xpr.cpp new file mode 100644 index 0000000000..e23cf8fd8d --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/cwiseunaryview_nonconst_ctor_on_const_xpr.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void foo(CV_QUALIFIER Matrix3d &m){ + CwiseUnaryView,Matrix3d> t(m); +} + +int main() {} diff --git a/ground/gcs/src/libs/eigen/failtest/cwiseunaryview_on_const_type_actually_const.cpp b/ground/gcs/src/libs/eigen/failtest/cwiseunaryview_on_const_type_actually_const.cpp new file mode 100644 index 0000000000..fcd41dfdb8 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/cwiseunaryview_on_const_type_actually_const.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void foo(){ + MatrixXf m; + CwiseUnaryView,CV_QUALIFIER MatrixXf>(m).coeffRef(0, 0) = 1.0f; +} + +int main() {} diff --git a/ground/gcs/src/libs/eigen/failtest/selfadjointview_nonconst_ctor_on_const_xpr.cpp b/ground/gcs/src/libs/eigen/failtest/selfadjointview_nonconst_ctor_on_const_xpr.cpp new file mode 100644 index 0000000000..a240f81848 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/selfadjointview_nonconst_ctor_on_const_xpr.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void foo(CV_QUALIFIER Matrix3d &m){ + SelfAdjointView t(m); +} + +int main() {} diff --git a/ground/gcs/src/libs/eigen/failtest/selfadjointview_on_const_type_actually_const.cpp b/ground/gcs/src/libs/eigen/failtest/selfadjointview_on_const_type_actually_const.cpp new file mode 100644 index 0000000000..19aaad6d09 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/selfadjointview_on_const_type_actually_const.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void foo(){ + MatrixXf m; + SelfAdjointView(m).coeffRef(0, 0) = 1.0f; +} + +int main() {} diff --git a/ground/gcs/src/libs/eigen/failtest/sparse_ref_1.cpp b/ground/gcs/src/libs/eigen/failtest/sparse_ref_1.cpp new file mode 100644 index 0000000000..d78d1f9b11 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/sparse_ref_1.cpp @@ -0,0 +1,18 @@ +#include "../Eigen/Sparse" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void call_ref(Ref > a) { } + +int main() +{ + SparseMatrix a(10,10); + CV_QUALIFIER SparseMatrix& ac(a); + call_ref(ac); +} diff --git a/ground/gcs/src/libs/eigen/failtest/sparse_ref_2.cpp b/ground/gcs/src/libs/eigen/failtest/sparse_ref_2.cpp new file mode 100644 index 0000000000..46c9440c2f --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/sparse_ref_2.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Sparse" + +using namespace Eigen; + +void call_ref(Ref > a) { } + +int main() +{ + SparseMatrix A(10,10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(A.row(3)); +#else + call_ref(A.col(3)); +#endif +} diff --git a/ground/gcs/src/libs/eigen/failtest/sparse_ref_3.cpp b/ground/gcs/src/libs/eigen/failtest/sparse_ref_3.cpp new file mode 100644 index 0000000000..a9949b552c --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/sparse_ref_3.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Sparse" + +using namespace Eigen; + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +void call_ref(Ref > a) { } +#else +void call_ref(const Ref > &a) { } +#endif + +int main() +{ + SparseMatrix a(10,10); + call_ref(a+a); +} diff --git a/ground/gcs/src/libs/eigen/failtest/sparse_ref_4.cpp b/ground/gcs/src/libs/eigen/failtest/sparse_ref_4.cpp new file mode 100644 index 0000000000..57bb6a1fcd --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/sparse_ref_4.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Sparse" + +using namespace Eigen; + +void call_ref(Ref > a) {} + +int main() +{ + SparseMatrix A(10,10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(A.transpose()); +#else + call_ref(A); +#endif +} diff --git a/ground/gcs/src/libs/eigen/failtest/sparse_ref_5.cpp b/ground/gcs/src/libs/eigen/failtest/sparse_ref_5.cpp new file mode 100644 index 0000000000..4478f6f2f1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/sparse_ref_5.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Sparse" + +using namespace Eigen; + +void call_ref(Ref > a) { } + +int main() +{ + SparseMatrix a(10,10); + SparseMatrixBase > &ac(a); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(ac); +#else + call_ref(ac.derived()); +#endif +} diff --git a/ground/gcs/src/libs/eigen/failtest/sparse_storage_mismatch.cpp b/ground/gcs/src/libs/eigen/failtest/sparse_storage_mismatch.cpp new file mode 100644 index 0000000000..51840d416a --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/sparse_storage_mismatch.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Sparse" +using namespace Eigen; + +typedef SparseMatrix Mat1; +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +typedef SparseMatrix Mat2; +#else +typedef SparseMatrix Mat2; +#endif + +int main() +{ + Mat1 a(10,10); + Mat2 b(10,10); + a += b; +} diff --git a/ground/gcs/src/libs/eigen/failtest/swap_1.cpp b/ground/gcs/src/libs/eigen/failtest/swap_1.cpp new file mode 100644 index 0000000000..106379720a --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/swap_1.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +int main() +{ + VectorXf a(10), b(10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + const DenseBase &ac(a); +#else + DenseBase &ac(a); +#endif + b.swap(ac); +} diff --git a/ground/gcs/src/libs/eigen/failtest/swap_2.cpp b/ground/gcs/src/libs/eigen/failtest/swap_2.cpp new file mode 100644 index 0000000000..c130ba6e4e --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/swap_2.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +int main() +{ + VectorXf a(10), b(10); + VectorXf const &ac(a); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + b.swap(ac); +#else + b.swap(ac.const_cast_derived()); +#endif +} \ No newline at end of file diff --git a/ground/gcs/src/libs/eigen/failtest/ternary_1.cpp b/ground/gcs/src/libs/eigen/failtest/ternary_1.cpp new file mode 100644 index 0000000000..b40bcb0cc2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/ternary_1.cpp @@ -0,0 +1,13 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +int main(int argc,char **) +{ + VectorXf a(10), b(10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + b = argc>1 ? 2*a : -a; +#else + b = argc>1 ? 2*a : VectorXf(-a); +#endif +} diff --git a/ground/gcs/src/libs/eigen/failtest/ternary_2.cpp b/ground/gcs/src/libs/eigen/failtest/ternary_2.cpp new file mode 100644 index 0000000000..a46b12b2b9 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/ternary_2.cpp @@ -0,0 +1,13 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +int main(int argc,char **) +{ + VectorXf a(10), b(10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + b = argc>1 ? 2*a : a+a; +#else + b = argc>1 ? VectorXf(2*a) : VectorXf(a+a); +#endif +} diff --git a/ground/gcs/src/libs/eigen/failtest/triangularview_nonconst_ctor_on_const_xpr.cpp b/ground/gcs/src/libs/eigen/failtest/triangularview_nonconst_ctor_on_const_xpr.cpp new file mode 100644 index 0000000000..807447e4bf --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/triangularview_nonconst_ctor_on_const_xpr.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void foo(CV_QUALIFIER Matrix3d &m){ + TriangularView t(m); +} + +int main() {} diff --git a/ground/gcs/src/libs/eigen/failtest/triangularview_on_const_type_actually_const.cpp b/ground/gcs/src/libs/eigen/failtest/triangularview_on_const_type_actually_const.cpp new file mode 100644 index 0000000000..0a381a6126 --- /dev/null +++ b/ground/gcs/src/libs/eigen/failtest/triangularview_on_const_type_actually_const.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void foo(){ + MatrixXf m; + TriangularView(m).coeffRef(0, 0) = 1.0f; +} + +int main() {} diff --git a/ground/gcs/src/libs/eigen/lapack/complex_double.cpp b/ground/gcs/src/libs/eigen/lapack/complex_double.cpp index 424d2b8ca3..c9c5752731 100644 --- a/ground/gcs/src/libs/eigen/lapack/complex_double.cpp +++ b/ground/gcs/src/libs/eigen/lapack/complex_double.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2009-2011 Gael Guennebaud +// Copyright (C) 2009-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -15,3 +15,4 @@ #include "cholesky.cpp" #include "lu.cpp" +#include "svd.cpp" diff --git a/ground/gcs/src/libs/eigen/lapack/complex_single.cpp b/ground/gcs/src/libs/eigen/lapack/complex_single.cpp index c0b2d29aeb..6d11b26cdf 100644 --- a/ground/gcs/src/libs/eigen/lapack/complex_single.cpp +++ b/ground/gcs/src/libs/eigen/lapack/complex_single.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2009-2011 Gael Guennebaud +// Copyright (C) 2009-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -15,3 +15,4 @@ #include "cholesky.cpp" #include "lu.cpp" +#include "svd.cpp" diff --git a/ground/gcs/src/libs/eigen/lapack/double.cpp b/ground/gcs/src/libs/eigen/lapack/double.cpp index d86549e193..ea78bb6621 100644 --- a/ground/gcs/src/libs/eigen/lapack/double.cpp +++ b/ground/gcs/src/libs/eigen/lapack/double.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2009-2011 Gael Guennebaud +// Copyright (C) 2009-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -15,3 +15,4 @@ #include "cholesky.cpp" #include "lu.cpp" #include "eigenvalues.cpp" +#include "svd.cpp" diff --git a/ground/gcs/src/libs/eigen/lapack/eigenvalues.cpp b/ground/gcs/src/libs/eigen/lapack/eigenvalues.cpp index a1526ebcd0..921c515697 100644 --- a/ground/gcs/src/libs/eigen/lapack/eigenvalues.cpp +++ b/ground/gcs/src/libs/eigen/lapack/eigenvalues.cpp @@ -7,10 +7,10 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. -#include "common.h" +#include "lapack_common.h" #include -// computes an LU factorization of a general M-by-N matrix A using partial pivoting with row interchanges +// computes eigen values and vectors of a general N-by-N matrix A EIGEN_LAPACK_FUNC(syev,(char *jobz, char *uplo, int* n, Scalar* a, int *lda, Scalar* w, Scalar* /*work*/, int* lwork, int *info)) { // TODO exploit the work buffer @@ -22,24 +22,7 @@ EIGEN_LAPACK_FUNC(syev,(char *jobz, char *uplo, int* n, Scalar* a, int *lda, Sca else if(*n<0) *info = -3; else if(*lda +// Copyright (C) 2010-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -11,6 +11,7 @@ #define EIGEN_LAPACK_COMMON_H #include "../blas/common.h" +#include "../Eigen/src/misc/lapack.h" #define EIGEN_LAPACK_FUNC(FUNC,ARGLIST) \ extern "C" { int EIGEN_BLAS_FUNC(FUNC) ARGLIST; } \ @@ -18,6 +19,11 @@ typedef Eigen::Map > PivotsType; +#if ISCOMPLEX +#define EIGEN_LAPACK_ARG_IF_COMPLEX(X) X, +#else +#define EIGEN_LAPACK_ARG_IF_COMPLEX(X) +#endif #endif // EIGEN_LAPACK_COMMON_H diff --git a/ground/gcs/src/libs/eigen/lapack/single.cpp b/ground/gcs/src/libs/eigen/lapack/single.cpp index a64ed44e1c..c7da3effad 100644 --- a/ground/gcs/src/libs/eigen/lapack/single.cpp +++ b/ground/gcs/src/libs/eigen/lapack/single.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2009-2011 Gael Guennebaud +// Copyright (C) 2009-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -15,3 +15,4 @@ #include "cholesky.cpp" #include "lu.cpp" #include "eigenvalues.cpp" +#include "svd.cpp" diff --git a/ground/gcs/src/libs/eigen/lapack/svd.cpp b/ground/gcs/src/libs/eigen/lapack/svd.cpp new file mode 100644 index 0000000000..77b302b6b4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/lapack/svd.cpp @@ -0,0 +1,138 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "lapack_common.h" +#include + +// computes the singular values/vectors a general M-by-N matrix A using divide-and-conquer +EIGEN_LAPACK_FUNC(gesdd,(char *jobz, int *m, int* n, Scalar* a, int *lda, RealScalar *s, Scalar *u, int *ldu, Scalar *vt, int *ldvt, Scalar* /*work*/, int* lwork, + EIGEN_LAPACK_ARG_IF_COMPLEX(RealScalar */*rwork*/) int * /*iwork*/, int *info)) +{ + // TODO exploit the work buffer + bool query_size = *lwork==-1; + int diag_size = (std::min)(*m,*n); + + *info = 0; + if(*jobz!='A' && *jobz!='S' && *jobz!='O' && *jobz!='N') *info = -1; + else if(*m<0) *info = -2; + else if(*n<0) *info = -3; + else if(*lda=*n && *ldvt<*n)) *info = -10; + + if(*info!=0) + { + int e = -*info; + return xerbla_(SCALAR_SUFFIX_UP"GESDD ", &e, 6); + } + + if(query_size) + { + *lwork = 0; + return 0; + } + + if(*n==0 || *m==0) + return 0; + + PlainMatrixType mat(*m,*n); + mat = matrix(a,*m,*n,*lda); + + int option = *jobz=='A' ? ComputeFullU|ComputeFullV + : *jobz=='S' ? ComputeThinU|ComputeThinV + : *jobz=='O' ? ComputeThinU|ComputeThinV + : 0; + + BDCSVD svd(mat,option); + + make_vector(s,diag_size) = svd.singularValues().head(diag_size); + + if(*jobz=='A') + { + matrix(u,*m,*m,*ldu) = svd.matrixU(); + matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint(); + } + else if(*jobz=='S') + { + matrix(u,*m,diag_size,*ldu) = svd.matrixU(); + matrix(vt,diag_size,*n,*ldvt) = svd.matrixV().adjoint(); + } + else if(*jobz=='O' && *m>=*n) + { + matrix(a,*m,*n,*lda) = svd.matrixU(); + matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint(); + } + else if(*jobz=='O') + { + matrix(u,*m,*m,*ldu) = svd.matrixU(); + matrix(a,diag_size,*n,*lda) = svd.matrixV().adjoint(); + } + + return 0; +} + +// computes the singular values/vectors a general M-by-N matrix A using two sided jacobi algorithm +EIGEN_LAPACK_FUNC(gesvd,(char *jobu, char *jobv, int *m, int* n, Scalar* a, int *lda, RealScalar *s, Scalar *u, int *ldu, Scalar *vt, int *ldvt, Scalar* /*work*/, int* lwork, + EIGEN_LAPACK_ARG_IF_COMPLEX(RealScalar */*rwork*/) int *info)) +{ + // TODO exploit the work buffer + bool query_size = *lwork==-1; + int diag_size = (std::min)(*m,*n); + + *info = 0; + if( *jobu!='A' && *jobu!='S' && *jobu!='O' && *jobu!='N') *info = -1; + else if((*jobv!='A' && *jobv!='S' && *jobv!='O' && *jobv!='N') + || (*jobu=='O' && *jobv=='O')) *info = -2; + else if(*m<0) *info = -3; + else if(*n<0) *info = -4; + else if(*lda svd(mat,option); + + make_vector(s,diag_size) = svd.singularValues().head(diag_size); + { + if(*jobu=='A') matrix(u,*m,*m,*ldu) = svd.matrixU(); + else if(*jobu=='S') matrix(u,*m,diag_size,*ldu) = svd.matrixU(); + else if(*jobu=='O') matrix(a,*m,diag_size,*lda) = svd.matrixU(); + } + { + if(*jobv=='A') matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint(); + else if(*jobv=='S') matrix(vt,diag_size,*n,*ldvt) = svd.matrixV().adjoint(); + else if(*jobv=='O') matrix(a,diag_size,*n,*lda) = svd.matrixV().adjoint(); + } + return 0; +} diff --git a/ground/gcs/src/libs/eigen/scripts/buildtests.in b/ground/gcs/src/libs/eigen/scripts/buildtests.in index 7026373cfc..526d5b74b9 100755 --- a/ground/gcs/src/libs/eigen/scripts/buildtests.in +++ b/ground/gcs/src/libs/eigen/scripts/buildtests.in @@ -2,7 +2,7 @@ if [[ $# != 1 || $1 == *help ]] then - echo "usage: ./check regexp" + echo "usage: $0 regexp" echo " Builds tests matching the regexp." echo " The EIGEN_MAKE_ARGS environment variable allows to pass args to 'make'." echo " For example, to launch 5 concurrent builds, use EIGEN_MAKE_ARGS='-j5'" @@ -14,9 +14,9 @@ targets_to_make=`echo "$TESTSLIST" | egrep "$1" | xargs echo` if [ -n "${EIGEN_MAKE_ARGS:+x}" ] then - make $targets_to_make ${EIGEN_MAKE_ARGS} + @CMAKE_MAKE_PROGRAM@ $targets_to_make ${EIGEN_MAKE_ARGS} else - make $targets_to_make + @CMAKE_MAKE_PROGRAM@ $targets_to_make @EIGEN_TEST_BUILD_FLAGS@ fi exit $? diff --git a/ground/gcs/src/libs/eigen/scripts/check.in b/ground/gcs/src/libs/eigen/scripts/check.in index a90061a579..7717e2d939 100755 --- a/ground/gcs/src/libs/eigen/scripts/check.in +++ b/ground/gcs/src/libs/eigen/scripts/check.in @@ -3,7 +3,7 @@ if [[ $# != 1 || $1 == *help ]] then - echo "usage: ./check regexp" + echo "usage: $0 regexp" echo " Builds and runs tests matching the regexp." echo " The EIGEN_MAKE_ARGS environment variable allows to pass args to 'make'." echo " For example, to launch 5 concurrent builds, use EIGEN_MAKE_ARGS='-j5'" diff --git a/ground/gcs/src/libs/eigen/scripts/eigen_gen_docs b/ground/gcs/src/libs/eigen/scripts/eigen_gen_docs index 0e6f9ada24..787dcb325f 100644 --- a/ground/gcs/src/libs/eigen/scripts/eigen_gen_docs +++ b/ground/gcs/src/libs/eigen/scripts/eigen_gen_docs @@ -4,7 +4,7 @@ # You should call this script with USER set as you want, else some default # will be used USER=${USER:-'orzel'} -UPLOAD_DIR=dox +UPLOAD_DIR=dox-devel #ulimit -v 1024000 diff --git a/ground/gcs/src/libs/eigen/test/CMakeLists.txt b/ground/gcs/src/libs/eigen/test/CMakeLists.txt index c0d8a4e288..2141d07c2b 100644 --- a/ground/gcs/src/libs/eigen/test/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/test/CMakeLists.txt @@ -1,22 +1,38 @@ - -# generate split test header file -message(STATUS ${CMAKE_CURRENT_BINARY_DIR}) -file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/split_test_helper.h "") -foreach(i RANGE 1 999) - file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/split_test_helper.h - "#ifdef EIGEN_TEST_PART_${i}\n" - "#define CALL_SUBTEST_${i}(FUNC) CALL_SUBTEST(FUNC)\n" - "#else\n" - "#define CALL_SUBTEST_${i}(FUNC)\n" - "#endif\n\n" +# generate split test header file only if it does not yet exist +# in order to prevent a rebuild everytime cmake is configured +if(NOT EXISTS ${CMAKE_CURRENT_BINARY_DIR}/split_test_helper.h) + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/split_test_helper.h "") + foreach(i RANGE 1 999) + file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/split_test_helper.h + "#ifdef EIGEN_TEST_PART_${i}\n" + "#define CALL_SUBTEST_${i}(FUNC) CALL_SUBTEST(FUNC)\n" + "#else\n" + "#define CALL_SUBTEST_${i}(FUNC)\n" + "#endif\n\n" ) -endforeach() + endforeach() +endif() + +# check if we have a Fortran compiler +include("../cmake/language_support.cmake") + +workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) + +if(EIGEN_Fortran_COMPILER_WORKS) + enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() +endif() + +if(NOT EIGEN_Fortran_COMPILER_WORKS) + # search for a default Lapack library to complete Eigen's one + find_package(LAPACK) +endif() # configure blas/lapack (use Eigen's ones) -set(BLAS_FOUND TRUE) -set(LAPACK_FOUND TRUE) -set(BLAS_LIBRARIES eigen_blas) -set(LAPACK_LIBRARIES eigen_lapack) +set(EIGEN_BLAS_LIBRARIES eigen_blas) +set(EIGEN_LAPACK_LIBRARIES eigen_lapack) set(EIGEN_TEST_MATRIX_DIR "" CACHE STRING "Enable testing of realword sparse matrices contained in the specified path") if(EIGEN_TEST_MATRIX_DIR) @@ -31,33 +47,33 @@ endif(EIGEN_TEST_MATRIX_DIR) set(SPARSE_LIBS " ") find_package(Cholmod) -if(CHOLMOD_FOUND AND BLAS_FOUND AND LAPACK_FOUND) +if(CHOLMOD_FOUND) add_definitions("-DEIGEN_CHOLMOD_SUPPORT") include_directories(${CHOLMOD_INCLUDES}) - set(SPARSE_LIBS ${SPARSE_LIBS} ${CHOLMOD_LIBRARIES} ${BLAS_LIBRARIES} ${LAPACK_LIBRARIES}) - set(CHOLMOD_ALL_LIBS ${CHOLMOD_LIBRARIES} ${BLAS_LIBRARIES} ${LAPACK_LIBRARIES}) + set(SPARSE_LIBS ${SPARSE_LIBS} ${CHOLMOD_LIBRARIES} ${EIGEN_BLAS_LIBRARIES} ${EIGEN_LAPACK_LIBRARIES}) + set(CHOLMOD_ALL_LIBS ${CHOLMOD_LIBRARIES} ${EIGEN_BLAS_LIBRARIES} ${EIGEN_LAPACK_LIBRARIES}) ei_add_property(EIGEN_TESTED_BACKENDS "Cholmod, ") else() ei_add_property(EIGEN_MISSING_BACKENDS "Cholmod, ") endif() find_package(Umfpack) -if(UMFPACK_FOUND AND BLAS_FOUND) +if(UMFPACK_FOUND) add_definitions("-DEIGEN_UMFPACK_SUPPORT") include_directories(${UMFPACK_INCLUDES}) - set(SPARSE_LIBS ${SPARSE_LIBS} ${UMFPACK_LIBRARIES} ${BLAS_LIBRARIES}) - set(UMFPACK_ALL_LIBS ${UMFPACK_LIBRARIES} ${BLAS_LIBRARIES}) + set(SPARSE_LIBS ${SPARSE_LIBS} ${UMFPACK_LIBRARIES} ${EIGEN_BLAS_LIBRARIES}) + set(UMFPACK_ALL_LIBS ${UMFPACK_LIBRARIES} ${EIGEN_BLAS_LIBRARIES}) ei_add_property(EIGEN_TESTED_BACKENDS "UmfPack, ") else() ei_add_property(EIGEN_MISSING_BACKENDS "UmfPack, ") endif() -find_package(SuperLU) -if(SUPERLU_FOUND AND BLAS_FOUND) +find_package(SuperLU 4.0) +if(SUPERLU_FOUND) add_definitions("-DEIGEN_SUPERLU_SUPPORT") include_directories(${SUPERLU_INCLUDES}) - set(SPARSE_LIBS ${SPARSE_LIBS} ${SUPERLU_LIBRARIES} ${BLAS_LIBRARIES}) - set(SUPERLU_ALL_LIBS ${SUPERLU_LIBRARIES} ${BLAS_LIBRARIES}) + set(SPARSE_LIBS ${SPARSE_LIBS} ${SUPERLU_LIBRARIES} ${EIGEN_BLAS_LIBRARIES}) + set(SUPERLU_ALL_LIBS ${SUPERLU_LIBRARIES} ${EIGEN_BLAS_LIBRARIES}) ei_add_property(EIGEN_TESTED_BACKENDS "SuperLU, ") else() ei_add_property(EIGEN_MISSING_BACKENDS "SuperLU, ") @@ -67,7 +83,7 @@ endif() find_package(Pastix) find_package(Scotch) find_package(Metis 5.0 REQUIRED) -if(PASTIX_FOUND AND BLAS_FOUND) +if(PASTIX_FOUND) add_definitions("-DEIGEN_PASTIX_SUPPORT") include_directories(${PASTIX_INCLUDES}) if(SCOTCH_FOUND) @@ -79,8 +95,8 @@ if(PASTIX_FOUND AND BLAS_FOUND) else(SCOTCH_FOUND) ei_add_property(EIGEN_MISSING_BACKENDS "PaStiX, ") endif(SCOTCH_FOUND) - set(SPARSE_LIBS ${SPARSE_LIBS} ${PASTIX_LIBRARIES} ${ORDERING_LIBRARIES} ${BLAS_LIBRARIES}) - set(PASTIX_ALL_LIBS ${PASTIX_LIBRARIES} ${BLAS_LIBRARIES}) + set(SPARSE_LIBS ${SPARSE_LIBS} ${PASTIX_LIBRARIES} ${ORDERING_LIBRARIES} ${EIGEN_BLAS_LIBRARIES}) + set(PASTIX_ALL_LIBS ${PASTIX_LIBRARIES} ${EIGEN_BLAS_LIBRARIES}) ei_add_property(EIGEN_TESTED_BACKENDS "PaStiX, ") else() ei_add_property(EIGEN_MISSING_BACKENDS "PaStiX, ") @@ -95,16 +111,14 @@ else() endif() find_package(SPQR) -if(SPQR_FOUND AND BLAS_FOUND AND LAPACK_FOUND) - if(CHOLMOD_FOUND) - add_definitions("-DEIGEN_SPQR_SUPPORT") - include_directories(${SPQR_INCLUDES}) - set(SPQR_ALL_LIBS ${SPQR_LIBRARIES} ${CHOLMOD_LIBRARIES} ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) - set(SPARSE_LIBS ${SPARSE_LIBS} ${SPQR_ALL_LIBS}) - ei_add_property(EIGEN_TESTED_BACKENDS "SPQR, ") - else(CHOLMOD_FOUND) - ei_add_property(EIGEN_MISSING_BACKENDS "SPQR, ") - endif(CHOLMOD_FOUND) +if(SPQR_FOUND AND CHOLMOD_FOUND AND (EIGEN_Fortran_COMPILER_WORKS OR LAPACK_FOUND) ) + add_definitions("-DEIGEN_SPQR_SUPPORT") + include_directories(${SPQR_INCLUDES}) + set(SPQR_ALL_LIBS ${SPQR_LIBRARIES} ${CHOLMOD_LIBRARIES} ${EIGEN_LAPACK_LIBRARIES} ${EIGEN_BLAS_LIBRARIES} ${LAPACK_LIBRARIES}) + set(SPARSE_LIBS ${SPARSE_LIBS} ${SPQR_ALL_LIBS}) + ei_add_property(EIGEN_TESTED_BACKENDS "SPQR, ") +else() + ei_add_property(EIGEN_MISSING_BACKENDS "SPQR, ") endif() option(EIGEN_TEST_NOQT "Disable Qt support in unit tests" OFF) @@ -125,25 +139,32 @@ endif(TEST_LIB) set_property(GLOBAL PROPERTY EIGEN_CURRENT_SUBPROJECT "Official") add_custom_target(BuildOfficial) +ei_add_test(rand) ei_add_test(meta) ei_add_test(sizeof) ei_add_test(dynalloc) ei_add_test(nomalloc) ei_add_test(first_aligned) +ei_add_test(nullary) ei_add_test(mixingtypes) -ei_add_test(packetmath) +ei_add_test(packetmath "-DEIGEN_FAST_MATH=1") ei_add_test(unalignedassert) ei_add_test(vectorization_logic) ei_add_test(basicstuff) +ei_add_test(constructor) ei_add_test(linearstructure) ei_add_test(integer_types) -ei_add_test(cwiseop) ei_add_test(unalignedcount) -ei_add_test(exceptions) +if(NOT EIGEN_TEST_NO_EXCEPTIONS) + ei_add_test(exceptions) +endif() ei_add_test(redux) ei_add_test(visitor) ei_add_test(block) ei_add_test(corners) +ei_add_test(swap) +ei_add_test(resize) +ei_add_test(conservative_resize) ei_add_test(product_small) ei_add_test(product_large) ei_add_test(product_extra) @@ -161,6 +182,7 @@ ei_add_test(array_for_matrix) ei_add_test(array_replicate) ei_add_test(array_reverse) ei_add_test(ref) +ei_add_test(is_same_dense) ei_add_test(triangular) ei_add_test(selfadjoint) ei_add_test(product_selfadjoint) @@ -172,6 +194,7 @@ ei_add_test(product_trsolve) ei_add_test(product_mmtr) ei_add_test(product_notemporary) ei_add_test(stable_norm) +ei_add_test(permutationmatrices) ei_add_test(bandmatrix) ei_add_test(cholesky) ei_add_test(lu) @@ -191,56 +214,75 @@ ei_add_test(real_qz) ei_add_test(eigensolver_generalized_real) ei_add_test(jacobi) ei_add_test(jacobisvd) +ei_add_test(bdcsvd) +ei_add_test(householder) ei_add_test(geo_orthomethods) -ei_add_test(geo_homogeneous) ei_add_test(geo_quaternion) -ei_add_test(geo_transformations) ei_add_test(geo_eulerangles) -ei_add_test(geo_hyperplane) ei_add_test(geo_parametrizedline) ei_add_test(geo_alignedbox) +ei_add_test(geo_hyperplane) +ei_add_test(geo_transformations) +ei_add_test(geo_homogeneous) ei_add_test(stdvector) ei_add_test(stdvector_overload) ei_add_test(stdlist) ei_add_test(stdlist_overload) ei_add_test(stddeque) ei_add_test(stddeque_overload) -ei_add_test(resize) -ei_add_test(sparse_vector) ei_add_test(sparse_basic) +ei_add_test(sparse_block) +ei_add_test(sparse_vector) ei_add_test(sparse_product) +ei_add_test(sparse_ref) ei_add_test(sparse_solvers) -ei_add_test(umeyama) -ei_add_test(householder) -ei_add_test(swap) -ei_add_test(conservative_resize) -ei_add_test(permutationmatrices) ei_add_test(sparse_permutations) -ei_add_test(nullary) +ei_add_test(simplicial_cholesky) +ei_add_test(conjugate_gradient) +ei_add_test(incomplete_cholesky) +ei_add_test(bicgstab) +ei_add_test(lscg) +ei_add_test(sparselu) +ei_add_test(sparseqr) +ei_add_test(umeyama) ei_add_test(nesting_ops "${CMAKE_CXX_FLAGS_DEBUG}") ei_add_test(zerosized) ei_add_test(dontalign) -ei_add_test(sizeoverflow) +ei_add_test(evaluators) +if(NOT EIGEN_TEST_NO_EXCEPTIONS) + ei_add_test(sizeoverflow) +endif() ei_add_test(prec_inverse_4x4) ei_add_test(vectorwiseop) ei_add_test(special_numbers) ei_add_test(rvalue_types) +ei_add_test(dense_storage) +ei_add_test(ctorleak) ei_add_test(mpl2only) +ei_add_test(inplace_decomposition) +ei_add_test(half_float) +ei_add_test(array_of_string) -ei_add_test(simplicial_cholesky) -ei_add_test(conjugate_gradient) -ei_add_test(bicgstab) -ei_add_test(sparselu) -ei_add_test(sparseqr) +add_executable(bug1213 bug1213.cpp bug1213_main.cpp) + +check_cxx_compiler_flag("-ffast-math" COMPILER_SUPPORT_FASTMATH) +if(COMPILER_SUPPORT_FASTMATH) + set(EIGEN_FASTMATH_FLAGS "-ffast-math") +else() + check_cxx_compiler_flag("/fp:fast" COMPILER_SUPPORT_FPFAST) + if(COMPILER_SUPPORT_FPFAST) + set(EIGEN_FASTMATH_FLAGS "/fp:fast") + endif() +endif() -# ei_add_test(denseLM) +ei_add_test(fastmath " ${EIGEN_FASTMATH_FLAGS} ") + +# # ei_add_test(denseLM) if(QT4_FOUND) ei_add_test(qtvector "" "${QT_QTCORE_LIBRARY}") endif(QT4_FOUND) -ei_add_test(eigen2support) - if(UMFPACK_FOUND) ei_add_test(umfpack_support "" "${UMFPACK_ALL_LIBS}") endif() @@ -285,9 +327,54 @@ ei_add_property(EIGEN_TESTING_SUMMARY "Sparse lib flags: ${SPARSE_LIBS}\n") option(EIGEN_TEST_EIGEN2 "Run whole Eigen2 test suite against EIGEN2_SUPPORT" OFF) mark_as_advanced(EIGEN_TEST_EIGEN2) if(EIGEN_TEST_EIGEN2) - add_subdirectory(eigen2) + message(WARNING "The Eigen2 test suite has been removed") +endif() + +# boost MP unit test +find_package(Boost) +if(Boost_FOUND) + include_directories(${Boost_INCLUDE_DIRS}) + ei_add_test(boostmultiprec "" "${Boost_LIBRARIES}") + ei_add_property(EIGEN_TESTED_BACKENDS "Boost.Multiprecision, ") +else() + ei_add_property(EIGEN_MISSING_BACKENDS "Boost.Multiprecision, ") +endif() + + +# CUDA unit tests +option(EIGEN_TEST_CUDA "Enable CUDA support in unit tests" OFF) +option(EIGEN_TEST_CUDA_CLANG "Use clang instead of nvcc to compile the CUDA tests" OFF) + +if(EIGEN_TEST_CUDA_CLANG AND NOT CMAKE_CXX_COMPILER MATCHES "clang") + message(WARNING "EIGEN_TEST_CUDA_CLANG is set, but CMAKE_CXX_COMPILER does not appear to be clang.") endif() +if(EIGEN_TEST_CUDA) + +find_package(CUDA 5.0) +if(CUDA_FOUND) + + set(CUDA_PROPAGATE_HOST_FLAGS OFF) + if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + set(CUDA_NVCC_FLAGS "-ccbin ${CMAKE_C_COMPILER}" CACHE STRING "nvcc flags" FORCE) + endif() + if(EIGEN_TEST_CUDA_CLANG) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 --cuda-gpu-arch=sm_30") + endif() + cuda_include_directories(${CMAKE_CURRENT_BINARY_DIR}) + set(EIGEN_ADD_TEST_FILENAME_EXTENSION "cu") + + ei_add_test(cuda_basic) + + unset(EIGEN_ADD_TEST_FILENAME_EXTENSION) + +endif(CUDA_FOUND) + +endif(EIGEN_TEST_CUDA) + + +file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/failtests) +add_test(NAME failtests WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/failtests COMMAND ${CMAKE_COMMAND} ${Eigen_SOURCE_DIR} -G "${CMAKE_GENERATOR}" -DEIGEN_FAILTEST=ON) option(EIGEN_TEST_BUILD_DOCUMENTATION "Test building the doxygen documentation" OFF) IF(EIGEN_TEST_BUILD_DOCUMENTATION) diff --git a/ground/gcs/src/libs/eigen/test/adjoint.cpp b/ground/gcs/src/libs/eigen/test/adjoint.cpp index ea36f7841d..bdea51c108 100644 --- a/ground/gcs/src/libs/eigen/test/adjoint.cpp +++ b/ground/gcs/src/libs/eigen/test/adjoint.cpp @@ -42,6 +42,17 @@ template<> struct adjoint_specific { VERIFY_IS_APPROX(v1, v1.norm() * v3); VERIFY_IS_APPROX(v3, v1.normalized()); VERIFY_IS_APPROX(v3.norm(), RealScalar(1)); + + // check null inputs + VERIFY_IS_APPROX((v1*0).normalized(), (v1*0)); +#if (!EIGEN_ARCH_i386) || defined(EIGEN_VECTORIZE) + RealScalar very_small = (std::numeric_limits::min)(); + VERIFY( (v1*very_small).norm() == 0 ); + VERIFY_IS_APPROX((v1*very_small).normalized(), (v1*very_small)); + v3 = v1*very_small; + v3.normalize(); + VERIFY_IS_APPROX(v3, (v1*very_small)); +#endif // check compatibility of dot and adjoint ref = NumTraits::IsInteger ? 0 : (std::max)((std::max)(v1.norm(),v2.norm()),(std::max)((square * v2).norm(),(square.adjoint() * v1).norm())); @@ -64,6 +75,7 @@ template void adjoint(const MatrixType& m) typedef typename NumTraits::Real RealScalar; typedef Matrix VectorType; typedef Matrix SquareMatrixType; + const Index PacketSize = internal::packet_traits::size; Index rows = m.rows(); Index cols = m.cols(); @@ -108,6 +120,17 @@ template void adjoint(const MatrixType& m) VERIFY_IS_APPROX(m3,m1.transpose()); m3.transposeInPlace(); VERIFY_IS_APPROX(m3,m1); + + if(PacketSize(0,m3.rows()-PacketSize); + Index j = internal::random(0,m3.cols()-PacketSize); + m3.template block(i,j).transposeInPlace(); + VERIFY_IS_APPROX( (m3.template block(i,j)), (m1.template block(i,j).transpose()) ); + m3.template block(i,j).transposeInPlace(); + VERIFY_IS_APPROX(m3,m1); + } // check inplace adjoint m3 = m1; @@ -129,14 +152,24 @@ void test_adjoint() CALL_SUBTEST_1( adjoint(Matrix()) ); CALL_SUBTEST_2( adjoint(Matrix3d()) ); CALL_SUBTEST_3( adjoint(Matrix4f()) ); + CALL_SUBTEST_4( adjoint(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_5( adjoint(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_6( adjoint(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + + // Complement for 128 bits vectorization: + CALL_SUBTEST_8( adjoint(Matrix2d()) ); + CALL_SUBTEST_9( adjoint(Matrix()) ); + + // 256 bits vectorization: + CALL_SUBTEST_10( adjoint(Matrix()) ); + CALL_SUBTEST_11( adjoint(Matrix()) ); + CALL_SUBTEST_12( adjoint(Matrix()) ); } // test a large static matrix only once CALL_SUBTEST_7( adjoint(Matrix()) ); -#ifdef EIGEN_TEST_PART_4 +#ifdef EIGEN_TEST_PART_13 { MatrixXcf a(10,10), b(10,10); VERIFY_RAISES_ASSERT(a = a.transpose()); @@ -154,6 +187,13 @@ void test_adjoint() a.transpose() = a.adjoint(); a.transpose() += a.adjoint(); a.transpose() += a.adjoint() + b; + + // regression tests for check_for_aliasing + MatrixXd c(10,10); + c = 1.0 * MatrixXd::Ones(10,10) + c; + c = MatrixXd::Ones(10,10) * 1.0 + c; + c = c + MatrixXd::Ones(10,10) .cwiseProduct( MatrixXd::Zero(10,10) ); + c = MatrixXd::Ones(10,10) * MatrixXd::Zero(10,10); } #endif } diff --git a/ground/gcs/src/libs/eigen/test/array.cpp b/ground/gcs/src/libs/eigen/test/array.cpp index 68f6b3d7ab..15c3266a98 100644 --- a/ground/gcs/src/libs/eigen/test/array.cpp +++ b/ground/gcs/src/libs/eigen/test/array.cpp @@ -13,6 +13,7 @@ template void array(const ArrayType& m) { typedef typename ArrayType::Index Index; typedef typename ArrayType::Scalar Scalar; + typedef typename ArrayType::RealScalar RealScalar; typedef Array ColVectorType; typedef Array RowVectorType; @@ -22,6 +23,8 @@ template void array(const ArrayType& m) ArrayType m1 = ArrayType::Random(rows, cols), m2 = ArrayType::Random(rows, cols), m3(rows, cols); + ArrayType m4 = m1; // copy constructor + VERIFY_IS_APPROX(m1, m4); ColVectorType cv1 = ColVectorType::Random(rows); RowVectorType rv1 = RowVectorType::Random(cols); @@ -70,7 +73,7 @@ template void array(const ArrayType& m) VERIFY_IS_MUCH_SMALLER_THAN(abs(m1.rowwise().sum().sum() - m1.sum()), m1.abs().sum()); if (!internal::isMuchSmallerThan(abs(m1.sum() - (m1+m2).sum()), m1.abs().sum(), test_precision())) VERIFY_IS_NOT_APPROX(((m1+m2).rowwise().sum()).sum(), m1.sum()); - VERIFY_IS_APPROX(m1.colwise().sum(), m1.colwise().redux(internal::scalar_sum_op())); + VERIFY_IS_APPROX(m1.colwise().sum(), m1.colwise().redux(internal::scalar_sum_op())); // vector-wise ops m3 = m1; @@ -81,6 +84,47 @@ template void array(const ArrayType& m) VERIFY_IS_APPROX(m3.rowwise() += rv1, m1.rowwise() + rv1); m3 = m1; VERIFY_IS_APPROX(m3.rowwise() -= rv1, m1.rowwise() - rv1); + + // Conversion from scalar + VERIFY_IS_APPROX((m3 = s1), ArrayType::Constant(rows,cols,s1)); + VERIFY_IS_APPROX((m3 = 1), ArrayType::Constant(rows,cols,1)); + VERIFY_IS_APPROX((m3.topLeftCorner(rows,cols) = 1), ArrayType::Constant(rows,cols,1)); + typedef Array FixedArrayType; + FixedArrayType f1(s1); + VERIFY_IS_APPROX(f1, FixedArrayType::Constant(s1)); + FixedArrayType f2(numext::real(s1)); + VERIFY_IS_APPROX(f2, FixedArrayType::Constant(numext::real(s1))); + FixedArrayType f3((int)100*numext::real(s1)); + VERIFY_IS_APPROX(f3, FixedArrayType::Constant((int)100*numext::real(s1))); + f1.setRandom(); + FixedArrayType f4(f1.data()); + VERIFY_IS_APPROX(f4, f1); + + // pow + VERIFY_IS_APPROX(m1.pow(2), m1.square()); + VERIFY_IS_APPROX(pow(m1,2), m1.square()); + VERIFY_IS_APPROX(m1.pow(3), m1.cube()); + VERIFY_IS_APPROX(pow(m1,3), m1.cube()); + VERIFY_IS_APPROX((-m1).pow(3), -m1.cube()); + VERIFY_IS_APPROX(pow(2*m1,3), 8*m1.cube()); + ArrayType exponents = ArrayType::Constant(rows, cols, RealScalar(2)); + VERIFY_IS_APPROX(Eigen::pow(m1,exponents), m1.square()); + VERIFY_IS_APPROX(m1.pow(exponents), m1.square()); + VERIFY_IS_APPROX(Eigen::pow(2*m1,exponents), 4*m1.square()); + VERIFY_IS_APPROX((2*m1).pow(exponents), 4*m1.square()); + VERIFY_IS_APPROX(Eigen::pow(m1,2*exponents), m1.square().square()); + VERIFY_IS_APPROX(m1.pow(2*exponents), m1.square().square()); + VERIFY_IS_APPROX(Eigen::pow(m1(0,0), exponents), ArrayType::Constant(rows,cols,m1(0,0)*m1(0,0))); + + // Check possible conflicts with 1D ctor + typedef Array OneDArrayType; + OneDArrayType o1(rows); + VERIFY(o1.size()==rows); + OneDArrayType o4((int)rows); + VERIFY(o4.size()==rows); } template void comparisons(const ArrayType& m) @@ -97,8 +141,11 @@ template void comparisons(const ArrayType& m) c = internal::random(0, cols-1); ArrayType m1 = ArrayType::Random(rows, cols), - m2 = ArrayType::Random(rows, cols), - m3(rows, cols); + m2 = ArrayType::Random(rows, cols), + m3(rows, cols), + m4 = m1; + + m4 = (m4.abs()==Scalar(0)).select(1,m4); VERIFY(((m1 + Scalar(1)) > m1).all()); VERIFY(((m1 - Scalar(1)) < m1).all()); @@ -112,11 +159,17 @@ template void comparisons(const ArrayType& m) VERIFY(!(m1 > m2 && m1 < m2).any()); VERIFY((m1 <= m2 || m1 >= m2).all()); - // comparisons to scalar + // comparisons array to scalar VERIFY( (m1 != (m1(r,c)+1) ).any() ); - VERIFY( (m1 > (m1(r,c)-1) ).any() ); - VERIFY( (m1 < (m1(r,c)+1) ).any() ); - VERIFY( (m1 == m1(r,c) ).any() ); + VERIFY( (m1 > (m1(r,c)-1) ).any() ); + VERIFY( (m1 < (m1(r,c)+1) ).any() ); + VERIFY( (m1 == m1(r,c) ).any() ); + + // comparisons scalar to array + VERIFY( ( (m1(r,c)+1) != m1).any() ); + VERIFY( ( (m1(r,c)-1) < m1).any() ); + VERIFY( ( (m1(r,c)+1) > m1).any() ); + VERIFY( ( m1(r,c) == m1).any() ); // test Select VERIFY_IS_APPROX( (m1 void array_real(const ArrayType& m) ArrayType m1 = ArrayType::Random(rows, cols), m2 = ArrayType::Random(rows, cols), - m3(rows, cols); + m3(rows, cols), + m4 = m1; + + m4 = (m4.abs()==Scalar(0)).select(1,m4); Scalar s1 = internal::random(); - // these tests are mostly to check possible compilation issues. + // these tests are mostly to check possible compilation issues with free-functions. VERIFY_IS_APPROX(m1.sin(), sin(m1)); VERIFY_IS_APPROX(m1.cos(), cos(m1)); + VERIFY_IS_APPROX(m1.tan(), tan(m1)); VERIFY_IS_APPROX(m1.asin(), asin(m1)); VERIFY_IS_APPROX(m1.acos(), acos(m1)); - VERIFY_IS_APPROX(m1.tan(), tan(m1)); - + VERIFY_IS_APPROX(m1.atan(), atan(m1)); + VERIFY_IS_APPROX(m1.sinh(), sinh(m1)); + VERIFY_IS_APPROX(m1.cosh(), cosh(m1)); + VERIFY_IS_APPROX(m1.tanh(), tanh(m1)); + + VERIFY_IS_APPROX(m1.arg(), arg(m1)); + VERIFY_IS_APPROX(m1.round(), round(m1)); + VERIFY_IS_APPROX(m1.floor(), floor(m1)); + VERIFY_IS_APPROX(m1.ceil(), ceil(m1)); + VERIFY((m1.isNaN() == (Eigen::isnan)(m1)).all()); + VERIFY((m1.isInf() == (Eigen::isinf)(m1)).all()); + VERIFY((m1.isFinite() == (Eigen::isfinite)(m1)).all()); + VERIFY_IS_APPROX(m1.inverse(), inverse(m1)); + VERIFY_IS_APPROX(m1.abs(), abs(m1)); + VERIFY_IS_APPROX(m1.abs2(), abs2(m1)); + VERIFY_IS_APPROX(m1.square(), square(m1)); + VERIFY_IS_APPROX(m1.cube(), cube(m1)); VERIFY_IS_APPROX(cos(m1+RealScalar(3)*m2), cos((m1+RealScalar(3)*m2).eval())); + VERIFY_IS_APPROX(m1.sign(), sign(m1)); + - VERIFY_IS_APPROX(m1.abs().sqrt(), sqrt(abs(m1))); - VERIFY_IS_APPROX(m1.abs(), sqrt(numext::abs2(m1))); + // avoid NaNs with abs() so verification doesn't fail + m3 = m1.abs(); + VERIFY_IS_APPROX(m3.sqrt(), sqrt(abs(m1))); + VERIFY_IS_APPROX(m3.rsqrt(), Scalar(1)/sqrt(abs(m1))); + VERIFY_IS_APPROX(rsqrt(m3), Scalar(1)/sqrt(abs(m1))); + VERIFY_IS_APPROX(m3.log(), log(m3)); + VERIFY_IS_APPROX(m3.log1p(), log1p(m3)); + VERIFY_IS_APPROX(m3.log10(), log10(m3)); + + + VERIFY((!(m1>m2) == (m1<=m2)).all()); + + VERIFY_IS_APPROX(sin(m1.asin()), m1); + VERIFY_IS_APPROX(cos(m1.acos()), m1); + VERIFY_IS_APPROX(tan(m1.atan()), m1); + VERIFY_IS_APPROX(sinh(m1), 0.5*(exp(m1)-exp(-m1))); + VERIFY_IS_APPROX(cosh(m1), 0.5*(exp(m1)+exp(-m1))); + VERIFY_IS_APPROX(tanh(m1), (0.5*(exp(m1)-exp(-m1)))/(0.5*(exp(m1)+exp(-m1)))); + VERIFY_IS_APPROX(arg(m1), ((m1<0).template cast())*std::acos(-1.0)); + VERIFY((round(m1) <= ceil(m1) && round(m1) >= floor(m1)).all()); + VERIFY((Eigen::isnan)((m1*0.0)/0.0).all()); + VERIFY((Eigen::isinf)(m4/0.0).all()); + VERIFY(((Eigen::isfinite)(m1) && (!(Eigen::isfinite)(m1*0.0/0.0)) && (!(Eigen::isfinite)(m4/0.0))).all()); + VERIFY_IS_APPROX(inverse(inverse(m1)),m1); + VERIFY((abs(m1) == m1 || abs(m1) == -m1).all()); + VERIFY_IS_APPROX(m3, sqrt(abs2(m1))); + VERIFY_IS_APPROX( m1.sign(), -(-m1).sign() ); + VERIFY_IS_APPROX( m1*m1.sign(),m1.abs()); + VERIFY_IS_APPROX(m1.sign() * m1.abs(), m1); VERIFY_IS_APPROX(numext::abs2(numext::real(m1)) + numext::abs2(numext::imag(m1)), numext::abs2(m1)); VERIFY_IS_APPROX(numext::abs2(real(m1)) + numext::abs2(imag(m1)), numext::abs2(m1)); @@ -187,52 +288,138 @@ template void array_real(const ArrayType& m) // shift argument of logarithm so that it is not zero Scalar smallNumber = NumTraits::dummy_precision(); - VERIFY_IS_APPROX((m1.abs() + smallNumber).log() , log(abs(m1) + smallNumber)); + VERIFY_IS_APPROX((m3 + smallNumber).log() , log(abs(m1) + smallNumber)); + VERIFY_IS_APPROX((m3 + smallNumber + 1).log() , log1p(abs(m1) + smallNumber)); VERIFY_IS_APPROX(m1.exp() * m2.exp(), exp(m1+m2)); VERIFY_IS_APPROX(m1.exp(), exp(m1)); VERIFY_IS_APPROX(m1.exp() / m2.exp(),(m1-m2).exp()); - VERIFY_IS_APPROX(m1.pow(2), m1.square()); - VERIFY_IS_APPROX(pow(m1,2), m1.square()); - - ArrayType exponents = ArrayType::Constant(rows, cols, RealScalar(2)); - VERIFY_IS_APPROX(Eigen::pow(m1,exponents), m1.square()); - - m3 = m1.abs(); VERIFY_IS_APPROX(m3.pow(RealScalar(0.5)), m3.sqrt()); VERIFY_IS_APPROX(pow(m3,RealScalar(0.5)), m3.sqrt()); + VERIFY_IS_APPROX(m3.pow(RealScalar(-0.5)), m3.rsqrt()); + VERIFY_IS_APPROX(pow(m3,RealScalar(-0.5)), m3.rsqrt()); + + VERIFY_IS_APPROX(log10(m3), log(m3)/log(10)); + // scalar by array division const RealScalar tiny = sqrt(std::numeric_limits::epsilon()); s1 += Scalar(tiny); m1 += ArrayType::Constant(rows,cols,Scalar(tiny)); VERIFY_IS_APPROX(s1/m1, s1 * m1.inverse()); - + // check inplace transpose m3 = m1; m3.transposeInPlace(); - VERIFY_IS_APPROX(m3,m1.transpose()); + VERIFY_IS_APPROX(m3, m1.transpose()); m3.transposeInPlace(); - VERIFY_IS_APPROX(m3,m1); + VERIFY_IS_APPROX(m3, m1); } template void array_complex(const ArrayType& m) { typedef typename ArrayType::Index Index; + typedef typename ArrayType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; Index rows = m.rows(); Index cols = m.cols(); ArrayType m1 = ArrayType::Random(rows, cols), - m2(rows, cols); + m2(rows, cols), + m4 = m1; + + m4.real() = (m4.real().abs()==RealScalar(0)).select(RealScalar(1),m4.real()); + m4.imag() = (m4.imag().abs()==RealScalar(0)).select(RealScalar(1),m4.imag()); + + Array m3(rows, cols); for (Index i = 0; i < m.rows(); ++i) for (Index j = 0; j < m.cols(); ++j) m2(i,j) = sqrt(m1(i,j)); - VERIFY_IS_APPROX(m1.sqrt(), m2); - VERIFY_IS_APPROX(m1.sqrt(), Eigen::sqrt(m1)); + // these tests are mostly to check possible compilation issues with free-functions. + VERIFY_IS_APPROX(m1.sin(), sin(m1)); + VERIFY_IS_APPROX(m1.cos(), cos(m1)); + VERIFY_IS_APPROX(m1.tan(), tan(m1)); + VERIFY_IS_APPROX(m1.sinh(), sinh(m1)); + VERIFY_IS_APPROX(m1.cosh(), cosh(m1)); + VERIFY_IS_APPROX(m1.tanh(), tanh(m1)); + VERIFY_IS_APPROX(m1.arg(), arg(m1)); + VERIFY((m1.isNaN() == (Eigen::isnan)(m1)).all()); + VERIFY((m1.isInf() == (Eigen::isinf)(m1)).all()); + VERIFY((m1.isFinite() == (Eigen::isfinite)(m1)).all()); + VERIFY_IS_APPROX(m1.inverse(), inverse(m1)); + VERIFY_IS_APPROX(m1.log(), log(m1)); + VERIFY_IS_APPROX(m1.log10(), log10(m1)); + VERIFY_IS_APPROX(m1.abs(), abs(m1)); + VERIFY_IS_APPROX(m1.abs2(), abs2(m1)); + VERIFY_IS_APPROX(m1.sqrt(), sqrt(m1)); + VERIFY_IS_APPROX(m1.square(), square(m1)); + VERIFY_IS_APPROX(m1.cube(), cube(m1)); + VERIFY_IS_APPROX(cos(m1+RealScalar(3)*m2), cos((m1+RealScalar(3)*m2).eval())); + VERIFY_IS_APPROX(m1.sign(), sign(m1)); + + + VERIFY_IS_APPROX(m1.exp() * m2.exp(), exp(m1+m2)); + VERIFY_IS_APPROX(m1.exp(), exp(m1)); + VERIFY_IS_APPROX(m1.exp() / m2.exp(),(m1-m2).exp()); + + VERIFY_IS_APPROX(sinh(m1), 0.5*(exp(m1)-exp(-m1))); + VERIFY_IS_APPROX(cosh(m1), 0.5*(exp(m1)+exp(-m1))); + VERIFY_IS_APPROX(tanh(m1), (0.5*(exp(m1)-exp(-m1)))/(0.5*(exp(m1)+exp(-m1)))); + + for (Index i = 0; i < m.rows(); ++i) + for (Index j = 0; j < m.cols(); ++j) + m3(i,j) = std::atan2(imag(m1(i,j)), real(m1(i,j))); + VERIFY_IS_APPROX(arg(m1), m3); + + std::complex zero(0.0,0.0); + VERIFY((Eigen::isnan)(m1*zero/zero).all()); +#if EIGEN_COMP_MSVC + // msvc complex division is not robust + VERIFY((Eigen::isinf)(m4/RealScalar(0)).all()); +#else +#if EIGEN_COMP_CLANG + // clang's complex division is notoriously broken too + if((numext::isinf)(m4(0,0)/RealScalar(0))) { +#endif + VERIFY((Eigen::isinf)(m4/zero).all()); +#if EIGEN_COMP_CLANG + } + else + { + VERIFY((Eigen::isinf)(m4.real()/zero.real()).all()); + } +#endif +#endif // MSVC + + VERIFY(((Eigen::isfinite)(m1) && (!(Eigen::isfinite)(m1*zero/zero)) && (!(Eigen::isfinite)(m1/zero))).all()); + + VERIFY_IS_APPROX(inverse(inverse(m1)),m1); + VERIFY_IS_APPROX(conj(m1.conjugate()), m1); + VERIFY_IS_APPROX(abs(m1), sqrt(square(real(m1))+square(imag(m1)))); + VERIFY_IS_APPROX(abs(m1), sqrt(abs2(m1))); + VERIFY_IS_APPROX(log10(m1), log(m1)/log(10)); + + VERIFY_IS_APPROX( m1.sign(), -(-m1).sign() ); + VERIFY_IS_APPROX( m1.sign() * m1.abs(), m1); + + // scalar by array division + Scalar s1 = internal::random(); + const RealScalar tiny = std::sqrt(std::numeric_limits::epsilon()); + s1 += Scalar(tiny); + m1 += ArrayType::Constant(rows,cols,Scalar(tiny)); + VERIFY_IS_APPROX(s1/m1, s1 * m1.inverse()); + + // check inplace transpose + m2 = m1; + m2.transposeInPlace(); + VERIFY_IS_APPROX(m2, m1.transpose()); + m2.transposeInPlace(); + VERIFY_IS_APPROX(m2, m1); + } template void min_max(const ArrayType& m) @@ -301,7 +488,7 @@ void test_array() VERIFY((internal::is_same< internal::global_math_functions_filtering_base::type, int >::value)); VERIFY((internal::is_same< internal::global_math_functions_filtering_base::type, float >::value)); VERIFY((internal::is_same< internal::global_math_functions_filtering_base::type, ArrayBase >::value)); - typedef CwiseUnaryOp, ArrayXd > Xpr; + typedef CwiseUnaryOp, ArrayXd > Xpr; VERIFY((internal::is_same< internal::global_math_functions_filtering_base::type, ArrayBase >::value)); diff --git a/ground/gcs/src/libs/eigen/test/array_for_matrix.cpp b/ground/gcs/src/libs/eigen/test/array_for_matrix.cpp index 9667e1f143..c1501947b9 100644 --- a/ground/gcs/src/libs/eigen/test/array_for_matrix.cpp +++ b/ground/gcs/src/libs/eigen/test/array_for_matrix.cpp @@ -45,7 +45,7 @@ template void array_for_matrix(const MatrixType& m) VERIFY_IS_MUCH_SMALLER_THAN(m1.rowwise().sum().sum() - m1.sum(), m1.squaredNorm()); VERIFY_IS_MUCH_SMALLER_THAN(m1.colwise().sum() + m2.colwise().sum() - (m1+m2).colwise().sum(), (m1+m2).squaredNorm()); VERIFY_IS_MUCH_SMALLER_THAN(m1.rowwise().sum() - m2.rowwise().sum() - (m1-m2).rowwise().sum(), (m1-m2).squaredNorm()); - VERIFY_IS_APPROX(m1.colwise().sum(), m1.colwise().redux(internal::scalar_sum_op())); + VERIFY_IS_APPROX(m1.colwise().sum(), m1.colwise().redux(internal::scalar_sum_op())); // vector-wise ops m3 = m1; @@ -68,6 +68,16 @@ template void array_for_matrix(const MatrixType& m) const Scalar& ref_a2 = m.array().matrix().coeffRef(0,0); VERIFY(&ref_a1 == &ref_m1); VERIFY(&ref_a2 == &ref_m2); + + // Check write accessors: + m1.array().coeffRef(0,0) = 1; + VERIFY_IS_APPROX(m1(0,0),Scalar(1)); + m1.array()(0,0) = 2; + VERIFY_IS_APPROX(m1(0,0),Scalar(2)); + m1.array().matrix().coeffRef(0,0) = 3; + VERIFY_IS_APPROX(m1(0,0),Scalar(3)); + m1.array().matrix()(0,0) = 4; + VERIFY_IS_APPROX(m1(0,0),Scalar(4)); } template void comparisons(const MatrixType& m) @@ -124,6 +134,12 @@ template void comparisons(const MatrixType& m) // count VERIFY(((m1.array().abs()+1)>RealScalar(0.1)).count() == rows*cols); + // and/or + VERIFY( ((m1.array()RealScalar(0)).matrix()).count() == 0); + VERIFY( ((m1.array()=RealScalar(0)).matrix()).count() == rows*cols); + RealScalar a = m1.cwiseAbs().mean(); + VERIFY( ((m1.array()<-a).matrix() || (m1.array()>a).matrix()).count() == (m1.cwiseAbs().array()>a).count()); + typedef Matrix VectorOfIndices; // TODO allows colwise/rowwise for array @@ -134,9 +150,21 @@ template void comparisons(const MatrixType& m) template void lpNorm(const VectorType& v) { using std::sqrt; + typedef typename VectorType::RealScalar RealScalar; VectorType u = VectorType::Random(v.size()); - VERIFY_IS_APPROX(u.template lpNorm(), u.cwiseAbs().maxCoeff()); + if(v.size()==0) + { + VERIFY_IS_APPROX(u.template lpNorm(), RealScalar(0)); + VERIFY_IS_APPROX(u.template lpNorm<1>(), RealScalar(0)); + VERIFY_IS_APPROX(u.template lpNorm<2>(), RealScalar(0)); + VERIFY_IS_APPROX(u.template lpNorm<5>(), RealScalar(0)); + } + else + { + VERIFY_IS_APPROX(u.template lpNorm(), u.cwiseAbs().maxCoeff()); + } + VERIFY_IS_APPROX(u.template lpNorm<1>(), u.cwiseAbs().sum()); VERIFY_IS_APPROX(u.template lpNorm<2>(), sqrt(u.array().abs().square().sum())); VERIFY_IS_APPROX(numext::pow(u.template lpNorm<5>(), typename VectorType::RealScalar(5)), u.array().abs().pow(5).sum()); @@ -245,6 +273,8 @@ void test_array_for_matrix() CALL_SUBTEST_5( lpNorm(VectorXf(internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_4( lpNorm(VectorXcf(internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } + CALL_SUBTEST_5( lpNorm(VectorXf(0)) ); + CALL_SUBTEST_4( lpNorm(VectorXcf(0)) ); for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_4( resize(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_5( resize(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); diff --git a/ground/gcs/src/libs/eigen/test/array_of_string.cpp b/ground/gcs/src/libs/eigen/test/array_of_string.cpp new file mode 100644 index 0000000000..e23b7c59e6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/array_of_string.cpp @@ -0,0 +1,32 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +void test_array_of_string() +{ + typedef Array ArrayXs; + ArrayXs a1(3), a2(3), a3(3), a3ref(3); + a1 << "one", "two", "three"; + a2 << "1", "2", "3"; + a3ref << "one (1)", "two (2)", "three (3)"; + std::stringstream s1; + s1 << a1; + VERIFY_IS_EQUAL(s1.str(), std::string(" one two three")); + a3 = a1 + std::string(" (") + a2 + std::string(")"); + VERIFY((a3==a3ref).all()); + + a3 = a1; + a3 += std::string(" (") + a2 + std::string(")"); + VERIFY((a3==a3ref).all()); + + a1.swap(a3); + VERIFY((a1==a3ref).all()); + VERIFY((a3!=a3ref).all()); +} diff --git a/ground/gcs/src/libs/eigen/test/array_replicate.cpp b/ground/gcs/src/libs/eigen/test/array_replicate.cpp index f412d1aed7..779c8fc2f7 100644 --- a/ground/gcs/src/libs/eigen/test/array_replicate.cpp +++ b/ground/gcs/src/libs/eigen/test/array_replicate.cpp @@ -44,6 +44,19 @@ template void replicate(const MatrixType& m) x2 << m2, m2, m2, m2, m2, m2; VERIFY_IS_APPROX(x2, (m2.template replicate<2,3>())); + + x2.resize(rows,3*cols); + x2 << m2, m2, m2; + VERIFY_IS_APPROX(x2, (m2.template replicate<1,3>())); + + vx1.resize(3*rows,cols); + vx1 << m2, m2, m2; + VERIFY_IS_APPROX(vx1+vx1, vx1+(m2.template replicate<3,1>())); + + vx1=m2+(m2.colwise().replicate(1)); + + if(m2.cols()==1) + VERIFY_IS_APPROX(m2.coeff(0), (m2.template replicate<3,1>().coeff(m2.rows()))); x2.resize(rows,f1); for (int j=0; j void reverse(const MatrixType& m) // this test relies a lot on Random.h, and there's not much more that we can do // to test it, hence I consider that we will have tested Random.h - MatrixType m1 = MatrixType::Random(rows, cols); + MatrixType m1 = MatrixType::Random(rows, cols), m2; VectorType v1 = VectorType::Random(rows); MatrixType m1_r = m1.reverse(); @@ -96,14 +96,32 @@ template void reverse(const MatrixType& m) m1.reverse()(r, c) = x; VERIFY_IS_APPROX(x, m1(rows - 1 - r, cols - 1 - c)); + + m2 = m1; + m2.reverseInPlace(); + VERIFY_IS_APPROX(m2,m1.reverse().eval()); + + m2 = m1; + m2.col(0).reverseInPlace(); + VERIFY_IS_APPROX(m2.col(0),m1.col(0).reverse().eval()); + + m2 = m1; + m2.row(0).reverseInPlace(); + VERIFY_IS_APPROX(m2.row(0),m1.row(0).reverse().eval()); + + m2 = m1; + m2.rowwise().reverseInPlace(); + VERIFY_IS_APPROX(m2,m1.rowwise().reverse().eval()); + + m2 = m1; + m2.colwise().reverseInPlace(); + VERIFY_IS_APPROX(m2,m1.colwise().reverse().eval()); - /* m1.colwise().reverse()(r, c) = x; VERIFY_IS_APPROX(x, m1(rows - 1 - r, c)); m1.rowwise().reverse()(r, c) = x; VERIFY_IS_APPROX(x, m1(r, cols - 1 - c)); - */ } void test_array_reverse() @@ -113,11 +131,11 @@ void test_array_reverse() CALL_SUBTEST_2( reverse(Matrix2f()) ); CALL_SUBTEST_3( reverse(Matrix4f()) ); CALL_SUBTEST_4( reverse(Matrix4d()) ); - CALL_SUBTEST_5( reverse(MatrixXcf(3, 3)) ); - CALL_SUBTEST_6( reverse(MatrixXi(6, 3)) ); - CALL_SUBTEST_7( reverse(MatrixXcd(20, 20)) ); + CALL_SUBTEST_5( reverse(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_6( reverse(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_7( reverse(MatrixXcd(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_8( reverse(Matrix()) ); - CALL_SUBTEST_9( reverse(Matrix(6,3)) ); + CALL_SUBTEST_9( reverse(Matrix(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } #ifdef EIGEN_TEST_PART_3 Vector4f x; x << 1, 2, 3, 4; diff --git a/ground/gcs/src/libs/eigen/test/bandmatrix.cpp b/ground/gcs/src/libs/eigen/test/bandmatrix.cpp index 5e4e8e07b1..f8c38f7c31 100644 --- a/ground/gcs/src/libs/eigen/test/bandmatrix.cpp +++ b/ground/gcs/src/libs/eigen/test/bandmatrix.cpp @@ -11,7 +11,6 @@ template void bandmatrix(const MatrixType& _m) { - typedef typename MatrixType::Index Index; typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; typedef Matrix DenseMatrixType; @@ -62,8 +61,6 @@ using Eigen::internal::BandMatrix; void test_bandmatrix() { - typedef BandMatrix::Index Index; - for(int i = 0; i < 10*g_repeat ; i++) { Index rows = internal::random(1,10); Index cols = internal::random(1,10); diff --git a/ground/gcs/src/libs/eigen/test/basicstuff.cpp b/ground/gcs/src/libs/eigen/test/basicstuff.cpp index 8c0621ecd2..99d91f9daf 100644 --- a/ground/gcs/src/libs/eigen/test/basicstuff.cpp +++ b/ground/gcs/src/libs/eigen/test/basicstuff.cpp @@ -126,6 +126,20 @@ template void basicStuff(const MatrixType& m) for(typename MatrixType::Index i=0;i(0,10)>5; + m3 = b ? m1 : m2; + if(b) VERIFY_IS_APPROX(m3,m1); + else VERIFY_IS_APPROX(m3,m2); + m3 = b ? -m1 : m2; + if(b) VERIFY_IS_APPROX(m3,-m1); + else VERIFY_IS_APPROX(m3,m2); + m3 = b ? m1 : -m2; + if(b) VERIFY_IS_APPROX(m3,m1); + else VERIFY_IS_APPROX(m3,-m2); + } } template void basicStuffComplex(const MatrixType& m) @@ -180,15 +194,64 @@ void casting() template void fixedSizeMatrixConstruction() { - const Scalar raw[3] = {1,2,3}; - Matrix m(raw); - Array a(raw); - VERIFY(m(0) == 1); - VERIFY(m(1) == 2); - VERIFY(m(2) == 3); - VERIFY(a(0) == 1); - VERIFY(a(1) == 2); - VERIFY(a(2) == 3); + Scalar raw[4]; + for(int k=0; k<4; ++k) + raw[k] = internal::random(); + + { + Matrix m(raw); + Array a(raw); + for(int k=0; k<4; ++k) VERIFY(m(k) == raw[k]); + for(int k=0; k<4; ++k) VERIFY(a(k) == raw[k]); + VERIFY_IS_EQUAL(m,(Matrix(raw[0],raw[1],raw[2],raw[3]))); + VERIFY((a==(Array(raw[0],raw[1],raw[2],raw[3]))).all()); + } + { + Matrix m(raw); + Array a(raw); + for(int k=0; k<3; ++k) VERIFY(m(k) == raw[k]); + for(int k=0; k<3; ++k) VERIFY(a(k) == raw[k]); + VERIFY_IS_EQUAL(m,(Matrix(raw[0],raw[1],raw[2]))); + VERIFY((a==Array(raw[0],raw[1],raw[2])).all()); + } + { + Matrix m(raw), m2( (DenseIndex(raw[0])), (DenseIndex(raw[1])) ); + Array a(raw), a2( (DenseIndex(raw[0])), (DenseIndex(raw[1])) ); + for(int k=0; k<2; ++k) VERIFY(m(k) == raw[k]); + for(int k=0; k<2; ++k) VERIFY(a(k) == raw[k]); + VERIFY_IS_EQUAL(m,(Matrix(raw[0],raw[1]))); + VERIFY((a==Array(raw[0],raw[1])).all()); + for(int k=0; k<2; ++k) VERIFY(m2(k) == DenseIndex(raw[k])); + for(int k=0; k<2; ++k) VERIFY(a2(k) == DenseIndex(raw[k])); + } + { + Matrix m(raw), + m2( (DenseIndex(raw[0])), (DenseIndex(raw[1])) ), + m3( (int(raw[0])), (int(raw[1])) ), + m4( (float(raw[0])), (float(raw[1])) ); + Array a(raw), a2( (DenseIndex(raw[0])), (DenseIndex(raw[1])) ); + for(int k=0; k<2; ++k) VERIFY(m(k) == raw[k]); + for(int k=0; k<2; ++k) VERIFY(a(k) == raw[k]); + VERIFY_IS_EQUAL(m,(Matrix(raw[0],raw[1]))); + VERIFY((a==Array(raw[0],raw[1])).all()); + for(int k=0; k<2; ++k) VERIFY(m2(k) == DenseIndex(raw[k])); + for(int k=0; k<2; ++k) VERIFY(a2(k) == DenseIndex(raw[k])); + for(int k=0; k<2; ++k) VERIFY(m3(k) == int(raw[k])); + for(int k=0; k<2; ++k) VERIFY((m4(k)) == Scalar(float(raw[k]))); + } + { + Matrix m(raw), m1(raw[0]), m2( (DenseIndex(raw[0])) ), m3( (int(raw[0])) ); + Array a(raw), a1(raw[0]), a2( (DenseIndex(raw[0])) ); + VERIFY(m(0) == raw[0]); + VERIFY(a(0) == raw[0]); + VERIFY(m1(0) == raw[0]); + VERIFY(a1(0) == raw[0]); + VERIFY(m2(0) == DenseIndex(raw[0])); + VERIFY(a2(0) == DenseIndex(raw[0])); + VERIFY(m3(0) == int(raw[0])); + VERIFY_IS_EQUAL(m,(Matrix(raw[0]))); + VERIFY((a==Array(raw[0])).all()); + } } void test_basicstuff() @@ -207,8 +270,11 @@ void test_basicstuff() } CALL_SUBTEST_1(fixedSizeMatrixConstruction()); + CALL_SUBTEST_1(fixedSizeMatrixConstruction()); CALL_SUBTEST_1(fixedSizeMatrixConstruction()); - CALL_SUBTEST_1(fixedSizeMatrixConstruction()); + CALL_SUBTEST_1(fixedSizeMatrixConstruction()); + CALL_SUBTEST_1(fixedSizeMatrixConstruction()); + CALL_SUBTEST_1(fixedSizeMatrixConstruction()); CALL_SUBTEST_2(casting()); } diff --git a/ground/gcs/src/libs/eigen/test/bdcsvd.cpp b/ground/gcs/src/libs/eigen/test/bdcsvd.cpp new file mode 100644 index 0000000000..f9f687aacd --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/bdcsvd.cpp @@ -0,0 +1,111 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2013 Gauthier Brun +// Copyright (C) 2013 Nicolas Carre +// Copyright (C) 2013 Jean Ceccato +// Copyright (C) 2013 Pierre Zoppitelli +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/ + +// discard stack allocation as that too bypasses malloc +#define EIGEN_STACK_ALLOCATION_LIMIT 0 +#define EIGEN_RUNTIME_NO_MALLOC + +#include "main.h" +#include +#include +#include + + +#define SVD_DEFAULT(M) BDCSVD +#define SVD_FOR_MIN_NORM(M) BDCSVD +#include "svd_common.h" + +// Check all variants of JacobiSVD +template +void bdcsvd(const MatrixType& a = MatrixType(), bool pickrandom = true) +{ + MatrixType m = a; + if(pickrandom) + svd_fill_random(m); + + CALL_SUBTEST(( svd_test_all_computation_options >(m, false) )); +} + +template +void bdcsvd_method() +{ + enum { Size = MatrixType::RowsAtCompileTime }; + typedef typename MatrixType::RealScalar RealScalar; + typedef Matrix RealVecType; + MatrixType m = MatrixType::Identity(); + VERIFY_IS_APPROX(m.bdcSvd().singularValues(), RealVecType::Ones()); + VERIFY_RAISES_ASSERT(m.bdcSvd().matrixU()); + VERIFY_RAISES_ASSERT(m.bdcSvd().matrixV()); + VERIFY_IS_APPROX(m.bdcSvd(ComputeFullU|ComputeFullV).solve(m), m); +} + +// compare the Singular values returned with Jacobi and Bdc +template +void compare_bdc_jacobi(const MatrixType& a = MatrixType(), unsigned int computationOptions = 0) +{ + MatrixType m = MatrixType::Random(a.rows(), a.cols()); + BDCSVD bdc_svd(m); + JacobiSVD jacobi_svd(m); + VERIFY_IS_APPROX(bdc_svd.singularValues(), jacobi_svd.singularValues()); + if(computationOptions & ComputeFullU) VERIFY_IS_APPROX(bdc_svd.matrixU(), jacobi_svd.matrixU()); + if(computationOptions & ComputeThinU) VERIFY_IS_APPROX(bdc_svd.matrixU(), jacobi_svd.matrixU()); + if(computationOptions & ComputeFullV) VERIFY_IS_APPROX(bdc_svd.matrixV(), jacobi_svd.matrixV()); + if(computationOptions & ComputeThinV) VERIFY_IS_APPROX(bdc_svd.matrixV(), jacobi_svd.matrixV()); +} + +void test_bdcsvd() +{ + CALL_SUBTEST_3(( svd_verify_assert >(Matrix3f()) )); + CALL_SUBTEST_4(( svd_verify_assert >(Matrix4d()) )); + CALL_SUBTEST_7(( svd_verify_assert >(MatrixXf(10,12)) )); + CALL_SUBTEST_8(( svd_verify_assert >(MatrixXcd(7,5)) )); + + CALL_SUBTEST_101(( svd_all_trivial_2x2(bdcsvd) )); + CALL_SUBTEST_102(( svd_all_trivial_2x2(bdcsvd) )); + + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_3(( bdcsvd() )); + CALL_SUBTEST_4(( bdcsvd() )); + CALL_SUBTEST_5(( bdcsvd >() )); + + int r = internal::random(1, EIGEN_TEST_MAX_SIZE/2), + c = internal::random(1, EIGEN_TEST_MAX_SIZE/2); + + TEST_SET_BUT_UNUSED_VARIABLE(r) + TEST_SET_BUT_UNUSED_VARIABLE(c) + + CALL_SUBTEST_6(( bdcsvd(Matrix(r,2)) )); + CALL_SUBTEST_7(( bdcsvd(MatrixXf(r,c)) )); + CALL_SUBTEST_7(( compare_bdc_jacobi(MatrixXf(r,c)) )); + CALL_SUBTEST_10(( bdcsvd(MatrixXd(r,c)) )); + CALL_SUBTEST_10(( compare_bdc_jacobi(MatrixXd(r,c)) )); + CALL_SUBTEST_8(( bdcsvd(MatrixXcd(r,c)) )); + CALL_SUBTEST_8(( compare_bdc_jacobi(MatrixXcd(r,c)) )); + + // Test on inf/nan matrix + CALL_SUBTEST_7( (svd_inf_nan, MatrixXf>()) ); + CALL_SUBTEST_10( (svd_inf_nan, MatrixXd>()) ); + } + + // test matrixbase method + CALL_SUBTEST_1(( bdcsvd_method() )); + CALL_SUBTEST_3(( bdcsvd_method() )); + + // Test problem size constructors + CALL_SUBTEST_7( BDCSVD(10,10) ); + + // Check that preallocation avoids subsequent mallocs + CALL_SUBTEST_9( svd_preallocate() ); + + CALL_SUBTEST_2( svd_underoverflow() ); +} + diff --git a/ground/gcs/src/libs/eigen/test/bicgstab.cpp b/ground/gcs/src/libs/eigen/test/bicgstab.cpp index f327e2facf..4cc0dd31cf 100644 --- a/ground/gcs/src/libs/eigen/test/bicgstab.cpp +++ b/ground/gcs/src/libs/eigen/test/bicgstab.cpp @@ -10,13 +10,16 @@ #include "sparse_solver.h" #include -template void test_bicgstab_T() +template void test_bicgstab_T() { - BiCGSTAB, DiagonalPreconditioner > bicgstab_colmajor_diag; - BiCGSTAB, IdentityPreconditioner > bicgstab_colmajor_I; - BiCGSTAB, IncompleteLUT > bicgstab_colmajor_ilut; + BiCGSTAB, DiagonalPreconditioner > bicgstab_colmajor_diag; + BiCGSTAB, IdentityPreconditioner > bicgstab_colmajor_I; + BiCGSTAB, IncompleteLUT > bicgstab_colmajor_ilut; //BiCGSTAB, SSORPreconditioner > bicgstab_colmajor_ssor; + bicgstab_colmajor_diag.setTolerance(NumTraits::epsilon()*4); + bicgstab_colmajor_ilut.setTolerance(NumTraits::epsilon()*4); + CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_diag) ); // CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_I) ); CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_ilut) ); @@ -25,6 +28,7 @@ template void test_bicgstab_T() void test_bicgstab() { - CALL_SUBTEST_1(test_bicgstab_T()); - CALL_SUBTEST_2(test_bicgstab_T >()); + CALL_SUBTEST_1((test_bicgstab_T()) ); + CALL_SUBTEST_2((test_bicgstab_T, int>())); + CALL_SUBTEST_3((test_bicgstab_T())); } diff --git a/ground/gcs/src/libs/eigen/test/block.cpp b/ground/gcs/src/libs/eigen/test/block.cpp index 9ed5d7bc5e..39565af831 100644 --- a/ground/gcs/src/libs/eigen/test/block.cpp +++ b/ground/gcs/src/libs/eigen/test/block.cpp @@ -130,6 +130,14 @@ template void block(const MatrixType& m) VERIFY(numext::real(ones.col(c1).dot(ones.col(c2))) == RealScalar(rows)); VERIFY(numext::real(ones.row(r1).dot(ones.row(r2))) == RealScalar(cols)); + + // chekc that linear acccessors works on blocks + m1 = m1_copy; + if((MatrixType::Flags&RowMajorBit)==0) + VERIFY_IS_EQUAL(m1.leftCols(c1).coeff(r1+c1*rows), m1(r1,c1)); + else + VERIFY_IS_EQUAL(m1.topRows(r1).coeff(c1+r1*cols), m1(r1,c1)); + // now test some block-inside-of-block. @@ -141,11 +149,11 @@ template void block(const MatrixType& m) VERIFY_IS_EQUAL( (m1.transpose().block(c1,r1,c2-c1+1,r2-r1+1).col(0)) , (m1.row(r1).segment(c1,c2-c1+1)).transpose() ); // expressions without direct access - VERIFY_IS_EQUAL( ((m1+m2).block(r1,c1,rows-r1,cols-c1).block(r2-r1,c2-c1,rows-r2,cols-c2)) , ((m1+m2).block(r2,c2,rows-r2,cols-c2)) ); - VERIFY_IS_EQUAL( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).row(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)) ); - VERIFY_IS_EQUAL( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).col(0)) , ((m1+m2).col(c1).segment(r1,r2-r1+1)) ); - VERIFY_IS_EQUAL( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).transpose().col(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)).transpose() ); - VERIFY_IS_EQUAL( ((m1+m2).transpose().block(c1,r1,c2-c1+1,r2-r1+1).col(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)).transpose() ); + VERIFY_IS_APPROX( ((m1+m2).block(r1,c1,rows-r1,cols-c1).block(r2-r1,c2-c1,rows-r2,cols-c2)) , ((m1+m2).block(r2,c2,rows-r2,cols-c2)) ); + VERIFY_IS_APPROX( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).row(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)) ); + VERIFY_IS_APPROX( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).col(0)) , ((m1+m2).col(c1).segment(r1,r2-r1+1)) ); + VERIFY_IS_APPROX( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).transpose().col(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)).transpose() ); + VERIFY_IS_APPROX( ((m1+m2).transpose().block(c1,r1,c2-c1+1,r2-r1+1).col(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)).transpose() ); // evaluation into plain matrices from expressions with direct access (stress MapBase) DynamicMatrixType dm; @@ -173,6 +181,19 @@ template void block(const MatrixType& m) dm = m1.row(r1).segment(c1,c2-c1+1).transpose(); dv = m1.transpose().block(c1,r1,c2-c1+1,r2-r1+1).col(0); VERIFY_IS_EQUAL(dv, dm); + + VERIFY_IS_EQUAL( (m1.template block(1,0,0,1)), m1.block(1,0,0,1)); + VERIFY_IS_EQUAL( (m1.template block<1,Dynamic>(0,1,1,0)), m1.block(0,1,1,0)); + VERIFY_IS_EQUAL( ((m1*1).template block(1,0,0,1)), m1.block(1,0,0,1)); + VERIFY_IS_EQUAL( ((m1*1).template block<1,Dynamic>(0,1,1,0)), m1.block(0,1,1,0)); + + if (rows>=2 && cols>=2) + { + VERIFY_RAISES_ASSERT( m1 += m1.col(0) ); + VERIFY_RAISES_ASSERT( m1 -= m1.col(0) ); + VERIFY_RAISES_ASSERT( m1.array() *= m1.col(0).array() ); + VERIFY_RAISES_ASSERT( m1.array() /= m1.col(0).array() ); + } } diff --git a/ground/gcs/src/libs/eigen/test/boostmultiprec.cpp b/ground/gcs/src/libs/eigen/test/boostmultiprec.cpp new file mode 100644 index 0000000000..e06e9bdaf6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/boostmultiprec.cpp @@ -0,0 +1,201 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include + +#ifdef EIGEN_TEST_MAX_SIZE +#undef EIGEN_TEST_MAX_SIZE +#endif + +#define EIGEN_TEST_MAX_SIZE 50 + +#ifdef EIGEN_TEST_PART_1 +#include "cholesky.cpp" +#endif + +#ifdef EIGEN_TEST_PART_2 +#include "lu.cpp" +#endif + +#ifdef EIGEN_TEST_PART_3 +#include "qr.cpp" +#endif + +#ifdef EIGEN_TEST_PART_4 +#include "qr_colpivoting.cpp" +#endif + +#ifdef EIGEN_TEST_PART_5 +#include "qr_fullpivoting.cpp" +#endif + +#ifdef EIGEN_TEST_PART_6 +#include "eigensolver_selfadjoint.cpp" +#endif + +#ifdef EIGEN_TEST_PART_7 +#include "eigensolver_generic.cpp" +#endif + +#ifdef EIGEN_TEST_PART_8 +#include "eigensolver_generalized_real.cpp" +#endif + +#ifdef EIGEN_TEST_PART_9 +#include "jacobisvd.cpp" +#endif + +#ifdef EIGEN_TEST_PART_10 +#include "bdcsvd.cpp" +#endif + +#include + +#undef min +#undef max +#undef isnan +#undef isinf +#undef isfinite + +#include +#include +#include +#include + +namespace mp = boost::multiprecision; +typedef mp::number, mp::et_on> Real; + +namespace Eigen { + template<> struct NumTraits : GenericNumTraits { + static inline Real dummy_precision() { return 1e-50; } + }; + + template + struct NumTraits > : NumTraits {}; + + template<> + Real test_precision() { return 1e-50; } + + // needed in C++93 mode where number does not support explicit cast. + namespace internal { + template + struct cast_impl { + static inline NewType run(const Real& x) { + return x.template convert_to(); + } + }; + + template<> + struct cast_impl > { + static inline std::complex run(const Real& x) { + return std::complex(x); + } + }; + } +} + +namespace boost { +namespace multiprecision { + // to make ADL works as expected: + using boost::math::isfinite; + using boost::math::isnan; + using boost::math::isinf; + using boost::math::copysign; + using boost::math::hypot; + + // The following is needed for std::complex: + Real fabs(const Real& a) { return abs EIGEN_NOT_A_MACRO (a); } + Real fmax(const Real& a, const Real& b) { using std::max; return max(a,b); } + + // some specialization for the unit tests: + inline bool test_isMuchSmallerThan(const Real& a, const Real& b) { + return internal::isMuchSmallerThan(a, b, test_precision()); + } + + inline bool test_isApprox(const Real& a, const Real& b) { + return internal::isApprox(a, b, test_precision()); + } + + inline bool test_isApproxOrLessThan(const Real& a, const Real& b) { + return internal::isApproxOrLessThan(a, b, test_precision()); + } + + Real get_test_precision(const Real&) { + return test_precision(); + } + + Real test_relative_error(const Real &a, const Real &b) { + using Eigen::numext::abs2; + return sqrt(abs2(a-b)/Eigen::numext::mini(abs2(a),abs2(b))); + } +} +} + +namespace Eigen { + +} + +void test_boostmultiprec() +{ + typedef Matrix Mat; + typedef Matrix,Dynamic,Dynamic> MatC; + + std::cout << "NumTraits::epsilon() = " << NumTraits::epsilon() << std::endl; + std::cout << "NumTraits::dummy_precision() = " << NumTraits::dummy_precision() << std::endl; + std::cout << "NumTraits::lowest() = " << NumTraits::lowest() << std::endl; + std::cout << "NumTraits::highest() = " << NumTraits::highest() << std::endl; + std::cout << "NumTraits::digits10() = " << NumTraits::digits10() << std::endl; + + // chekc stream output + { + Mat A(10,10); + A.setRandom(); + std::stringstream ss; + ss << A; + } + { + MatC A(10,10); + A.setRandom(); + std::stringstream ss; + ss << A; + } + + for(int i = 0; i < g_repeat; i++) { + int s = internal::random(1,EIGEN_TEST_MAX_SIZE); + + CALL_SUBTEST_1( cholesky(Mat(s,s)) ); + + CALL_SUBTEST_2( lu_non_invertible() ); + CALL_SUBTEST_2( lu_invertible() ); + CALL_SUBTEST_2( lu_non_invertible() ); + CALL_SUBTEST_2( lu_invertible() ); + + CALL_SUBTEST_3( qr(Mat(internal::random(1,EIGEN_TEST_MAX_SIZE),internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_3( qr_invertible() ); + + CALL_SUBTEST_4( qr() ); + CALL_SUBTEST_4( cod() ); + CALL_SUBTEST_4( qr_invertible() ); + + CALL_SUBTEST_5( qr() ); + CALL_SUBTEST_5( qr_invertible() ); + + CALL_SUBTEST_6( selfadjointeigensolver(Mat(s,s)) ); + + CALL_SUBTEST_7( eigensolver(Mat(s,s)) ); + + CALL_SUBTEST_8( generalized_eigensolver_real(Mat(s,s)) ); + + TEST_SET_BUT_UNUSED_VARIABLE(s) + } + + CALL_SUBTEST_9(( jacobisvd(Mat(internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE), internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) )); + CALL_SUBTEST_10(( bdcsvd(Mat(internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE), internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) )); +} + diff --git a/ground/gcs/src/libs/eigen/test/bug1213.cpp b/ground/gcs/src/libs/eigen/test/bug1213.cpp new file mode 100644 index 0000000000..581760c1a6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/bug1213.cpp @@ -0,0 +1,13 @@ + +// This anonymous enum is essential to trigger the linking issue +enum { + Foo +}; + +#include "bug1213.h" + +bool bug1213_1(const Eigen::Vector3f& x) +{ + return bug1213_2(x); +} + diff --git a/ground/gcs/src/libs/eigen/test/bug1213.h b/ground/gcs/src/libs/eigen/test/bug1213.h new file mode 100644 index 0000000000..040e5a470d --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/bug1213.h @@ -0,0 +1,8 @@ + +#include + +template +bool bug1213_2(const Eigen::Matrix& x); + +bool bug1213_1(const Eigen::Vector3f& x); + diff --git a/ground/gcs/src/libs/eigen/test/bug1213_main.cpp b/ground/gcs/src/libs/eigen/test/bug1213_main.cpp new file mode 100644 index 0000000000..4802c00032 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/bug1213_main.cpp @@ -0,0 +1,18 @@ + +// This is a regression unit regarding a weird linking issue with gcc. + +#include "bug1213.h" + +int main() +{ + return 0; +} + + +template +bool bug1213_2(const Eigen::Matrix& ) +{ + return true; +} + +template bool bug1213_2(const Eigen::Vector3f&); diff --git a/ground/gcs/src/libs/eigen/test/cholesky.cpp b/ground/gcs/src/libs/eigen/test/cholesky.cpp index 56885deb75..8ad5ac6397 100644 --- a/ground/gcs/src/libs/eigen/test/cholesky.cpp +++ b/ground/gcs/src/libs/eigen/test/cholesky.cpp @@ -11,20 +11,17 @@ #define EIGEN_NO_ASSERTION_CHECKING #endif -static int nb_temporaries; - -#define EIGEN_DENSE_STORAGE_CTOR_PLUGIN { if(size!=0) nb_temporaries++; } +#define TEST_ENABLE_TEMPORARY_TRACKING #include "main.h" #include #include -#define VERIFY_EVALUATION_COUNT(XPR,N) {\ - nb_temporaries = 0; \ - XPR; \ - if(nb_temporaries!=N) std::cerr << "nb_temporaries == " << nb_temporaries << "\n"; \ - VERIFY( (#XPR) && nb_temporaries==N ); \ - } +template +typename MatrixType::RealScalar matrix_l1_norm(const MatrixType& m) { + MatrixType symm = m.template selfadjointView(); + return symm.cwiseAbs().colwise().sum().maxCoeff(); +} template class CholType> void test_chol_update(const MatrixType& symm) { @@ -83,14 +80,10 @@ template void cholesky(const MatrixType& m) symm += a1 * a1.adjoint(); } - // to test if really Cholesky only uses the upper triangular part, uncomment the following - // FIXME: currently that fails !! - //symm.template part().setZero(); - { SquareMatrixType symmUp = symm.template triangularView(); SquareMatrixType symmLo = symm.template triangularView(); - + LLT chollo(symmLo); VERIFY_IS_APPROX(symm, chollo.reconstructedMatrix()); vecX = chollo.solve(vecB); @@ -98,6 +91,14 @@ template void cholesky(const MatrixType& m) matX = chollo.solve(matB); VERIFY_IS_APPROX(symm * matX, matB); + const MatrixType symmLo_inverse = chollo.solve(MatrixType::Identity(rows,cols)); + RealScalar rcond = (RealScalar(1) / matrix_l1_norm(symmLo)) / + matrix_l1_norm(symmLo_inverse); + RealScalar rcond_est = chollo.rcond(); + // Verify that the estimated condition number is within a factor of 10 of the + // truth. + VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10); + // test the upper mode LLT cholup(symmUp); VERIFY_IS_APPROX(symm, cholup.reconstructedMatrix()); @@ -106,6 +107,15 @@ template void cholesky(const MatrixType& m) matX = cholup.solve(matB); VERIFY_IS_APPROX(symm * matX, matB); + // Verify that the estimated condition number is within a factor of 10 of the + // truth. + const MatrixType symmUp_inverse = cholup.solve(MatrixType::Identity(rows,cols)); + rcond = (RealScalar(1) / matrix_l1_norm(symmUp)) / + matrix_l1_norm(symmUp_inverse); + rcond_est = cholup.rcond(); + VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10); + + MatrixType neg = -symmLo; chollo.compute(neg); VERIFY(chollo.info()==NumericalIssue); @@ -114,7 +124,7 @@ template void cholesky(const MatrixType& m) VERIFY_IS_APPROX(MatrixType(chollo.matrixU().transpose().conjugate()), MatrixType(chollo.matrixL())); VERIFY_IS_APPROX(MatrixType(cholup.matrixL().transpose().conjugate()), MatrixType(cholup.matrixU())); VERIFY_IS_APPROX(MatrixType(cholup.matrixU().transpose().conjugate()), MatrixType(cholup.matrixL())); - + // test some special use cases of SelfCwiseBinaryOp: MatrixType m1 = MatrixType::Random(rows,cols), m2(rows,cols); m2 = m1; @@ -144,19 +154,38 @@ template void cholesky(const MatrixType& m) SquareMatrixType symmLo = symm.template triangularView(); LDLT ldltlo(symmLo); + VERIFY(ldltlo.info()==Success); VERIFY_IS_APPROX(symm, ldltlo.reconstructedMatrix()); vecX = ldltlo.solve(vecB); VERIFY_IS_APPROX(symm * vecX, vecB); matX = ldltlo.solve(matB); VERIFY_IS_APPROX(symm * matX, matB); + const MatrixType symmLo_inverse = ldltlo.solve(MatrixType::Identity(rows,cols)); + RealScalar rcond = (RealScalar(1) / matrix_l1_norm(symmLo)) / + matrix_l1_norm(symmLo_inverse); + RealScalar rcond_est = ldltlo.rcond(); + // Verify that the estimated condition number is within a factor of 10 of the + // truth. + VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10); + + LDLT ldltup(symmUp); + VERIFY(ldltup.info()==Success); VERIFY_IS_APPROX(symm, ldltup.reconstructedMatrix()); vecX = ldltup.solve(vecB); VERIFY_IS_APPROX(symm * vecX, vecB); matX = ldltup.solve(matB); VERIFY_IS_APPROX(symm * matX, matB); + // Verify that the estimated condition number is within a factor of 10 of the + // truth. + const MatrixType symmUp_inverse = ldltup.solve(MatrixType::Identity(rows,cols)); + rcond = (RealScalar(1) / matrix_l1_norm(symmUp)) / + matrix_l1_norm(symmUp_inverse); + rcond_est = ldltup.rcond(); + VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10); + VERIFY_IS_APPROX(MatrixType(ldltlo.matrixL().transpose().conjugate()), MatrixType(ldltlo.matrixU())); VERIFY_IS_APPROX(MatrixType(ldltlo.matrixU().transpose().conjugate()), MatrixType(ldltlo.matrixL())); VERIFY_IS_APPROX(MatrixType(ldltup.matrixL().transpose().conjugate()), MatrixType(ldltup.matrixU())); @@ -185,7 +214,7 @@ template void cholesky(const MatrixType& m) if(rows>=3) { SquareMatrixType A = symm; - int c = internal::random(0,rows-2); + Index c = internal::random(0,rows-2); A.bottomRightCorner(c,c).setZero(); // Make sure a solution exists: vecX.setRandom(); @@ -196,11 +225,11 @@ template void cholesky(const MatrixType& m) vecX = ldltlo.solve(vecB); VERIFY_IS_APPROX(A * vecX, vecB); } - + // check non-full rank matrices if(rows>=3) { - int r = internal::random(1,rows-1); + Index r = internal::random(1,rows-1); Matrix a = Matrix::Random(rows,r); SquareMatrixType A = a * a.adjoint(); // Make sure a solution exists: @@ -212,15 +241,17 @@ template void cholesky(const MatrixType& m) vecX = ldltlo.solve(vecB); VERIFY_IS_APPROX(A * vecX, vecB); } - + // check matrices with a wide spectrum if(rows>=3) { + using std::pow; + using std::sqrt; RealScalar s = (std::min)(16,std::numeric_limits::max_exponent10/8); Matrix a = Matrix::Random(rows,rows); Matrix d = Matrix::Random(rows); - for(int k=0; k(-s,s)); + for(Index k=0; k(-s,s)); SquareMatrixType A = a * d.asDiagonal() * a.adjoint(); // Make sure a solution exists: vecX.setRandom(); @@ -229,7 +260,20 @@ template void cholesky(const MatrixType& m) ldltlo.compute(A); VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix()); vecX = ldltlo.solve(vecB); - VERIFY_IS_APPROX(A * vecX, vecB); + + if(ldltlo.vectorD().real().cwiseAbs().minCoeff()>RealScalar(0)) + { + VERIFY_IS_APPROX(A * vecX,vecB); + } + else + { + RealScalar large_tol = sqrt(test_precision()); + VERIFY((A * vecX).isApprox(vecB, large_tol)); + + ++g_test_level; + VERIFY_IS_APPROX(A * vecX,vecB); + --g_test_level; + } } } @@ -289,6 +333,7 @@ template void cholesky_cplx(const MatrixType& m) RealMatrixType symmLo = symm.template triangularView(); LDLT ldltlo(symmLo); + VERIFY(ldltlo.info()==Success); VERIFY_IS_APPROX(symm, ldltlo.reconstructedMatrix()); vecX = ldltlo.solve(vecB); VERIFY_IS_APPROX(symm * vecX, vecB); @@ -314,46 +359,101 @@ template void cholesky_bug241(const MatrixType& m) } // LDLT is not guaranteed to work for indefinite matrices, but happens to work fine if matrix is diagonal. -// This test checks that LDLT reports correctly that matrix is indefinite. +// This test checks that LDLT reports correctly that matrix is indefinite. // See http://forum.kde.org/viewtopic.php?f=74&t=106942 and bug 736 template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; LDLT ldlt(2); - + { mat << 1, 0, 0, -1; ldlt.compute(mat); + VERIFY(ldlt.info()==Success); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 1, 2, 2, 1; ldlt.compute(mat); + VERIFY(ldlt.info()==Success); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 0, 0, 0, 0; ldlt.compute(mat); + VERIFY(ldlt.info()==Success); VERIFY(ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << 0, 0, 0, 1; ldlt.compute(mat); + VERIFY(ldlt.info()==Success); VERIFY(!ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << -1, 0, 0, 0; ldlt.compute(mat); + VERIFY(ldlt.info()==Success); VERIFY(ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } } +template +void cholesky_faillure_cases() +{ + MatrixXd mat; + LDLT ldlt; + + { + mat.resize(2,2); + mat << 0, 1, 1, 0; + ldlt.compute(mat); + VERIFY_IS_NOT_APPROX(mat,ldlt.reconstructedMatrix()); + VERIFY(ldlt.info()==NumericalIssue); + } +#if (!EIGEN_ARCH_i386) || defined(EIGEN_VECTORIZE_SSE2) + { + mat.resize(3,3); + mat << -1, -3, 3, + -3, -8.9999999999999999999, 1, + 3, 1, 0; + ldlt.compute(mat); + VERIFY(ldlt.info()==NumericalIssue); + VERIFY_IS_NOT_APPROX(mat,ldlt.reconstructedMatrix()); + } +#endif + { + mat.resize(3,3); + mat << 1, 2, 3, + 2, 4, 1, + 3, 1, 0; + ldlt.compute(mat); + VERIFY(ldlt.info()==NumericalIssue); + VERIFY_IS_NOT_APPROX(mat,ldlt.reconstructedMatrix()); + } + + { + mat.resize(8,8); + mat << 0.1, 0, -0.1, 0, 0, 0, 1, 0, + 0, 4.24667, 0, 2.00333, 0, 0, 0, 0, + -0.1, 0, 0.2, 0, -0.1, 0, 0, 0, + 0, 2.00333, 0, 8.49333, 0, 2.00333, 0, 0, + 0, 0, -0.1, 0, 0.1, 0, 0, 1, + 0, 0, 0, 2.00333, 0, 4.24667, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0; + ldlt.compute(mat); + VERIFY(ldlt.info()==NumericalIssue); + VERIFY_IS_NOT_APPROX(mat,ldlt.reconstructedMatrix()); + } +} + template void cholesky_verify_assert() { MatrixType tmp; @@ -384,10 +484,14 @@ void test_cholesky() CALL_SUBTEST_3( cholesky_definiteness(Matrix2d()) ); CALL_SUBTEST_4( cholesky(Matrix3f()) ); CALL_SUBTEST_5( cholesky(Matrix4d()) ); + s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_2( cholesky(MatrixXd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_6( cholesky_cplx(MatrixXcd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } CALL_SUBTEST_4( cholesky_verify_assert() ); @@ -398,7 +502,8 @@ void test_cholesky() // Test problem size constructors CALL_SUBTEST_9( LLT(10) ); CALL_SUBTEST_9( LDLT(10) ); - - TEST_SET_BUT_UNUSED_VARIABLE(s) + + CALL_SUBTEST_2( cholesky_faillure_cases() ); + TEST_SET_BUT_UNUSED_VARIABLE(nb_temporaries) } diff --git a/ground/gcs/src/libs/eigen/test/cholmod_support.cpp b/ground/gcs/src/libs/eigen/test/cholmod_support.cpp index 8f8be3c0e0..a7eda28f79 100644 --- a/ground/gcs/src/libs/eigen/test/cholmod_support.cpp +++ b/ground/gcs/src/libs/eigen/test/cholmod_support.cpp @@ -7,6 +7,7 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS #include "sparse_solver.h" #include @@ -40,13 +41,13 @@ template void test_cholmod_T() check_sparse_spd_solving(llt_colmajor_upper); check_sparse_spd_solving(ldlt_colmajor_lower); check_sparse_spd_solving(ldlt_colmajor_upper); - -// check_sparse_spd_determinant(chol_colmajor_lower); -// check_sparse_spd_determinant(chol_colmajor_upper); -// check_sparse_spd_determinant(llt_colmajor_lower); -// check_sparse_spd_determinant(llt_colmajor_upper); -// check_sparse_spd_determinant(ldlt_colmajor_lower); -// check_sparse_spd_determinant(ldlt_colmajor_upper); + + check_sparse_spd_determinant(chol_colmajor_lower); + check_sparse_spd_determinant(chol_colmajor_upper); + check_sparse_spd_determinant(llt_colmajor_lower); + check_sparse_spd_determinant(llt_colmajor_upper); + check_sparse_spd_determinant(ldlt_colmajor_lower); + check_sparse_spd_determinant(ldlt_colmajor_upper); } void test_cholmod_support() diff --git a/ground/gcs/src/libs/eigen/test/commainitializer.cpp b/ground/gcs/src/libs/eigen/test/commainitializer.cpp index 99102b966b..9844adbd22 100644 --- a/ground/gcs/src/libs/eigen/test/commainitializer.cpp +++ b/ground/gcs/src/libs/eigen/test/commainitializer.cpp @@ -9,6 +9,62 @@ #include "main.h" + +template +void test_blocks() +{ + Matrix m_fixed; + MatrixXi m_dynamic(M1+M2, N1+N2); + + Matrix mat11; mat11.setRandom(); + Matrix mat12; mat12.setRandom(); + Matrix mat21; mat21.setRandom(); + Matrix mat22; mat22.setRandom(); + + MatrixXi matx11 = mat11, matx12 = mat12, matx21 = mat21, matx22 = mat22; + + { + VERIFY_IS_EQUAL((m_fixed << mat11, mat12, mat21, matx22).finished(), (m_dynamic << mat11, matx12, mat21, matx22).finished()); + VERIFY_IS_EQUAL((m_fixed.template topLeftCorner()), mat11); + VERIFY_IS_EQUAL((m_fixed.template topRightCorner()), mat12); + VERIFY_IS_EQUAL((m_fixed.template bottomLeftCorner()), mat21); + VERIFY_IS_EQUAL((m_fixed.template bottomRightCorner()), mat22); + VERIFY_IS_EQUAL((m_fixed << mat12, mat11, matx21, mat22).finished(), (m_dynamic << mat12, matx11, matx21, mat22).finished()); + } + + if(N1 > 0) + { + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat12, mat11, mat21, mat22)); + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat12, mat21, mat21, mat22)); + } + else + { + // allow insertion of zero-column blocks: + VERIFY_IS_EQUAL((m_fixed << mat11, mat12, mat11, mat11, mat21, mat21, mat22).finished(), (m_dynamic << mat12, mat22).finished()); + } + if(M1 != M2) + { + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat21, mat12, mat22)); + } +} + + +template +struct test_block_recursion +{ + static void run() + { + test_blocks<(N>>6)&3, (N>>4)&3, (N>>2)&3, N & 3>(); + test_block_recursion::run(); + } +}; + +template<> +struct test_block_recursion<-1> +{ + static void run() { } +}; + void test_commainitializer() { Matrix3d m3; @@ -43,4 +99,8 @@ void test_commainitializer() 4, 5, 6, vec[2].transpose(); VERIFY_IS_APPROX(m3, ref); + + + // recursively test all block-sizes from 0 to 3: + test_block_recursion<(1<<8) - 1>(); } diff --git a/ground/gcs/src/libs/eigen/test/conjugate_gradient.cpp b/ground/gcs/src/libs/eigen/test/conjugate_gradient.cpp index 019cc4d64f..9622fd86dd 100644 --- a/ground/gcs/src/libs/eigen/test/conjugate_gradient.cpp +++ b/ground/gcs/src/libs/eigen/test/conjugate_gradient.cpp @@ -10,13 +10,14 @@ #include "sparse_solver.h" #include -template void test_conjugate_gradient_T() +template void test_conjugate_gradient_T() { - ConjugateGradient, Lower > cg_colmajor_lower_diag; - ConjugateGradient, Upper > cg_colmajor_upper_diag; - ConjugateGradient, Lower|Upper> cg_colmajor_loup_diag; - ConjugateGradient, Lower, IdentityPreconditioner> cg_colmajor_lower_I; - ConjugateGradient, Upper, IdentityPreconditioner> cg_colmajor_upper_I; + typedef SparseMatrix SparseMatrixType; + ConjugateGradient cg_colmajor_lower_diag; + ConjugateGradient cg_colmajor_upper_diag; + ConjugateGradient cg_colmajor_loup_diag; + ConjugateGradient cg_colmajor_lower_I; + ConjugateGradient cg_colmajor_upper_I; CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_diag) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_diag) ); @@ -27,6 +28,7 @@ template void test_conjugate_gradient_T() void test_conjugate_gradient() { - CALL_SUBTEST_1(test_conjugate_gradient_T()); - CALL_SUBTEST_2(test_conjugate_gradient_T >()); + CALL_SUBTEST_1(( test_conjugate_gradient_T() )); + CALL_SUBTEST_2(( test_conjugate_gradient_T, int>() )); + CALL_SUBTEST_3(( test_conjugate_gradient_T() )); } diff --git a/ground/gcs/src/libs/eigen/test/constructor.cpp b/ground/gcs/src/libs/eigen/test/constructor.cpp new file mode 100644 index 0000000000..eec9e21929 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/constructor.cpp @@ -0,0 +1,84 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2017 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + + +#define TEST_ENABLE_TEMPORARY_TRACKING + +#include "main.h" + +template struct Wrapper +{ + MatrixType m_mat; + inline Wrapper(const MatrixType &x) : m_mat(x) {} + inline operator const MatrixType& () const { return m_mat; } + inline operator MatrixType& () { return m_mat; } +}; + +template void ctor_init1(const MatrixType& m) +{ + // Check logic in PlainObjectBase::_init1 + Index rows = m.rows(); + Index cols = m.cols(); + + MatrixType m0 = MatrixType::Random(rows,cols); + + VERIFY_EVALUATION_COUNT( MatrixType m1(m0), 1); + VERIFY_EVALUATION_COUNT( MatrixType m2(m0+m0), 1); + VERIFY_EVALUATION_COUNT( MatrixType m2(m0.block(0,0,rows,cols)) , 1); + + Wrapper wrapper(m0); + VERIFY_EVALUATION_COUNT( MatrixType m3(wrapper) , 1); +} + + +void test_constructor() +{ + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1( ctor_init1(Matrix()) ); + CALL_SUBTEST_1( ctor_init1(Matrix4d()) ); + CALL_SUBTEST_1( ctor_init1(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_1( ctor_init1(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + } + { + Matrix a(123); + VERIFY_IS_EQUAL(a[0], 123); + } + { + Matrix a(123.0); + VERIFY_IS_EQUAL(a[0], 123); + } + { + Matrix a(123); + VERIFY_IS_EQUAL(a[0], 123.f); + } + { + Array a(123); + VERIFY_IS_EQUAL(a[0], 123); + } + { + Array a(123.0); + VERIFY_IS_EQUAL(a[0], 123); + } + { + Array a(123); + VERIFY_IS_EQUAL(a[0], 123.f); + } + { + Array a(123); + VERIFY_IS_EQUAL(a(4), 123); + } + { + Array a(123.0); + VERIFY_IS_EQUAL(a(4), 123); + } + { + Array a(123); + VERIFY_IS_EQUAL(a(4), 123.f); + } +} diff --git a/ground/gcs/src/libs/eigen/test/ctorleak.cpp b/ground/gcs/src/libs/eigen/test/ctorleak.cpp new file mode 100644 index 0000000000..c158f5e4ee --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/ctorleak.cpp @@ -0,0 +1,69 @@ +#include "main.h" + +#include // std::exception + +struct Foo +{ + static Index object_count; + static Index object_limit; + int dummy; + + Foo() + { +#ifdef EIGEN_EXCEPTIONS + // TODO: Is this the correct way to handle this? + if (Foo::object_count > Foo::object_limit) { std::cout << "\nThrow!\n"; throw Foo::Fail(); } +#endif + std::cout << '+'; + ++Foo::object_count; + } + + ~Foo() + { + std::cout << '-'; + --Foo::object_count; + } + + class Fail : public std::exception {}; +}; + +Index Foo::object_count = 0; +Index Foo::object_limit = 0; + +#undef EIGEN_TEST_MAX_SIZE +#define EIGEN_TEST_MAX_SIZE 3 + +void test_ctorleak() +{ + typedef Matrix MatrixX; + typedef Matrix VectorX; + Foo::object_count = 0; + for(int i = 0; i < g_repeat; i++) { + Index rows = internal::random(2,EIGEN_TEST_MAX_SIZE), cols = internal::random(2,EIGEN_TEST_MAX_SIZE); + Foo::object_limit = internal::random(0, rows*cols - 2); + std::cout << "object_limit =" << Foo::object_limit << std::endl; +#ifdef EIGEN_EXCEPTIONS + try + { +#endif + std::cout << "\nMatrixX m(" << rows << ", " << cols << ");\n"; + MatrixX m(rows, cols); +#ifdef EIGEN_EXCEPTIONS + VERIFY(false); // not reached if exceptions are enabled + } + catch (const Foo::Fail&) { /* ignore */ } +#endif + VERIFY_IS_EQUAL(Index(0), Foo::object_count); + + { + Foo::object_limit = (rows+1)*(cols+1); + MatrixX A(rows, cols); + VERIFY_IS_EQUAL(Foo::object_count, rows*cols); + VectorX v=A.row(0); + VERIFY_IS_EQUAL(Foo::object_count, (rows+1)*cols); + v = A.col(0); + VERIFY_IS_EQUAL(Foo::object_count, rows*(cols+1)); + } + VERIFY_IS_EQUAL(Index(0), Foo::object_count); + } +} diff --git a/ground/gcs/src/libs/eigen/test/cuda_basic.cu b/ground/gcs/src/libs/eigen/test/cuda_basic.cu new file mode 100644 index 0000000000..cb2e4167a4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/cuda_basic.cu @@ -0,0 +1,173 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015-2016 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +// workaround issue between gcc >= 4.7 and cuda 5.5 +#if (defined __GNUC__) && (__GNUC__>4 || __GNUC_MINOR__>=7) + #undef _GLIBCXX_ATOMIC_BUILTINS + #undef _GLIBCXX_USE_INT128 +#endif + +#define EIGEN_TEST_NO_LONGDOUBLE +#define EIGEN_TEST_NO_COMPLEX +#define EIGEN_TEST_FUNC cuda_basic +#define EIGEN_DEFAULT_DENSE_INDEX_TYPE int + +#include +#include +#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500 +#include +#endif +#include "main.h" +#include "cuda_common.h" + +// Check that dense modules can be properly parsed by nvcc +#include + +// struct Foo{ +// EIGEN_DEVICE_FUNC +// void operator()(int i, const float* mats, float* vecs) const { +// using namespace Eigen; +// // Matrix3f M(data); +// // Vector3f x(data+9); +// // Map(data+9) = M.inverse() * x; +// Matrix3f M(mats+i/16); +// Vector3f x(vecs+i*3); +// // using std::min; +// // using std::sqrt; +// Map(vecs+i*3) << x.minCoeff(), 1, 2;// / x.dot(x);//(M.inverse() * x) / x.x(); +// //x = x*2 + x.y() * x + x * x.maxCoeff() - x / x.sum(); +// } +// }; + +template +struct coeff_wise { + EIGEN_DEVICE_FUNC + void operator()(int i, const typename T::Scalar* in, typename T::Scalar* out) const + { + using namespace Eigen; + T x1(in+i); + T x2(in+i+1); + T x3(in+i+2); + Map res(out+i*T::MaxSizeAtCompileTime); + + res.array() += (in[0] * x1 + x2).array() * x3.array(); + } +}; + +template +struct replicate { + EIGEN_DEVICE_FUNC + void operator()(int i, const typename T::Scalar* in, typename T::Scalar* out) const + { + using namespace Eigen; + T x1(in+i); + int step = x1.size() * 4; + int stride = 3 * step; + + typedef Map > MapType; + MapType(out+i*stride+0*step, x1.rows()*2, x1.cols()*2) = x1.replicate(2,2); + MapType(out+i*stride+1*step, x1.rows()*3, x1.cols()) = in[i] * x1.colwise().replicate(3); + MapType(out+i*stride+2*step, x1.rows(), x1.cols()*3) = in[i] * x1.rowwise().replicate(3); + } +}; + +template +struct redux { + EIGEN_DEVICE_FUNC + void operator()(int i, const typename T::Scalar* in, typename T::Scalar* out) const + { + using namespace Eigen; + int N = 10; + T x1(in+i); + out[i*N+0] = x1.minCoeff(); + out[i*N+1] = x1.maxCoeff(); + out[i*N+2] = x1.sum(); + out[i*N+3] = x1.prod(); + out[i*N+4] = x1.matrix().squaredNorm(); + out[i*N+5] = x1.matrix().norm(); + out[i*N+6] = x1.colwise().sum().maxCoeff(); + out[i*N+7] = x1.rowwise().maxCoeff().sum(); + out[i*N+8] = x1.matrix().colwise().squaredNorm().sum(); + } +}; + +template +struct prod_test { + EIGEN_DEVICE_FUNC + void operator()(int i, const typename T1::Scalar* in, typename T1::Scalar* out) const + { + using namespace Eigen; + typedef Matrix T3; + T1 x1(in+i); + T2 x2(in+i+1); + Map res(out+i*T3::MaxSizeAtCompileTime); + res += in[i] * x1 * x2; + } +}; + +template +struct diagonal { + EIGEN_DEVICE_FUNC + void operator()(int i, const typename T1::Scalar* in, typename T1::Scalar* out) const + { + using namespace Eigen; + T1 x1(in+i); + Map res(out+i*T2::MaxSizeAtCompileTime); + res += x1.diagonal(); + } +}; + +template +struct eigenvalues { + EIGEN_DEVICE_FUNC + void operator()(int i, const typename T::Scalar* in, typename T::Scalar* out) const + { + using namespace Eigen; + typedef Matrix Vec; + T M(in+i); + Map res(out+i*Vec::MaxSizeAtCompileTime); + T A = M*M.adjoint(); + SelfAdjointEigenSolver eig; + eig.computeDirect(M); + res = eig.eigenvalues(); + } +}; + +void test_cuda_basic() +{ + ei_test_init_cuda(); + + int nthreads = 100; + Eigen::VectorXf in, out; + + #ifndef __CUDA_ARCH__ + int data_size = nthreads * 512; + in.setRandom(data_size); + out.setRandom(data_size); + #endif + + CALL_SUBTEST( run_and_compare_to_cuda(coeff_wise(), nthreads, in, out) ); + CALL_SUBTEST( run_and_compare_to_cuda(coeff_wise(), nthreads, in, out) ); + + CALL_SUBTEST( run_and_compare_to_cuda(replicate(), nthreads, in, out) ); + CALL_SUBTEST( run_and_compare_to_cuda(replicate(), nthreads, in, out) ); + + CALL_SUBTEST( run_and_compare_to_cuda(redux(), nthreads, in, out) ); + CALL_SUBTEST( run_and_compare_to_cuda(redux(), nthreads, in, out) ); + + CALL_SUBTEST( run_and_compare_to_cuda(prod_test(), nthreads, in, out) ); + CALL_SUBTEST( run_and_compare_to_cuda(prod_test(), nthreads, in, out) ); + + CALL_SUBTEST( run_and_compare_to_cuda(diagonal(), nthreads, in, out) ); + CALL_SUBTEST( run_and_compare_to_cuda(diagonal(), nthreads, in, out) ); + + CALL_SUBTEST( run_and_compare_to_cuda(eigenvalues(), nthreads, in, out) ); + CALL_SUBTEST( run_and_compare_to_cuda(eigenvalues(), nthreads, in, out) ); + +} diff --git a/ground/gcs/src/libs/eigen/test/cuda_common.h b/ground/gcs/src/libs/eigen/test/cuda_common.h new file mode 100644 index 0000000000..9737693acf --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/cuda_common.h @@ -0,0 +1,101 @@ + +#ifndef EIGEN_TEST_CUDA_COMMON_H +#define EIGEN_TEST_CUDA_COMMON_H + +#include +#include +#include +#include + +#ifndef __CUDACC__ +dim3 threadIdx, blockDim, blockIdx; +#endif + +template +void run_on_cpu(const Kernel& ker, int n, const Input& in, Output& out) +{ + for(int i=0; i +__global__ +void run_on_cuda_meta_kernel(const Kernel ker, int n, const Input* in, Output* out) +{ + int i = threadIdx.x + blockIdx.x*blockDim.x; + if(i +void run_on_cuda(const Kernel& ker, int n, const Input& in, Output& out) +{ + typename Input::Scalar* d_in; + typename Output::Scalar* d_out; + std::ptrdiff_t in_bytes = in.size() * sizeof(typename Input::Scalar); + std::ptrdiff_t out_bytes = out.size() * sizeof(typename Output::Scalar); + + cudaMalloc((void**)(&d_in), in_bytes); + cudaMalloc((void**)(&d_out), out_bytes); + + cudaMemcpy(d_in, in.data(), in_bytes, cudaMemcpyHostToDevice); + cudaMemcpy(d_out, out.data(), out_bytes, cudaMemcpyHostToDevice); + + // Simple and non-optimal 1D mapping assuming n is not too large + // That's only for unit testing! + dim3 Blocks(128); + dim3 Grids( (n+int(Blocks.x)-1)/int(Blocks.x) ); + + cudaThreadSynchronize(); + run_on_cuda_meta_kernel<<>>(ker, n, d_in, d_out); + cudaThreadSynchronize(); + + // check inputs have not been modified + cudaMemcpy(const_cast(in.data()), d_in, in_bytes, cudaMemcpyDeviceToHost); + cudaMemcpy(out.data(), d_out, out_bytes, cudaMemcpyDeviceToHost); + + cudaFree(d_in); + cudaFree(d_out); +} + + +template +void run_and_compare_to_cuda(const Kernel& ker, int n, const Input& in, Output& out) +{ + Input in_ref, in_cuda; + Output out_ref, out_cuda; + #ifndef __CUDA_ARCH__ + in_ref = in_cuda = in; + out_ref = out_cuda = out; + #endif + run_on_cpu (ker, n, in_ref, out_ref); + run_on_cuda(ker, n, in_cuda, out_cuda); + #ifndef __CUDA_ARCH__ + VERIFY_IS_APPROX(in_ref, in_cuda); + VERIFY_IS_APPROX(out_ref, out_cuda); + #endif +} + + +void ei_test_init_cuda() +{ + int device = 0; + cudaDeviceProp deviceProp; + cudaGetDeviceProperties(&deviceProp, device); + std::cout << "CUDA device info:\n"; + std::cout << " name: " << deviceProp.name << "\n"; + std::cout << " capability: " << deviceProp.major << "." << deviceProp.minor << "\n"; + std::cout << " multiProcessorCount: " << deviceProp.multiProcessorCount << "\n"; + std::cout << " maxThreadsPerMultiProcessor: " << deviceProp.maxThreadsPerMultiProcessor << "\n"; + std::cout << " warpSize: " << deviceProp.warpSize << "\n"; + std::cout << " regsPerBlock: " << deviceProp.regsPerBlock << "\n"; + std::cout << " concurrentKernels: " << deviceProp.concurrentKernels << "\n"; + std::cout << " clockRate: " << deviceProp.clockRate << "\n"; + std::cout << " canMapHostMemory: " << deviceProp.canMapHostMemory << "\n"; + std::cout << " computeMode: " << deviceProp.computeMode << "\n"; +} + +#endif // EIGEN_TEST_CUDA_COMMON_H diff --git a/ground/gcs/src/libs/eigen/test/cwiseop.cpp b/ground/gcs/src/libs/eigen/test/cwiseop.cpp deleted file mode 100644 index e3361da17e..0000000000 --- a/ground/gcs/src/libs/eigen/test/cwiseop.cpp +++ /dev/null @@ -1,184 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN2_SUPPORT -#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING - -#define EIGEN_NO_STATIC_ASSERT -#include "main.h" -#include - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif - -using namespace std; - -template struct AddIfNull { - const Scalar operator() (const Scalar a, const Scalar b) const {return a<=1e-3 ? b : a;} - enum { Cost = NumTraits::AddCost }; -}; - -template -typename Eigen::internal::enable_if::IsInteger,typename MatrixType::Scalar>::type -cwiseops_real_only(MatrixType& m1, MatrixType& m2, MatrixType& m3, MatrixType& mones) -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - - VERIFY_IS_APPROX(m1.cwise() / m2, m1.cwise() * (m2.cwise().inverse())); - m3 = m1.cwise().abs().cwise().sqrt(); - VERIFY_IS_APPROX(m3.cwise().square(), m1.cwise().abs()); - VERIFY_IS_APPROX(m1.cwise().square().cwise().sqrt(), m1.cwise().abs()); - VERIFY_IS_APPROX(m1.cwise().abs().cwise().log().cwise().exp() , m1.cwise().abs()); - - VERIFY_IS_APPROX(m1.cwise().pow(2), m1.cwise().square()); - m3 = (m1.cwise().abs().cwise()<=RealScalar(0.01)).select(mones,m1); - VERIFY_IS_APPROX(m3.cwise().pow(-1), m3.cwise().inverse()); - m3 = m1.cwise().abs(); - VERIFY_IS_APPROX(m3.cwise().pow(RealScalar(0.5)), m3.cwise().sqrt()); - -// VERIFY_IS_APPROX(m1.cwise().tan(), m1.cwise().sin().cwise() / m1.cwise().cos()); - VERIFY_IS_APPROX(mones, m1.cwise().sin().cwise().square() + m1.cwise().cos().cwise().square()); - m3 = m1; - m3.cwise() /= m2; - VERIFY_IS_APPROX(m3, m1.cwise() / m2); - - return Scalar(0); -} - -template -typename Eigen::internal::enable_if::IsInteger,typename MatrixType::Scalar>::type -cwiseops_real_only(MatrixType& , MatrixType& , MatrixType& , MatrixType& ) -{ - return 0; -} - -template void cwiseops(const MatrixType& m) -{ - typedef typename MatrixType::Index Index; - typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; - - Index rows = m.rows(); - Index cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols), - m1bis = m1, - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - m4(rows, cols), - mzero = MatrixType::Zero(rows, cols), - mones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows); - VectorType vzero = VectorType::Zero(rows), - vones = VectorType::Ones(rows), - v3(rows); - - Index r = internal::random(0, rows-1), - c = internal::random(0, cols-1); - - Scalar s1 = internal::random(); - - // test Zero, Ones, Constant, and the set* variants - m3 = MatrixType::Constant(rows, cols, s1); - for (int j=0; j >(mones); - - VERIFY_IS_APPROX(m1.cwise().pow(2), m1.cwise().abs2()); - VERIFY_IS_APPROX(m1.cwise().pow(2), m1.cwise().square()); - VERIFY_IS_APPROX(m1.cwise().pow(3), m1.cwise().cube()); - - VERIFY_IS_APPROX(m1 + mones, m1.cwise()+Scalar(1)); - VERIFY_IS_APPROX(m1 - mones, m1.cwise()-Scalar(1)); - m3 = m1; m3.cwise() += 1; - VERIFY_IS_APPROX(m1 + mones, m3); - m3 = m1; m3.cwise() -= 1; - VERIFY_IS_APPROX(m1 - mones, m3); - - VERIFY_IS_APPROX(m2, m2.cwise() * mones); - VERIFY_IS_APPROX(m1.cwise() * m2, m2.cwise() * m1); - m3 = m1; - m3.cwise() *= m2; - VERIFY_IS_APPROX(m3, m1.cwise() * m2); - - VERIFY_IS_APPROX(mones, m2.cwise()/m2); - - // check min - VERIFY_IS_APPROX( m1.cwise().min(m2), m2.cwise().min(m1) ); - VERIFY_IS_APPROX( m1.cwise().min(m1+mones), m1 ); - VERIFY_IS_APPROX( m1.cwise().min(m1-mones), m1-mones ); - - // check max - VERIFY_IS_APPROX( m1.cwise().max(m2), m2.cwise().max(m1) ); - VERIFY_IS_APPROX( m1.cwise().max(m1-mones), m1 ); - VERIFY_IS_APPROX( m1.cwise().max(m1+mones), m1+mones ); - - VERIFY( (m1.cwise() == m1).all() ); - VERIFY( (m1.cwise() != m2).any() ); - VERIFY(!(m1.cwise() == (m1+mones)).any() ); - if (rows*cols>1) - { - m3 = m1; - m3(r,c) += 1; - VERIFY( (m1.cwise() == m3).any() ); - VERIFY( !(m1.cwise() == m3).all() ); - } - VERIFY( (m1.cwise().min(m2).cwise() <= m2).all() ); - VERIFY( (m1.cwise().max(m2).cwise() >= m2).all() ); - VERIFY( (m1.cwise().min(m2).cwise() < (m1+mones)).all() ); - VERIFY( (m1.cwise().max(m2).cwise() > (m1-mones)).all() ); - - VERIFY( (m1.cwise()(), Scalar(1)))).all() ); - VERIFY( !(m1.cwise()(), Scalar(1)))).all() ); - VERIFY( !(m1.cwise()>m1bis.unaryExpr(bind2nd(plus(), Scalar(1)))).any() ); - - cwiseops_real_only(m1, m2, m3, mones); -} - -void test_cwiseop() -{ - for(int i = 0; i < g_repeat ; i++) { - CALL_SUBTEST_1( cwiseops(Matrix()) ); - CALL_SUBTEST_2( cwiseops(Matrix4d()) ); - CALL_SUBTEST_3( cwiseops(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); - CALL_SUBTEST_4( cwiseops(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); - CALL_SUBTEST_5( cwiseops(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); - CALL_SUBTEST_6( cwiseops(MatrixXd(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/dense_storage.cpp b/ground/gcs/src/libs/eigen/test/dense_storage.cpp new file mode 100644 index 0000000000..e63712b1a4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/dense_storage.cpp @@ -0,0 +1,76 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2013 Hauke Heibel +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +#include + +template +void dense_storage_copy() +{ + static const int Size = ((Rows==Dynamic || Cols==Dynamic) ? Dynamic : Rows*Cols); + typedef DenseStorage DenseStorageType; + + const int rows = (Rows==Dynamic) ? 4 : Rows; + const int cols = (Cols==Dynamic) ? 3 : Cols; + const int size = rows*cols; + DenseStorageType reference(size, rows, cols); + T* raw_reference = reference.data(); + for (int i=0; i(i); + + DenseStorageType copied_reference(reference); + const T* raw_copied_reference = copied_reference.data(); + for (int i=0; i +void dense_storage_assignment() +{ + static const int Size = ((Rows==Dynamic || Cols==Dynamic) ? Dynamic : Rows*Cols); + typedef DenseStorage DenseStorageType; + + const int rows = (Rows==Dynamic) ? 4 : Rows; + const int cols = (Cols==Dynamic) ? 3 : Cols; + const int size = rows*cols; + DenseStorageType reference(size, rows, cols); + T* raw_reference = reference.data(); + for (int i=0; i(i); + + DenseStorageType copied_reference; + copied_reference = reference; + const T* raw_copied_reference = copied_reference.data(); + for (int i=0; i(); + dense_storage_copy(); + dense_storage_copy(); + dense_storage_copy(); + + dense_storage_copy(); + dense_storage_copy(); + dense_storage_copy(); + dense_storage_copy(); + + dense_storage_assignment(); + dense_storage_assignment(); + dense_storage_assignment(); + dense_storage_assignment(); + + dense_storage_assignment(); + dense_storage_assignment(); + dense_storage_assignment(); + dense_storage_assignment(); +} diff --git a/ground/gcs/src/libs/eigen/test/diagonal.cpp b/ground/gcs/src/libs/eigen/test/diagonal.cpp index 53814a5885..c1546e97da 100644 --- a/ground/gcs/src/libs/eigen/test/diagonal.cpp +++ b/ground/gcs/src/libs/eigen/test/diagonal.cpp @@ -20,6 +20,8 @@ template void diagonal(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols); + Scalar s1 = internal::random(); + //check diagonal() VERIFY_IS_APPROX(m1.diagonal(), m1.transpose().diagonal()); m2.diagonal() = 2 * m1.diagonal(); @@ -58,6 +60,26 @@ template void diagonal(const MatrixType& m) VERIFY_IS_APPROX(m2.template diagonal(), static_cast(2) * m1.diagonal(N2)); m2.diagonal(N2)[0] *= 3; VERIFY_IS_APPROX(m2.diagonal(N2)[0], static_cast(6) * m1.diagonal(N2)[0]); + + m2.diagonal(N2).x() = s1; + VERIFY_IS_APPROX(m2.diagonal(N2).x(), s1); + m2.diagonal(N2).coeffRef(0) = Scalar(2)*s1; + VERIFY_IS_APPROX(m2.diagonal(N2).coeff(0), Scalar(2)*s1); + } +} + +template void diagonal_assert(const MatrixType& m) { + Index rows = m.rows(); + Index cols = m.cols(); + + MatrixType m1 = MatrixType::Random(rows, cols); + + if (rows>=2 && cols>=2) + { + VERIFY_RAISES_ASSERT( m1 += m1.diagonal() ); + VERIFY_RAISES_ASSERT( m1 -= m1.diagonal() ); + VERIFY_RAISES_ASSERT( m1.array() *= m1.diagonal().array() ); + VERIFY_RAISES_ASSERT( m1.array() /= m1.diagonal().array() ); } } @@ -74,4 +96,6 @@ void test_diagonal() CALL_SUBTEST_1( diagonal(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_1( diagonal(Matrix(3, 4)) ); } + + CALL_SUBTEST_1( diagonal_assert(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } diff --git a/ground/gcs/src/libs/eigen/test/diagonalmatrices.cpp b/ground/gcs/src/libs/eigen/test/diagonalmatrices.cpp index 149f1db2f7..cd6dc8cf05 100644 --- a/ground/gcs/src/libs/eigen/test/diagonalmatrices.cpp +++ b/ground/gcs/src/libs/eigen/test/diagonalmatrices.cpp @@ -17,6 +17,7 @@ template void diagonalmatrices(const MatrixType& m) typedef Matrix VectorType; typedef Matrix RowVectorType; typedef Matrix SquareMatrixType; + typedef Matrix DynMatrixType; typedef DiagonalMatrix LeftDiagonalMatrix; typedef DiagonalMatrix RightDiagonalMatrix; typedef Matrix BigMatrix; @@ -64,6 +65,13 @@ template void diagonalmatrices(const MatrixType& m) VERIFY_IS_APPROX( (((v1+v2).asDiagonal() * (m1+m2))(i,j)) , (v1+v2)(i) * (m1+m2)(i,j) ); VERIFY_IS_APPROX( ((m1 * (rv1+rv2).asDiagonal())(i,j)) , (rv1+rv2)(j) * m1(i,j) ); VERIFY_IS_APPROX( (((m1+m2) * (rv1+rv2).asDiagonal())(i,j)) , (rv1+rv2)(j) * (m1+m2)(i,j) ); + + if(rows>1) + { + DynMatrixType tmp = m1.topRows(rows/2), res; + VERIFY_IS_APPROX( (res = m1.topRows(rows/2) * rv1.asDiagonal()), tmp * rv1.asDiagonal() ); + VERIFY_IS_APPROX( (res = v1.head(rows/2).asDiagonal()*m1.topRows(rows/2)), v1.head(rows/2).asDiagonal()*tmp ); + } BigMatrix big; big.setZero(2*rows, 2*cols); @@ -84,6 +92,24 @@ template void diagonalmatrices(const MatrixType& m) VERIFY_IS_APPROX(m1 * (rdm1 * s1), (m1 * rdm1) * s1); VERIFY_IS_APPROX(m1 * (s1 * rdm1), (m1 * rdm1) * s1); + + // Diagonal to dense + sq_m1.setRandom(); + sq_m2 = sq_m1; + VERIFY_IS_APPROX( (sq_m1 += (s1*v1).asDiagonal()), sq_m2 += (s1*v1).asDiagonal().toDenseMatrix() ); + VERIFY_IS_APPROX( (sq_m1 -= (s1*v1).asDiagonal()), sq_m2 -= (s1*v1).asDiagonal().toDenseMatrix() ); + VERIFY_IS_APPROX( (sq_m1 = (s1*v1).asDiagonal()), (s1*v1).asDiagonal().toDenseMatrix() ); +} + +template +void bug987() +{ + Matrix3Xd points = Matrix3Xd::Random(3, 3); + Vector2d diag = Vector2d::Random(); + Matrix2Xd tmp1 = points.topRows<2>(), res1, res2; + VERIFY_IS_APPROX( res1 = diag.asDiagonal() * points.topRows<2>(), res2 = diag.asDiagonal() * tmp1 ); + Matrix2d tmp2 = points.topLeftCorner<2,2>(); + VERIFY_IS_APPROX(( res1 = points.topLeftCorner<2,2>()*diag.asDiagonal()) , res2 = tmp2*diag.asDiagonal() ); } void test_diagonalmatrices() @@ -99,4 +125,5 @@ void test_diagonalmatrices() CALL_SUBTEST_8( diagonalmatrices(Matrix(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_9( diagonalmatrices(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } + CALL_SUBTEST_10( bug987<0>() ); } diff --git a/ground/gcs/src/libs/eigen/test/dynalloc.cpp b/ground/gcs/src/libs/eigen/test/dynalloc.cpp index ef92c0507c..f1cc70beeb 100644 --- a/ground/gcs/src/libs/eigen/test/dynalloc.cpp +++ b/ground/gcs/src/libs/eigen/test/dynalloc.cpp @@ -9,18 +9,20 @@ #include "main.h" -#if EIGEN_ALIGN -#define ALIGNMENT 16 +#if EIGEN_MAX_ALIGN_BYTES>0 +#define ALIGNMENT EIGEN_MAX_ALIGN_BYTES #else #define ALIGNMENT 1 #endif +typedef Matrix Vector8f; + void check_handmade_aligned_malloc() { for(int i = 1; i < 1000; i++) { char *p = (char*)internal::handmade_aligned_malloc(i); - VERIFY(size_t(p)%ALIGNMENT==0); + VERIFY(internal::UIntPtr(p)%ALIGNMENT==0); // if the buffer is wrongly allocated this will give a bad write --> check with valgrind for(int j = 0; j < i; j++) p[j]=0; internal::handmade_aligned_free(p); @@ -29,10 +31,10 @@ void check_handmade_aligned_malloc() void check_aligned_malloc() { - for(int i = 1; i < 1000; i++) + for(int i = ALIGNMENT; i < 1000; i++) { char *p = (char*)internal::aligned_malloc(i); - VERIFY(size_t(p)%ALIGNMENT==0); + VERIFY(internal::UIntPtr(p)%ALIGNMENT==0); // if the buffer is wrongly allocated this will give a bad write --> check with valgrind for(int j = 0; j < i; j++) p[j]=0; internal::aligned_free(p); @@ -41,10 +43,10 @@ void check_aligned_malloc() void check_aligned_new() { - for(int i = 1; i < 1000; i++) + for(int i = ALIGNMENT; i < 1000; i++) { float *p = internal::aligned_new(i); - VERIFY(size_t(p)%ALIGNMENT==0); + VERIFY(internal::UIntPtr(p)%ALIGNMENT==0); // if the buffer is wrongly allocated this will give a bad write --> check with valgrind for(int j = 0; j < i; j++) p[j]=0; internal::aligned_delete(p,i); @@ -53,10 +55,10 @@ void check_aligned_new() void check_aligned_stack_alloc() { - for(int i = 1; i < 400; i++) + for(int i = ALIGNMENT; i < 400; i++) { ei_declare_aligned_stack_constructed_variable(float,p,i,0); - VERIFY(size_t(p)%ALIGNMENT==0); + VERIFY(internal::UIntPtr(p)%ALIGNMENT==0); // if the buffer is wrongly allocated this will give a bad write --> check with valgrind for(int j = 0; j < i; j++) p[j]=0; } @@ -68,7 +70,7 @@ struct MyStruct { EIGEN_MAKE_ALIGNED_OPERATOR_NEW char dummychar; - Vector4f avec; + Vector8f avec; }; class MyClassA @@ -76,15 +78,19 @@ class MyClassA public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW char dummychar; - Vector4f avec; + Vector8f avec; }; template void check_dynaligned() { - T* obj = new T; - VERIFY(T::NeedsToAlign==1); - VERIFY(size_t(obj)%ALIGNMENT==0); - delete obj; + // TODO have to be updated once we support multiple alignment values + if(T::SizeAtCompileTime % ALIGNMENT == 0) + { + T* obj = new T; + VERIFY(T::NeedsToAlign==1); + VERIFY(internal::UIntPtr(obj)%ALIGNMENT==0); + delete obj; + } } template void check_custom_new_delete() @@ -100,7 +106,7 @@ template void check_custom_new_delete() delete[] t; } -#ifdef EIGEN_ALIGN +#if EIGEN_MAX_ALIGN_BYTES>0 { T* t = static_cast((T::operator new)(sizeof(T))); (T::operator delete)(t, sizeof(T)); @@ -120,9 +126,17 @@ void test_dynalloc() CALL_SUBTEST(check_aligned_malloc()); CALL_SUBTEST(check_aligned_new()); CALL_SUBTEST(check_aligned_stack_alloc()); + + for (int i=0; i() ); + CALL_SUBTEST( check_custom_new_delete() ); + CALL_SUBTEST( check_custom_new_delete() ); + CALL_SUBTEST( check_custom_new_delete() ); + } // check static allocation, who knows ? - #if EIGEN_ALIGN_STATICALLY + #if EIGEN_MAX_STATIC_ALIGN_BYTES for (int i=0; i() ); @@ -130,23 +144,19 @@ void test_dynalloc() CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); - - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); + CALL_SUBTEST(check_dynaligned() ); } { - MyStruct foo0; VERIFY(size_t(foo0.avec.data())%ALIGNMENT==0); - MyClassA fooA; VERIFY(size_t(fooA.avec.data())%ALIGNMENT==0); + MyStruct foo0; VERIFY(internal::UIntPtr(foo0.avec.data())%ALIGNMENT==0); + MyClassA fooA; VERIFY(internal::UIntPtr(fooA.avec.data())%ALIGNMENT==0); } // dynamic allocation, single object for (int i=0; iavec.data())%ALIGNMENT==0); - MyClassA *fooA = new MyClassA(); VERIFY(size_t(fooA->avec.data())%ALIGNMENT==0); + MyStruct *foo0 = new MyStruct(); VERIFY(internal::UIntPtr(foo0->avec.data())%ALIGNMENT==0); + MyClassA *fooA = new MyClassA(); VERIFY(internal::UIntPtr(fooA->avec.data())%ALIGNMENT==0); delete foo0; delete fooA; } @@ -155,8 +165,8 @@ void test_dynalloc() const int N = 10; for (int i=0; iavec.data())%ALIGNMENT==0); - MyClassA *fooA = new MyClassA[N]; VERIFY(size_t(fooA->avec.data())%ALIGNMENT==0); + MyStruct *foo0 = new MyStruct[N]; VERIFY(internal::UIntPtr(foo0->avec.data())%ALIGNMENT==0); + MyClassA *fooA = new MyClassA[N]; VERIFY(internal::UIntPtr(fooA->avec.data())%ALIGNMENT==0); delete[] foo0; delete[] fooA; } diff --git a/ground/gcs/src/libs/eigen/test/eigen2/CMakeLists.txt b/ground/gcs/src/libs/eigen/test/eigen2/CMakeLists.txt deleted file mode 100644 index 9615a6026e..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -add_custom_target(eigen2_buildtests) -add_custom_target(eigen2_check COMMAND "ctest -R eigen2") -add_dependencies(eigen2_check eigen2_buildtests) -add_dependencies(buildtests eigen2_buildtests) - -add_definitions("-DEIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API") -add_definitions("-DEIGEN_NO_EIGEN2_DEPRECATED_WARNING") - -ei_add_test(eigen2_meta) -ei_add_test(eigen2_sizeof) -ei_add_test(eigen2_dynalloc) -ei_add_test(eigen2_nomalloc) -#ei_add_test(eigen2_first_aligned) -ei_add_test(eigen2_mixingtypes) -#ei_add_test(eigen2_packetmath) -ei_add_test(eigen2_unalignedassert) -#ei_add_test(eigen2_vectorization_logic) -ei_add_test(eigen2_basicstuff) -ei_add_test(eigen2_linearstructure) -ei_add_test(eigen2_cwiseop) -ei_add_test(eigen2_sum) -ei_add_test(eigen2_product_small) -ei_add_test(eigen2_product_large ${EI_OFLAG}) -ei_add_test(eigen2_adjoint) -ei_add_test(eigen2_submatrices) -ei_add_test(eigen2_miscmatrices) -ei_add_test(eigen2_commainitializer) -ei_add_test(eigen2_smallvectors) -ei_add_test(eigen2_map) -ei_add_test(eigen2_array) -ei_add_test(eigen2_triangular) -ei_add_test(eigen2_cholesky " " "${GSL_LIBRARIES}") -ei_add_test(eigen2_lu ${EI_OFLAG}) -ei_add_test(eigen2_determinant ${EI_OFLAG}) -ei_add_test(eigen2_inverse) -ei_add_test(eigen2_qr) -ei_add_test(eigen2_eigensolver " " "${GSL_LIBRARIES}") -ei_add_test(eigen2_svd) -ei_add_test(eigen2_geometry) -ei_add_test(eigen2_geometry_with_eigen2_prefix) -ei_add_test(eigen2_hyperplane) -ei_add_test(eigen2_parametrizedline) -ei_add_test(eigen2_alignedbox) -ei_add_test(eigen2_regression) -ei_add_test(eigen2_stdvector) -ei_add_test(eigen2_newstdvector) -if(QT4_FOUND) - ei_add_test(eigen2_qtvector " " "${QT_QTCORE_LIBRARY}") -endif(QT4_FOUND) -# no support for eigen2 sparse module -# if(NOT EIGEN_DEFAULT_TO_ROW_MAJOR) -# ei_add_test(eigen2_sparse_vector) -# ei_add_test(eigen2_sparse_basic) -# ei_add_test(eigen2_sparse_solvers " " "${SPARSE_LIBS}") -# ei_add_test(eigen2_sparse_product) -# endif() -ei_add_test(eigen2_swap) -ei_add_test(eigen2_visitor) -ei_add_test(eigen2_bug_132) - -ei_add_test(eigen2_prec_inverse_4x4 ${EI_OFLAG}) diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_adjoint.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_adjoint.cpp deleted file mode 100644 index c0f8114599..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_adjoint.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void adjoint(const MatrixType& m) -{ - /* this test covers the following files: - Transpose.h Conjugate.h Dot.h - */ - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - typedef Matrix SquareMatrixType; - int rows = m.rows(); - int cols = m.cols(); - - RealScalar largerEps = test_precision(); - if (ei_is_same_type::ret) - largerEps = RealScalar(1e-3f); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - square = SquareMatrixType::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - v3 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); - - Scalar s1 = ei_random(), - s2 = ei_random(); - - // check basic compatibility of adjoint, transpose, conjugate - VERIFY_IS_APPROX(m1.transpose().conjugate().adjoint(), m1); - VERIFY_IS_APPROX(m1.adjoint().conjugate().transpose(), m1); - - // check multiplicative behavior - VERIFY_IS_APPROX((m1.adjoint() * m2).adjoint(), m2.adjoint() * m1); - VERIFY_IS_APPROX((s1 * m1).adjoint(), ei_conj(s1) * m1.adjoint()); - - // check basic properties of dot, norm, norm2 - typedef typename NumTraits::Real RealScalar; - VERIFY(ei_isApprox((s1 * v1 + s2 * v2).eigen2_dot(v3), s1 * v1.eigen2_dot(v3) + s2 * v2.eigen2_dot(v3), largerEps)); - VERIFY(ei_isApprox(v3.eigen2_dot(s1 * v1 + s2 * v2), ei_conj(s1)*v3.eigen2_dot(v1)+ei_conj(s2)*v3.eigen2_dot(v2), largerEps)); - VERIFY_IS_APPROX(ei_conj(v1.eigen2_dot(v2)), v2.eigen2_dot(v1)); - VERIFY_IS_APPROX(ei_real(v1.eigen2_dot(v1)), v1.squaredNorm()); - if(NumTraits::HasFloatingPoint) - VERIFY_IS_APPROX(v1.squaredNorm(), v1.norm() * v1.norm()); - VERIFY_IS_MUCH_SMALLER_THAN(ei_abs(vzero.eigen2_dot(v1)), static_cast(1)); - if(NumTraits::HasFloatingPoint) - VERIFY_IS_MUCH_SMALLER_THAN(vzero.norm(), static_cast(1)); - - // check compatibility of dot and adjoint - VERIFY(ei_isApprox(v1.eigen2_dot(square * v2), (square.adjoint() * v1).eigen2_dot(v2), largerEps)); - - // like in testBasicStuff, test operator() to check const-qualification - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - VERIFY_IS_APPROX(m1.conjugate()(r,c), ei_conj(m1(r,c))); - VERIFY_IS_APPROX(m1.adjoint()(c,r), ei_conj(m1(r,c))); - - if(NumTraits::HasFloatingPoint) - { - // check that Random().normalized() works: tricky as the random xpr must be evaluated by - // normalized() in order to produce a consistent result. - VERIFY_IS_APPROX(VectorType::Random(rows).normalized().norm(), RealScalar(1)); - } - - // check inplace transpose - m3 = m1; - m3.transposeInPlace(); - VERIFY_IS_APPROX(m3,m1.transpose()); - m3.transposeInPlace(); - VERIFY_IS_APPROX(m3,m1); - -} - -void test_eigen2_adjoint() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( adjoint(Matrix()) ); - CALL_SUBTEST_2( adjoint(Matrix3d()) ); - CALL_SUBTEST_3( adjoint(Matrix4f()) ); - CALL_SUBTEST_4( adjoint(MatrixXcf(4, 4)) ); - CALL_SUBTEST_5( adjoint(MatrixXi(8, 12)) ); - CALL_SUBTEST_6( adjoint(MatrixXf(21, 21)) ); - } - // test a large matrix only once - CALL_SUBTEST_7( adjoint(Matrix()) ); -} - diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_alignedbox.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_alignedbox.cpp deleted file mode 100644 index 35043b9587..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_alignedbox.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include -#include - -template void alignedbox(const BoxType& _box) -{ - /* this test covers the following files: - AlignedBox.h - */ - - const int dim = _box.dim(); - typedef typename BoxType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - - VectorType p0 = VectorType::Random(dim); - VectorType p1 = VectorType::Random(dim); - RealScalar s1 = ei_random(0,1); - - BoxType b0(dim); - BoxType b1(VectorType::Random(dim),VectorType::Random(dim)); - BoxType b2; - - b0.extend(p0); - b0.extend(p1); - VERIFY(b0.contains(p0*s1+(Scalar(1)-s1)*p1)); - VERIFY(!b0.contains(p0 + (1+s1)*(p1-p0))); - - (b2 = b0).extend(b1); - VERIFY(b2.contains(b0)); - VERIFY(b2.contains(b1)); - VERIFY_IS_APPROX(b2.clamp(b0), b0); - - // casting - const int Dim = BoxType::AmbientDimAtCompileTime; - typedef typename GetDifferentType::type OtherScalar; - AlignedBox hp1f = b0.template cast(); - VERIFY_IS_APPROX(hp1f.template cast(),b0); - AlignedBox hp1d = b0.template cast(); - VERIFY_IS_APPROX(hp1d.template cast(),b0); -} - -void test_eigen2_alignedbox() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( alignedbox(AlignedBox()) ); - CALL_SUBTEST_2( alignedbox(AlignedBox()) ); - CALL_SUBTEST_3( alignedbox(AlignedBox()) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_array.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_array.cpp deleted file mode 100644 index c1ff40ce7d..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_array.cpp +++ /dev/null @@ -1,142 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template void array(const MatrixType& m) -{ - /* this test covers the following files: - Array.cpp - */ - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - - int rows = m.rows(); - int cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols); - - Scalar s1 = ei_random(), - s2 = ei_random(); - - // scalar addition - VERIFY_IS_APPROX(m1.cwise() + s1, s1 + m1.cwise()); - VERIFY_IS_APPROX(m1.cwise() + s1, MatrixType::Constant(rows,cols,s1) + m1); - VERIFY_IS_APPROX((m1*Scalar(2)).cwise() - s2, (m1+m1) - MatrixType::Constant(rows,cols,s2) ); - m3 = m1; - m3.cwise() += s2; - VERIFY_IS_APPROX(m3, m1.cwise() + s2); - m3 = m1; - m3.cwise() -= s1; - VERIFY_IS_APPROX(m3, m1.cwise() - s1); - - // reductions - VERIFY_IS_APPROX(m1.colwise().sum().sum(), m1.sum()); - VERIFY_IS_APPROX(m1.rowwise().sum().sum(), m1.sum()); - if (!ei_isApprox(m1.sum(), (m1+m2).sum())) - VERIFY_IS_NOT_APPROX(((m1+m2).rowwise().sum()).sum(), m1.sum()); - VERIFY_IS_APPROX(m1.colwise().sum(), m1.colwise().redux(internal::scalar_sum_op())); -} - -template void comparisons(const MatrixType& m) -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - - int rows = m.rows(); - int cols = m.cols(); - - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols); - - VERIFY(((m1.cwise() + Scalar(1)).cwise() > m1).all()); - VERIFY(((m1.cwise() - Scalar(1)).cwise() < m1).all()); - if (rows*cols>1) - { - m3 = m1; - m3(r,c) += 1; - VERIFY(! (m1.cwise() < m3).all() ); - VERIFY(! (m1.cwise() > m3).all() ); - } - - // comparisons to scalar - VERIFY( (m1.cwise() != (m1(r,c)+1) ).any() ); - VERIFY( (m1.cwise() > (m1(r,c)-1) ).any() ); - VERIFY( (m1.cwise() < (m1(r,c)+1) ).any() ); - VERIFY( (m1.cwise() == m1(r,c) ).any() ); - - // test Select - VERIFY_IS_APPROX( (m1.cwise()m2).select(m1,m2), m1.cwise().max(m2) ); - Scalar mid = (m1.cwise().abs().minCoeff() + m1.cwise().abs().maxCoeff())/Scalar(2); - for (int j=0; j=MatrixType::Constant(rows,cols,mid)) - .select(m1,0), m3); - // even shorter version: - VERIFY_IS_APPROX( (m1.cwise().abs().cwise()RealScalar(0.1)).count() == rows*cols); - VERIFY_IS_APPROX(((m1.cwise().abs().cwise()+1).cwise()>RealScalar(0.1)).colwise().count().template cast(), RowVectorXi::Constant(cols,rows)); - VERIFY_IS_APPROX(((m1.cwise().abs().cwise()+1).cwise()>RealScalar(0.1)).rowwise().count().template cast(), VectorXi::Constant(rows, cols)); -} - -template void lpNorm(const VectorType& v) -{ - VectorType u = VectorType::Random(v.size()); - - VERIFY_IS_APPROX(u.template lpNorm(), u.cwise().abs().maxCoeff()); - VERIFY_IS_APPROX(u.template lpNorm<1>(), u.cwise().abs().sum()); - VERIFY_IS_APPROX(u.template lpNorm<2>(), ei_sqrt(u.cwise().abs().cwise().square().sum())); - VERIFY_IS_APPROX(ei_pow(u.template lpNorm<5>(), typename VectorType::RealScalar(5)), u.cwise().abs().cwise().pow(5).sum()); -} - -void test_eigen2_array() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( array(Matrix()) ); - CALL_SUBTEST_2( array(Matrix2f()) ); - CALL_SUBTEST_3( array(Matrix4d()) ); - CALL_SUBTEST_4( array(MatrixXcf(3, 3)) ); - CALL_SUBTEST_5( array(MatrixXf(8, 12)) ); - CALL_SUBTEST_6( array(MatrixXi(8, 12)) ); - } - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( comparisons(Matrix()) ); - CALL_SUBTEST_2( comparisons(Matrix2f()) ); - CALL_SUBTEST_3( comparisons(Matrix4d()) ); - CALL_SUBTEST_5( comparisons(MatrixXf(8, 12)) ); - CALL_SUBTEST_6( comparisons(MatrixXi(8, 12)) ); - } - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( lpNorm(Matrix()) ); - CALL_SUBTEST_2( lpNorm(Vector2f()) ); - CALL_SUBTEST_3( lpNorm(Vector3d()) ); - CALL_SUBTEST_4( lpNorm(Vector4f()) ); - CALL_SUBTEST_5( lpNorm(VectorXf(16)) ); - CALL_SUBTEST_7( lpNorm(VectorXcd(10)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_basicstuff.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_basicstuff.cpp deleted file mode 100644 index dd2dec1eff..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_basicstuff.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void basicStuff(const MatrixType& m) -{ - typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; - - int rows = m.rows(); - int cols = m.cols(); - - // this test relies a lot on Random.h, and there's not much more that we can do - // to test it, hence I consider that we will have tested Random.h - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - square = Matrix::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); - - Scalar x = ei_random(); - - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - - m1.coeffRef(r,c) = x; - VERIFY_IS_APPROX(x, m1.coeff(r,c)); - m1(r,c) = x; - VERIFY_IS_APPROX(x, m1(r,c)); - v1.coeffRef(r) = x; - VERIFY_IS_APPROX(x, v1.coeff(r)); - v1(r) = x; - VERIFY_IS_APPROX(x, v1(r)); - v1[r] = x; - VERIFY_IS_APPROX(x, v1[r]); - - VERIFY_IS_APPROX( v1, v1); - VERIFY_IS_NOT_APPROX( v1, 2*v1); - VERIFY_IS_MUCH_SMALLER_THAN( vzero, v1); - if(NumTraits::HasFloatingPoint) - VERIFY_IS_MUCH_SMALLER_THAN( vzero, v1.norm()); - VERIFY_IS_NOT_MUCH_SMALLER_THAN(v1, v1); - VERIFY_IS_APPROX( vzero, v1-v1); - VERIFY_IS_APPROX( m1, m1); - VERIFY_IS_NOT_APPROX( m1, 2*m1); - VERIFY_IS_MUCH_SMALLER_THAN( mzero, m1); - VERIFY_IS_NOT_MUCH_SMALLER_THAN(m1, m1); - VERIFY_IS_APPROX( mzero, m1-m1); - - // always test operator() on each read-only expression class, - // in order to check const-qualifiers. - // indeed, if an expression class (here Zero) is meant to be read-only, - // hence has no _write() method, the corresponding MatrixBase method (here zero()) - // should return a const-qualified object so that it is the const-qualified - // operator() that gets called, which in turn calls _read(). - VERIFY_IS_MUCH_SMALLER_THAN(MatrixType::Zero(rows,cols)(r,c), static_cast(1)); - - // now test copying a row-vector into a (column-)vector and conversely. - square.col(r) = square.row(r).eval(); - Matrix rv(rows); - Matrix cv(rows); - rv = square.row(r); - cv = square.col(r); - VERIFY_IS_APPROX(rv, cv.transpose()); - - if(cols!=1 && rows!=1 && MatrixType::SizeAtCompileTime!=Dynamic) - { - VERIFY_RAISES_ASSERT(m1 = (m2.block(0,0, rows-1, cols-1))); - } - - VERIFY_IS_APPROX(m3 = m1,m1); - MatrixType m4; - VERIFY_IS_APPROX(m4 = m1,m1); - - // test swap - m3 = m1; - m1.swap(m2); - VERIFY_IS_APPROX(m3, m2); - if(rows*cols>=3) - { - VERIFY_IS_NOT_APPROX(m3, m1); - } -} - -void test_eigen2_basicstuff() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( basicStuff(Matrix()) ); - CALL_SUBTEST_2( basicStuff(Matrix4d()) ); - CALL_SUBTEST_3( basicStuff(MatrixXcf(3, 3)) ); - CALL_SUBTEST_4( basicStuff(MatrixXi(8, 12)) ); - CALL_SUBTEST_5( basicStuff(MatrixXcd(20, 20)) ); - CALL_SUBTEST_6( basicStuff(Matrix()) ); - CALL_SUBTEST_7( basicStuff(Matrix(10,10)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_bug_132.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_bug_132.cpp deleted file mode 100644 index 7fe3610ce8..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_bug_132.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2010 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -void test_eigen2_bug_132() { - int size = 100; - MatrixXd A(size, size); - VectorXd b(size), c(size); - { - VectorXd y = A.transpose() * (b-c); // bug 132: infinite recursion in coeffRef - VectorXd z = (b-c).transpose() * A; // bug 132: infinite recursion in coeffRef - } - - // the following ones weren't failing, but let's include them for completeness: - { - VectorXd y = A * (b-c); - VectorXd z = (b-c).transpose() * A.transpose(); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_cholesky.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_cholesky.cpp deleted file mode 100644 index 9c4b6f5619..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_cholesky.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN_NO_ASSERTION_CHECKING -#include "main.h" -#include -#include - -#ifdef HAS_GSL -#include "gsl_helper.h" -#endif - -template void cholesky(const MatrixType& m) -{ - /* this test covers the following files: - LLT.h LDLT.h - */ - int rows = m.rows(); - int cols = m.cols(); - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix SquareMatrixType; - typedef Matrix VectorType; - - MatrixType a0 = MatrixType::Random(rows,cols); - VectorType vecB = VectorType::Random(rows), vecX(rows); - MatrixType matB = MatrixType::Random(rows,cols), matX(rows,cols); - SquareMatrixType symm = a0 * a0.adjoint(); - // let's make sure the matrix is not singular or near singular - MatrixType a1 = MatrixType::Random(rows,cols); - symm += a1 * a1.adjoint(); - - #ifdef HAS_GSL - if (ei_is_same_type::ret) - { - typedef GslTraits Gsl; - typename Gsl::Matrix gMatA=0, gSymm=0; - typename Gsl::Vector gVecB=0, gVecX=0; - convert(symm, gSymm); - convert(symm, gMatA); - convert(vecB, gVecB); - convert(vecB, gVecX); - Gsl::cholesky(gMatA); - Gsl::cholesky_solve(gMatA, gVecB, gVecX); - VectorType vecX(rows), _vecX, _vecB; - convert(gVecX, _vecX); - symm.llt().solve(vecB, &vecX); - Gsl::prod(gSymm, gVecX, gVecB); - convert(gVecB, _vecB); - // test gsl itself ! - VERIFY_IS_APPROX(vecB, _vecB); - VERIFY_IS_APPROX(vecX, _vecX); - - Gsl::free(gMatA); - Gsl::free(gSymm); - Gsl::free(gVecB); - Gsl::free(gVecX); - } - #endif - - { - LDLT ldlt(symm); - VERIFY(ldlt.isPositiveDefinite()); - // in eigen3, LDLT is pivoting - //VERIFY_IS_APPROX(symm, ldlt.matrixL() * ldlt.vectorD().asDiagonal() * ldlt.matrixL().adjoint()); - ldlt.solve(vecB, &vecX); - VERIFY_IS_APPROX(symm * vecX, vecB); - ldlt.solve(matB, &matX); - VERIFY_IS_APPROX(symm * matX, matB); - } - - { - LLT chol(symm); - VERIFY(chol.isPositiveDefinite()); - VERIFY_IS_APPROX(symm, chol.matrixL() * chol.matrixL().adjoint()); - chol.solve(vecB, &vecX); - VERIFY_IS_APPROX(symm * vecX, vecB); - chol.solve(matB, &matX); - VERIFY_IS_APPROX(symm * matX, matB); - } - -#if 0 // cholesky is not rank-revealing anyway - // test isPositiveDefinite on non definite matrix - if (rows>4) - { - SquareMatrixType symm = a0.block(0,0,rows,cols-4) * a0.block(0,0,rows,cols-4).adjoint(); - LLT chol(symm); - VERIFY(!chol.isPositiveDefinite()); - LDLT cholnosqrt(symm); - VERIFY(!cholnosqrt.isPositiveDefinite()); - } -#endif -} - -void test_eigen2_cholesky() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( cholesky(Matrix()) ); - CALL_SUBTEST_2( cholesky(Matrix2d()) ); - CALL_SUBTEST_3( cholesky(Matrix3f()) ); - CALL_SUBTEST_4( cholesky(Matrix4d()) ); - CALL_SUBTEST_5( cholesky(MatrixXcd(7,7)) ); - CALL_SUBTEST_6( cholesky(MatrixXf(17,17)) ); - CALL_SUBTEST_7( cholesky(MatrixXd(33,33)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_commainitializer.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_commainitializer.cpp deleted file mode 100644 index e0f901e0bd..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_commainitializer.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -void test_eigen2_commainitializer() -{ - Matrix3d m3; - Matrix4d m4; - - VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8) ); - - #ifndef _MSC_VER - VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10) ); - #endif - - double data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; - Matrix3d ref = Map >(data); - - m3 = Matrix3d::Random(); - m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9; - VERIFY_IS_APPROX(m3, ref ); - - Vector3d vec[3]; - vec[0] << 1, 4, 7; - vec[1] << 2, 5, 8; - vec[2] << 3, 6, 9; - m3 = Matrix3d::Random(); - m3 << vec[0], vec[1], vec[2]; - VERIFY_IS_APPROX(m3, ref); - - vec[0] << 1, 2, 3; - vec[1] << 4, 5, 6; - vec[2] << 7, 8, 9; - m3 = Matrix3d::Random(); - m3 << vec[0].transpose(), - 4, 5, 6, - vec[2].transpose(); - VERIFY_IS_APPROX(m3, ref); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_cwiseop.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_cwiseop.cpp deleted file mode 100644 index 22e1cc3429..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_cwiseop.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include - -using namespace std; - -template struct AddIfNull { - const Scalar operator() (const Scalar a, const Scalar b) const {return a<=1e-3 ? b : a;} - enum { Cost = NumTraits::AddCost }; -}; - -template void cwiseops(const MatrixType& m) -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - - int rows = m.rows(); - int cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - m4(rows, cols), - mzero = MatrixType::Zero(rows, cols), - mones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows); - VectorType vzero = VectorType::Zero(rows), - vones = VectorType::Ones(rows), - v3(rows); - - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - - Scalar s1 = ei_random(); - - // test Zero, Ones, Constant, and the set* variants - m3 = MatrixType::Constant(rows, cols, s1); - for (int j=0; j >(mones); - - VERIFY_IS_APPROX(m1.cwise().pow(2), m1.cwise().abs2()); - VERIFY_IS_APPROX(m1.cwise().pow(2), m1.cwise().square()); - VERIFY_IS_APPROX(m1.cwise().pow(3), m1.cwise().cube()); - - VERIFY_IS_APPROX(m1 + mones, m1.cwise()+Scalar(1)); - VERIFY_IS_APPROX(m1 - mones, m1.cwise()-Scalar(1)); - m3 = m1; m3.cwise() += 1; - VERIFY_IS_APPROX(m1 + mones, m3); - m3 = m1; m3.cwise() -= 1; - VERIFY_IS_APPROX(m1 - mones, m3); - - VERIFY_IS_APPROX(m2, m2.cwise() * mones); - VERIFY_IS_APPROX(m1.cwise() * m2, m2.cwise() * m1); - m3 = m1; - m3.cwise() *= m2; - VERIFY_IS_APPROX(m3, m1.cwise() * m2); - - VERIFY_IS_APPROX(mones, m2.cwise()/m2); - if(NumTraits::HasFloatingPoint) - { - VERIFY_IS_APPROX(m1.cwise() / m2, m1.cwise() * (m2.cwise().inverse())); - m3 = m1.cwise().abs().cwise().sqrt(); - VERIFY_IS_APPROX(m3.cwise().square(), m1.cwise().abs()); - VERIFY_IS_APPROX(m1.cwise().square().cwise().sqrt(), m1.cwise().abs()); - VERIFY_IS_APPROX(m1.cwise().abs().cwise().log().cwise().exp() , m1.cwise().abs()); - - VERIFY_IS_APPROX(m1.cwise().pow(2), m1.cwise().square()); - m3 = (m1.cwise().abs().cwise()<=RealScalar(0.01)).select(mones,m1); - VERIFY_IS_APPROX(m3.cwise().pow(-1), m3.cwise().inverse()); - m3 = m1.cwise().abs(); - VERIFY_IS_APPROX(m3.cwise().pow(RealScalar(0.5)), m3.cwise().sqrt()); - -// VERIFY_IS_APPROX(m1.cwise().tan(), m1.cwise().sin().cwise() / m1.cwise().cos()); - VERIFY_IS_APPROX(mones, m1.cwise().sin().cwise().square() + m1.cwise().cos().cwise().square()); - m3 = m1; - m3.cwise() /= m2; - VERIFY_IS_APPROX(m3, m1.cwise() / m2); - } - - // check min - VERIFY_IS_APPROX( m1.cwise().min(m2), m2.cwise().min(m1) ); - VERIFY_IS_APPROX( m1.cwise().min(m1+mones), m1 ); - VERIFY_IS_APPROX( m1.cwise().min(m1-mones), m1-mones ); - - // check max - VERIFY_IS_APPROX( m1.cwise().max(m2), m2.cwise().max(m1) ); - VERIFY_IS_APPROX( m1.cwise().max(m1-mones), m1 ); - VERIFY_IS_APPROX( m1.cwise().max(m1+mones), m1+mones ); - - VERIFY( (m1.cwise() == m1).all() ); - VERIFY( (m1.cwise() != m2).any() ); - VERIFY(!(m1.cwise() == (m1+mones)).any() ); - if (rows*cols>1) - { - m3 = m1; - m3(r,c) += 1; - VERIFY( (m1.cwise() == m3).any() ); - VERIFY( !(m1.cwise() == m3).all() ); - } - VERIFY( (m1.cwise().min(m2).cwise() <= m2).all() ); - VERIFY( (m1.cwise().max(m2).cwise() >= m2).all() ); - VERIFY( (m1.cwise().min(m2).cwise() < (m1+mones)).all() ); - VERIFY( (m1.cwise().max(m2).cwise() > (m1-mones)).all() ); - - VERIFY( (m1.cwise()(), Scalar(1)))).all() ); - VERIFY( !(m1.cwise()(), Scalar(1)))).all() ); - VERIFY( !(m1.cwise()>m1.unaryExpr(bind2nd(plus(), Scalar(1)))).any() ); -} - -void test_eigen2_cwiseop() -{ - for(int i = 0; i < g_repeat ; i++) { - CALL_SUBTEST_1( cwiseops(Matrix()) ); - CALL_SUBTEST_2( cwiseops(Matrix4d()) ); - CALL_SUBTEST_3( cwiseops(MatrixXf(3, 3)) ); - CALL_SUBTEST_3( cwiseops(MatrixXf(22, 22)) ); - CALL_SUBTEST_4( cwiseops(MatrixXi(8, 12)) ); - CALL_SUBTEST_5( cwiseops(MatrixXd(20, 20)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_determinant.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_determinant.cpp deleted file mode 100644 index c7b4ad0533..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_determinant.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Benoit Jacob -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template void determinant(const MatrixType& m) -{ - /* this test covers the following files: - Determinant.h - */ - int size = m.rows(); - - MatrixType m1(size, size), m2(size, size); - m1.setRandom(); - m2.setRandom(); - typedef typename MatrixType::Scalar Scalar; - Scalar x = ei_random(); - VERIFY_IS_APPROX(MatrixType::Identity(size, size).determinant(), Scalar(1)); - VERIFY_IS_APPROX((m1*m2).determinant(), m1.determinant() * m2.determinant()); - if(size==1) return; - int i = ei_random(0, size-1); - int j; - do { - j = ei_random(0, size-1); - } while(j==i); - m2 = m1; - m2.row(i).swap(m2.row(j)); - VERIFY_IS_APPROX(m2.determinant(), -m1.determinant()); - m2 = m1; - m2.col(i).swap(m2.col(j)); - VERIFY_IS_APPROX(m2.determinant(), -m1.determinant()); - VERIFY_IS_APPROX(m2.determinant(), m2.transpose().determinant()); - VERIFY_IS_APPROX(ei_conj(m2.determinant()), m2.adjoint().determinant()); - m2 = m1; - m2.row(i) += x*m2.row(j); - VERIFY_IS_APPROX(m2.determinant(), m1.determinant()); - m2 = m1; - m2.row(i) *= x; - VERIFY_IS_APPROX(m2.determinant(), m1.determinant() * x); -} - -void test_eigen2_determinant() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( determinant(Matrix()) ); - CALL_SUBTEST_2( determinant(Matrix()) ); - CALL_SUBTEST_3( determinant(Matrix()) ); - CALL_SUBTEST_4( determinant(Matrix()) ); - CALL_SUBTEST_5( determinant(Matrix, 10, 10>()) ); - CALL_SUBTEST_6( determinant(MatrixXd(20, 20)) ); - } - CALL_SUBTEST_6( determinant(MatrixXd(200, 200)) ); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_dynalloc.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_dynalloc.cpp deleted file mode 100644 index 1891a9e333..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_dynalloc.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -#if EIGEN_ARCH_WANTS_ALIGNMENT -#define ALIGNMENT 16 -#else -#define ALIGNMENT 1 -#endif - -void check_handmade_aligned_malloc() -{ - for(int i = 1; i < 1000; i++) - { - char *p = (char*)ei_handmade_aligned_malloc(i); - VERIFY(std::size_t(p)%ALIGNMENT==0); - // if the buffer is wrongly allocated this will give a bad write --> check with valgrind - for(int j = 0; j < i; j++) p[j]=0; - ei_handmade_aligned_free(p); - } -} - -void check_aligned_malloc() -{ - for(int i = 1; i < 1000; i++) - { - char *p = (char*)ei_aligned_malloc(i); - VERIFY(std::size_t(p)%ALIGNMENT==0); - // if the buffer is wrongly allocated this will give a bad write --> check with valgrind - for(int j = 0; j < i; j++) p[j]=0; - ei_aligned_free(p); - } -} - -void check_aligned_new() -{ - for(int i = 1; i < 1000; i++) - { - float *p = ei_aligned_new(i); - VERIFY(std::size_t(p)%ALIGNMENT==0); - // if the buffer is wrongly allocated this will give a bad write --> check with valgrind - for(int j = 0; j < i; j++) p[j]=0; - ei_aligned_delete(p,i); - } -} - -void check_aligned_stack_alloc() -{ - for(int i = 1; i < 1000; i++) - { - ei_declare_aligned_stack_constructed_variable(float, p, i, 0); - VERIFY(std::size_t(p)%ALIGNMENT==0); - // if the buffer is wrongly allocated this will give a bad write --> check with valgrind - for(int j = 0; j < i; j++) p[j]=0; - } -} - - -// test compilation with both a struct and a class... -struct MyStruct -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - char dummychar; - Vector4f avec; -}; - -class MyClassA -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - char dummychar; - Vector4f avec; -}; - -template void check_dynaligned() -{ - T* obj = new T; - VERIFY(std::size_t(obj)%ALIGNMENT==0); - delete obj; -} - -void test_eigen2_dynalloc() -{ - // low level dynamic memory allocation - CALL_SUBTEST(check_handmade_aligned_malloc()); - CALL_SUBTEST(check_aligned_malloc()); - CALL_SUBTEST(check_aligned_new()); - CALL_SUBTEST(check_aligned_stack_alloc()); - - for (int i=0; i() ); - CALL_SUBTEST( check_dynaligned() ); - CALL_SUBTEST( check_dynaligned() ); - CALL_SUBTEST( check_dynaligned() ); - CALL_SUBTEST( check_dynaligned() ); - } - - // check static allocation, who knows ? - { - MyStruct foo0; VERIFY(std::size_t(foo0.avec.data())%ALIGNMENT==0); - MyClassA fooA; VERIFY(std::size_t(fooA.avec.data())%ALIGNMENT==0); - } - - // dynamic allocation, single object - for (int i=0; iavec.data())%ALIGNMENT==0); - MyClassA *fooA = new MyClassA(); VERIFY(std::size_t(fooA->avec.data())%ALIGNMENT==0); - delete foo0; - delete fooA; - } - - // dynamic allocation, array - const int N = 10; - for (int i=0; iavec.data())%ALIGNMENT==0); - MyClassA *fooA = new MyClassA[N]; VERIFY(std::size_t(fooA->avec.data())%ALIGNMENT==0); - delete[] foo0; - delete[] fooA; - } - -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_eigensolver.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_eigensolver.cpp deleted file mode 100644 index 48b4ace436..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_eigensolver.cpp +++ /dev/null @@ -1,146 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -#ifdef HAS_GSL -#include "gsl_helper.h" -#endif - -template void selfadjointeigensolver(const MatrixType& m) -{ - /* this test covers the following files: - EigenSolver.h, SelfAdjointEigenSolver.h (and indirectly: Tridiagonalization.h) - */ - int rows = m.rows(); - int cols = m.cols(); - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - typedef Matrix RealVectorType; - typedef typename std::complex::Real> Complex; - - RealScalar largerEps = 10*test_precision(); - - MatrixType a = MatrixType::Random(rows,cols); - MatrixType a1 = MatrixType::Random(rows,cols); - MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; - - MatrixType b = MatrixType::Random(rows,cols); - MatrixType b1 = MatrixType::Random(rows,cols); - MatrixType symmB = b.adjoint() * b + b1.adjoint() * b1; - - SelfAdjointEigenSolver eiSymm(symmA); - // generalized eigen pb - SelfAdjointEigenSolver eiSymmGen(symmA, symmB); - - #ifdef HAS_GSL - if (ei_is_same_type::ret) - { - typedef GslTraits Gsl; - typename Gsl::Matrix gEvec=0, gSymmA=0, gSymmB=0; - typename GslTraits::Vector gEval=0; - RealVectorType _eval; - MatrixType _evec; - convert(symmA, gSymmA); - convert(symmB, gSymmB); - convert(symmA, gEvec); - gEval = GslTraits::createVector(rows); - - Gsl::eigen_symm(gSymmA, gEval, gEvec); - convert(gEval, _eval); - convert(gEvec, _evec); - - // test gsl itself ! - VERIFY((symmA * _evec).isApprox(_evec * _eval.asDiagonal(), largerEps)); - - // compare with eigen - VERIFY_IS_APPROX(_eval, eiSymm.eigenvalues()); - VERIFY_IS_APPROX(_evec.cwise().abs(), eiSymm.eigenvectors().cwise().abs()); - - // generalized pb - Gsl::eigen_symm_gen(gSymmA, gSymmB, gEval, gEvec); - convert(gEval, _eval); - convert(gEvec, _evec); - // test GSL itself: - VERIFY((symmA * _evec).isApprox(symmB * (_evec * _eval.asDiagonal()), largerEps)); - - // compare with eigen - MatrixType normalized_eivec = eiSymmGen.eigenvectors()*eiSymmGen.eigenvectors().colwise().norm().asDiagonal().inverse(); - VERIFY_IS_APPROX(_eval, eiSymmGen.eigenvalues()); - VERIFY_IS_APPROX(_evec.cwiseAbs(), normalized_eivec.cwiseAbs()); - - Gsl::free(gSymmA); - Gsl::free(gSymmB); - GslTraits::free(gEval); - Gsl::free(gEvec); - } - #endif - - VERIFY((symmA * eiSymm.eigenvectors()).isApprox( - eiSymm.eigenvectors() * eiSymm.eigenvalues().asDiagonal(), largerEps)); - - // generalized eigen problem Ax = lBx - VERIFY((symmA * eiSymmGen.eigenvectors()).isApprox( - symmB * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); - - MatrixType sqrtSymmA = eiSymm.operatorSqrt(); - VERIFY_IS_APPROX(symmA, sqrtSymmA*sqrtSymmA); - VERIFY_IS_APPROX(sqrtSymmA, symmA*eiSymm.operatorInverseSqrt()); -} - -template void eigensolver(const MatrixType& m) -{ - /* this test covers the following files: - EigenSolver.h - */ - int rows = m.rows(); - int cols = m.cols(); - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - typedef Matrix RealVectorType; - typedef typename std::complex::Real> Complex; - - // RealScalar largerEps = 10*test_precision(); - - MatrixType a = MatrixType::Random(rows,cols); - MatrixType a1 = MatrixType::Random(rows,cols); - MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; - - EigenSolver ei0(symmA); - VERIFY_IS_APPROX(symmA * ei0.pseudoEigenvectors(), ei0.pseudoEigenvectors() * ei0.pseudoEigenvalueMatrix()); - VERIFY_IS_APPROX((symmA.template cast()) * (ei0.pseudoEigenvectors().template cast()), - (ei0.pseudoEigenvectors().template cast()) * (ei0.eigenvalues().asDiagonal())); - - EigenSolver ei1(a); - VERIFY_IS_APPROX(a * ei1.pseudoEigenvectors(), ei1.pseudoEigenvectors() * ei1.pseudoEigenvalueMatrix()); - VERIFY_IS_APPROX(a.template cast() * ei1.eigenvectors(), - ei1.eigenvectors() * ei1.eigenvalues().asDiagonal()); - -} - -void test_eigen2_eigensolver() -{ - for(int i = 0; i < g_repeat; i++) { - // very important to test a 3x3 matrix since we provide a special path for it - CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) ); - CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) ); - CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(7,7)) ); - CALL_SUBTEST_4( selfadjointeigensolver(MatrixXcd(5,5)) ); - CALL_SUBTEST_5( selfadjointeigensolver(MatrixXd(19,19)) ); - - CALL_SUBTEST_6( eigensolver(Matrix4f()) ); - CALL_SUBTEST_5( eigensolver(MatrixXd(17,17)) ); - } -} - diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_first_aligned.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_first_aligned.cpp deleted file mode 100644 index 51bb3cad13..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_first_aligned.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template -void test_eigen2_first_aligned_helper(Scalar *array, int size) -{ - const int packet_size = sizeof(Scalar) * ei_packet_traits::size; - VERIFY(((std::size_t(array) + sizeof(Scalar) * ei_alignmentOffset(array, size)) % packet_size) == 0); -} - -template -void test_eigen2_none_aligned_helper(Scalar *array, int size) -{ - VERIFY(ei_packet_traits::size == 1 || ei_alignmentOffset(array, size) == size); -} - -struct some_non_vectorizable_type { float x; }; - -void test_eigen2_first_aligned() -{ - EIGEN_ALIGN_128 float array_float[100]; - test_first_aligned_helper(array_float, 50); - test_first_aligned_helper(array_float+1, 50); - test_first_aligned_helper(array_float+2, 50); - test_first_aligned_helper(array_float+3, 50); - test_first_aligned_helper(array_float+4, 50); - test_first_aligned_helper(array_float+5, 50); - - EIGEN_ALIGN_128 double array_double[100]; - test_first_aligned_helper(array_double, 50); - test_first_aligned_helper(array_double+1, 50); - test_first_aligned_helper(array_double+2, 50); - - double *array_double_plus_4_bytes = (double*)(std::size_t(array_double)+4); - test_none_aligned_helper(array_double_plus_4_bytes, 50); - test_none_aligned_helper(array_double_plus_4_bytes+1, 50); - - some_non_vectorizable_type array_nonvec[100]; - test_first_aligned_helper(array_nonvec, 100); - test_none_aligned_helper(array_nonvec, 100); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_geometry.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_geometry.cpp deleted file mode 100644 index 5140407740..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_geometry.cpp +++ /dev/null @@ -1,432 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include -#include - -template void geometry(void) -{ - /* this test covers the following files: - Cross.h Quaternion.h, Transform.cpp - */ - - typedef Matrix Matrix2; - typedef Matrix Matrix3; - typedef Matrix Matrix4; - typedef Matrix Vector2; - typedef Matrix Vector3; - typedef Matrix Vector4; - typedef Quaternion Quaternionx; - typedef AngleAxis AngleAxisx; - typedef Transform Transform2; - typedef Transform Transform3; - typedef Scaling Scaling2; - typedef Scaling Scaling3; - typedef Translation Translation2; - typedef Translation Translation3; - - Scalar largeEps = test_precision(); - if (ei_is_same_type::ret) - largeEps = 1e-2f; - - Vector3 v0 = Vector3::Random(), - v1 = Vector3::Random(), - v2 = Vector3::Random(); - Vector2 u0 = Vector2::Random(); - Matrix3 matrot1; - - Scalar a = ei_random(-Scalar(M_PI), Scalar(M_PI)); - - // cross product - VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).eigen2_dot(v1), Scalar(1)); - Matrix3 m; - m << v0.normalized(), - (v0.cross(v1)).normalized(), - (v0.cross(v1).cross(v0)).normalized(); - VERIFY(m.isUnitary()); - - // Quaternion: Identity(), setIdentity(); - Quaternionx q1, q2; - q2.setIdentity(); - VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs()); - q1.coeffs().setRandom(); - VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs()); - - // unitOrthogonal - VERIFY_IS_MUCH_SMALLER_THAN(u0.unitOrthogonal().eigen2_dot(u0), Scalar(1)); - VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().eigen2_dot(v0), Scalar(1)); - VERIFY_IS_APPROX(u0.unitOrthogonal().norm(), Scalar(1)); - VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), Scalar(1)); - - - VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); - VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0); - VERIFY_IS_APPROX(ei_cos(a)*v0.squaredNorm(), v0.eigen2_dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); - m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); - VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); - VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); - - q1 = AngleAxisx(a, v0.normalized()); - q2 = AngleAxisx(a, v1.normalized()); - - // angular distance - Scalar refangle = ei_abs(AngleAxisx(q1.inverse()*q2).angle()); - if (refangle>Scalar(M_PI)) - refangle = Scalar(2)*Scalar(M_PI) - refangle; - - if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps) - { - VERIFY(ei_isApprox(q1.angularDistance(q2), refangle, largeEps)); - } - - // rotation matrix conversion - VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2); - VERIFY_IS_APPROX(q1 * q2 * v2, - q1.toRotationMatrix() * q2.toRotationMatrix() * v2); - - VERIFY( (q2*q1).isApprox(q1*q2, largeEps) || !(q2 * q1 * v2).isApprox( - q1.toRotationMatrix() * q2.toRotationMatrix() * v2)); - - q2 = q1.toRotationMatrix(); - VERIFY_IS_APPROX(q1*v1,q2*v1); - - matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX()) - * AngleAxisx(Scalar(0.2), Vector3::UnitY()) - * AngleAxisx(Scalar(0.3), Vector3::UnitZ()); - VERIFY_IS_APPROX(matrot1 * v1, - AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix() - * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix() - * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1))); - - // angle-axis conversion - AngleAxisx aa = q1; - VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); - - // from two vector creation - VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized()); - VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized()); - - // inverse and conjugate - VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1); - VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1); - - // AngleAxis - VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), - Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix()); - - AngleAxisx aa1; - m = q1.toRotationMatrix(); - aa1 = m; - VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(), - Quaternionx(m).toRotationMatrix()); - - // Transform - // TODO complete the tests ! - a = 0; - while (ei_abs(a)(-Scalar(0.4)*Scalar(M_PI), Scalar(0.4)*Scalar(M_PI)); - q1 = AngleAxisx(a, v0.normalized()); - Transform3 t0, t1, t2; - // first test setIdentity() and Identity() - t0.setIdentity(); - VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); - t0.matrix().setZero(); - t0 = Transform3::Identity(); - VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); - - t0.linear() = q1.toRotationMatrix(); - t1.setIdentity(); - t1.linear() = q1.toRotationMatrix(); - - v0 << 50, 2, 1;//= ei_random_matrix().cwiseProduct(Vector3(10,2,0.5)); - t0.scale(v0); - t1.prescale(v0); - - VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).norm(), v0.x()); - //VERIFY(!ei_isApprox((t1 * Vector3(1,0,0)).norm(), v0.x())); - - t0.setIdentity(); - t1.setIdentity(); - v1 << 1, 2, 3; - t0.linear() = q1.toRotationMatrix(); - t0.pretranslate(v0); - t0.scale(v1); - t1.linear() = q1.conjugate().toRotationMatrix(); - t1.prescale(v1.cwise().inverse()); - t1.translate(-v0); - - VERIFY((t0.matrix() * t1.matrix()).isIdentity(test_precision())); - - t1.fromPositionOrientationScale(v0, q1, v1); - VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); - VERIFY_IS_APPROX(t1*v1, t0*v1); - - t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix()); - t1.setIdentity(); t1.scale(v0).rotate(q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix()); - VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix()); - - // More transform constructors, operator=, operator*= - - Matrix3 mat3 = Matrix3::Random(); - Matrix4 mat4; - mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose(); - Transform3 tmat3(mat3), tmat4(mat4); - tmat4.matrix()(3,3) = Scalar(1); - VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix()); - - Scalar a3 = ei_random(-Scalar(M_PI), Scalar(M_PI)); - Vector3 v3 = Vector3::Random().normalized(); - AngleAxisx aa3(a3, v3); - Transform3 t3(aa3); - Transform3 t4; - t4 = aa3; - VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); - t4.rotate(AngleAxisx(-a3,v3)); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); - t4 *= aa3; - VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); - - v3 = Vector3::Random(); - Translation3 tv3(v3); - Transform3 t5(tv3); - t4 = tv3; - VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); - t4.translate(-v3); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); - t4 *= tv3; - VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); - - Scaling3 sv3(v3); - Transform3 t6(sv3); - t4 = sv3; - VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); - t4.scale(v3.cwise().inverse()); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); - t4 *= sv3; - VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); - - // matrix * transform - VERIFY_IS_APPROX(Transform3(t3.matrix()*t4).matrix(), Transform3(t3*t4).matrix()); - - // chained Transform product - VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix()); - - // check that Transform product doesn't have aliasing problems - t5 = t4; - t5 = t5*t5; - VERIFY_IS_APPROX(t5, t4*t4); - - // 2D transformation - Transform2 t20, t21; - Vector2 v20 = Vector2::Random(); - Vector2 v21 = Vector2::Random(); - for (int k=0; k<2; ++k) - if (ei_abs(v21[k])(a).toRotationMatrix(); - VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(), - t21.pretranslate(v20).scale(v21).matrix()); - - t21.setIdentity(); - t21.linear() = Rotation2D(-a).toRotationMatrix(); - VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) - * (t21.prescale(v21.cwise().inverse()).translate(-v20))).matrix().isIdentity(test_precision()) ); - - // Transform - new API - // 3D - t0.setIdentity(); - t0.rotate(q1).scale(v0).translate(v0); - // mat * scaling and mat * translation - t1 = (Matrix3(q1) * Scaling3(v0)) * Translation3(v0); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // mat * transformation and scaling * translation - t1 = Matrix3(q1) * (Scaling3(v0) * Translation3(v0)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - t0.setIdentity(); - t0.prerotate(q1).prescale(v0).pretranslate(v0); - // translation * scaling and transformation * mat - t1 = (Translation3(v0) * Scaling3(v0)) * Matrix3(q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // scaling * mat and translation * mat - t1 = Translation3(v0) * (Scaling3(v0) * Matrix3(q1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - t0.setIdentity(); - t0.scale(v0).translate(v0).rotate(q1); - // translation * mat and scaling * transformation - t1 = Scaling3(v0) * (Translation3(v0) * Matrix3(q1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // transformation * scaling - t0.scale(v0); - t1 = t1 * Scaling3(v0); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // transformation * translation - t0.translate(v0); - t1 = t1 * Translation3(v0); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // translation * transformation - t0.pretranslate(v0); - t1 = Translation3(v0) * t1; - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // transform * quaternion - t0.rotate(q1); - t1 = t1 * q1; - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // translation * quaternion - t0.translate(v1).rotate(q1); - t1 = t1 * (Translation3(v1) * q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // scaling * quaternion - t0.scale(v1).rotate(q1); - t1 = t1 * (Scaling3(v1) * q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // quaternion * transform - t0.prerotate(q1); - t1 = q1 * t1; - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // quaternion * translation - t0.rotate(q1).translate(v1); - t1 = t1 * (q1 * Translation3(v1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // quaternion * scaling - t0.rotate(q1).scale(v1); - t1 = t1 * (q1 * Scaling3(v1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // translation * vector - t0.setIdentity(); - t0.translate(v0); - VERIFY_IS_APPROX(t0 * v1, Translation3(v0) * v1); - - // scaling * vector - t0.setIdentity(); - t0.scale(v0); - VERIFY_IS_APPROX(t0 * v1, Scaling3(v0) * v1); - - // test transform inversion - t0.setIdentity(); - t0.translate(v0); - t0.linear().setRandom(); - VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse()); - t0.setIdentity(); - t0.translate(v0).rotate(q1); - VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse()); - - // test extract rotation and scaling - t0.setIdentity(); - t0.translate(v0).rotate(q1).scale(v1); - VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1); - - Matrix3 mat_rotation, mat_scaling; - t0.setIdentity(); - t0.translate(v0).rotate(q1).scale(v1); - t0.computeRotationScaling(&mat_rotation, &mat_scaling); - VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling); - VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); - VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); - t0.computeScalingRotation(&mat_scaling, &mat_rotation); - VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation); - VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); - VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); - - // test casting - Transform t1f = t1.template cast(); - VERIFY_IS_APPROX(t1f.template cast(),t1); - Transform t1d = t1.template cast(); - VERIFY_IS_APPROX(t1d.template cast(),t1); - - Translation3 tr1(v0); - Translation tr1f = tr1.template cast(); - VERIFY_IS_APPROX(tr1f.template cast(),tr1); - Translation tr1d = tr1.template cast(); - VERIFY_IS_APPROX(tr1d.template cast(),tr1); - - Scaling3 sc1(v0); - Scaling sc1f = sc1.template cast(); - VERIFY_IS_APPROX(sc1f.template cast(),sc1); - Scaling sc1d = sc1.template cast(); - VERIFY_IS_APPROX(sc1d.template cast(),sc1); - - Quaternion q1f = q1.template cast(); - VERIFY_IS_APPROX(q1f.template cast(),q1); - Quaternion q1d = q1.template cast(); - VERIFY_IS_APPROX(q1d.template cast(),q1); - - AngleAxis aa1f = aa1.template cast(); - VERIFY_IS_APPROX(aa1f.template cast(),aa1); - AngleAxis aa1d = aa1.template cast(); - VERIFY_IS_APPROX(aa1d.template cast(),aa1); - - Rotation2D r2d1(ei_random()); - Rotation2D r2d1f = r2d1.template cast(); - VERIFY_IS_APPROX(r2d1f.template cast(),r2d1); - Rotation2D r2d1d = r2d1.template cast(); - VERIFY_IS_APPROX(r2d1d.template cast(),r2d1); - - m = q1; -// m.col(1) = Vector3(0,ei_random(),ei_random()).normalized(); -// m.col(0) = Vector3(-1,0,0).normalized(); -// m.col(2) = m.col(0).cross(m.col(1)); - #define VERIFY_EULER(I,J,K, X,Y,Z) { \ - Vector3 ea = m.eulerAngles(I,J,K); \ - Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ - VERIFY_IS_APPROX(m, m1); \ - VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ - } - VERIFY_EULER(0,1,2, X,Y,Z); - VERIFY_EULER(0,1,0, X,Y,X); - VERIFY_EULER(0,2,1, X,Z,Y); - VERIFY_EULER(0,2,0, X,Z,X); - - VERIFY_EULER(1,2,0, Y,Z,X); - VERIFY_EULER(1,2,1, Y,Z,Y); - VERIFY_EULER(1,0,2, Y,X,Z); - VERIFY_EULER(1,0,1, Y,X,Y); - - VERIFY_EULER(2,0,1, Z,X,Y); - VERIFY_EULER(2,0,2, Z,X,Z); - VERIFY_EULER(2,1,0, Z,Y,X); - VERIFY_EULER(2,1,2, Z,Y,Z); - - // colwise/rowwise cross product - mat3.setRandom(); - Vector3 vec3 = Vector3::Random(); - Matrix3 mcross; - int i = ei_random(0,2); - mcross = mat3.colwise().cross(vec3); - VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3)); - mcross = mat3.rowwise().cross(vec3); - VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3)); - - -} - -void test_eigen2_geometry() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( geometry() ); - CALL_SUBTEST_2( geometry() ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp deleted file mode 100644 index 12d4a71c3c..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp +++ /dev/null @@ -1,435 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN2_SUPPORT_STAGE15_RESOLVE_API_CONFLICTS_WARN - -#include "main.h" -#include -#include -#include - -template void geometry(void) -{ - /* this test covers the following files: - Cross.h Quaternion.h, Transform.cpp - */ - - typedef Matrix Matrix2; - typedef Matrix Matrix3; - typedef Matrix Matrix4; - typedef Matrix Vector2; - typedef Matrix Vector3; - typedef Matrix Vector4; - typedef eigen2_Quaternion Quaternionx; - typedef eigen2_AngleAxis AngleAxisx; - typedef eigen2_Transform Transform2; - typedef eigen2_Transform Transform3; - typedef eigen2_Scaling Scaling2; - typedef eigen2_Scaling Scaling3; - typedef eigen2_Translation Translation2; - typedef eigen2_Translation Translation3; - - Scalar largeEps = test_precision(); - if (ei_is_same_type::ret) - largeEps = 1e-2f; - - Vector3 v0 = Vector3::Random(), - v1 = Vector3::Random(), - v2 = Vector3::Random(); - Vector2 u0 = Vector2::Random(); - Matrix3 matrot1; - - Scalar a = ei_random(-Scalar(M_PI), Scalar(M_PI)); - - // cross product - VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).eigen2_dot(v1), Scalar(1)); - Matrix3 m; - m << v0.normalized(), - (v0.cross(v1)).normalized(), - (v0.cross(v1).cross(v0)).normalized(); - VERIFY(m.isUnitary()); - - // Quaternion: Identity(), setIdentity(); - Quaternionx q1, q2; - q2.setIdentity(); - VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs()); - q1.coeffs().setRandom(); - VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs()); - - // unitOrthogonal - VERIFY_IS_MUCH_SMALLER_THAN(u0.unitOrthogonal().eigen2_dot(u0), Scalar(1)); - VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().eigen2_dot(v0), Scalar(1)); - VERIFY_IS_APPROX(u0.unitOrthogonal().norm(), Scalar(1)); - VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), Scalar(1)); - - - VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); - VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0); - VERIFY_IS_APPROX(ei_cos(a)*v0.squaredNorm(), v0.eigen2_dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); - m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); - VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); - VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); - - q1 = AngleAxisx(a, v0.normalized()); - q2 = AngleAxisx(a, v1.normalized()); - - // angular distance - Scalar refangle = ei_abs(AngleAxisx(q1.inverse()*q2).angle()); - if (refangle>Scalar(M_PI)) - refangle = Scalar(2)*Scalar(M_PI) - refangle; - - if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps) - { - VERIFY(ei_isApprox(q1.angularDistance(q2), refangle, largeEps)); - } - - // rotation matrix conversion - VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2); - VERIFY_IS_APPROX(q1 * q2 * v2, - q1.toRotationMatrix() * q2.toRotationMatrix() * v2); - - VERIFY( (q2*q1).isApprox(q1*q2, largeEps) || !(q2 * q1 * v2).isApprox( - q1.toRotationMatrix() * q2.toRotationMatrix() * v2)); - - q2 = q1.toRotationMatrix(); - VERIFY_IS_APPROX(q1*v1,q2*v1); - - matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX()) - * AngleAxisx(Scalar(0.2), Vector3::UnitY()) - * AngleAxisx(Scalar(0.3), Vector3::UnitZ()); - VERIFY_IS_APPROX(matrot1 * v1, - AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix() - * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix() - * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1))); - - // angle-axis conversion - AngleAxisx aa = q1; - VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); - - // from two vector creation - VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized()); - VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized()); - - // inverse and conjugate - VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1); - VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1); - - // AngleAxis - VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), - Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix()); - - AngleAxisx aa1; - m = q1.toRotationMatrix(); - aa1 = m; - VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(), - Quaternionx(m).toRotationMatrix()); - - // Transform - // TODO complete the tests ! - a = 0; - while (ei_abs(a)(-Scalar(0.4)*Scalar(M_PI), Scalar(0.4)*Scalar(M_PI)); - q1 = AngleAxisx(a, v0.normalized()); - Transform3 t0, t1, t2; - // first test setIdentity() and Identity() - t0.setIdentity(); - VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); - t0.matrix().setZero(); - t0 = Transform3::Identity(); - VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); - - t0.linear() = q1.toRotationMatrix(); - t1.setIdentity(); - t1.linear() = q1.toRotationMatrix(); - - v0 << 50, 2, 1;//= ei_random_matrix().cwiseProduct(Vector3(10,2,0.5)); - t0.scale(v0); - t1.prescale(v0); - - VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).norm(), v0.x()); - //VERIFY(!ei_isApprox((t1 * Vector3(1,0,0)).norm(), v0.x())); - - t0.setIdentity(); - t1.setIdentity(); - v1 << 1, 2, 3; - t0.linear() = q1.toRotationMatrix(); - t0.pretranslate(v0); - t0.scale(v1); - t1.linear() = q1.conjugate().toRotationMatrix(); - t1.prescale(v1.cwise().inverse()); - t1.translate(-v0); - - VERIFY((t0.matrix() * t1.matrix()).isIdentity(test_precision())); - - t1.fromPositionOrientationScale(v0, q1, v1); - VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); - VERIFY_IS_APPROX(t1*v1, t0*v1); - - t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix()); - t1.setIdentity(); t1.scale(v0).rotate(q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix()); - VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix()); - - // More transform constructors, operator=, operator*= - - Matrix3 mat3 = Matrix3::Random(); - Matrix4 mat4; - mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose(); - Transform3 tmat3(mat3), tmat4(mat4); - tmat4.matrix()(3,3) = Scalar(1); - VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix()); - - Scalar a3 = ei_random(-Scalar(M_PI), Scalar(M_PI)); - Vector3 v3 = Vector3::Random().normalized(); - AngleAxisx aa3(a3, v3); - Transform3 t3(aa3); - Transform3 t4; - t4 = aa3; - VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); - t4.rotate(AngleAxisx(-a3,v3)); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); - t4 *= aa3; - VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); - - v3 = Vector3::Random(); - Translation3 tv3(v3); - Transform3 t5(tv3); - t4 = tv3; - VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); - t4.translate(-v3); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); - t4 *= tv3; - VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); - - Scaling3 sv3(v3); - Transform3 t6(sv3); - t4 = sv3; - VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); - t4.scale(v3.cwise().inverse()); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); - t4 *= sv3; - VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); - - // matrix * transform - VERIFY_IS_APPROX(Transform3(t3.matrix()*t4).matrix(), Transform3(t3*t4).matrix()); - - // chained Transform product - VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix()); - - // check that Transform product doesn't have aliasing problems - t5 = t4; - t5 = t5*t5; - VERIFY_IS_APPROX(t5, t4*t4); - - // 2D transformation - Transform2 t20, t21; - Vector2 v20 = Vector2::Random(); - Vector2 v21 = Vector2::Random(); - for (int k=0; k<2; ++k) - if (ei_abs(v21[k])(a).toRotationMatrix(); - VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(), - t21.pretranslate(v20).scale(v21).matrix()); - - t21.setIdentity(); - t21.linear() = Rotation2D(-a).toRotationMatrix(); - VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) - * (t21.prescale(v21.cwise().inverse()).translate(-v20))).matrix().isIdentity(test_precision()) ); - - // Transform - new API - // 3D - t0.setIdentity(); - t0.rotate(q1).scale(v0).translate(v0); - // mat * scaling and mat * translation - t1 = (Matrix3(q1) * Scaling3(v0)) * Translation3(v0); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // mat * transformation and scaling * translation - t1 = Matrix3(q1) * (Scaling3(v0) * Translation3(v0)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - t0.setIdentity(); - t0.prerotate(q1).prescale(v0).pretranslate(v0); - // translation * scaling and transformation * mat - t1 = (Translation3(v0) * Scaling3(v0)) * Matrix3(q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // scaling * mat and translation * mat - t1 = Translation3(v0) * (Scaling3(v0) * Matrix3(q1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - t0.setIdentity(); - t0.scale(v0).translate(v0).rotate(q1); - // translation * mat and scaling * transformation - t1 = Scaling3(v0) * (Translation3(v0) * Matrix3(q1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // transformation * scaling - t0.scale(v0); - t1 = t1 * Scaling3(v0); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // transformation * translation - t0.translate(v0); - t1 = t1 * Translation3(v0); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // translation * transformation - t0.pretranslate(v0); - t1 = Translation3(v0) * t1; - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // transform * quaternion - t0.rotate(q1); - t1 = t1 * q1; - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // translation * quaternion - t0.translate(v1).rotate(q1); - t1 = t1 * (Translation3(v1) * q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // scaling * quaternion - t0.scale(v1).rotate(q1); - t1 = t1 * (Scaling3(v1) * q1); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // quaternion * transform - t0.prerotate(q1); - t1 = q1 * t1; - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // quaternion * translation - t0.rotate(q1).translate(v1); - t1 = t1 * (q1 * Translation3(v1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // quaternion * scaling - t0.rotate(q1).scale(v1); - t1 = t1 * (q1 * Scaling3(v1)); - VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - - // translation * vector - t0.setIdentity(); - t0.translate(v0); - VERIFY_IS_APPROX(t0 * v1, Translation3(v0) * v1); - - // scaling * vector - t0.setIdentity(); - t0.scale(v0); - VERIFY_IS_APPROX(t0 * v1, Scaling3(v0) * v1); - - // test transform inversion - t0.setIdentity(); - t0.translate(v0); - t0.linear().setRandom(); - VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse()); - t0.setIdentity(); - t0.translate(v0).rotate(q1); - VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse()); - - // test extract rotation and scaling - t0.setIdentity(); - t0.translate(v0).rotate(q1).scale(v1); - VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1); - - Matrix3 mat_rotation, mat_scaling; - t0.setIdentity(); - t0.translate(v0).rotate(q1).scale(v1); - t0.computeRotationScaling(&mat_rotation, &mat_scaling); - VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling); - VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); - VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); - t0.computeScalingRotation(&mat_scaling, &mat_rotation); - VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation); - VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); - VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); - - // test casting - eigen2_Transform t1f = t1.template cast(); - VERIFY_IS_APPROX(t1f.template cast(),t1); - eigen2_Transform t1d = t1.template cast(); - VERIFY_IS_APPROX(t1d.template cast(),t1); - - Translation3 tr1(v0); - eigen2_Translation tr1f = tr1.template cast(); - VERIFY_IS_APPROX(tr1f.template cast(),tr1); - eigen2_Translation tr1d = tr1.template cast(); - VERIFY_IS_APPROX(tr1d.template cast(),tr1); - - Scaling3 sc1(v0); - eigen2_Scaling sc1f = sc1.template cast(); - VERIFY_IS_APPROX(sc1f.template cast(),sc1); - eigen2_Scaling sc1d = sc1.template cast(); - VERIFY_IS_APPROX(sc1d.template cast(),sc1); - - eigen2_Quaternion q1f = q1.template cast(); - VERIFY_IS_APPROX(q1f.template cast(),q1); - eigen2_Quaternion q1d = q1.template cast(); - VERIFY_IS_APPROX(q1d.template cast(),q1); - - eigen2_AngleAxis aa1f = aa1.template cast(); - VERIFY_IS_APPROX(aa1f.template cast(),aa1); - eigen2_AngleAxis aa1d = aa1.template cast(); - VERIFY_IS_APPROX(aa1d.template cast(),aa1); - - eigen2_Rotation2D r2d1(ei_random()); - eigen2_Rotation2D r2d1f = r2d1.template cast(); - VERIFY_IS_APPROX(r2d1f.template cast(),r2d1); - eigen2_Rotation2D r2d1d = r2d1.template cast(); - VERIFY_IS_APPROX(r2d1d.template cast(),r2d1); - - m = q1; -// m.col(1) = Vector3(0,ei_random(),ei_random()).normalized(); -// m.col(0) = Vector3(-1,0,0).normalized(); -// m.col(2) = m.col(0).cross(m.col(1)); - #define VERIFY_EULER(I,J,K, X,Y,Z) { \ - Vector3 ea = m.eulerAngles(I,J,K); \ - Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ - VERIFY_IS_APPROX(m, m1); \ - VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ - } - VERIFY_EULER(0,1,2, X,Y,Z); - VERIFY_EULER(0,1,0, X,Y,X); - VERIFY_EULER(0,2,1, X,Z,Y); - VERIFY_EULER(0,2,0, X,Z,X); - - VERIFY_EULER(1,2,0, Y,Z,X); - VERIFY_EULER(1,2,1, Y,Z,Y); - VERIFY_EULER(1,0,2, Y,X,Z); - VERIFY_EULER(1,0,1, Y,X,Y); - - VERIFY_EULER(2,0,1, Z,X,Y); - VERIFY_EULER(2,0,2, Z,X,Z); - VERIFY_EULER(2,1,0, Z,Y,X); - VERIFY_EULER(2,1,2, Z,Y,Z); - - // colwise/rowwise cross product - mat3.setRandom(); - Vector3 vec3 = Vector3::Random(); - Matrix3 mcross; - int i = ei_random(0,2); - mcross = mat3.colwise().cross(vec3); - VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3)); - mcross = mat3.rowwise().cross(vec3); - VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3)); - - -} - -void test_eigen2_geometry_with_eigen2_prefix() -{ - std::cout << "eigen2 support: " << EIGEN2_SUPPORT_STAGE << std::endl; - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( geometry() ); - CALL_SUBTEST_2( geometry() ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_hyperplane.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_hyperplane.cpp deleted file mode 100644 index f3f85e14df..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_hyperplane.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include -#include - -template void hyperplane(const HyperplaneType& _plane) -{ - /* this test covers the following files: - Hyperplane.h - */ - - const int dim = _plane.dim(); - typedef typename HyperplaneType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - typedef Matrix MatrixType; - - VectorType p0 = VectorType::Random(dim); - VectorType p1 = VectorType::Random(dim); - - VectorType n0 = VectorType::Random(dim).normalized(); - VectorType n1 = VectorType::Random(dim).normalized(); - - HyperplaneType pl0(n0, p0); - HyperplaneType pl1(n1, p1); - HyperplaneType pl2 = pl1; - - Scalar s0 = ei_random(); - Scalar s1 = ei_random(); - - VERIFY_IS_APPROX( n1.eigen2_dot(n1), Scalar(1) ); - - VERIFY_IS_MUCH_SMALLER_THAN( pl0.absDistance(p0), Scalar(1) ); - VERIFY_IS_APPROX( pl1.signedDistance(p1 + n1 * s0), s0 ); - VERIFY_IS_MUCH_SMALLER_THAN( pl1.signedDistance(pl1.projection(p0)), Scalar(1) ); - VERIFY_IS_MUCH_SMALLER_THAN( pl1.absDistance(p1 + pl1.normal().unitOrthogonal() * s1), Scalar(1) ); - - // transform - if (!NumTraits::IsComplex) - { - MatrixType rot = MatrixType::Random(dim,dim).qr().matrixQ(); - Scaling scaling(VectorType::Random()); - Translation translation(VectorType::Random()); - - pl2 = pl1; - VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot).absDistance(rot * p1), Scalar(1) ); - pl2 = pl1; - VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot,Isometry).absDistance(rot * p1), Scalar(1) ); - pl2 = pl1; - VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling).absDistance((rot*scaling) * p1), Scalar(1) ); - pl2 = pl1; - VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling*translation) - .absDistance((rot*scaling*translation) * p1), Scalar(1) ); - pl2 = pl1; - VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*translation,Isometry) - .absDistance((rot*translation) * p1), Scalar(1) ); - } - - // casting - const int Dim = HyperplaneType::AmbientDimAtCompileTime; - typedef typename GetDifferentType::type OtherScalar; - Hyperplane hp1f = pl1.template cast(); - VERIFY_IS_APPROX(hp1f.template cast(),pl1); - Hyperplane hp1d = pl1.template cast(); - VERIFY_IS_APPROX(hp1d.template cast(),pl1); -} - -template void lines() -{ - typedef Hyperplane HLine; - typedef ParametrizedLine PLine; - typedef Matrix Vector; - typedef Matrix CoeffsType; - - for(int i = 0; i < 10; i++) - { - Vector center = Vector::Random(); - Vector u = Vector::Random(); - Vector v = Vector::Random(); - Scalar a = ei_random(); - while (ei_abs(a-1) < 1e-4) a = ei_random(); - while (u.norm() < 1e-4) u = Vector::Random(); - while (v.norm() < 1e-4) v = Vector::Random(); - - HLine line_u = HLine::Through(center + u, center + a*u); - HLine line_v = HLine::Through(center + v, center + a*v); - - // the line equations should be normalized so that a^2+b^2=1 - VERIFY_IS_APPROX(line_u.normal().norm(), Scalar(1)); - VERIFY_IS_APPROX(line_v.normal().norm(), Scalar(1)); - - Vector result = line_u.intersection(line_v); - - // the lines should intersect at the point we called "center" - VERIFY_IS_APPROX(result, center); - - // check conversions between two types of lines - PLine pl(line_u); // gcc 3.3 will commit suicide if we don't name this variable - CoeffsType converted_coeffs(HLine(pl).coeffs()); - converted_coeffs *= line_u.coeffs()(0)/converted_coeffs(0); - VERIFY(line_u.coeffs().isApprox(converted_coeffs)); - } -} - -void test_eigen2_hyperplane() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( hyperplane(Hyperplane()) ); - CALL_SUBTEST_2( hyperplane(Hyperplane()) ); - CALL_SUBTEST_3( hyperplane(Hyperplane()) ); - CALL_SUBTEST_4( hyperplane(Hyperplane,5>()) ); - CALL_SUBTEST_5( lines() ); - CALL_SUBTEST_6( lines() ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_inverse.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_inverse.cpp deleted file mode 100644 index ccd24a194d..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_inverse.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template void inverse(const MatrixType& m) -{ - /* this test covers the following files: - Inverse.h - */ - int rows = m.rows(); - int cols = m.cols(); - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - - MatrixType m1 = MatrixType::Random(rows, cols), - m2(rows, cols), - identity = MatrixType::Identity(rows, rows); - - while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8) - { - m1 = MatrixType::Random(rows, cols); - } - - m2 = m1.inverse(); - VERIFY_IS_APPROX(m1, m2.inverse() ); - - m1.computeInverse(&m2); - VERIFY_IS_APPROX(m1, m2.inverse() ); - - VERIFY_IS_APPROX((Scalar(2)*m2).inverse(), m2.inverse()*Scalar(0.5)); - - VERIFY_IS_APPROX(identity, m1.inverse() * m1 ); - VERIFY_IS_APPROX(identity, m1 * m1.inverse() ); - - VERIFY_IS_APPROX(m1, m1.inverse().inverse() ); - - // since for the general case we implement separately row-major and col-major, test that - VERIFY_IS_APPROX(m1.transpose().inverse(), m1.inverse().transpose()); -} - -void test_eigen2_inverse() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( inverse(Matrix()) ); - CALL_SUBTEST_2( inverse(Matrix2d()) ); - CALL_SUBTEST_3( inverse(Matrix3f()) ); - CALL_SUBTEST_4( inverse(Matrix4f()) ); - CALL_SUBTEST_5( inverse(MatrixXf(8,8)) ); - CALL_SUBTEST_6( inverse(MatrixXcd(7,7)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_linearstructure.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_linearstructure.cpp deleted file mode 100644 index 488f4c4850..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_linearstructure.cpp +++ /dev/null @@ -1,83 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void linearStructure(const MatrixType& m) -{ - /* this test covers the following files: - Sum.h Difference.h Opposite.h ScalarMultiple.h - */ - - typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; - - int rows = m.rows(); - int cols = m.cols(); - - // this test relies a lot on Random.h, and there's not much more that we can do - // to test it, hence I consider that we will have tested Random.h - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols); - - Scalar s1 = ei_random(); - while (ei_abs(s1)<1e-3) s1 = ei_random(); - - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - - VERIFY_IS_APPROX(-(-m1), m1); - VERIFY_IS_APPROX(m1+m1, 2*m1); - VERIFY_IS_APPROX(m1+m2-m1, m2); - VERIFY_IS_APPROX(-m2+m1+m2, m1); - VERIFY_IS_APPROX(m1*s1, s1*m1); - VERIFY_IS_APPROX((m1+m2)*s1, s1*m1+s1*m2); - VERIFY_IS_APPROX((-m1+m2)*s1, -s1*m1+s1*m2); - m3 = m2; m3 += m1; - VERIFY_IS_APPROX(m3, m1+m2); - m3 = m2; m3 -= m1; - VERIFY_IS_APPROX(m3, m2-m1); - m3 = m2; m3 *= s1; - VERIFY_IS_APPROX(m3, s1*m2); - if(NumTraits::HasFloatingPoint) - { - m3 = m2; m3 /= s1; - VERIFY_IS_APPROX(m3, m2/s1); - } - - // again, test operator() to check const-qualification - VERIFY_IS_APPROX((-m1)(r,c), -(m1(r,c))); - VERIFY_IS_APPROX((m1-m2)(r,c), (m1(r,c))-(m2(r,c))); - VERIFY_IS_APPROX((m1+m2)(r,c), (m1(r,c))+(m2(r,c))); - VERIFY_IS_APPROX((s1*m1)(r,c), s1*(m1(r,c))); - VERIFY_IS_APPROX((m1*s1)(r,c), (m1(r,c))*s1); - if(NumTraits::HasFloatingPoint) - VERIFY_IS_APPROX((m1/s1)(r,c), (m1(r,c))/s1); - - // use .block to disable vectorization and compare to the vectorized version - VERIFY_IS_APPROX(m1+m1.block(0,0,rows,cols), m1+m1); - VERIFY_IS_APPROX(m1.cwise() * m1.block(0,0,rows,cols), m1.cwise() * m1); - VERIFY_IS_APPROX(m1 - m1.block(0,0,rows,cols), m1 - m1); - VERIFY_IS_APPROX(m1.block(0,0,rows,cols) * s1, m1 * s1); -} - -void test_eigen2_linearstructure() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( linearStructure(Matrix()) ); - CALL_SUBTEST_2( linearStructure(Matrix2f()) ); - CALL_SUBTEST_3( linearStructure(Vector3d()) ); - CALL_SUBTEST_4( linearStructure(Matrix4d()) ); - CALL_SUBTEST_5( linearStructure(MatrixXcf(3, 3)) ); - CALL_SUBTEST_6( linearStructure(MatrixXf(8, 12)) ); - CALL_SUBTEST_7( linearStructure(MatrixXi(8, 12)) ); - CALL_SUBTEST_8( linearStructure(MatrixXcd(20, 20)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_lu.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_lu.cpp deleted file mode 100644 index e993b1c7c0..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_lu.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template -void doSomeRankPreservingOperations(Eigen::MatrixBase& m) -{ - typedef typename Derived::RealScalar RealScalar; - for(int a = 0; a < 3*(m.rows()+m.cols()); a++) - { - RealScalar d = Eigen::ei_random(-1,1); - int i = Eigen::ei_random(0,m.rows()-1); // i is a random row number - int j; - do { - j = Eigen::ei_random(0,m.rows()-1); - } while (i==j); // j is another one (must be different) - m.row(i) += d * m.row(j); - - i = Eigen::ei_random(0,m.cols()-1); // i is a random column number - do { - j = Eigen::ei_random(0,m.cols()-1); - } while (i==j); // j is another one (must be different) - m.col(i) += d * m.col(j); - } -} - -template void lu_non_invertible() -{ - /* this test covers the following files: - LU.h - */ - // NOTE there seems to be a problem with too small sizes -- could easily lie in the doSomeRankPreservingOperations function - int rows = ei_random(20,200), cols = ei_random(20,200), cols2 = ei_random(20,200); - int rank = ei_random(1, std::min(rows, cols)-1); - - MatrixType m1(rows, cols), m2(cols, cols2), m3(rows, cols2), k(1,1); - m1 = MatrixType::Random(rows,cols); - if(rows <= cols) - for(int i = rank; i < rows; i++) m1.row(i).setZero(); - else - for(int i = rank; i < cols; i++) m1.col(i).setZero(); - doSomeRankPreservingOperations(m1); - - LU lu(m1); - typename LU::KernelResultType m1kernel = lu.kernel(); - typename LU::ImageResultType m1image = lu.image(); - - VERIFY(rank == lu.rank()); - VERIFY(cols - lu.rank() == lu.dimensionOfKernel()); - VERIFY(!lu.isInjective()); - VERIFY(!lu.isInvertible()); - VERIFY(lu.isSurjective() == (lu.rank() == rows)); - VERIFY((m1 * m1kernel).isMuchSmallerThan(m1)); - VERIFY(m1image.lu().rank() == rank); - MatrixType sidebyside(m1.rows(), m1.cols() + m1image.cols()); - sidebyside << m1, m1image; - VERIFY(sidebyside.lu().rank() == rank); - m2 = MatrixType::Random(cols,cols2); - m3 = m1*m2; - m2 = MatrixType::Random(cols,cols2); - lu.solve(m3, &m2); - VERIFY_IS_APPROX(m3, m1*m2); - /* solve now always returns true - m3 = MatrixType::Random(rows,cols2); - VERIFY(!lu.solve(m3, &m2)); - */ -} - -template void lu_invertible() -{ - /* this test covers the following files: - LU.h - */ - typedef typename NumTraits::Real RealScalar; - int size = ei_random(10,200); - - MatrixType m1(size, size), m2(size, size), m3(size, size); - m1 = MatrixType::Random(size,size); - - if (ei_is_same_type::ret) - { - // let's build a matrix more stable to inverse - MatrixType a = MatrixType::Random(size,size*2); - m1 += a * a.adjoint(); - } - - LU lu(m1); - VERIFY(0 == lu.dimensionOfKernel()); - VERIFY(size == lu.rank()); - VERIFY(lu.isInjective()); - VERIFY(lu.isSurjective()); - VERIFY(lu.isInvertible()); - VERIFY(lu.image().lu().isInvertible()); - m3 = MatrixType::Random(size,size); - lu.solve(m3, &m2); - VERIFY_IS_APPROX(m3, m1*m2); - VERIFY_IS_APPROX(m2, lu.inverse()*m3); - m3 = MatrixType::Random(size,size); - VERIFY(lu.solve(m3, &m2)); -} - -void test_eigen2_lu() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( lu_non_invertible() ); - CALL_SUBTEST_2( lu_non_invertible() ); - CALL_SUBTEST_3( lu_non_invertible() ); - CALL_SUBTEST_4( lu_non_invertible() ); - CALL_SUBTEST_1( lu_invertible() ); - CALL_SUBTEST_2( lu_invertible() ); - CALL_SUBTEST_3( lu_invertible() ); - CALL_SUBTEST_4( lu_invertible() ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_map.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_map.cpp deleted file mode 100644 index 4a1c4e11ad..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_map.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2007-2010 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void map_class_vector(const VectorType& m) -{ - typedef typename VectorType::Scalar Scalar; - - int size = m.size(); - - // test Map.h - Scalar* array1 = ei_aligned_new(size); - Scalar* array2 = ei_aligned_new(size); - Scalar* array3 = new Scalar[size+1]; - Scalar* array3unaligned = std::size_t(array3)%16 == 0 ? array3+1 : array3; - - Map(array1, size) = VectorType::Random(size); - Map(array2, size) = Map(array1, size); - Map(array3unaligned, size) = Map((const Scalar*)array1, size); // test non-const-correctness support in eigen2 - VectorType ma1 = Map(array1, size); - VectorType ma2 = Map(array2, size); - VectorType ma3 = Map(array3unaligned, size); - VERIFY_IS_APPROX(ma1, ma2); - VERIFY_IS_APPROX(ma1, ma3); - - ei_aligned_delete(array1, size); - ei_aligned_delete(array2, size); - delete[] array3; -} - -template void map_class_matrix(const MatrixType& m) -{ - typedef typename MatrixType::Scalar Scalar; - - int rows = m.rows(), cols = m.cols(), size = rows*cols; - - // test Map.h - Scalar* array1 = ei_aligned_new(size); - for(int i = 0; i < size; i++) array1[i] = Scalar(1); - Scalar* array2 = ei_aligned_new(size); - for(int i = 0; i < size; i++) array2[i] = Scalar(1); - Scalar* array3 = new Scalar[size+1]; - for(int i = 0; i < size+1; i++) array3[i] = Scalar(1); - Scalar* array3unaligned = std::size_t(array3)%16 == 0 ? array3+1 : array3; - Map(array1, rows, cols) = MatrixType::Ones(rows,cols); - Map(array2, rows, cols) = Map((const Scalar*)array1, rows, cols); // test non-const-correctness support in eigen2 - Map(array3unaligned, rows, cols) = Map(array1, rows, cols); - MatrixType ma1 = Map(array1, rows, cols); - MatrixType ma2 = Map(array2, rows, cols); - VERIFY_IS_APPROX(ma1, ma2); - MatrixType ma3 = Map(array3unaligned, rows, cols); - VERIFY_IS_APPROX(ma1, ma3); - - ei_aligned_delete(array1, size); - ei_aligned_delete(array2, size); - delete[] array3; -} - -template void map_static_methods(const VectorType& m) -{ - typedef typename VectorType::Scalar Scalar; - - int size = m.size(); - - // test Map.h - Scalar* array1 = ei_aligned_new(size); - Scalar* array2 = ei_aligned_new(size); - Scalar* array3 = new Scalar[size+1]; - Scalar* array3unaligned = std::size_t(array3)%16 == 0 ? array3+1 : array3; - - VectorType::MapAligned(array1, size) = VectorType::Random(size); - VectorType::Map(array2, size) = VectorType::Map(array1, size); - VectorType::Map(array3unaligned, size) = VectorType::Map(array1, size); - VectorType ma1 = VectorType::Map(array1, size); - VectorType ma2 = VectorType::MapAligned(array2, size); - VectorType ma3 = VectorType::Map(array3unaligned, size); - VERIFY_IS_APPROX(ma1, ma2); - VERIFY_IS_APPROX(ma1, ma3); - - ei_aligned_delete(array1, size); - ei_aligned_delete(array2, size); - delete[] array3; -} - - -void test_eigen2_map() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( map_class_vector(Matrix()) ); - CALL_SUBTEST_2( map_class_vector(Vector4d()) ); - CALL_SUBTEST_3( map_class_vector(RowVector4f()) ); - CALL_SUBTEST_4( map_class_vector(VectorXcf(8)) ); - CALL_SUBTEST_5( map_class_vector(VectorXi(12)) ); - - CALL_SUBTEST_1( map_class_matrix(Matrix()) ); - CALL_SUBTEST_2( map_class_matrix(Matrix4d()) ); - CALL_SUBTEST_6( map_class_matrix(Matrix()) ); - CALL_SUBTEST_4( map_class_matrix(MatrixXcf(ei_random(1,10),ei_random(1,10))) ); - CALL_SUBTEST_5( map_class_matrix(MatrixXi(ei_random(1,10),ei_random(1,10))) ); - - CALL_SUBTEST_1( map_static_methods(Matrix()) ); - CALL_SUBTEST_2( map_static_methods(Vector3f()) ); - CALL_SUBTEST_7( map_static_methods(RowVector3d()) ); - CALL_SUBTEST_4( map_static_methods(VectorXcd(8)) ); - CALL_SUBTEST_5( map_static_methods(VectorXf(12)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_meta.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_meta.cpp deleted file mode 100644 index 1d01bd84d5..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_meta.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -void test_eigen2_meta() -{ - typedef float & FloatRef; - typedef const float & ConstFloatRef; - - VERIFY((ei_meta_if<(3<4),ei_meta_true, ei_meta_false>::ret::ret)); - VERIFY(( ei_is_same_type::ret)); - VERIFY((!ei_is_same_type::ret)); - VERIFY((!ei_is_same_type::ret)); - VERIFY((!ei_is_same_type::ret)); - - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - VERIFY(( ei_is_same_type::type >::ret)); - - VERIFY(ei_meta_sqrt<1>::ret == 1); - #define VERIFY_META_SQRT(X) VERIFY(ei_meta_sqrt::ret == int(ei_sqrt(double(X)))) - VERIFY_META_SQRT(2); - VERIFY_META_SQRT(3); - VERIFY_META_SQRT(4); - VERIFY_META_SQRT(5); - VERIFY_META_SQRT(6); - VERIFY_META_SQRT(8); - VERIFY_META_SQRT(9); - VERIFY_META_SQRT(15); - VERIFY_META_SQRT(16); - VERIFY_META_SQRT(17); - VERIFY_META_SQRT(255); - VERIFY_META_SQRT(256); - VERIFY_META_SQRT(257); - VERIFY_META_SQRT(1023); - VERIFY_META_SQRT(1024); - VERIFY_META_SQRT(1025); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_miscmatrices.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_miscmatrices.cpp deleted file mode 100644 index 8bbb20cc81..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_miscmatrices.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void miscMatrices(const MatrixType& m) -{ - /* this test covers the following files: - DiagonalMatrix.h Ones.h - */ - - typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; - typedef Matrix RowVectorType; - int rows = m.rows(); - int cols = m.cols(); - - int r = ei_random(0, rows-1), r2 = ei_random(0, rows-1), c = ei_random(0, cols-1); - VERIFY_IS_APPROX(MatrixType::Ones(rows,cols)(r,c), static_cast(1)); - MatrixType m1 = MatrixType::Ones(rows,cols); - VERIFY_IS_APPROX(m1(r,c), static_cast(1)); - VectorType v1 = VectorType::Random(rows); - v1[0]; - Matrix - square = v1.asDiagonal(); - if(r==r2) VERIFY_IS_APPROX(square(r,r2), v1[r]); - else VERIFY_IS_MUCH_SMALLER_THAN(square(r,r2), static_cast(1)); - square = MatrixType::Zero(rows, rows); - square.diagonal() = VectorType::Ones(rows); - VERIFY_IS_APPROX(square, MatrixType::Identity(rows, rows)); -} - -void test_eigen2_miscmatrices() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( miscMatrices(Matrix()) ); - CALL_SUBTEST_2( miscMatrices(Matrix4d()) ); - CALL_SUBTEST_3( miscMatrices(MatrixXcf(3, 3)) ); - CALL_SUBTEST_4( miscMatrices(MatrixXi(8, 12)) ); - CALL_SUBTEST_5( miscMatrices(MatrixXcd(20, 20)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_mixingtypes.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_mixingtypes.cpp deleted file mode 100644 index fb5ac5ddad..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_mixingtypes.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_NO_STATIC_ASSERT -#define EIGEN_NO_STATIC_ASSERT // turn static asserts into runtime asserts in order to check them -#endif - -#ifndef EIGEN_DONT_VECTORIZE -#define EIGEN_DONT_VECTORIZE // SSE intrinsics aren't designed to allow mixing types -#endif - -#include "main.h" - - -template void mixingtypes(int size = SizeAtCompileType) -{ - typedef Matrix Mat_f; - typedef Matrix Mat_d; - typedef Matrix, SizeAtCompileType, SizeAtCompileType> Mat_cf; - typedef Matrix, SizeAtCompileType, SizeAtCompileType> Mat_cd; - typedef Matrix Vec_f; - typedef Matrix Vec_d; - typedef Matrix, SizeAtCompileType, 1> Vec_cf; - typedef Matrix, SizeAtCompileType, 1> Vec_cd; - - Mat_f mf(size,size); - Mat_d md(size,size); - Mat_cf mcf(size,size); - Mat_cd mcd(size,size); - Vec_f vf(size,1); - Vec_d vd(size,1); - Vec_cf vcf(size,1); - Vec_cd vcd(size,1); - - mf+mf; - VERIFY_RAISES_ASSERT(mf+md); - VERIFY_RAISES_ASSERT(mf+mcf); - VERIFY_RAISES_ASSERT(vf=vd); - VERIFY_RAISES_ASSERT(vf+=vd); - VERIFY_RAISES_ASSERT(mcd=md); - - mf*mf; - md*mcd; - mcd*md; - mf*vcf; - mcf*vf; - mcf *= mf; - vcd = md*vcd; - vcf = mcf*vf; -#if 0 - // these are know generating hard build errors in eigen3 - VERIFY_RAISES_ASSERT(mf*md); - VERIFY_RAISES_ASSERT(mcf*mcd); - VERIFY_RAISES_ASSERT(mcf*vcd); - VERIFY_RAISES_ASSERT(vcf = mf*vf); - - vf.eigen2_dot(vf); - VERIFY_RAISES_ASSERT(vd.eigen2_dot(vf)); - VERIFY_RAISES_ASSERT(vcf.eigen2_dot(vf)); // yeah eventually we should allow this but i'm too lazy to make that change now in Dot.h - // especially as that might be rewritten as cwise product .sum() which would make that automatic. -#endif -} - -void test_eigen2_mixingtypes() -{ - // check that our operator new is indeed called: - CALL_SUBTEST_1(mixingtypes<3>()); - CALL_SUBTEST_2(mixingtypes<4>()); - CALL_SUBTEST_3(mixingtypes(20)); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_newstdvector.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_newstdvector.cpp deleted file mode 100644 index 5f90099019..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_newstdvector.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN_USE_NEW_STDVECTOR -#include "main.h" -#include -#include - -template -void check_stdvector_matrix(const MatrixType& m) -{ - int rows = m.rows(); - int cols = m.cols(); - MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::vector > v(10, MatrixType(rows,cols)), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.resize(22,y); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((std::size_t)&(v[22]) == (std::size_t)&(v[21]) + sizeof(MatrixType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - MatrixType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; i -void check_stdvector_transform(const TransformType&) -{ - typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - std::vector > v(10), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.resize(22,y); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((std::size_t)&(v[22]) == (std::size_t)&(v[21]) + sizeof(TransformType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - TransformType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; i -void check_stdvector_quaternion(const QuaternionType&) -{ - typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::vector > v(10), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.resize(22,y); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((std::size_t)&(v[22]) == (std::size_t)&(v[21]) + sizeof(QuaternionType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - QuaternionType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; i -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -// this hack is needed to make this file compiles with -pedantic (gcc) -#ifdef __GNUC__ -#define throw(X) -#endif -// discard stack allocation as that too bypasses malloc -#define EIGEN_STACK_ALLOCATION_LIMIT 0 -// any heap allocation will raise an assert -#define EIGEN_NO_MALLOC - -#include "main.h" - -template void nomalloc(const MatrixType& m) -{ - /* this test check no dynamic memory allocation are issued with fixed-size matrices - */ - - typedef typename MatrixType::Scalar Scalar; - - int rows = m.rows(); - int cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols); - - Scalar s1 = ei_random(); - - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - - VERIFY_IS_APPROX((m1+m2)*s1, s1*m1+s1*m2); - VERIFY_IS_APPROX((m1+m2)(r,c), (m1(r,c))+(m2(r,c))); - VERIFY_IS_APPROX(m1.cwise() * m1.block(0,0,rows,cols), m1.cwise() * m1); - VERIFY_IS_APPROX((m1*m1.transpose())*m2, m1*(m1.transpose()*m2)); -} - -void test_eigen2_nomalloc() -{ - // check that our operator new is indeed called: - VERIFY_RAISES_ASSERT(MatrixXd dummy = MatrixXd::Random(3,3)); - CALL_SUBTEST_1( nomalloc(Matrix()) ); - CALL_SUBTEST_2( nomalloc(Matrix4d()) ); - CALL_SUBTEST_3( nomalloc(Matrix()) ); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_packetmath.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_packetmath.cpp deleted file mode 100644 index b1f325fe76..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_packetmath.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -// using namespace Eigen; - -template bool areApprox(const Scalar* a, const Scalar* b, int size) -{ - for (int i=0; i const complex& min(const complex& a, const complex& b) -{ return a.real() < b.real() ? a : b; } - -template<> const complex& max(const complex& a, const complex& b) -{ return a.real() < b.real() ? b : a; } - -} - -template void packetmath() -{ - typedef typename ei_packet_traits::type Packet; - const int PacketSize = ei_packet_traits::size; - - const int size = PacketSize*4; - EIGEN_ALIGN_128 Scalar data1[ei_packet_traits::size*4]; - EIGEN_ALIGN_128 Scalar data2[ei_packet_traits::size*4]; - EIGEN_ALIGN_128 Packet packets[PacketSize*2]; - EIGEN_ALIGN_128 Scalar ref[ei_packet_traits::size*4]; - for (int i=0; i(); - data2[i] = ei_random(); - } - - ei_pstore(data2, ei_pload(data1)); - VERIFY(areApprox(data1, data2, PacketSize) && "aligned load/store"); - - for (int offset=0; offset(packets[0], packets[1]); - else if (offset==1) ei_palign<1>(packets[0], packets[1]); - else if (offset==2) ei_palign<2>(packets[0], packets[1]); - else if (offset==3) ei_palign<3>(packets[0], packets[1]); - ei_pstore(data2, packets[0]); - - for (int i=0; i Vector; - VERIFY(areApprox(ref, data2, PacketSize) && "ei_palign"); - } - - CHECK_CWISE(REF_ADD, ei_padd); - CHECK_CWISE(REF_SUB, ei_psub); - CHECK_CWISE(REF_MUL, ei_pmul); - #ifndef EIGEN_VECTORIZE_ALTIVEC - if (!ei_is_same_type::ret) - CHECK_CWISE(REF_DIV, ei_pdiv); - #endif - CHECK_CWISE(std::min, ei_pmin); - CHECK_CWISE(std::max, ei_pmax); - - for (int i=0; i() ); - CALL_SUBTEST_2( packetmath() ); - CALL_SUBTEST_3( packetmath() ); - CALL_SUBTEST_4( packetmath >() ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_parametrizedline.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_parametrizedline.cpp deleted file mode 100644 index 8147288709..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_parametrizedline.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include -#include - -template void parametrizedline(const LineType& _line) -{ - /* this test covers the following files: - ParametrizedLine.h - */ - - const int dim = _line.dim(); - typedef typename LineType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; - typedef Matrix MatrixType; - - VectorType p0 = VectorType::Random(dim); - VectorType p1 = VectorType::Random(dim); - - VectorType d0 = VectorType::Random(dim).normalized(); - - LineType l0(p0, d0); - - Scalar s0 = ei_random(); - Scalar s1 = ei_abs(ei_random()); - - VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(p0), RealScalar(1) ); - VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(p0+s0*d0), RealScalar(1) ); - VERIFY_IS_APPROX( (l0.projection(p1)-p1).norm(), l0.distance(p1) ); - VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(l0.projection(p1)), RealScalar(1) ); - VERIFY_IS_APPROX( Scalar(l0.distance((p0+s0*d0) + d0.unitOrthogonal() * s1)), s1 ); - - // casting - const int Dim = LineType::AmbientDimAtCompileTime; - typedef typename GetDifferentType::type OtherScalar; - ParametrizedLine hp1f = l0.template cast(); - VERIFY_IS_APPROX(hp1f.template cast(),l0); - ParametrizedLine hp1d = l0.template cast(); - VERIFY_IS_APPROX(hp1d.template cast(),l0); -} - -void test_eigen2_parametrizedline() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( parametrizedline(ParametrizedLine()) ); - CALL_SUBTEST_2( parametrizedline(ParametrizedLine()) ); - CALL_SUBTEST_3( parametrizedline(ParametrizedLine()) ); - CALL_SUBTEST_4( parametrizedline(ParametrizedLine,5>()) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_prec_inverse_4x4.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_prec_inverse_4x4.cpp deleted file mode 100644 index 8bfa556948..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_prec_inverse_4x4.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include - -template std::string type_name() { return "other"; } -template<> std::string type_name() { return "float"; } -template<> std::string type_name() { return "double"; } -template<> std::string type_name() { return "int"; } -template<> std::string type_name >() { return "complex"; } -template<> std::string type_name >() { return "complex"; } -template<> std::string type_name >() { return "complex"; } - -#define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl; - -template inline typename NumTraits::Real epsilon() -{ - return std::numeric_limits::Real>::epsilon(); -} - -template void inverse_permutation_4x4() -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename MatrixType::RealScalar RealScalar; - Vector4i indices(0,1,2,3); - for(int i = 0; i < 24; ++i) - { - MatrixType m = MatrixType::Zero(); - m(indices(0),0) = 1; - m(indices(1),1) = 1; - m(indices(2),2) = 1; - m(indices(3),3) = 1; - MatrixType inv = m.inverse(); - double error = double( (m*inv-MatrixType::Identity()).norm() / epsilon() ); - VERIFY(error == 0.0); - std::next_permutation(indices.data(),indices.data()+4); - } -} - -template void inverse_general_4x4(int repeat) -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename MatrixType::RealScalar RealScalar; - double error_sum = 0., error_max = 0.; - for(int i = 0; i < repeat; ++i) - { - MatrixType m; - RealScalar absdet; - do { - m = MatrixType::Random(); - absdet = ei_abs(m.determinant()); - } while(absdet < 10 * epsilon()); - MatrixType inv = m.inverse(); - double error = double( (m*inv-MatrixType::Identity()).norm() * absdet / epsilon() ); - error_sum += error; - error_max = std::max(error_max, error); - } - std::cerr << "inverse_general_4x4, Scalar = " << type_name() << std::endl; - double error_avg = error_sum / repeat; - EIGEN_DEBUG_VAR(error_avg); - EIGEN_DEBUG_VAR(error_max); - VERIFY(error_avg < (NumTraits::IsComplex ? 8.0 : 1.25)); - VERIFY(error_max < (NumTraits::IsComplex ? 64.0 : 20.0)); -} - -void test_eigen2_prec_inverse_4x4() -{ - CALL_SUBTEST_1((inverse_permutation_4x4())); - CALL_SUBTEST_1(( inverse_general_4x4(200000 * g_repeat) )); - - CALL_SUBTEST_2((inverse_permutation_4x4 >())); - CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); - - CALL_SUBTEST_3((inverse_permutation_4x4())); - CALL_SUBTEST_3((inverse_general_4x4(50000 * g_repeat))); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_product_large.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_product_large.cpp deleted file mode 100644 index 5149ef7483..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_product_large.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "product.h" - -void test_eigen2_product_large() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( product(MatrixXf(ei_random(1,320), ei_random(1,320))) ); - CALL_SUBTEST_2( product(MatrixXd(ei_random(1,320), ei_random(1,320))) ); - CALL_SUBTEST_3( product(MatrixXi(ei_random(1,320), ei_random(1,320))) ); - CALL_SUBTEST_4( product(MatrixXcf(ei_random(1,50), ei_random(1,50))) ); - CALL_SUBTEST_5( product(Matrix(ei_random(1,320), ei_random(1,320))) ); - } - -#ifdef EIGEN_TEST_PART_6 - { - // test a specific issue in DiagonalProduct - int N = 1000000; - VectorXf v = VectorXf::Ones(N); - MatrixXf m = MatrixXf::Ones(N,3); - m = (v+v).asDiagonal() * m; - VERIFY_IS_APPROX(m, MatrixXf::Constant(N,3,2)); - } - - { - // test deferred resizing in Matrix::operator= - MatrixXf a = MatrixXf::Random(10,4), b = MatrixXf::Random(4,10), c = a; - VERIFY_IS_APPROX((a = a * b), (c * b).eval()); - } - - { - MatrixXf mat1(10,10); mat1.setRandom(); - MatrixXf mat2(32,10); mat2.setRandom(); - MatrixXf result = mat1.row(2)*mat2.transpose(); - VERIFY_IS_APPROX(result, (mat1.row(2)*mat2.transpose()).eval()); - } -#endif -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_product_small.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_product_small.cpp deleted file mode 100644 index 4cd8c102f3..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_product_small.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN_NO_STATIC_ASSERT -#include "product.h" - -void test_eigen2_product_small() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( product(Matrix()) ); - CALL_SUBTEST_2( product(Matrix()) ); - CALL_SUBTEST_3( product(Matrix3d()) ); - CALL_SUBTEST_4( product(Matrix4d()) ); - CALL_SUBTEST_5( product(Matrix4f()) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_qr.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_qr.cpp deleted file mode 100644 index 76977e4c1c..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_qr.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template void qr(const MatrixType& m) -{ - /* this test covers the following files: - QR.h - */ - int rows = m.rows(); - int cols = m.cols(); - - typedef typename MatrixType::Scalar Scalar; - typedef Matrix SquareMatrixType; - typedef Matrix VectorType; - - MatrixType a = MatrixType::Random(rows,cols); - QR qrOfA(a); - VERIFY_IS_APPROX(a, qrOfA.matrixQ() * qrOfA.matrixR()); - VERIFY_IS_NOT_APPROX(a+MatrixType::Identity(rows, cols), qrOfA.matrixQ() * qrOfA.matrixR()); - - #if 0 // eigenvalues module not yet ready - SquareMatrixType b = a.adjoint() * a; - - // check tridiagonalization - Tridiagonalization tridiag(b); - VERIFY_IS_APPROX(b, tridiag.matrixQ() * tridiag.matrixT() * tridiag.matrixQ().adjoint()); - - // check hessenberg decomposition - HessenbergDecomposition hess(b); - VERIFY_IS_APPROX(b, hess.matrixQ() * hess.matrixH() * hess.matrixQ().adjoint()); - VERIFY_IS_APPROX(tridiag.matrixT(), hess.matrixH()); - b = SquareMatrixType::Random(cols,cols); - hess.compute(b); - VERIFY_IS_APPROX(b, hess.matrixQ() * hess.matrixH() * hess.matrixQ().adjoint()); - #endif -} - -void test_eigen2_qr() -{ - for(int i = 0; i < 1; i++) { - CALL_SUBTEST_1( qr(Matrix2f()) ); - CALL_SUBTEST_2( qr(Matrix4d()) ); - CALL_SUBTEST_3( qr(MatrixXf(12,8)) ); - CALL_SUBTEST_4( qr(MatrixXcd(5,5)) ); - CALL_SUBTEST_4( qr(MatrixXcd(7,3)) ); - } - -#ifdef EIGEN_TEST_PART_5 - // small isFullRank test - { - Matrix3d mat; - mat << 1, 45, 1, 2, 2, 2, 1, 2, 3; - VERIFY(mat.qr().isFullRank()); - mat << 1, 1, 1, 2, 2, 2, 1, 2, 3; - //always returns true in eigen2support - //VERIFY(!mat.qr().isFullRank()); - } - -#endif -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_qtvector.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_qtvector.cpp deleted file mode 100644 index 6cfb58a26c..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_qtvector.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN_WORK_AROUND_QT_BUG_CALLING_WRONG_OPERATOR_NEW_FIXED_IN_QT_4_5 - -#include "main.h" - -#include -#include - -#include - -template -void check_qtvector_matrix(const MatrixType& m) -{ - int rows = m.rows(); - int cols = m.cols(); - MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - QVector v(10, MatrixType(rows,cols)), w(20, y); - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], y); - } - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.fill(y,22); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(MatrixType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - MatrixType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(int i=23; i -void check_qtvector_transform(const TransformType&) -{ - typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - QVector v(10), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.fill(y,22); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(TransformType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - TransformType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; int(i) -void check_qtvector_quaternion(const QuaternionType&) -{ - typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - QVector v(10), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.fill(y,22); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(QuaternionType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - QuaternionType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; int(i) -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template -void makeNoisyCohyperplanarPoints(int numPoints, - VectorType **points, - HyperplaneType *hyperplane, - typename VectorType::Scalar noiseAmplitude) -{ - typedef typename VectorType::Scalar Scalar; - const int size = points[0]->size(); - // pick a random hyperplane, store the coefficients of its equation - hyperplane->coeffs().resize(size + 1); - for(int j = 0; j < size + 1; j++) - { - do { - hyperplane->coeffs().coeffRef(j) = ei_random(); - } while(ei_abs(hyperplane->coeffs().coeff(j)) < 0.5); - } - - // now pick numPoints random points on this hyperplane - for(int i = 0; i < numPoints; i++) - { - VectorType& cur_point = *(points[i]); - do - { - cur_point = VectorType::Random(size)/*.normalized()*/; - // project cur_point onto the hyperplane - Scalar x = - (hyperplane->coeffs().start(size).cwise()*cur_point).sum(); - cur_point *= hyperplane->coeffs().coeff(size) / x; - } while( cur_point.norm() < 0.5 - || cur_point.norm() > 2.0 ); - } - - // add some noise to these points - for(int i = 0; i < numPoints; i++ ) - *(points[i]) += noiseAmplitude * VectorType::Random(size); -} - -template -void check_linearRegression(int numPoints, - VectorType **points, - const VectorType& original, - typename VectorType::Scalar tolerance) -{ - int size = points[0]->size(); - assert(size==2); - VectorType result(size); - linearRegression(numPoints, points, &result, 1); - typename VectorType::Scalar error = (result - original).norm() / original.norm(); - VERIFY(ei_abs(error) < ei_abs(tolerance)); -} - -template -void check_fitHyperplane(int numPoints, - VectorType **points, - const HyperplaneType& original, - typename VectorType::Scalar tolerance) -{ - int size = points[0]->size(); - HyperplaneType result(size); - fitHyperplane(numPoints, points, &result); - result.coeffs() *= original.coeffs().coeff(size)/result.coeffs().coeff(size); - typename VectorType::Scalar error = (result.coeffs() - original.coeffs()).norm() / original.coeffs().norm(); - std::cout << ei_abs(error) << " xxx " << ei_abs(tolerance) << std::endl; - VERIFY(ei_abs(error) < ei_abs(tolerance)); -} - -void test_eigen2_regression() -{ - for(int i = 0; i < g_repeat; i++) - { -#ifdef EIGEN_TEST_PART_1 - { - Vector2f points2f [1000]; - Vector2f *points2f_ptrs [1000]; - for(int i = 0; i < 1000; i++) points2f_ptrs[i] = &(points2f[i]); - Vector2f coeffs2f; - Hyperplane coeffs3f; - makeNoisyCohyperplanarPoints(1000, points2f_ptrs, &coeffs3f, 0.01f); - coeffs2f[0] = -coeffs3f.coeffs()[0]/coeffs3f.coeffs()[1]; - coeffs2f[1] = -coeffs3f.coeffs()[2]/coeffs3f.coeffs()[1]; - CALL_SUBTEST(check_linearRegression(10, points2f_ptrs, coeffs2f, 0.05f)); - CALL_SUBTEST(check_linearRegression(100, points2f_ptrs, coeffs2f, 0.01f)); - CALL_SUBTEST(check_linearRegression(1000, points2f_ptrs, coeffs2f, 0.002f)); - } -#endif -#ifdef EIGEN_TEST_PART_2 - { - Vector2f points2f [1000]; - Vector2f *points2f_ptrs [1000]; - for(int i = 0; i < 1000; i++) points2f_ptrs[i] = &(points2f[i]); - Hyperplane coeffs3f; - makeNoisyCohyperplanarPoints(1000, points2f_ptrs, &coeffs3f, 0.01f); - CALL_SUBTEST(check_fitHyperplane(10, points2f_ptrs, coeffs3f, 0.05f)); - CALL_SUBTEST(check_fitHyperplane(100, points2f_ptrs, coeffs3f, 0.01f)); - CALL_SUBTEST(check_fitHyperplane(1000, points2f_ptrs, coeffs3f, 0.002f)); - } -#endif -#ifdef EIGEN_TEST_PART_3 - { - Vector4d points4d [1000]; - Vector4d *points4d_ptrs [1000]; - for(int i = 0; i < 1000; i++) points4d_ptrs[i] = &(points4d[i]); - Hyperplane coeffs5d; - makeNoisyCohyperplanarPoints(1000, points4d_ptrs, &coeffs5d, 0.01); - CALL_SUBTEST(check_fitHyperplane(10, points4d_ptrs, coeffs5d, 0.05)); - CALL_SUBTEST(check_fitHyperplane(100, points4d_ptrs, coeffs5d, 0.01)); - CALL_SUBTEST(check_fitHyperplane(1000, points4d_ptrs, coeffs5d, 0.002)); - } -#endif -#ifdef EIGEN_TEST_PART_4 - { - VectorXcd *points11cd_ptrs[1000]; - for(int i = 0; i < 1000; i++) points11cd_ptrs[i] = new VectorXcd(11); - Hyperplane,Dynamic> *coeffs12cd = new Hyperplane,Dynamic>(11); - makeNoisyCohyperplanarPoints(1000, points11cd_ptrs, coeffs12cd, 0.01); - CALL_SUBTEST(check_fitHyperplane(100, points11cd_ptrs, *coeffs12cd, 0.025)); - CALL_SUBTEST(check_fitHyperplane(1000, points11cd_ptrs, *coeffs12cd, 0.006)); - delete coeffs12cd; - for(int i = 0; i < 1000; i++) delete points11cd_ptrs[i]; - } -#endif - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sizeof.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sizeof.cpp deleted file mode 100644 index ec1af5a066..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sizeof.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void verifySizeOf(const MatrixType&) -{ - typedef typename MatrixType::Scalar Scalar; - if (MatrixType::RowsAtCompileTime!=Dynamic && MatrixType::ColsAtCompileTime!=Dynamic) - VERIFY(sizeof(MatrixType)==sizeof(Scalar)*MatrixType::SizeAtCompileTime); - else - VERIFY(sizeof(MatrixType)==sizeof(Scalar*) + 2 * sizeof(typename MatrixType::Index)); -} - -void test_eigen2_sizeof() -{ - CALL_SUBTEST( verifySizeOf(Matrix()) ); - CALL_SUBTEST( verifySizeOf(Matrix4d()) ); - CALL_SUBTEST( verifySizeOf(Matrix()) ); - CALL_SUBTEST( verifySizeOf(Matrix()) ); - CALL_SUBTEST( verifySizeOf(MatrixXcf(3, 3)) ); - CALL_SUBTEST( verifySizeOf(MatrixXi(8, 12)) ); - CALL_SUBTEST( verifySizeOf(MatrixXcd(20, 20)) ); - CALL_SUBTEST( verifySizeOf(Matrix()) ); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_smallvectors.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_smallvectors.cpp deleted file mode 100644 index 03962b17d9..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_smallvectors.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void smallVectors() -{ - typedef Matrix V2; - typedef Matrix V3; - typedef Matrix V4; - Scalar x1 = ei_random(), - x2 = ei_random(), - x3 = ei_random(), - x4 = ei_random(); - V2 v2(x1, x2); - V3 v3(x1, x2, x3); - V4 v4(x1, x2, x3, x4); - VERIFY_IS_APPROX(x1, v2.x()); - VERIFY_IS_APPROX(x1, v3.x()); - VERIFY_IS_APPROX(x1, v4.x()); - VERIFY_IS_APPROX(x2, v2.y()); - VERIFY_IS_APPROX(x2, v3.y()); - VERIFY_IS_APPROX(x2, v4.y()); - VERIFY_IS_APPROX(x3, v3.z()); - VERIFY_IS_APPROX(x3, v4.z()); - VERIFY_IS_APPROX(x4, v4.w()); -} - -void test_eigen2_smallvectors() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST( smallVectors() ); - CALL_SUBTEST( smallVectors() ); - CALL_SUBTEST( smallVectors() ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_basic.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_basic.cpp deleted file mode 100644 index 049077670e..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_basic.cpp +++ /dev/null @@ -1,317 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Daniel Gomez Ferro -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "sparse.h" - -template -bool test_random_setter(SparseMatrix& sm, const DenseType& ref, const std::vector& nonzeroCoords) -{ - typedef SparseMatrix SparseType; - { - sm.setZero(); - SetterType w(sm); - std::vector remaining = nonzeroCoords; - while(!remaining.empty()) - { - int i = ei_random(0,remaining.size()-1); - w(remaining[i].x(),remaining[i].y()) = ref.coeff(remaining[i].x(),remaining[i].y()); - remaining[i] = remaining.back(); - remaining.pop_back(); - } - } - return sm.isApprox(ref); -} - -template -bool test_random_setter(DynamicSparseMatrix& sm, const DenseType& ref, const std::vector& nonzeroCoords) -{ - sm.setZero(); - std::vector remaining = nonzeroCoords; - while(!remaining.empty()) - { - int i = ei_random(0,remaining.size()-1); - sm.coeffRef(remaining[i].x(),remaining[i].y()) = ref.coeff(remaining[i].x(),remaining[i].y()); - remaining[i] = remaining.back(); - remaining.pop_back(); - } - return sm.isApprox(ref); -} - -template void sparse_basic(const SparseMatrixType& ref) -{ - const int rows = ref.rows(); - const int cols = ref.cols(); - typedef typename SparseMatrixType::Scalar Scalar; - enum { Flags = SparseMatrixType::Flags }; - - double density = std::max(8./(rows*cols), 0.01); - typedef Matrix DenseMatrix; - typedef Matrix DenseVector; - Scalar eps = 1e-6; - - SparseMatrixType m(rows, cols); - DenseMatrix refMat = DenseMatrix::Zero(rows, cols); - DenseVector vec1 = DenseVector::Random(rows); - Scalar s1 = ei_random(); - - std::vector zeroCoords; - std::vector nonzeroCoords; - initSparse(density, refMat, m, 0, &zeroCoords, &nonzeroCoords); - - if (zeroCoords.size()==0 || nonzeroCoords.size()==0) - return; - - // test coeff and coeffRef - for (int i=0; i<(int)zeroCoords.size(); ++i) - { - VERIFY_IS_MUCH_SMALLER_THAN( m.coeff(zeroCoords[i].x(),zeroCoords[i].y()), eps ); - if(ei_is_same_type >::ret) - VERIFY_RAISES_ASSERT( m.coeffRef(zeroCoords[0].x(),zeroCoords[0].y()) = 5 ); - } - VERIFY_IS_APPROX(m, refMat); - - m.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); - refMat.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); - - VERIFY_IS_APPROX(m, refMat); - /* - // test InnerIterators and Block expressions - for (int t=0; t<10; ++t) - { - int j = ei_random(0,cols-1); - int i = ei_random(0,rows-1); - int w = ei_random(1,cols-j-1); - int h = ei_random(1,rows-i-1); - -// VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); - for(int c=0; c w(m); -// for (int i=0; icoeffRef(nonzeroCoords[i].x(),nonzeroCoords[i].y()) = refMat.coeff(nonzeroCoords[i].x(),nonzeroCoords[i].y()); -// } -// } -// VERIFY_IS_APPROX(m, refMat); - - // random setter -// { -// m.setZero(); -// VERIFY_IS_NOT_APPROX(m, refMat); -// SparseSetter w(m); -// std::vector remaining = nonzeroCoords; -// while(!remaining.empty()) -// { -// int i = ei_random(0,remaining.size()-1); -// w->coeffRef(remaining[i].x(),remaining[i].y()) = refMat.coeff(remaining[i].x(),remaining[i].y()); -// remaining[i] = remaining.back(); -// remaining.pop_back(); -// } -// } -// VERIFY_IS_APPROX(m, refMat); - - VERIFY(( test_random_setter >(m,refMat,nonzeroCoords) )); - #ifdef EIGEN_UNORDERED_MAP_SUPPORT - VERIFY(( test_random_setter >(m,refMat,nonzeroCoords) )); - #endif - #ifdef _DENSE_HASH_MAP_H_ - VERIFY(( test_random_setter >(m,refMat,nonzeroCoords) )); - #endif - #ifdef _SPARSE_HASH_MAP_H_ - VERIFY(( test_random_setter >(m,refMat,nonzeroCoords) )); - #endif - - // test fillrand - { - DenseMatrix m1(rows,cols); - m1.setZero(); - SparseMatrixType m2(rows,cols); - m2.startFill(); - for (int j=0; j(0,rows-1); - if (m1.coeff(i,j)==Scalar(0)) - m2.fillrand(i,j) = m1(i,j) = ei_random(); - } - } - m2.endFill(); - VERIFY_IS_APPROX(m2,m1); - } - - // test RandomSetter - /*{ - SparseMatrixType m1(rows,cols), m2(rows,cols); - DenseMatrix refM1 = DenseMatrix::Zero(rows, rows); - initSparse(density, refM1, m1); - { - Eigen::RandomSetter setter(m2); - for (int j=0; j(density, refM1, m1); - initSparse(density, refM2, m2); - initSparse(density, refM3, m3); - initSparse(density, refM4, m4); - - VERIFY_IS_APPROX(m1+m2, refM1+refM2); - VERIFY_IS_APPROX(m1+m2+m3, refM1+refM2+refM3); - VERIFY_IS_APPROX(m3.cwise()*(m1+m2), refM3.cwise()*(refM1+refM2)); - VERIFY_IS_APPROX(m1*s1-m2, refM1*s1-refM2); - - VERIFY_IS_APPROX(m1*=s1, refM1*=s1); - VERIFY_IS_APPROX(m1/=s1, refM1/=s1); - - VERIFY_IS_APPROX(m1+=m2, refM1+=refM2); - VERIFY_IS_APPROX(m1-=m2, refM1-=refM2); - - VERIFY_IS_APPROX(m1.col(0).eigen2_dot(refM2.row(0)), refM1.col(0).eigen2_dot(refM2.row(0))); - - refM4.setRandom(); - // sparse cwise* dense - VERIFY_IS_APPROX(m3.cwise()*refM4, refM3.cwise()*refM4); -// VERIFY_IS_APPROX(m3.cwise()/refM4, refM3.cwise()/refM4); - } - - // test innerVector() - { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); - initSparse(density, refMat2, m2); - int j0 = ei_random(0,rows-1); - int j1 = ei_random(0,rows-1); - VERIFY_IS_APPROX(m2.innerVector(j0), refMat2.col(j0)); - VERIFY_IS_APPROX(m2.innerVector(j0)+m2.innerVector(j1), refMat2.col(j0)+refMat2.col(j1)); - //m2.innerVector(j0) = 2*m2.innerVector(j1); - //refMat2.col(j0) = 2*refMat2.col(j1); - //VERIFY_IS_APPROX(m2, refMat2); - } - - // test innerVectors() - { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); - initSparse(density, refMat2, m2); - int j0 = ei_random(0,rows-2); - int j1 = ei_random(0,rows-2); - int n0 = ei_random(1,rows-std::max(j0,j1)); - VERIFY_IS_APPROX(m2.innerVectors(j0,n0), refMat2.block(0,j0,rows,n0)); - VERIFY_IS_APPROX(m2.innerVectors(j0,n0)+m2.innerVectors(j1,n0), - refMat2.block(0,j0,rows,n0)+refMat2.block(0,j1,rows,n0)); - //m2.innerVectors(j0,n0) = m2.innerVectors(j0,n0) + m2.innerVectors(j1,n0); - //refMat2.block(0,j0,rows,n0) = refMat2.block(0,j0,rows,n0) + refMat2.block(0,j1,rows,n0); - } - - // test transpose - { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); - initSparse(density, refMat2, m2); - VERIFY_IS_APPROX(m2.transpose().eval(), refMat2.transpose().eval()); - VERIFY_IS_APPROX(m2.transpose(), refMat2.transpose()); - } - - // test prune - { - SparseMatrixType m2(rows, rows); - DenseMatrix refM2(rows, rows); - refM2.setZero(); - int countFalseNonZero = 0; - int countTrueNonZero = 0; - m2.startFill(); - for (int j=0; j(0,1); - if (x<0.1) - { - // do nothing - } - else if (x<0.5) - { - countFalseNonZero++; - m2.fill(i,j) = Scalar(0); - } - else - { - countTrueNonZero++; - m2.fill(i,j) = refM2(i,j) = Scalar(1); - } - } - m2.endFill(); - VERIFY(countFalseNonZero+countTrueNonZero == m2.nonZeros()); - VERIFY_IS_APPROX(m2, refM2); - m2.prune(1); - VERIFY(countTrueNonZero==m2.nonZeros()); - VERIFY_IS_APPROX(m2, refM2); - } -} - -void test_eigen2_sparse_basic() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( sparse_basic(SparseMatrix(8, 8)) ); - CALL_SUBTEST_2( sparse_basic(SparseMatrix >(16, 16)) ); - CALL_SUBTEST_1( sparse_basic(SparseMatrix(33, 33)) ); - - CALL_SUBTEST_3( sparse_basic(DynamicSparseMatrix(8, 8)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_product.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_product.cpp deleted file mode 100644 index d28e76dff7..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_product.cpp +++ /dev/null @@ -1,115 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Daniel Gomez Ferro -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "sparse.h" - -template void sparse_product(const SparseMatrixType& ref) -{ - const int rows = ref.rows(); - const int cols = ref.cols(); - typedef typename SparseMatrixType::Scalar Scalar; - enum { Flags = SparseMatrixType::Flags }; - - double density = std::max(8./(rows*cols), 0.01); - typedef Matrix DenseMatrix; - typedef Matrix DenseVector; - - // test matrix-matrix product - { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - DenseMatrix refMat3 = DenseMatrix::Zero(rows, rows); - DenseMatrix refMat4 = DenseMatrix::Zero(rows, rows); - DenseMatrix dm4 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); - SparseMatrixType m3(rows, rows); - SparseMatrixType m4(rows, rows); - initSparse(density, refMat2, m2); - initSparse(density, refMat3, m3); - initSparse(density, refMat4, m4); - VERIFY_IS_APPROX(m4=m2*m3, refMat4=refMat2*refMat3); - VERIFY_IS_APPROX(m4=m2.transpose()*m3, refMat4=refMat2.transpose()*refMat3); - VERIFY_IS_APPROX(m4=m2.transpose()*m3.transpose(), refMat4=refMat2.transpose()*refMat3.transpose()); - VERIFY_IS_APPROX(m4=m2*m3.transpose(), refMat4=refMat2*refMat3.transpose()); - - // sparse * dense - VERIFY_IS_APPROX(dm4=m2*refMat3, refMat4=refMat2*refMat3); - VERIFY_IS_APPROX(dm4=m2*refMat3.transpose(), refMat4=refMat2*refMat3.transpose()); - VERIFY_IS_APPROX(dm4=m2.transpose()*refMat3, refMat4=refMat2.transpose()*refMat3); - VERIFY_IS_APPROX(dm4=m2.transpose()*refMat3.transpose(), refMat4=refMat2.transpose()*refMat3.transpose()); - - // dense * sparse - VERIFY_IS_APPROX(dm4=refMat2*m3, refMat4=refMat2*refMat3); - VERIFY_IS_APPROX(dm4=refMat2*m3.transpose(), refMat4=refMat2*refMat3.transpose()); - VERIFY_IS_APPROX(dm4=refMat2.transpose()*m3, refMat4=refMat2.transpose()*refMat3); - VERIFY_IS_APPROX(dm4=refMat2.transpose()*m3.transpose(), refMat4=refMat2.transpose()*refMat3.transpose()); - - VERIFY_IS_APPROX(m3=m3*m3, refMat3=refMat3*refMat3); - } - - // test matrix - diagonal product - if(false) // it compiles, but the precision is terrible. probably doesn't matter in this branch.... - { - DenseMatrix refM2 = DenseMatrix::Zero(rows, rows); - DenseMatrix refM3 = DenseMatrix::Zero(rows, rows); - DiagonalMatrix d1(DenseVector::Random(rows)); - SparseMatrixType m2(rows, rows); - SparseMatrixType m3(rows, rows); - initSparse(density, refM2, m2); - initSparse(density, refM3, m3); - VERIFY_IS_APPROX(m3=m2*d1, refM3=refM2*d1); - VERIFY_IS_APPROX(m3=m2.transpose()*d1, refM3=refM2.transpose()*d1); - VERIFY_IS_APPROX(m3=d1*m2, refM3=d1*refM2); - VERIFY_IS_APPROX(m3=d1*m2.transpose(), refM3=d1 * refM2.transpose()); - } - - // test self adjoint products - { - DenseMatrix b = DenseMatrix::Random(rows, rows); - DenseMatrix x = DenseMatrix::Random(rows, rows); - DenseMatrix refX = DenseMatrix::Random(rows, rows); - DenseMatrix refUp = DenseMatrix::Zero(rows, rows); - DenseMatrix refLo = DenseMatrix::Zero(rows, rows); - DenseMatrix refS = DenseMatrix::Zero(rows, rows); - SparseMatrixType mUp(rows, rows); - SparseMatrixType mLo(rows, rows); - SparseMatrixType mS(rows, rows); - do { - initSparse(density, refUp, mUp, ForceRealDiag|/*ForceNonZeroDiag|*/MakeUpperTriangular); - } while (refUp.isZero()); - refLo = refUp.transpose().conjugate(); - mLo = mUp.transpose().conjugate(); - refS = refUp + refLo; - refS.diagonal() *= 0.5; - mS = mUp + mLo; - for (int k=0; k()*b, refX=refS*b); - VERIFY_IS_APPROX(x=mLo.template marked()*b, refX=refS*b); - VERIFY_IS_APPROX(x=mS.template marked()*b, refX=refS*b); - } - -} - -void test_eigen2_sparse_product() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( sparse_product(SparseMatrix(8, 8)) ); - CALL_SUBTEST_2( sparse_product(SparseMatrix >(16, 16)) ); - CALL_SUBTEST_1( sparse_product(SparseMatrix(33, 33)) ); - - CALL_SUBTEST_3( sparse_product(DynamicSparseMatrix(8, 8)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_solvers.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_solvers.cpp deleted file mode 100644 index 3aef27ab40..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_solvers.cpp +++ /dev/null @@ -1,200 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Daniel Gomez Ferro -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "sparse.h" - -template void -initSPD(double density, - Matrix& refMat, - SparseMatrix& sparseMat) -{ - Matrix aux(refMat.rows(),refMat.cols()); - initSparse(density,refMat,sparseMat); - refMat = refMat * refMat.adjoint(); - for (int k=0; k<2; ++k) - { - initSparse(density,aux,sparseMat,ForceNonZeroDiag); - refMat += aux * aux.adjoint(); - } - sparseMat.startFill(); - for (int j=0 ; j void sparse_solvers(int rows, int cols) -{ - double density = std::max(8./(rows*cols), 0.01); - typedef Matrix DenseMatrix; - typedef Matrix DenseVector; - // Scalar eps = 1e-6; - - DenseVector vec1 = DenseVector::Random(rows); - - std::vector zeroCoords; - std::vector nonzeroCoords; - - // test triangular solver - { - DenseVector vec2 = vec1, vec3 = vec1; - SparseMatrix m2(rows, cols); - DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols); - - // lower - initSparse(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords); - VERIFY_IS_APPROX(refMat2.template marked().solveTriangular(vec2), - m2.template marked().solveTriangular(vec3)); - - // lower - transpose - initSparse(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords); - VERIFY_IS_APPROX(refMat2.template marked().transpose().solveTriangular(vec2), - m2.template marked().transpose().solveTriangular(vec3)); - - // upper - initSparse(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, &zeroCoords, &nonzeroCoords); - VERIFY_IS_APPROX(refMat2.template marked().solveTriangular(vec2), - m2.template marked().solveTriangular(vec3)); - - // upper - transpose - initSparse(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, &zeroCoords, &nonzeroCoords); - VERIFY_IS_APPROX(refMat2.template marked().transpose().solveTriangular(vec2), - m2.template marked().transpose().solveTriangular(vec3)); - } - - // test LLT - { - // TODO fix the issue with complex (see SparseLLT::solveInPlace) - SparseMatrix m2(rows, cols); - DenseMatrix refMat2(rows, cols); - - DenseVector b = DenseVector::Random(cols); - DenseVector refX(cols), x(cols); - - initSPD(density, refMat2, m2); - - refMat2.llt().solve(b, &refX); - typedef SparseMatrix SparseSelfAdjointMatrix; - if (!NumTraits::IsComplex) - { - x = b; - SparseLLT (m2).solveInPlace(x); - VERIFY(refX.isApprox(x,test_precision()) && "LLT: default"); - } - #ifdef EIGEN_CHOLMOD_SUPPORT - x = b; - SparseLLT(m2).solveInPlace(x); - VERIFY(refX.isApprox(x,test_precision()) && "LLT: cholmod"); - #endif - if (!NumTraits::IsComplex) - { - #ifdef EIGEN_TAUCS_SUPPORT - x = b; - SparseLLT(m2,IncompleteFactorization).solveInPlace(x); - VERIFY(refX.isApprox(x,test_precision()) && "LLT: taucs (IncompleteFactorization)"); - x = b; - SparseLLT(m2,SupernodalMultifrontal).solveInPlace(x); - VERIFY(refX.isApprox(x,test_precision()) && "LLT: taucs (SupernodalMultifrontal)"); - x = b; - SparseLLT(m2,SupernodalLeftLooking).solveInPlace(x); - VERIFY(refX.isApprox(x,test_precision()) && "LLT: taucs (SupernodalLeftLooking)"); - #endif - } - } - - // test LDLT - if (!NumTraits::IsComplex) - { - // TODO fix the issue with complex (see SparseLDLT::solveInPlace) - SparseMatrix m2(rows, cols); - DenseMatrix refMat2(rows, cols); - - DenseVector b = DenseVector::Random(cols); - DenseVector refX(cols), x(cols); - - //initSPD(density, refMat2, m2); - initSparse(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, 0, 0); - refMat2 += refMat2.adjoint(); - refMat2.diagonal() *= 0.5; - - refMat2.ldlt().solve(b, &refX); - typedef SparseMatrix SparseSelfAdjointMatrix; - x = b; - SparseLDLT ldlt(m2); - if (ldlt.succeeded()) - ldlt.solveInPlace(x); - VERIFY(refX.isApprox(x,test_precision()) && "LDLT: default"); - } - - // test LU - { - static int count = 0; - SparseMatrix m2(rows, cols); - DenseMatrix refMat2(rows, cols); - - DenseVector b = DenseVector::Random(cols); - DenseVector refX(cols), x(cols); - - initSparse(density, refMat2, m2, ForceNonZeroDiag, &zeroCoords, &nonzeroCoords); - - LU refLu(refMat2); - refLu.solve(b, &refX); - #if defined(EIGEN_SUPERLU_SUPPORT) || defined(EIGEN_UMFPACK_SUPPORT) - Scalar refDet = refLu.determinant(); - #endif - x.setZero(); - // // SparseLU > (m2).solve(b,&x); - // // VERIFY(refX.isApprox(x,test_precision()) && "LU: default"); - #ifdef EIGEN_SUPERLU_SUPPORT - { - x.setZero(); - SparseLU,SuperLU> slu(m2); - if (slu.succeeded()) - { - if (slu.solve(b,&x)) { - VERIFY(refX.isApprox(x,test_precision()) && "LU: SuperLU"); - } - // std::cerr << refDet << " == " << slu.determinant() << "\n"; - if (count==0) { - VERIFY_IS_APPROX(refDet,slu.determinant()); // FIXME det is not very stable for complex - } - } - } - #endif - #ifdef EIGEN_UMFPACK_SUPPORT - { - // check solve - x.setZero(); - SparseLU,UmfPack> slu(m2); - if (slu.succeeded()) { - if (slu.solve(b,&x)) { - if (count==0) { - VERIFY(refX.isApprox(x,test_precision()) && "LU: umfpack"); // FIXME solve is not very stable for complex - } - } - VERIFY_IS_APPROX(refDet,slu.determinant()); - // TODO check the extracted data - //std::cerr << slu.matrixL() << "\n"; - } - } - #endif - count++; - } - -} - -void test_eigen2_sparse_solvers() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( sparse_solvers(8, 8) ); - CALL_SUBTEST_2( sparse_solvers >(16, 16) ); - CALL_SUBTEST_1( sparse_solvers(101, 101) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_vector.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_vector.cpp deleted file mode 100644 index e6d2d77a1b..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sparse_vector.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Daniel Gomez Ferro -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "sparse.h" - -template void sparse_vector(int rows, int cols) -{ - double densityMat = std::max(8./(rows*cols), 0.01); - double densityVec = std::max(8./float(rows), 0.1); - typedef Matrix DenseMatrix; - typedef Matrix DenseVector; - typedef SparseVector SparseVectorType; - typedef SparseMatrix SparseMatrixType; - Scalar eps = 1e-6; - - SparseMatrixType m1(rows,cols); - SparseVectorType v1(rows), v2(rows), v3(rows); - DenseMatrix refM1 = DenseMatrix::Zero(rows, cols); - DenseVector refV1 = DenseVector::Random(rows), - refV2 = DenseVector::Random(rows), - refV3 = DenseVector::Random(rows); - - std::vector zerocoords, nonzerocoords; - initSparse(densityVec, refV1, v1, &zerocoords, &nonzerocoords); - initSparse(densityMat, refM1, m1); - - initSparse(densityVec, refV2, v2); - initSparse(densityVec, refV3, v3); - - Scalar s1 = ei_random(); - - // test coeff and coeffRef - for (unsigned int i=0; i(8, 8) ); - CALL_SUBTEST_2( sparse_vector >(16, 16) ); - CALL_SUBTEST_1( sparse_vector(299, 535) ); - } -} - diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_stdvector.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_stdvector.cpp deleted file mode 100644 index 6ab05c20a3..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_stdvector.cpp +++ /dev/null @@ -1,148 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include -#include "main.h" -#include - -template -void check_stdvector_matrix(const MatrixType& m) -{ - int rows = m.rows(); - int cols = m.cols(); - MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::vector > v(10, MatrixType(rows,cols)), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.resize(22,y); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((std::size_t)&(v[22]) == (std::size_t)&(v[21]) + sizeof(MatrixType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - MatrixType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; i -void check_stdvector_transform(const TransformType&) -{ - typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - std::vector > v(10), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.resize(22,y); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((std::size_t)&(v[22]) == (std::size_t)&(v[21]) + sizeof(TransformType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - TransformType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; i -void check_stdvector_quaternion(const QuaternionType&) -{ - typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::vector > v(10), w(20, y); - v[5] = x; - w[6] = v[5]; - VERIFY_IS_APPROX(w[6], v[5]); - v = w; - for(int i = 0; i < 20; i++) - { - VERIFY_IS_APPROX(w[i], v[i]); - } - - v.resize(21); - v[20] = x; - VERIFY_IS_APPROX(v[20], x); - v.resize(22,y); - VERIFY_IS_APPROX(v[21], y); - v.push_back(x); - VERIFY_IS_APPROX(v[22], x); - VERIFY((std::size_t)&(v[22]) == (std::size_t)&(v[21]) + sizeof(QuaternionType)); - - // do a lot of push_back such that the vector gets internally resized - // (with memory reallocation) - QuaternionType* ref = &w[0]; - for(int i=0; i<30 || ((ref==&w[0]) && i<300); ++i) - v.push_back(w[i%w.size()]); - for(unsigned int i=23; i -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -// check minor separately in order to avoid the possible creation of a zero-sized -// array. Comes from a compilation error with gcc-3.4 or gcc-4 with -ansi -pedantic. -// Another solution would be to declare the array like this: T m_data[Size==0?1:Size]; in ei_matrix_storage -// but this is probably not bad to raise such an error at compile time... -template struct CheckMinor -{ - typedef Matrix MatrixType; - CheckMinor(MatrixType& m1, int r1, int c1) - { - int rows = m1.rows(); - int cols = m1.cols(); - - Matrix mi = m1.minor(0,0).eval(); - VERIFY_IS_APPROX(mi, m1.block(1,1,rows-1,cols-1)); - mi = m1.minor(r1,c1); - VERIFY_IS_APPROX(mi.transpose(), m1.transpose().minor(c1,r1)); - //check operator(), both constant and non-constant, on minor() - m1.minor(r1,c1)(0,0) = m1.minor(0,0)(0,0); - } -}; - -template struct CheckMinor -{ - typedef Matrix MatrixType; - CheckMinor(MatrixType&, int, int) {} -}; - -template void submatrices(const MatrixType& m) -{ - /* this test covers the following files: - Row.h Column.h Block.h Minor.h DiagonalCoeffs.h - */ - typedef typename MatrixType::Scalar Scalar; - typedef typename MatrixType::RealScalar RealScalar; - typedef Matrix VectorType; - typedef Matrix RowVectorType; - int rows = m.rows(); - int cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - ones = MatrixType::Ones(rows, cols), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows); - - Scalar s1 = ei_random(); - - int r1 = ei_random(0,rows-1); - int r2 = ei_random(r1,rows-1); - int c1 = ei_random(0,cols-1); - int c2 = ei_random(c1,cols-1); - - //check row() and col() - VERIFY_IS_APPROX(m1.col(c1).transpose(), m1.transpose().row(c1)); - VERIFY_IS_APPROX(square.row(r1).eigen2_dot(m1.col(c1)), (square.lazy() * m1.conjugate())(r1,c1)); - //check operator(), both constant and non-constant, on row() and col() - m1.row(r1) += s1 * m1.row(r2); - m1.col(c1) += s1 * m1.col(c2); - - //check block() - Matrix b1(1,1); b1(0,0) = m1(r1,c1); - RowVectorType br1(m1.block(r1,0,1,cols)); - VectorType bc1(m1.block(0,c1,rows,1)); - VERIFY_IS_APPROX(b1, m1.block(r1,c1,1,1)); - VERIFY_IS_APPROX(m1.row(r1), br1); - VERIFY_IS_APPROX(m1.col(c1), bc1); - //check operator(), both constant and non-constant, on block() - m1.block(r1,c1,r2-r1+1,c2-c1+1) = s1 * m2.block(0, 0, r2-r1+1,c2-c1+1); - m1.block(r1,c1,r2-r1+1,c2-c1+1)(r2-r1,c2-c1) = m2.block(0, 0, r2-r1+1,c2-c1+1)(0,0); - - //check minor() - CheckMinor checkminor(m1,r1,c1); - - //check diagonal() - VERIFY_IS_APPROX(m1.diagonal(), m1.transpose().diagonal()); - m2.diagonal() = 2 * m1.diagonal(); - m2.diagonal()[0] *= 3; - VERIFY_IS_APPROX(m2.diagonal()[0], static_cast(6) * m1.diagonal()[0]); - - enum { - BlockRows = EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::RowsAtCompileTime,2), - BlockCols = EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::ColsAtCompileTime,5) - }; - if (rows>=5 && cols>=8) - { - // test fixed block() as lvalue - m1.template block(1,1) *= s1; - // test operator() on fixed block() both as constant and non-constant - m1.template block(1,1)(0, 3) = m1.template block<2,5>(1,1)(1,2); - // check that fixed block() and block() agree - Matrix b = m1.template block(3,3); - VERIFY_IS_APPROX(b, m1.block(3,3,BlockRows,BlockCols)); - } - - if (rows>2) - { - // test sub vectors - VERIFY_IS_APPROX(v1.template start<2>(), v1.block(0,0,2,1)); - VERIFY_IS_APPROX(v1.template start<2>(), v1.start(2)); - VERIFY_IS_APPROX(v1.template start<2>(), v1.segment(0,2)); - VERIFY_IS_APPROX(v1.template start<2>(), v1.template segment<2>(0)); - int i = rows-2; - VERIFY_IS_APPROX(v1.template end<2>(), v1.block(i,0,2,1)); - VERIFY_IS_APPROX(v1.template end<2>(), v1.end(2)); - VERIFY_IS_APPROX(v1.template end<2>(), v1.segment(i,2)); - VERIFY_IS_APPROX(v1.template end<2>(), v1.template segment<2>(i)); - i = ei_random(0,rows-2); - VERIFY_IS_APPROX(v1.segment(i,2), v1.template segment<2>(i)); - } - - // stress some basic stuffs with block matrices - VERIFY(ei_real(ones.col(c1).sum()) == RealScalar(rows)); - VERIFY(ei_real(ones.row(r1).sum()) == RealScalar(cols)); - - VERIFY(ei_real(ones.col(c1).eigen2_dot(ones.col(c2))) == RealScalar(rows)); - VERIFY(ei_real(ones.row(r1).eigen2_dot(ones.row(r2))) == RealScalar(cols)); -} - -void test_eigen2_submatrices() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( submatrices(Matrix()) ); - CALL_SUBTEST_2( submatrices(Matrix4d()) ); - CALL_SUBTEST_3( submatrices(MatrixXcf(3, 3)) ); - CALL_SUBTEST_4( submatrices(MatrixXi(8, 12)) ); - CALL_SUBTEST_5( submatrices(MatrixXcd(20, 20)) ); - CALL_SUBTEST_6( submatrices(MatrixXf(20, 20)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sum.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sum.cpp deleted file mode 100644 index b47057caab..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_sum.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void matrixSum(const MatrixType& m) -{ - typedef typename MatrixType::Scalar Scalar; - - int rows = m.rows(); - int cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols); - - VERIFY_IS_MUCH_SMALLER_THAN(MatrixType::Zero(rows, cols).sum(), Scalar(1)); - VERIFY_IS_APPROX(MatrixType::Ones(rows, cols).sum(), Scalar(float(rows*cols))); // the float() here to shut up excessive MSVC warning about int->complex conversion being lossy - Scalar x = Scalar(0); - for(int i = 0; i < rows; i++) for(int j = 0; j < cols; j++) x += m1(i,j); - VERIFY_IS_APPROX(m1.sum(), x); -} - -template void vectorSum(const VectorType& w) -{ - typedef typename VectorType::Scalar Scalar; - int size = w.size(); - - VectorType v = VectorType::Random(size); - for(int i = 1; i < size; i++) - { - Scalar s = Scalar(0); - for(int j = 0; j < i; j++) s += v[j]; - VERIFY_IS_APPROX(s, v.start(i).sum()); - } - - for(int i = 0; i < size-1; i++) - { - Scalar s = Scalar(0); - for(int j = i; j < size; j++) s += v[j]; - VERIFY_IS_APPROX(s, v.end(size-i).sum()); - } - - for(int i = 0; i < size/2; i++) - { - Scalar s = Scalar(0); - for(int j = i; j < size-i; j++) s += v[j]; - VERIFY_IS_APPROX(s, v.segment(i, size-2*i).sum()); - } -} - -void test_eigen2_sum() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( matrixSum(Matrix()) ); - CALL_SUBTEST_2( matrixSum(Matrix2f()) ); - CALL_SUBTEST_3( matrixSum(Matrix4d()) ); - CALL_SUBTEST_4( matrixSum(MatrixXcf(3, 3)) ); - CALL_SUBTEST_5( matrixSum(MatrixXf(8, 12)) ); - CALL_SUBTEST_6( matrixSum(MatrixXi(8, 12)) ); - } - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_5( vectorSum(VectorXf(5)) ); - CALL_SUBTEST_7( vectorSum(VectorXd(10)) ); - CALL_SUBTEST_5( vectorSum(VectorXf(33)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_svd.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_svd.cpp deleted file mode 100644 index d4689a56f4..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_svd.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include - -template void svd(const MatrixType& m) -{ - /* this test covers the following files: - SVD.h - */ - int rows = m.rows(); - int cols = m.cols(); - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - MatrixType a = MatrixType::Random(rows,cols); - Matrix b = - Matrix::Random(rows,1); - Matrix x(cols,1), x2(cols,1); - - RealScalar largerEps = test_precision(); - if (ei_is_same_type::ret) - largerEps = 1e-3f; - - { - SVD svd(a); - MatrixType sigma = MatrixType::Zero(rows,cols); - MatrixType matU = MatrixType::Zero(rows,rows); - sigma.block(0,0,cols,cols) = svd.singularValues().asDiagonal(); - matU.block(0,0,rows,cols) = svd.matrixU(); - VERIFY_IS_APPROX(a, matU * sigma * svd.matrixV().transpose()); - } - - - if (rows==cols) - { - if (ei_is_same_type::ret) - { - MatrixType a1 = MatrixType::Random(rows,cols); - a += a * a.adjoint() + a1 * a1.adjoint(); - } - SVD svd(a); - svd.solve(b, &x); - VERIFY_IS_APPROX(a * x,b); - } - - - if(rows==cols) - { - SVD svd(a); - MatrixType unitary, positive; - svd.computeUnitaryPositive(&unitary, &positive); - VERIFY_IS_APPROX(unitary * unitary.adjoint(), MatrixType::Identity(unitary.rows(),unitary.rows())); - VERIFY_IS_APPROX(positive, positive.adjoint()); - for(int i = 0; i < rows; i++) VERIFY(positive.diagonal()[i] >= 0); // cheap necessary (not sufficient) condition for positivity - VERIFY_IS_APPROX(unitary*positive, a); - - svd.computePositiveUnitary(&positive, &unitary); - VERIFY_IS_APPROX(unitary * unitary.adjoint(), MatrixType::Identity(unitary.rows(),unitary.rows())); - VERIFY_IS_APPROX(positive, positive.adjoint()); - for(int i = 0; i < rows; i++) VERIFY(positive.diagonal()[i] >= 0); // cheap necessary (not sufficient) condition for positivity - VERIFY_IS_APPROX(positive*unitary, a); - } -} - -void test_eigen2_svd() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( svd(Matrix3f()) ); - CALL_SUBTEST_2( svd(Matrix4d()) ); - CALL_SUBTEST_3( svd(MatrixXf(7,7)) ); - CALL_SUBTEST_4( svd(MatrixXd(14,7)) ); - // complex are not implemented yet -// CALL_SUBTEST( svd(MatrixXcd(6,6)) ); -// CALL_SUBTEST( svd(MatrixXcf(3,3)) ); - SVD s; - MatrixXf m = MatrixXf::Random(10,1); - s.compute(m); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_swap.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_swap.cpp deleted file mode 100644 index f3a8846d94..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_swap.cpp +++ /dev/null @@ -1,83 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#define EIGEN_NO_STATIC_ASSERT -#include "main.h" - -template -struct other_matrix_type -{ - typedef int type; -}; - -template -struct other_matrix_type > -{ - typedef Matrix<_Scalar, _Rows, _Cols, _Options^RowMajor, _MaxRows, _MaxCols> type; -}; - -template void swap(const MatrixType& m) -{ - typedef typename other_matrix_type::type OtherMatrixType; - typedef typename MatrixType::Scalar Scalar; - - ei_assert((!ei_is_same_type::ret)); - int rows = m.rows(); - int cols = m.cols(); - - // construct 3 matrix guaranteed to be distinct - MatrixType m1 = MatrixType::Random(rows,cols); - MatrixType m2 = MatrixType::Random(rows,cols) + Scalar(100) * MatrixType::Identity(rows,cols); - OtherMatrixType m3 = OtherMatrixType::Random(rows,cols) + Scalar(200) * OtherMatrixType::Identity(rows,cols); - - MatrixType m1_copy = m1; - MatrixType m2_copy = m2; - OtherMatrixType m3_copy = m3; - - // test swapping 2 matrices of same type - m1.swap(m2); - VERIFY_IS_APPROX(m1,m2_copy); - VERIFY_IS_APPROX(m2,m1_copy); - m1 = m1_copy; - m2 = m2_copy; - - // test swapping 2 matrices of different types - m1.swap(m3); - VERIFY_IS_APPROX(m1,m3_copy); - VERIFY_IS_APPROX(m3,m1_copy); - m1 = m1_copy; - m3 = m3_copy; - - // test swapping matrix with expression - m1.swap(m2.block(0,0,rows,cols)); - VERIFY_IS_APPROX(m1,m2_copy); - VERIFY_IS_APPROX(m2,m1_copy); - m1 = m1_copy; - m2 = m2_copy; - - // test swapping two expressions of different types - m1.transpose().swap(m3.transpose()); - VERIFY_IS_APPROX(m1,m3_copy); - VERIFY_IS_APPROX(m3,m1_copy); - m1 = m1_copy; - m3 = m3_copy; - - // test assertion on mismatching size -- matrix case - VERIFY_RAISES_ASSERT(m1.swap(m1.row(0))); - // test assertion on mismatching size -- xpr case - VERIFY_RAISES_ASSERT(m1.row(0).swap(m1)); -} - -void test_eigen2_swap() -{ - CALL_SUBTEST_1( swap(Matrix3f()) ); // fixed size, no vectorization - CALL_SUBTEST_1( swap(Matrix4d()) ); // fixed size, possible vectorization - CALL_SUBTEST_1( swap(MatrixXd(3,3)) ); // dyn size, no vectorization - CALL_SUBTEST_1( swap(MatrixXf(30,30)) ); // dyn size, possible vectorization -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_triangular.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_triangular.cpp deleted file mode 100644 index 6f17b7dffe..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_triangular.cpp +++ /dev/null @@ -1,148 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void triangular(const MatrixType& m) -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - - RealScalar largerEps = 10*test_precision(); - - int rows = m.rows(); - int cols = m.cols(); - - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - m4(rows, cols), - r1(rows, cols), - r2(rows, cols); - - MatrixType m1up = m1.template part(); - MatrixType m2up = m2.template part(); - - if (rows*cols>1) - { - VERIFY(m1up.isUpperTriangular()); - VERIFY(m2up.transpose().isLowerTriangular()); - VERIFY(!m2.isLowerTriangular()); - } - -// VERIFY_IS_APPROX(m1up.transpose() * m2, m1.upper().transpose().lower() * m2); - - // test overloaded operator+= - r1.setZero(); - r2.setZero(); - r1.template part() += m1; - r2 += m1up; - VERIFY_IS_APPROX(r1,r2); - - // test overloaded operator= - m1.setZero(); - m1.template part() = (m2.transpose() * m2).lazy(); - m3 = m2.transpose() * m2; - VERIFY_IS_APPROX(m3.template part().transpose(), m1); - - // test overloaded operator= - m1.setZero(); - m1.template part() = (m2.transpose() * m2).lazy(); - VERIFY_IS_APPROX(m3.template part(), m1); - - VERIFY_IS_APPROX(m3.template part(), m3.diagonal().asDiagonal()); - - m1 = MatrixType::Random(rows, cols); - for (int i=0; i(); - - Transpose trm4(m4); - // test back and forward subsitution - m3 = m1.template part(); - VERIFY(m3.template marked().solveTriangular(m3).cwise().abs().isIdentity(test_precision())); - VERIFY(m3.transpose().template marked() - .solveTriangular(m3.transpose()).cwise().abs().isIdentity(test_precision())); - // check M * inv(L) using in place API - m4 = m3; - m3.transpose().template marked().solveTriangularInPlace(trm4); - VERIFY(m4.cwise().abs().isIdentity(test_precision())); - - m3 = m1.template part(); - VERIFY(m3.template marked().solveTriangular(m3).cwise().abs().isIdentity(test_precision())); - VERIFY(m3.transpose().template marked() - .solveTriangular(m3.transpose()).cwise().abs().isIdentity(test_precision())); - // check M * inv(U) using in place API - m4 = m3; - m3.transpose().template marked().solveTriangularInPlace(trm4); - VERIFY(m4.cwise().abs().isIdentity(test_precision())); - - m3 = m1.template part(); - VERIFY(m2.isApprox(m3 * (m3.template marked().solveTriangular(m2)), largerEps)); - m3 = m1.template part(); - VERIFY(m2.isApprox(m3 * (m3.template marked().solveTriangular(m2)), largerEps)); - - VERIFY((m1.template part() * m2.template part()).isUpperTriangular()); - - // test swap - m1.setOnes(); - m2.setZero(); - m2.template part().swap(m1); - m3.setZero(); - m3.template part().setOnes(); - VERIFY_IS_APPROX(m2,m3); - -} - -void selfadjoint() -{ - Matrix2i m; - m << 1, 2, - 3, 4; - - Matrix2i m1 = Matrix2i::Zero(); - m1.part() = m; - Matrix2i ref1; - ref1 << 1, 2, - 2, 4; - VERIFY(m1 == ref1); - - Matrix2i m2 = Matrix2i::Zero(); - m2.part() = m.part(); - Matrix2i ref2; - ref2 << 1, 2, - 2, 4; - VERIFY(m2 == ref2); - - Matrix2i m3 = Matrix2i::Zero(); - m3.part() = m.part(); - Matrix2i ref3; - ref3 << 1, 0, - 0, 4; - VERIFY(m3 == ref3); - - // example inspired from bug 159 - int array[] = {1, 2, 3, 4}; - Matrix2i::Map(array).part() = Matrix2i::Random().part(); - - std::cout << "hello\n" << array << std::endl; -} - -void test_eigen2_triangular() -{ - CALL_SUBTEST_8( selfadjoint() ); - for(int i = 0; i < g_repeat ; i++) { - CALL_SUBTEST_1( triangular(Matrix()) ); - CALL_SUBTEST_2( triangular(Matrix()) ); - CALL_SUBTEST_3( triangular(Matrix3d()) ); - CALL_SUBTEST_4( triangular(MatrixXcf(4, 4)) ); - CALL_SUBTEST_5( triangular(Matrix,8, 8>()) ); - CALL_SUBTEST_6( triangular(MatrixXd(17,17)) ); - CALL_SUBTEST_7( triangular(Matrix(5, 5)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_unalignedassert.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_unalignedassert.cpp deleted file mode 100644 index d10b6f5385..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_unalignedassert.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -struct Good1 -{ - MatrixXd m; // good: m will allocate its own array, taking care of alignment. - Good1() : m(20,20) {} -}; - -struct Good2 -{ - Matrix3d m; // good: m's size isn't a multiple of 16 bytes, so m doesn't have to be aligned -}; - -struct Good3 -{ - Vector2f m; // good: same reason -}; - -struct Bad4 -{ - Vector2d m; // bad: sizeof(m)%16==0 so alignment is required -}; - -struct Bad5 -{ - Matrix m; // bad: same reason -}; - -struct Bad6 -{ - Matrix m; // bad: same reason -}; - -struct Good7 -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Vector2d m; - float f; // make the struct have sizeof%16!=0 to make it a little more tricky when we allow an array of 2 such objects -}; - -struct Good8 -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - float f; // try the f at first -- the EIGEN_ALIGN_128 attribute of m should make that still work - Matrix4f m; -}; - -struct Good9 -{ - Matrix m; // good: no alignment requested - float f; -}; - -template struct Depends -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(Align) - Vector2d m; - float f; -}; - -template -void check_unalignedassert_good() -{ - T *x, *y; - x = new T; - delete x; - y = new T[2]; - delete[] y; -} - -#if EIGEN_ARCH_WANTS_ALIGNMENT -template -void check_unalignedassert_bad() -{ - float buf[sizeof(T)+16]; - float *unaligned = buf; - while((reinterpret_cast(unaligned)&0xf)==0) ++unaligned; // make sure unaligned is really unaligned - T *x = ::new(static_cast(unaligned)) T; - x->~T(); -} -#endif - -void unalignedassert() -{ - check_unalignedassert_good(); - check_unalignedassert_good(); - check_unalignedassert_good(); -#if EIGEN_ARCH_WANTS_ALIGNMENT - VERIFY_RAISES_ASSERT(check_unalignedassert_bad()); - VERIFY_RAISES_ASSERT(check_unalignedassert_bad()); - VERIFY_RAISES_ASSERT(check_unalignedassert_bad()); -#endif - - check_unalignedassert_good(); - check_unalignedassert_good(); - check_unalignedassert_good(); - check_unalignedassert_good >(); - -#if EIGEN_ARCH_WANTS_ALIGNMENT - VERIFY_RAISES_ASSERT(check_unalignedassert_bad >()); -#endif -} - -void test_eigen2_unalignedassert() -{ - CALL_SUBTEST(unalignedassert()); -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_visitor.cpp b/ground/gcs/src/libs/eigen/test/eigen2/eigen2_visitor.cpp deleted file mode 100644 index 4781991de7..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/eigen2_visitor.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" - -template void matrixVisitor(const MatrixType& p) -{ - typedef typename MatrixType::Scalar Scalar; - - int rows = p.rows(); - int cols = p.cols(); - - // construct a random matrix where all coefficients are different - MatrixType m; - m = MatrixType::Random(rows, cols); - for(int i = 0; i < m.size(); i++) - for(int i2 = 0; i2 < i; i2++) - while(m(i) == m(i2)) // yes, == - m(i) = ei_random(); - - Scalar minc = Scalar(1000), maxc = Scalar(-1000); - int minrow=0,mincol=0,maxrow=0,maxcol=0; - for(int j = 0; j < cols; j++) - for(int i = 0; i < rows; i++) - { - if(m(i,j) < minc) - { - minc = m(i,j); - minrow = i; - mincol = j; - } - if(m(i,j) > maxc) - { - maxc = m(i,j); - maxrow = i; - maxcol = j; - } - } - int eigen_minrow, eigen_mincol, eigen_maxrow, eigen_maxcol; - Scalar eigen_minc, eigen_maxc; - eigen_minc = m.minCoeff(&eigen_minrow,&eigen_mincol); - eigen_maxc = m.maxCoeff(&eigen_maxrow,&eigen_maxcol); - VERIFY(minrow == eigen_minrow); - VERIFY(maxrow == eigen_maxrow); - VERIFY(mincol == eigen_mincol); - VERIFY(maxcol == eigen_maxcol); - VERIFY_IS_APPROX(minc, eigen_minc); - VERIFY_IS_APPROX(maxc, eigen_maxc); - VERIFY_IS_APPROX(minc, m.minCoeff()); - VERIFY_IS_APPROX(maxc, m.maxCoeff()); -} - -template void vectorVisitor(const VectorType& w) -{ - typedef typename VectorType::Scalar Scalar; - - int size = w.size(); - - // construct a random vector where all coefficients are different - VectorType v; - v = VectorType::Random(size); - for(int i = 0; i < size; i++) - for(int i2 = 0; i2 < i; i2++) - while(v(i) == v(i2)) // yes, == - v(i) = ei_random(); - - Scalar minc = Scalar(1000), maxc = Scalar(-1000); - int minidx=0,maxidx=0; - for(int i = 0; i < size; i++) - { - if(v(i) < minc) - { - minc = v(i); - minidx = i; - } - if(v(i) > maxc) - { - maxc = v(i); - maxidx = i; - } - } - int eigen_minidx, eigen_maxidx; - Scalar eigen_minc, eigen_maxc; - eigen_minc = v.minCoeff(&eigen_minidx); - eigen_maxc = v.maxCoeff(&eigen_maxidx); - VERIFY(minidx == eigen_minidx); - VERIFY(maxidx == eigen_maxidx); - VERIFY_IS_APPROX(minc, eigen_minc); - VERIFY_IS_APPROX(maxc, eigen_maxc); - VERIFY_IS_APPROX(minc, v.minCoeff()); - VERIFY_IS_APPROX(maxc, v.maxCoeff()); -} - -void test_eigen2_visitor() -{ - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( matrixVisitor(Matrix()) ); - CALL_SUBTEST_2( matrixVisitor(Matrix2f()) ); - CALL_SUBTEST_3( matrixVisitor(Matrix4d()) ); - CALL_SUBTEST_4( matrixVisitor(MatrixXd(8, 12)) ); - CALL_SUBTEST_5( matrixVisitor(Matrix(20, 20)) ); - CALL_SUBTEST_6( matrixVisitor(MatrixXi(8, 12)) ); - } - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_7( vectorVisitor(Vector4f()) ); - CALL_SUBTEST_4( vectorVisitor(VectorXd(10)) ); - CALL_SUBTEST_4( vectorVisitor(RowVectorXd(10)) ); - CALL_SUBTEST_8( vectorVisitor(VectorXf(33)) ); - } -} diff --git a/ground/gcs/src/libs/eigen/test/eigen2/gsl_helper.h b/ground/gcs/src/libs/eigen/test/eigen2/gsl_helper.h deleted file mode 100644 index d1d8545333..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/gsl_helper.h +++ /dev/null @@ -1,175 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_GSL_HELPER -#define EIGEN_GSL_HELPER - -#include - -#include -#include -#include -#include -#include -#include - -namespace Eigen { - -template::IsComplex> struct GslTraits -{ - typedef gsl_matrix* Matrix; - typedef gsl_vector* Vector; - static Matrix createMatrix(int rows, int cols) { return gsl_matrix_alloc(rows,cols); } - static Vector createVector(int size) { return gsl_vector_alloc(size); } - static void free(Matrix& m) { gsl_matrix_free(m); m=0; } - static void free(Vector& m) { gsl_vector_free(m); m=0; } - static void prod(const Matrix& m, const Vector& v, Vector& x) { gsl_blas_dgemv(CblasNoTrans,1,m,v,0,x); } - static void cholesky(Matrix& m) { gsl_linalg_cholesky_decomp(m); } - static void cholesky_solve(const Matrix& m, const Vector& b, Vector& x) { gsl_linalg_cholesky_solve(m,b,x); } - static void eigen_symm(const Matrix& m, Vector& eval, Matrix& evec) - { - gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(m->size1); - Matrix a = createMatrix(m->size1, m->size2); - gsl_matrix_memcpy(a, m); - gsl_eigen_symmv(a,eval,evec,w); - gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC); - gsl_eigen_symmv_free(w); - free(a); - } - static void eigen_symm_gen(const Matrix& m, const Matrix& _b, Vector& eval, Matrix& evec) - { - gsl_eigen_gensymmv_workspace * w = gsl_eigen_gensymmv_alloc(m->size1); - Matrix a = createMatrix(m->size1, m->size2); - Matrix b = createMatrix(_b->size1, _b->size2); - gsl_matrix_memcpy(a, m); - gsl_matrix_memcpy(b, _b); - gsl_eigen_gensymmv(a,b,eval,evec,w); - gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC); - gsl_eigen_gensymmv_free(w); - free(a); - } -}; - -template struct GslTraits -{ - typedef gsl_matrix_complex* Matrix; - typedef gsl_vector_complex* Vector; - static Matrix createMatrix(int rows, int cols) { return gsl_matrix_complex_alloc(rows,cols); } - static Vector createVector(int size) { return gsl_vector_complex_alloc(size); } - static void free(Matrix& m) { gsl_matrix_complex_free(m); m=0; } - static void free(Vector& m) { gsl_vector_complex_free(m); m=0; } - static void cholesky(Matrix& m) { gsl_linalg_complex_cholesky_decomp(m); } - static void cholesky_solve(const Matrix& m, const Vector& b, Vector& x) { gsl_linalg_complex_cholesky_solve(m,b,x); } - static void prod(const Matrix& m, const Vector& v, Vector& x) - { gsl_blas_zgemv(CblasNoTrans,gsl_complex_rect(1,0),m,v,gsl_complex_rect(0,0),x); } - static void eigen_symm(const Matrix& m, gsl_vector* &eval, Matrix& evec) - { - gsl_eigen_hermv_workspace * w = gsl_eigen_hermv_alloc(m->size1); - Matrix a = createMatrix(m->size1, m->size2); - gsl_matrix_complex_memcpy(a, m); - gsl_eigen_hermv(a,eval,evec,w); - gsl_eigen_hermv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC); - gsl_eigen_hermv_free(w); - free(a); - } - static void eigen_symm_gen(const Matrix& m, const Matrix& _b, gsl_vector* &eval, Matrix& evec) - { - gsl_eigen_genhermv_workspace * w = gsl_eigen_genhermv_alloc(m->size1); - Matrix a = createMatrix(m->size1, m->size2); - Matrix b = createMatrix(_b->size1, _b->size2); - gsl_matrix_complex_memcpy(a, m); - gsl_matrix_complex_memcpy(b, _b); - gsl_eigen_genhermv(a,b,eval,evec,w); - gsl_eigen_hermv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC); - gsl_eigen_genhermv_free(w); - free(a); - } -}; - -template -void convert(const MatrixType& m, gsl_matrix* &res) -{ -// if (res) -// gsl_matrix_free(res); - res = gsl_matrix_alloc(m.rows(), m.cols()); - for (int i=0 ; i -void convert(const gsl_matrix* m, MatrixType& res) -{ - res.resize(int(m->size1), int(m->size2)); - for (int i=0 ; i -void convert(const VectorType& m, gsl_vector* &res) -{ - if (res) gsl_vector_free(res); - res = gsl_vector_alloc(m.size()); - for (int i=0 ; i -void convert(const gsl_vector* m, VectorType& res) -{ - res.resize (m->size); - for (int i=0 ; i -void convert(const MatrixType& m, gsl_matrix_complex* &res) -{ - res = gsl_matrix_complex_alloc(m.rows(), m.cols()); - for (int i=0 ; i -void convert(const gsl_matrix_complex* m, MatrixType& res) -{ - res.resize(int(m->size1), int(m->size2)); - for (int i=0 ; i -void convert(const VectorType& m, gsl_vector_complex* &res) -{ - res = gsl_vector_complex_alloc(m.size()); - for (int i=0 ; i -void convert(const gsl_vector_complex* m, VectorType& res) -{ - res.resize(m->size); - for (int i=0 ; i -// Copyright (C) 2008 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include -#include -#include -#include -#include - -#ifndef EIGEN_TEST_FUNC -#error EIGEN_TEST_FUNC must be defined -#endif - -#define DEFAULT_REPEAT 10 - -namespace Eigen -{ - static std::vector g_test_stack; - static int g_repeat; -} - -#define EI_PP_MAKE_STRING2(S) #S -#define EI_PP_MAKE_STRING(S) EI_PP_MAKE_STRING2(S) - -#define EI_PP_CAT2(a,b) a ## b -#define EI_PP_CAT(a,b) EI_PP_CAT2(a,b) - -#ifndef EIGEN_NO_ASSERTION_CHECKING - - namespace Eigen - { - static const bool should_raise_an_assert = false; - - // Used to avoid to raise two exceptions at a time in which - // case the exception is not properly caught. - // This may happen when a second exceptions is raise in a destructor. - static bool no_more_assert = false; - - struct eigen_assert_exception - { - eigen_assert_exception(void) {} - ~eigen_assert_exception() { Eigen::no_more_assert = false; } - }; - } - - // If EIGEN_DEBUG_ASSERTS is defined and if no assertion is raised while - // one should have been, then the list of excecuted assertions is printed out. - // - // EIGEN_DEBUG_ASSERTS is not enabled by default as it - // significantly increases the compilation time - // and might even introduce side effects that would hide - // some memory errors. - #ifdef EIGEN_DEBUG_ASSERTS - - namespace Eigen - { - static bool ei_push_assert = false; - static std::vector eigen_assert_list; - } - - #define eigen_assert(a) \ - if( (!(a)) && (!no_more_assert) ) \ - { \ - Eigen::no_more_assert = true; \ - throw Eigen::eigen_assert_exception(); \ - } \ - else if (Eigen::ei_push_assert) \ - { \ - eigen_assert_list.push_back(std::string(EI_PP_MAKE_STRING(__FILE__)" ("EI_PP_MAKE_STRING(__LINE__)") : "#a) ); \ - } - - #define VERIFY_RAISES_ASSERT(a) \ - { \ - Eigen::no_more_assert = false; \ - try { \ - Eigen::eigen_assert_list.clear(); \ - Eigen::ei_push_assert = true; \ - a; \ - Eigen::ei_push_assert = false; \ - std::cerr << "One of the following asserts should have been raised:\n"; \ - for (uint ai=0 ; ai - - -#define VERIFY(a) do { if (!(a)) { \ - std::cerr << "Test " << g_test_stack.back() << " failed in "EI_PP_MAKE_STRING(__FILE__) << " (" << EI_PP_MAKE_STRING(__LINE__) << ")" \ - << std::endl << " " << EI_PP_MAKE_STRING(a) << std::endl << std::endl; \ - abort(); \ - } } while (0) - -#define VERIFY_IS_APPROX(a, b) VERIFY(test_ei_isApprox(a, b)) -#define VERIFY_IS_NOT_APPROX(a, b) VERIFY(!test_ei_isApprox(a, b)) -#define VERIFY_IS_MUCH_SMALLER_THAN(a, b) VERIFY(test_ei_isMuchSmallerThan(a, b)) -#define VERIFY_IS_NOT_MUCH_SMALLER_THAN(a, b) VERIFY(!test_ei_isMuchSmallerThan(a, b)) -#define VERIFY_IS_APPROX_OR_LESS_THAN(a, b) VERIFY(test_ei_isApproxOrLessThan(a, b)) -#define VERIFY_IS_NOT_APPROX_OR_LESS_THAN(a, b) VERIFY(!test_ei_isApproxOrLessThan(a, b)) - -#define CALL_SUBTEST(FUNC) do { \ - g_test_stack.push_back(EI_PP_MAKE_STRING(FUNC)); \ - FUNC; \ - g_test_stack.pop_back(); \ - } while (0) - -namespace Eigen { - -template inline typename NumTraits::Real test_precision(); -template<> inline int test_precision() { return 0; } -template<> inline float test_precision() { return 1e-3f; } -template<> inline double test_precision() { return 1e-6; } -template<> inline float test_precision >() { return test_precision(); } -template<> inline double test_precision >() { return test_precision(); } -template<> inline long double test_precision() { return 1e-6; } - -inline bool test_ei_isApprox(const int& a, const int& b) -{ return ei_isApprox(a, b, test_precision()); } -inline bool test_ei_isMuchSmallerThan(const int& a, const int& b) -{ return ei_isMuchSmallerThan(a, b, test_precision()); } -inline bool test_ei_isApproxOrLessThan(const int& a, const int& b) -{ return ei_isApproxOrLessThan(a, b, test_precision()); } - -inline bool test_ei_isApprox(const float& a, const float& b) -{ return ei_isApprox(a, b, test_precision()); } -inline bool test_ei_isMuchSmallerThan(const float& a, const float& b) -{ return ei_isMuchSmallerThan(a, b, test_precision()); } -inline bool test_ei_isApproxOrLessThan(const float& a, const float& b) -{ return ei_isApproxOrLessThan(a, b, test_precision()); } - -inline bool test_ei_isApprox(const double& a, const double& b) -{ return ei_isApprox(a, b, test_precision()); } -inline bool test_ei_isMuchSmallerThan(const double& a, const double& b) -{ return ei_isMuchSmallerThan(a, b, test_precision()); } -inline bool test_ei_isApproxOrLessThan(const double& a, const double& b) -{ return ei_isApproxOrLessThan(a, b, test_precision()); } - -inline bool test_ei_isApprox(const std::complex& a, const std::complex& b) -{ return ei_isApprox(a, b, test_precision >()); } -inline bool test_ei_isMuchSmallerThan(const std::complex& a, const std::complex& b) -{ return ei_isMuchSmallerThan(a, b, test_precision >()); } - -inline bool test_ei_isApprox(const std::complex& a, const std::complex& b) -{ return ei_isApprox(a, b, test_precision >()); } -inline bool test_ei_isMuchSmallerThan(const std::complex& a, const std::complex& b) -{ return ei_isMuchSmallerThan(a, b, test_precision >()); } - -inline bool test_ei_isApprox(const long double& a, const long double& b) -{ return ei_isApprox(a, b, test_precision()); } -inline bool test_ei_isMuchSmallerThan(const long double& a, const long double& b) -{ return ei_isMuchSmallerThan(a, b, test_precision()); } -inline bool test_ei_isApproxOrLessThan(const long double& a, const long double& b) -{ return ei_isApproxOrLessThan(a, b, test_precision()); } - -template -inline bool test_ei_isApprox(const Type1& a, const Type2& b) -{ - return a.isApprox(b, test_precision()); -} - -template -inline bool test_ei_isMuchSmallerThan(const MatrixBase& m1, - const MatrixBase& m2) -{ - return m1.isMuchSmallerThan(m2, test_precision::Scalar>()); -} - -template -inline bool test_ei_isMuchSmallerThan(const MatrixBase& m, - const typename NumTraits::Scalar>::Real& s) -{ - return m.isMuchSmallerThan(s, test_precision::Scalar>()); -} - -} // end namespace Eigen - -template struct GetDifferentType; - -template<> struct GetDifferentType { typedef double type; }; -template<> struct GetDifferentType { typedef float type; }; -template struct GetDifferentType > -{ typedef std::complex::type> type; }; - -// forward declaration of the main test function -void EI_PP_CAT(test_,EIGEN_TEST_FUNC)(); - -using namespace Eigen; - -#ifdef EIGEN_TEST_PART_1 -#define CALL_SUBTEST_1(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_1(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_2 -#define CALL_SUBTEST_2(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_2(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_3 -#define CALL_SUBTEST_3(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_3(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_4 -#define CALL_SUBTEST_4(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_4(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_5 -#define CALL_SUBTEST_5(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_5(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_6 -#define CALL_SUBTEST_6(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_6(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_7 -#define CALL_SUBTEST_7(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_7(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_8 -#define CALL_SUBTEST_8(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_8(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_9 -#define CALL_SUBTEST_9(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_9(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_10 -#define CALL_SUBTEST_10(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_10(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_11 -#define CALL_SUBTEST_11(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_11(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_12 -#define CALL_SUBTEST_12(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_12(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_13 -#define CALL_SUBTEST_13(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_13(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_14 -#define CALL_SUBTEST_14(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_14(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_15 -#define CALL_SUBTEST_15(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_15(FUNC) -#endif - -#ifdef EIGEN_TEST_PART_16 -#define CALL_SUBTEST_16(FUNC) CALL_SUBTEST(FUNC) -#else -#define CALL_SUBTEST_16(FUNC) -#endif - - - -int main(int argc, char *argv[]) -{ - bool has_set_repeat = false; - bool has_set_seed = false; - bool need_help = false; - unsigned int seed = 0; - int repeat = DEFAULT_REPEAT; - - for(int i = 1; i < argc; i++) - { - if(argv[i][0] == 'r') - { - if(has_set_repeat) - { - std::cout << "Argument " << argv[i] << " conflicting with a former argument" << std::endl; - return 1; - } - repeat = std::atoi(argv[i]+1); - has_set_repeat = true; - if(repeat <= 0) - { - std::cout << "Invalid \'repeat\' value " << argv[i]+1 << std::endl; - return 1; - } - } - else if(argv[i][0] == 's') - { - if(has_set_seed) - { - std::cout << "Argument " << argv[i] << " conflicting with a former argument" << std::endl; - return 1; - } - seed = int(std::strtoul(argv[i]+1, 0, 10)); - has_set_seed = true; - bool ok = seed!=0; - if(!ok) - { - std::cout << "Invalid \'seed\' value " << argv[i]+1 << std::endl; - return 1; - } - } - else - { - need_help = true; - } - } - - if(need_help) - { - std::cout << "This test application takes the following optional arguments:" << std::endl; - std::cout << " rN Repeat each test N times (default: " << DEFAULT_REPEAT << ")" << std::endl; - std::cout << " sN Use N as seed for random numbers (default: based on current time)" << std::endl; - return 1; - } - - if(!has_set_seed) seed = (unsigned int) std::time(NULL); - if(!has_set_repeat) repeat = DEFAULT_REPEAT; - - std::cout << "Initializing random number generator with seed " << seed << std::endl; - std::srand(seed); - std::cout << "Repeating each test " << repeat << " times" << std::endl; - - Eigen::g_repeat = repeat; - Eigen::g_test_stack.push_back(EI_PP_MAKE_STRING(EIGEN_TEST_FUNC)); - - EI_PP_CAT(test_,EIGEN_TEST_FUNC)(); - return 0; -} - - - diff --git a/ground/gcs/src/libs/eigen/test/eigen2/product.h b/ground/gcs/src/libs/eigen/test/eigen2/product.h deleted file mode 100644 index ae1b4bae47..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/product.h +++ /dev/null @@ -1,129 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include "main.h" -#include -#include - -template -bool areNotApprox(const MatrixBase& m1, const MatrixBase& m2, typename Derived1::RealScalar epsilon = precision()) -{ - return !((m1-m2).cwise().abs2().maxCoeff() < epsilon * epsilon - * std::max(m1.cwise().abs2().maxCoeff(), m2.cwise().abs2().maxCoeff())); -} - -template void product(const MatrixType& m) -{ - /* this test covers the following files: - Identity.h Product.h - */ - - typedef typename MatrixType::Scalar Scalar; - typedef typename NumTraits::FloatingPoint FloatingPoint; - typedef Matrix RowVectorType; - typedef Matrix ColVectorType; - typedef Matrix RowSquareMatrixType; - typedef Matrix ColSquareMatrixType; - typedef Matrix OtherMajorMatrixType; - - int rows = m.rows(); - int cols = m.cols(); - - // this test relies a lot on Random.h, and there's not much more that we can do - // to test it, hence I consider that we will have tested Random.h - MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols); - RowSquareMatrixType - identity = RowSquareMatrixType::Identity(rows, rows), - square = RowSquareMatrixType::Random(rows, rows), - res = RowSquareMatrixType::Random(rows, rows); - ColSquareMatrixType - square2 = ColSquareMatrixType::Random(cols, cols), - res2 = ColSquareMatrixType::Random(cols, cols); - RowVectorType v1 = RowVectorType::Random(rows); - ColVectorType vc2 = ColVectorType::Random(cols), vcres(cols); - OtherMajorMatrixType tm1 = m1; - - Scalar s1 = ei_random(); - - int r = ei_random(0, rows-1), - c = ei_random(0, cols-1); - - // begin testing Product.h: only associativity for now - // (we use Transpose.h but this doesn't count as a test for it) - - VERIFY_IS_APPROX((m1*m1.transpose())*m2, m1*(m1.transpose()*m2)); - m3 = m1; - m3 *= m1.transpose() * m2; - VERIFY_IS_APPROX(m3, m1 * (m1.transpose()*m2)); - VERIFY_IS_APPROX(m3, m1.lazy() * (m1.transpose()*m2)); - - // continue testing Product.h: distributivity - VERIFY_IS_APPROX(square*(m1 + m2), square*m1+square*m2); - VERIFY_IS_APPROX(square*(m1 - m2), square*m1-square*m2); - - // continue testing Product.h: compatibility with ScalarMultiple.h - VERIFY_IS_APPROX(s1*(square*m1), (s1*square)*m1); - VERIFY_IS_APPROX(s1*(square*m1), square*(m1*s1)); - - // again, test operator() to check const-qualification - s1 += (square.lazy() * m1)(r,c); - - // test Product.h together with Identity.h - VERIFY_IS_APPROX(v1, identity*v1); - VERIFY_IS_APPROX(v1.transpose(), v1.transpose() * identity); - // again, test operator() to check const-qualification - VERIFY_IS_APPROX(MatrixType::Identity(rows, cols)(r,c), static_cast(r==c)); - - if (rows!=cols) - VERIFY_RAISES_ASSERT(m3 = m1*m1); - - // test the previous tests were not screwed up because operator* returns 0 - // (we use the more accurate default epsilon) - if (NumTraits::HasFloatingPoint && std::min(rows,cols)>1) - { - VERIFY(areNotApprox(m1.transpose()*m2,m2.transpose()*m1)); - } - - // test optimized operator+= path - res = square; - res += (m1 * m2.transpose()).lazy(); - VERIFY_IS_APPROX(res, square + m1 * m2.transpose()); - if (NumTraits::HasFloatingPoint && std::min(rows,cols)>1) - { - VERIFY(areNotApprox(res,square + m2 * m1.transpose())); - } - vcres = vc2; - vcres += (m1.transpose() * v1).lazy(); - VERIFY_IS_APPROX(vcres, vc2 + m1.transpose() * v1); - tm1 = m1; - VERIFY_IS_APPROX(tm1.transpose() * v1, m1.transpose() * v1); - VERIFY_IS_APPROX(v1.transpose() * tm1, v1.transpose() * m1); - - // test submatrix and matrix/vector product - for (int i=0; i::HasFloatingPoint && std::min(rows,cols)>1) - { - VERIFY(areNotApprox(res2,square2 + m2.transpose() * m1)); - } -} - diff --git a/ground/gcs/src/libs/eigen/test/eigen2/runtest.sh b/ground/gcs/src/libs/eigen/test/eigen2/runtest.sh deleted file mode 100755 index bc693af131..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/runtest.sh +++ /dev/null @@ -1,28 +0,0 @@ -#!/bin/bash - -black='\E[30m' -red='\E[31m' -green='\E[32m' -yellow='\E[33m' -blue='\E[34m' -magenta='\E[35m' -cyan='\E[36m' -white='\E[37m' - -if make test_$1 > /dev/null 2> .runtest.log ; then - if ! ./test_$1 r20 > /dev/null 2> .runtest.log ; then - echo -e $red Test $1 failed: $black - echo -e $blue - cat .runtest.log - echo -e $black - exit 1 - else - echo -e $green Test $1 passed$black - fi -else - echo -e $red Build of target $1 failed: $black - echo -e $blue - cat .runtest.log - echo -e $black - exit 1 -fi diff --git a/ground/gcs/src/libs/eigen/test/eigen2/sparse.h b/ground/gcs/src/libs/eigen/test/eigen2/sparse.h deleted file mode 100644 index e12f899903..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/sparse.h +++ /dev/null @@ -1,154 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. Eigen itself is part of the KDE project. -// -// Copyright (C) 2008 Daniel Gomez Ferro -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_TESTSPARSE_H - -#include "main.h" - -#if EIGEN_GNUC_AT_LEAST(4,0) && !defined __ICC -#include -#define EIGEN_UNORDERED_MAP_SUPPORT -namespace std { - using std::tr1::unordered_map; -} -#endif - -#ifdef EIGEN_GOOGLEHASH_SUPPORT - #include -#endif - -#include -#include -#include - -enum { - ForceNonZeroDiag = 1, - MakeLowerTriangular = 2, - MakeUpperTriangular = 4, - ForceRealDiag = 8 -}; - -/* Initializes both a sparse and dense matrix with same random values, - * and a ratio of \a density non zero entries. - * \param flags is a union of ForceNonZeroDiag, MakeLowerTriangular and MakeUpperTriangular - * allowing to control the shape of the matrix. - * \param zeroCoords and nonzeroCoords allows to get the coordinate lists of the non zero, - * and zero coefficients respectively. - */ -template void -initSparse(double density, - Matrix& refMat, - SparseMatrix& sparseMat, - int flags = 0, - std::vector* zeroCoords = 0, - std::vector* nonzeroCoords = 0) -{ - sparseMat.startFill(int(refMat.rows()*refMat.cols()*density)); - for(int j=0; j(0,1) < density) ? ei_random() : Scalar(0); - if ((flags&ForceNonZeroDiag) && (i==j)) - { - v = ei_random()*Scalar(3.); - v = v*v + Scalar(5.); - } - if ((flags & MakeLowerTriangular) && j>i) - v = Scalar(0); - else if ((flags & MakeUpperTriangular) && jpush_back(Vector2i(i,j)); - } - else if (zeroCoords) - { - zeroCoords->push_back(Vector2i(i,j)); - } - refMat(i,j) = v; - } - } - sparseMat.endFill(); -} - -template void -initSparse(double density, - Matrix& refMat, - DynamicSparseMatrix& sparseMat, - int flags = 0, - std::vector* zeroCoords = 0, - std::vector* nonzeroCoords = 0) -{ - sparseMat.startFill(int(refMat.rows()*refMat.cols()*density)); - for(int j=0; j(0,1) < density) ? ei_random() : Scalar(0); - if ((flags&ForceNonZeroDiag) && (i==j)) - { - v = ei_random()*Scalar(3.); - v = v*v + Scalar(5.); - } - if ((flags & MakeLowerTriangular) && j>i) - v = Scalar(0); - else if ((flags & MakeUpperTriangular) && jpush_back(Vector2i(i,j)); - } - else if (zeroCoords) - { - zeroCoords->push_back(Vector2i(i,j)); - } - refMat(i,j) = v; - } - } - sparseMat.endFill(); -} - -template void -initSparse(double density, - Matrix& refVec, - SparseVector& sparseVec, - std::vector* zeroCoords = 0, - std::vector* nonzeroCoords = 0) -{ - sparseVec.reserve(int(refVec.size()*density)); - sparseVec.setZero(); - for(int i=0; i(0,1) < density) ? ei_random() : Scalar(0); - if (v!=Scalar(0)) - { - sparseVec.fill(i) = v; - if (nonzeroCoords) - nonzeroCoords->push_back(i); - } - else if (zeroCoords) - zeroCoords->push_back(i); - refVec[i] = v; - } -} - -#endif // EIGEN_TESTSPARSE_H diff --git a/ground/gcs/src/libs/eigen/test/eigen2/testsuite.cmake b/ground/gcs/src/libs/eigen/test/eigen2/testsuite.cmake deleted file mode 100644 index 12b6bfa2eb..0000000000 --- a/ground/gcs/src/libs/eigen/test/eigen2/testsuite.cmake +++ /dev/null @@ -1,197 +0,0 @@ - -#################################################################### -# -# Usage: -# - create a new folder, let's call it cdash -# - in that folder, do: -# ctest -S path/to/eigen2/test/testsuite.cmake[,option1=value1[,option2=value2]] -# -# Options: -# - EIGEN_CXX: compiler, eg.: g++-4.2 -# default: default c++ compiler -# - EIGEN_SITE: eg, INRIA-Bdx_pc-gael, or the name of the contributor, etc. -# default: hostname -# - EIGEN_BUILD_STRING: a string which identify the system/compiler. It should be formed like that: -# --- -# with: -# = opensuse, debian, osx, windows, cygwin, freebsd, solaris, etc. -# = 11.1, XP, vista, leopard, etc. -# = i386, x86_64, ia64, powerpc, etc. -# = gcc-4.3.2, icc-11.0, MSVC-2008, etc. -# - EIGEN_EXPLICIT_VECTORIZATION: novec, SSE2, Altivec -# default: SSE2 for x86_64 systems, novec otherwise -# Its value is automatically appended to EIGEN_BUILD_STRING -# - EIGEN_CMAKE_DIR: path to cmake executable -# - EIGEN_MODE: dashboard model, can be Experimental, Nightly, or Continuous -# default: Nightly -# - EIGEN_WORK_DIR: directory used to download the source files and make the builds -# default: folder which contains this script -# - EIGEN_CMAKE_ARGS: additional arguments passed to cmake -# - CTEST_SOURCE_DIRECTORY: path to eigen's src (use a new and empty folder, not the one you are working on) -# default: /src -# - CTEST_BINARY_DIRECTORY: build directory -# default: /nightly- -# -# Here is an example running several compilers on a linux system: -# #!/bin/bash -# ARCH=`uname -m` -# SITE=`hostname` -# VERSION=opensuse-11.1 -# WORK_DIR=/home/gael/Coding/eigen2/cdash -# # get the last version of the script -# wget http://bitbucket.org/eigen/eigen/raw/tip/test/testsuite.cmake -o $WORK_DIR/testsuite.cmake -# COMMON="ctest -S $WORK_DIR/testsuite.cmake,EIGEN_WORK_DIR=$WORK_DIR,EIGEN_SITE=$SITE,EIGEN_MODE=$1,EIGEN_BUILD_STRING=$OS_VERSION-$ARCH" -# $COMMON-gcc-3.4.6,EIGEN_CXX=g++-3.4 -# $COMMON-gcc-4.0.1,EIGEN_CXX=g++-4.0.1 -# $COMMON-gcc-4.3.2,EIGEN_CXX=g++-4.3,EIGEN_EXPLICIT_VECTORIZATION=novec -# $COMMON-gcc-4.3.2,EIGEN_CXX=g++-4.3,EIGEN_EXPLICIT_VECTORIZATION=SSE2 -# $COMMON-icc-11.0,EIGEN_CXX=icpc -# -#################################################################### - -# process the arguments - -set(ARGLIST ${CTEST_SCRIPT_ARG}) -while(${ARGLIST} MATCHES ".+.*") - - # pick first - string(REGEX MATCH "([^,]*)(,.*)?" DUMMY ${ARGLIST}) - SET(TOP ${CMAKE_MATCH_1}) - - # remove first - string(REGEX MATCHALL "[^,]*,(.*)" DUMMY ${ARGLIST}) - SET(ARGLIST ${CMAKE_MATCH_1}) - - # decompose as a pair key=value - string(REGEX MATCH "([^=]*)(=.*)?" DUMMY ${TOP}) - SET(KEY ${CMAKE_MATCH_1}) - - string(REGEX MATCH "[^=]*=(.*)" DUMMY ${TOP}) - SET(VALUE ${CMAKE_MATCH_1}) - - # set the variable to the specified value - if(VALUE) - SET(${KEY} ${VALUE}) - else(VALUE) - SET(${KEY} ON) - endif(VALUE) - -endwhile(${ARGLIST} MATCHES ".+.*") - -#################################################################### -# Automatically set some user variables if they have not been defined manually -#################################################################### -cmake_minimum_required(VERSION 2.6 FATAL_ERROR) - -if(NOT EIGEN_SITE) - site_name(EIGEN_SITE) -endif(NOT EIGEN_SITE) - -if(NOT EIGEN_CMAKE_DIR) - SET(EIGEN_CMAKE_DIR "") -endif(NOT EIGEN_CMAKE_DIR) - -if(NOT EIGEN_BUILD_STRING) - - # let's try to find all information we need to make the build string ourself - - # OS - build_name(EIGEN_OS_VERSION) - - # arch - set(EIGEN_ARCH ${CMAKE_SYSTEM_PROCESSOR}) - if(WIN32) - set(EIGEN_ARCH $ENV{PROCESSOR_ARCHITECTURE}) - else(WIN32) - execute_process(COMMAND uname -m OUTPUT_VARIABLE EIGEN_ARCH OUTPUT_STRIP_TRAILING_WHITESPACE) - endif(WIN32) - - set(EIGEN_BUILD_STRING ${EIGEN_OS_VERSION}${EIGEN_ARCH}-${EIGEN_CXX}) - -endif(NOT EIGEN_BUILD_STRING) - -if(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - set(EIGEN_BUILD_STRING ${EIGEN_BUILD_STRING}-${EIGEN_EXPLICIT_VECTORIZATION}) -endif(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - -if(NOT EIGEN_WORK_DIR) - set(EIGEN_WORK_DIR ${CTEST_SCRIPT_DIRECTORY}) -endif(NOT EIGEN_WORK_DIR) - -if(NOT CTEST_SOURCE_DIRECTORY) - SET (CTEST_SOURCE_DIRECTORY "${EIGEN_WORK_DIR}/src") -endif(NOT CTEST_SOURCE_DIRECTORY) - -if(NOT CTEST_BINARY_DIRECTORY) - SET (CTEST_BINARY_DIRECTORY "${EIGEN_WORK_DIR}/nightly_${EIGEN_CXX}") -endif(NOT CTEST_BINARY_DIRECTORY) - -if(NOT EIGEN_MODE) - set(EIGEN_MODE Nightly) -endif(NOT EIGEN_MODE) - -## mandatory variables (the default should be ok in most cases): - -SET (CTEST_CVS_COMMAND "hg") -SET (CTEST_CVS_CHECKOUT "${CTEST_CVS_COMMAND} clone -r 2.0 http://bitbucket.org/eigen/eigen \"${CTEST_SOURCE_DIRECTORY}\"") - -# which ctest command to use for running the dashboard -SET (CTEST_COMMAND "${EIGEN_CMAKE_DIR}ctest -D ${EIGEN_MODE}") - -# what cmake command to use for configuring this dashboard -SET (CTEST_CMAKE_COMMAND "${EIGEN_CMAKE_DIR}cmake -DEIGEN_BUILD_TESTS=on ") - -#################################################################### -# The values in this section are optional you can either -# have them or leave them commented out -#################################################################### - -# this make sure we get consistent outputs -SET($ENV{LC_MESSAGES} "en_EN") - -# should ctest wipe the binary tree before running -SET(CTEST_START_WITH_EMPTY_BINARY_DIRECTORY TRUE) -SET(CTEST_BACKUP_AND_RESTORE TRUE) - -# this is the initial cache to use for the binary tree, be careful to escape -# any quotes inside of this string if you use it -if(WIN32 AND NOT UNIX) - #message(SEND_ERROR "win32") - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -G \"NMake Makefiles\" -DCMAKE_MAKE_PROGRAM=nmake") - SET (CTEST_INITIAL_CACHE " - MAKECOMMAND:STRING=nmake -i - CMAKE_MAKE_PROGRAM:FILEPATH=nmake - CMAKE_GENERATOR:INTERNAL=NMake Makefiles - BUILDNAME:STRING=${EIGEN_BUILD_STRING} - SITE:STRING=${EIGEN_SITE} - ") -else(WIN32 AND NOT UNIX) - SET (CTEST_INITIAL_CACHE " - BUILDNAME:STRING=${EIGEN_BUILD_STRING} - SITE:STRING=${EIGEN_SITE} - ") -endif(WIN32 AND NOT UNIX) - -# set any extra environment variables to use during the execution of the script here: - -if(EIGEN_CXX) - set(CTEST_ENVIRONMENT "CXX=${EIGEN_CXX}") -endif(EIGEN_CXX) - -if(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - if(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE2) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE3) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON -DEIGEN_TEST_SSE3=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES Altivec) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_ALTIVEC=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES novec) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_NO_EXPLICIT_VECTORIZATION=ON") - else(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE2) - message(FATAL_ERROR "Invalid value for EIGEN_EXPLICIT_VECTORIZATION (${EIGEN_EXPLICIT_VECTORIZATION}), must be: novec, SSE2, SSE3, Altivec") - endif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE2) -endif(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - -if(DEFINED EIGEN_CMAKE_ARGS) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} ${EIGEN_CMAKE_ARGS}") -endif(DEFINED EIGEN_CMAKE_ARGS) diff --git a/ground/gcs/src/libs/eigen/test/eigen2support.cpp b/ground/gcs/src/libs/eigen/test/eigen2support.cpp index 1fa49a8c87..ad1d980916 100644 --- a/ground/gcs/src/libs/eigen/test/eigen2support.cpp +++ b/ground/gcs/src/libs/eigen/test/eigen2support.cpp @@ -8,7 +8,6 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT -#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING #include "main.h" diff --git a/ground/gcs/src/libs/eigen/test/eigensolver_complex.cpp b/ground/gcs/src/libs/eigen/test/eigensolver_complex.cpp index c9d8c0877e..293b1b2656 100644 --- a/ground/gcs/src/libs/eigen/test/eigensolver_complex.cpp +++ b/ground/gcs/src/libs/eigen/test/eigensolver_complex.cpp @@ -13,20 +13,59 @@ #include #include -/* Check that two column vectors are approximately equal upto permutations, - by checking that the k-th power sums are equal for k = 1, ..., vec1.rows() */ +template bool find_pivot(typename MatrixType::Scalar tol, MatrixType &diffs, Index col=0) +{ + bool match = diffs.diagonal().sum() <= tol; + if(match || col==diffs.cols()) + { + return match; + } + else + { + Index n = diffs.cols(); + std::vector > transpositions; + for(Index i=col; i tol) + break; + + best_index += col; + + diffs.row(col).swap(diffs.row(best_index)); + if(find_pivot(tol,diffs,col+1)) return true; + diffs.row(col).swap(diffs.row(best_index)); + + // move current pivot to the end + diffs.row(n-(i-col)-1).swap(diffs.row(best_index)); + transpositions.push_back(std::pair(n-(i-col)-1,best_index)); + } + // restore + for(Index k=transpositions.size()-1; k>=0; --k) + diffs.row(transpositions[k].first).swap(diffs.row(transpositions[k].second)); + } + return false; +} + +/* Check that two column vectors are approximately equal upto permutations. + * Initially, this method checked that the k-th power sums are equal for all k = 1, ..., vec1.rows(), + * however this strategy is numerically inacurate because of numerical cancellation issues. + */ template void verify_is_approx_upto_permutation(const VectorType& vec1, const VectorType& vec2) { - typedef typename NumTraits::Real RealScalar; + typedef typename VectorType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; VERIFY(vec1.cols() == 1); VERIFY(vec2.cols() == 1); VERIFY(vec1.rows() == vec2.rows()); - for (int k = 1; k <= vec1.rows(); ++k) - { - VERIFY_IS_APPROX(vec1.array().pow(RealScalar(k)).sum(), vec2.array().pow(RealScalar(k)).sum()); - } + + Index n = vec1.rows(); + RealScalar tol = test_precision()*test_precision()*numext::maxi(vec1.squaredNorm(),vec2.squaredNorm()); + Matrix diffs = (vec1.rowwise().replicate(n) - vec2.rowwise().replicate(n).transpose()).cwiseAbs2(); + + VERIFY( find_pivot(tol, diffs) ); } @@ -79,13 +118,28 @@ template void eigensolver(const MatrixType& m) MatrixType id = MatrixType::Identity(rows, cols); VERIFY_IS_APPROX(id.operatorNorm(), RealScalar(1)); - if (rows > 1) + if (rows > 1 && rows < 20) { // Test matrix with NaN a(0,0) = std::numeric_limits::quiet_NaN(); ComplexEigenSolver eiNaN(a); VERIFY_IS_EQUAL(eiNaN.info(), NoConvergence); } + + // regression test for bug 1098 + { + ComplexEigenSolver eig(a.adjoint() * a); + eig.compute(a.adjoint() * a); + } + + // regression test for bug 478 + { + a.setZero(); + ComplexEigenSolver ei3(a); + VERIFY_IS_EQUAL(ei3.info(), Success); + VERIFY_IS_MUCH_SMALLER_THAN(ei3.eigenvalues().norm(),RealScalar(1)); + VERIFY((ei3.eigenvectors().transpose()*ei3.eigenvectors().transpose()).eval().isIdentity()); + } } template void eigensolver_verify_assert(const MatrixType& m) @@ -108,6 +162,7 @@ void test_eigensolver_complex() CALL_SUBTEST_2( eigensolver(MatrixXcd(s,s)) ); CALL_SUBTEST_3( eigensolver(Matrix, 1, 1>()) ); CALL_SUBTEST_4( eigensolver(Matrix3f()) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } CALL_SUBTEST_1( eigensolver_verify_assert(Matrix4cf()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); diff --git a/ground/gcs/src/libs/eigen/test/eigensolver_generalized_real.cpp b/ground/gcs/src/libs/eigen/test/eigensolver_generalized_real.cpp index 566a4bdc63..9c0838ba4e 100644 --- a/ground/gcs/src/libs/eigen/test/eigensolver_generalized_real.cpp +++ b/ground/gcs/src/libs/eigen/test/eigensolver_generalized_real.cpp @@ -1,15 +1,17 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2012 Gael Guennebaud +// Copyright (C) 2012-2016 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define EIGEN_RUNTIME_NO_MALLOC #include "main.h" #include #include +#include template void generalized_eigensolver_real(const MatrixType& m) { @@ -21,6 +23,7 @@ template void generalized_eigensolver_real(const MatrixType Index cols = m.cols(); typedef typename MatrixType::Scalar Scalar; + typedef std::complex ComplexScalar; typedef Matrix VectorType; MatrixType a = MatrixType::Random(rows,cols); @@ -31,14 +34,49 @@ template void generalized_eigensolver_real(const MatrixType MatrixType spdB = b.adjoint() * b + b1.adjoint() * b1; // lets compare to GeneralizedSelfAdjointEigenSolver - GeneralizedSelfAdjointEigenSolver symmEig(spdA, spdB); - GeneralizedEigenSolver eig(spdA, spdB); + { + GeneralizedSelfAdjointEigenSolver symmEig(spdA, spdB); + GeneralizedEigenSolver eig(spdA, spdB); - VERIFY_IS_EQUAL(eig.eigenvalues().imag().cwiseAbs().maxCoeff(), 0); + VERIFY_IS_EQUAL(eig.eigenvalues().imag().cwiseAbs().maxCoeff(), 0); - VectorType realEigenvalues = eig.eigenvalues().real(); - std::sort(realEigenvalues.data(), realEigenvalues.data()+realEigenvalues.size()); - VERIFY_IS_APPROX(realEigenvalues, symmEig.eigenvalues()); + VectorType realEigenvalues = eig.eigenvalues().real(); + std::sort(realEigenvalues.data(), realEigenvalues.data()+realEigenvalues.size()); + VERIFY_IS_APPROX(realEigenvalues, symmEig.eigenvalues()); + + // check eigenvectors + typename GeneralizedEigenSolver::EigenvectorsType D = eig.eigenvalues().asDiagonal(); + typename GeneralizedEigenSolver::EigenvectorsType V = eig.eigenvectors(); + VERIFY_IS_APPROX(spdA*V, spdB*V*D); + } + + // non symmetric case: + { + GeneralizedEigenSolver eig(rows); + // TODO enable full-prealocation of required memory, this probably requires an in-place mode for HessenbergDecomposition + //Eigen::internal::set_is_malloc_allowed(false); + eig.compute(a,b); + //Eigen::internal::set_is_malloc_allowed(true); + for(Index k=0; k tmp = (eig.betas()(k)*a).template cast() - eig.alphas()(k)*b; + if(tmp.size()>1 && tmp.norm()>(std::numeric_limits::min)()) + tmp /= tmp.norm(); + VERIFY_IS_MUCH_SMALLER_THAN( std::abs(tmp.determinant()), Scalar(1) ); + } + // check eigenvectors + typename GeneralizedEigenSolver::EigenvectorsType D = eig.eigenvalues().asDiagonal(); + typename GeneralizedEigenSolver::EigenvectorsType V = eig.eigenvectors(); + VERIFY_IS_APPROX(a*V, b*V*D); + } + + // regression test for bug 1098 + { + GeneralizedSelfAdjointEigenSolver eig1(a.adjoint() * a,b.adjoint() * b); + eig1.compute(a.adjoint() * a,b.adjoint() * b); + GeneralizedEigenSolver eig2(a.adjoint() * a,b.adjoint() * b); + eig2.compute(a.adjoint() * a,b.adjoint() * b); + } } void test_eigensolver_generalized_real() @@ -49,7 +87,7 @@ void test_eigensolver_generalized_real() s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(s,s)) ); - // some trivial but implementation-wise tricky cases + // some trivial but implementation-wise special cases CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(1,1)) ); CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(2,2)) ); CALL_SUBTEST_3( generalized_eigensolver_real(Matrix()) ); diff --git a/ground/gcs/src/libs/eigen/test/eigensolver_generic.cpp b/ground/gcs/src/libs/eigen/test/eigensolver_generic.cpp index 005af81ebb..d0e644d4be 100644 --- a/ground/gcs/src/libs/eigen/test/eigensolver_generic.cpp +++ b/ground/gcs/src/libs/eigen/test/eigensolver_generic.cpp @@ -63,13 +63,28 @@ template void eigensolver(const MatrixType& m) MatrixType id = MatrixType::Identity(rows, cols); VERIFY_IS_APPROX(id.operatorNorm(), RealScalar(1)); - if (rows > 2) + if (rows > 2 && rows < 20) { // Test matrix with NaN a(0,0) = std::numeric_limits::quiet_NaN(); EigenSolver eiNaN(a); VERIFY_IS_EQUAL(eiNaN.info(), NoConvergence); } + + // regression test for bug 1098 + { + EigenSolver eig(a.adjoint() * a); + eig.compute(a.adjoint() * a); + } + + // regression test for bug 478 + { + a.setZero(); + EigenSolver ei3(a); + VERIFY_IS_EQUAL(ei3.info(), Success); + VERIFY_IS_MUCH_SMALLER_THAN(ei3.eigenvalues().norm(),RealScalar(1)); + VERIFY((ei3.eigenvectors().transpose()*ei3.eigenvectors().transpose()).eval().isIdentity()); + } } template void eigensolver_verify_assert(const MatrixType& m) @@ -93,6 +108,7 @@ void test_eigensolver_generic() CALL_SUBTEST_1( eigensolver(Matrix4f()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_2( eigensolver(MatrixXd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) // some trivial but implementation-wise tricky cases CALL_SUBTEST_2( eigensolver(MatrixXd(1,1)) ); @@ -114,12 +130,37 @@ void test_eigensolver_generic() CALL_SUBTEST_2( { MatrixXd A(1,1); - A(0,0) = std::sqrt(-1.); + A(0,0) = std::sqrt(-1.); // is Not-a-Number Eigen::EigenSolver solver(A); - MatrixXd V(1, 1); - V(0,0) = solver.eigenvectors()(0,0).real(); + VERIFY_IS_EQUAL(solver.info(), NumericalIssue); } ); +#ifdef EIGEN_TEST_PART_2 + { + // regression test for bug 793 + MatrixXd a(3,3); + a << 0, 0, 1, + 1, 1, 1, + 1, 1e+200, 1; + Eigen::EigenSolver eig(a); + double scale = 1e-200; // scale to avoid overflow during the comparisons + VERIFY_IS_APPROX(a * eig.pseudoEigenvectors()*scale, eig.pseudoEigenvectors() * eig.pseudoEigenvalueMatrix()*scale); + VERIFY_IS_APPROX(a * eig.eigenvectors()*scale, eig.eigenvectors() * eig.eigenvalues().asDiagonal()*scale); + } + { + // check a case where all eigenvalues are null. + MatrixXd a(2,2); + a << 1, 1, + -1, -1; + Eigen::EigenSolver eig(a); + VERIFY_IS_APPROX(eig.pseudoEigenvectors().squaredNorm(), 2.); + VERIFY_IS_APPROX((a * eig.pseudoEigenvectors()).norm()+1., 1.); + VERIFY_IS_APPROX((eig.pseudoEigenvectors() * eig.pseudoEigenvalueMatrix()).norm()+1., 1.); + VERIFY_IS_APPROX((a * eig.eigenvectors()).norm()+1., 1.); + VERIFY_IS_APPROX((eig.eigenvectors() * eig.eigenvalues().asDiagonal()).norm()+1., 1.); + } +#endif + TEST_SET_BUT_UNUSED_VARIABLE(s) } diff --git a/ground/gcs/src/libs/eigen/test/eigensolver_selfadjoint.cpp b/ground/gcs/src/libs/eigen/test/eigensolver_selfadjoint.cpp index 38689cfbf3..39ad4130e8 100644 --- a/ground/gcs/src/libs/eigen/test/eigensolver_selfadjoint.cpp +++ b/ground/gcs/src/libs/eigen/test/eigensolver_selfadjoint.cpp @@ -9,8 +9,62 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "main.h" +#include "svd_fill.h" #include #include +#include + + +template void selfadjointeigensolver_essential_check(const MatrixType& m) +{ + typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + RealScalar eival_eps = numext::mini(test_precision(), NumTraits::dummy_precision()*20000); + + SelfAdjointEigenSolver eiSymm(m); + VERIFY_IS_EQUAL(eiSymm.info(), Success); + + RealScalar scaling = m.cwiseAbs().maxCoeff(); + + if(scaling<(std::numeric_limits::min)()) + { + VERIFY(eiSymm.eigenvalues().cwiseAbs().maxCoeff() <= (std::numeric_limits::min)()); + } + else + { + VERIFY_IS_APPROX((m.template selfadjointView() * eiSymm.eigenvectors())/scaling, + (eiSymm.eigenvectors() * eiSymm.eigenvalues().asDiagonal())/scaling); + } + VERIFY_IS_APPROX(m.template selfadjointView().eigenvalues(), eiSymm.eigenvalues()); + VERIFY_IS_UNITARY(eiSymm.eigenvectors()); + + if(m.cols()<=4) + { + SelfAdjointEigenSolver eiDirect; + eiDirect.computeDirect(m); + VERIFY_IS_EQUAL(eiDirect.info(), Success); + if(! eiSymm.eigenvalues().isApprox(eiDirect.eigenvalues(), eival_eps) ) + { + std::cerr << "reference eigenvalues: " << eiSymm.eigenvalues().transpose() << "\n" + << "obtained eigenvalues: " << eiDirect.eigenvalues().transpose() << "\n" + << "diff: " << (eiSymm.eigenvalues()-eiDirect.eigenvalues()).transpose() << "\n" + << "error (eps): " << (eiSymm.eigenvalues()-eiDirect.eigenvalues()).norm() / eiSymm.eigenvalues().norm() << " (" << eival_eps << ")\n"; + } + if(scaling<(std::numeric_limits::min)()) + { + VERIFY(eiDirect.eigenvalues().cwiseAbs().maxCoeff() <= (std::numeric_limits::min)()); + } + else + { + VERIFY_IS_APPROX(eiSymm.eigenvalues()/scaling, eiDirect.eigenvalues()/scaling); + VERIFY_IS_APPROX((m.template selfadjointView() * eiDirect.eigenvectors())/scaling, + (eiDirect.eigenvectors() * eiDirect.eigenvalues().asDiagonal())/scaling); + VERIFY_IS_APPROX(m.template selfadjointView().eigenvalues()/scaling, eiDirect.eigenvalues()/scaling); + } + + VERIFY_IS_UNITARY(eiDirect.eigenvectors()); + } +} template void selfadjointeigensolver(const MatrixType& m) { @@ -31,17 +85,8 @@ template void selfadjointeigensolver(const MatrixType& m) MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; MatrixType symmC = symmA; - // randomly nullify some rows/columns - { - Index count = 1;//internal::random(-cols,cols); - for(Index k=0; k(0,cols-1); - symmA.row(i).setZero(); - symmA.col(i).setZero(); - } - } - + svd_fill_random(symmA,Symmetric); + symmA.template triangularView().setZero(); symmC.template triangularView().setZero(); @@ -49,23 +94,13 @@ template void selfadjointeigensolver(const MatrixType& m) MatrixType b1 = MatrixType::Random(rows,cols); MatrixType symmB = b.adjoint() * b + b1.adjoint() * b1; symmB.template triangularView().setZero(); + + CALL_SUBTEST( selfadjointeigensolver_essential_check(symmA) ); SelfAdjointEigenSolver eiSymm(symmA); - SelfAdjointEigenSolver eiDirect; - eiDirect.computeDirect(symmA); // generalized eigen pb GeneralizedSelfAdjointEigenSolver eiSymmGen(symmC, symmB); - VERIFY_IS_EQUAL(eiSymm.info(), Success); - VERIFY((symmA.template selfadjointView() * eiSymm.eigenvectors()).isApprox( - eiSymm.eigenvectors() * eiSymm.eigenvalues().asDiagonal(), largerEps)); - VERIFY_IS_APPROX(symmA.template selfadjointView().eigenvalues(), eiSymm.eigenvalues()); - - VERIFY_IS_EQUAL(eiDirect.info(), Success); - VERIFY((symmA.template selfadjointView() * eiDirect.eigenvectors()).isApprox( - eiDirect.eigenvectors() * eiDirect.eigenvalues().asDiagonal(), largerEps)); - VERIFY_IS_APPROX(symmA.template selfadjointView().eigenvalues(), eiDirect.eigenvalues()); - SelfAdjointEigenSolver eiSymmNoEivecs(symmA, false); VERIFY_IS_EQUAL(eiSymmNoEivecs.info(), Success); VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues()); @@ -111,37 +146,111 @@ template void selfadjointeigensolver(const MatrixType& m) // test Tridiagonalization's methods Tridiagonalization tridiag(symmC); - // FIXME tridiag.matrixQ().adjoint() does not work + VERIFY_IS_APPROX(tridiag.diagonal(), tridiag.matrixT().diagonal()); + VERIFY_IS_APPROX(tridiag.subDiagonal(), tridiag.matrixT().template diagonal<-1>()); + Matrix T = tridiag.matrixT(); + if(rows>1 && cols>1) { + // FIXME check that upper and lower part are 0: + //VERIFY(T.topRightCorner(rows-2, cols-2).template triangularView().isZero()); + } + VERIFY_IS_APPROX(tridiag.diagonal(), T.diagonal()); + VERIFY_IS_APPROX(tridiag.subDiagonal(), T.template diagonal<1>()); VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT() * tridiag.matrixQ().adjoint()); - if (rows > 1) + // Test computation of eigenvalues from tridiagonal matrix + if(rows > 1) + { + SelfAdjointEigenSolver eiSymmTridiag; + eiSymmTridiag.computeFromTridiagonal(tridiag.matrixT().diagonal(), tridiag.matrixT().diagonal(-1), ComputeEigenvectors); + VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmTridiag.eigenvalues()); + VERIFY_IS_APPROX(tridiag.matrixT(), eiSymmTridiag.eigenvectors().real() * eiSymmTridiag.eigenvalues().asDiagonal() * eiSymmTridiag.eigenvectors().real().transpose()); + } + + if (rows > 1 && rows < 20) { // Test matrix with NaN symmC(0,0) = std::numeric_limits::quiet_NaN(); SelfAdjointEigenSolver eiSymmNaN(symmC); VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence); } + + // regression test for bug 1098 + { + SelfAdjointEigenSolver eig(a.adjoint() * a); + eig.compute(a.adjoint() * a); + } + + // regression test for bug 478 + { + a.setZero(); + SelfAdjointEigenSolver ei3(a); + VERIFY_IS_EQUAL(ei3.info(), Success); + VERIFY_IS_MUCH_SMALLER_THAN(ei3.eigenvalues().norm(),RealScalar(1)); + VERIFY((ei3.eigenvectors().transpose()*ei3.eigenvectors().transpose()).eval().isIdentity()); + } +} + +template +void bug_854() +{ + Matrix3d m; + m << 850.961, 51.966, 0, + 51.966, 254.841, 0, + 0, 0, 0; + selfadjointeigensolver_essential_check(m); +} + +template +void bug_1014() +{ + Matrix3d m; + m << 0.11111111111111114658, 0, 0, + 0, 0.11111111111111109107, 0, + 0, 0, 0.11111111111111107719; + selfadjointeigensolver_essential_check(m); +} + +template +void bug_1225() +{ + Matrix3d m1, m2; + m1.setRandom(); + m1 = m1*m1.transpose(); + m2 = m1.triangularView(); + SelfAdjointEigenSolver eig1(m1); + SelfAdjointEigenSolver eig2(m2.selfadjointView()); + VERIFY_IS_APPROX(eig1.eigenvalues(), eig2.eigenvalues()); +} + +template +void bug_1204() +{ + SparseMatrix A(2,2); + A.setIdentity(); + SelfAdjointEigenSolver > eig(A); } void test_eigensolver_selfadjoint() { int s = 0; for(int i = 0; i < g_repeat; i++) { + // trivial test for 1x1 matrices: + CALL_SUBTEST_1( selfadjointeigensolver(Matrix())); + CALL_SUBTEST_1( selfadjointeigensolver(Matrix())); // very important to test 3x3 and 2x2 matrices since we provide special paths for them - CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) ); - CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) ); - CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) ); - CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) ); + CALL_SUBTEST_12( selfadjointeigensolver(Matrix2f()) ); + CALL_SUBTEST_12( selfadjointeigensolver(Matrix2d()) ); + CALL_SUBTEST_13( selfadjointeigensolver(Matrix3f()) ); + CALL_SUBTEST_13( selfadjointeigensolver(Matrix3d()) ); CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) ); + s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) ); - s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(s,s)) ); - s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_5( selfadjointeigensolver(MatrixXcd(s,s)) ); - - s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_9( selfadjointeigensolver(Matrix,Dynamic,Dynamic,RowMajor>(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) // some trivial but implementation-wise tricky cases CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(1,1)) ); @@ -149,6 +258,11 @@ void test_eigensolver_selfadjoint() CALL_SUBTEST_6( selfadjointeigensolver(Matrix()) ); CALL_SUBTEST_7( selfadjointeigensolver(Matrix()) ); } + + CALL_SUBTEST_13( bug_854<0>() ); + CALL_SUBTEST_13( bug_1014<0>() ); + CALL_SUBTEST_13( bug_1204<0>() ); + CALL_SUBTEST_13( bug_1225<0>() ); // Test problem size constructors s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); diff --git a/ground/gcs/src/libs/eigen/test/evaluator_common.h b/ground/gcs/src/libs/eigen/test/evaluator_common.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ground/gcs/src/libs/eigen/test/evaluators.cpp b/ground/gcs/src/libs/eigen/test/evaluators.cpp new file mode 100644 index 0000000000..aed5a05a7f --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/evaluators.cpp @@ -0,0 +1,499 @@ + +#include "main.h" + +namespace Eigen { + + template + const Product + prod(const Lhs& lhs, const Rhs& rhs) + { + return Product(lhs,rhs); + } + + template + const Product + lazyprod(const Lhs& lhs, const Rhs& rhs) + { + return Product(lhs,rhs); + } + + template + EIGEN_STRONG_INLINE + DstXprType& copy_using_evaluator(const EigenBase &dst, const SrcXprType &src) + { + call_assignment(dst.const_cast_derived(), src.derived(), internal::assign_op()); + return dst.const_cast_derived(); + } + + template class StorageBase, typename SrcXprType> + EIGEN_STRONG_INLINE + const DstXprType& copy_using_evaluator(const NoAlias& dst, const SrcXprType &src) + { + call_assignment(dst, src.derived(), internal::assign_op()); + return dst.expression(); + } + + template + EIGEN_STRONG_INLINE + DstXprType& copy_using_evaluator(const PlainObjectBase &dst, const SrcXprType &src) + { + #ifdef EIGEN_NO_AUTOMATIC_RESIZING + eigen_assert((dst.size()==0 || (IsVectorAtCompileTime ? (dst.size() == src.size()) + : (dst.rows() == src.rows() && dst.cols() == src.cols()))) + && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined"); + #else + dst.const_cast_derived().resizeLike(src.derived()); + #endif + + call_assignment(dst.const_cast_derived(), src.derived(), internal::assign_op()); + return dst.const_cast_derived(); + } + + template + void add_assign_using_evaluator(const DstXprType& dst, const SrcXprType& src) + { + typedef typename DstXprType::Scalar Scalar; + call_assignment(const_cast(dst), src.derived(), internal::add_assign_op()); + } + + template + void subtract_assign_using_evaluator(const DstXprType& dst, const SrcXprType& src) + { + typedef typename DstXprType::Scalar Scalar; + call_assignment(const_cast(dst), src.derived(), internal::sub_assign_op()); + } + + template + void multiply_assign_using_evaluator(const DstXprType& dst, const SrcXprType& src) + { + typedef typename DstXprType::Scalar Scalar; + call_assignment(dst.const_cast_derived(), src.derived(), internal::mul_assign_op()); + } + + template + void divide_assign_using_evaluator(const DstXprType& dst, const SrcXprType& src) + { + typedef typename DstXprType::Scalar Scalar; + call_assignment(dst.const_cast_derived(), src.derived(), internal::div_assign_op()); + } + + template + void swap_using_evaluator(const DstXprType& dst, const SrcXprType& src) + { + typedef typename DstXprType::Scalar Scalar; + call_assignment(dst.const_cast_derived(), src.const_cast_derived(), internal::swap_assign_op()); + } + + namespace internal { + template class StorageBase, typename Src, typename Func> + EIGEN_DEVICE_FUNC void call_assignment(const NoAlias& dst, const Src& src, const Func& func) + { + call_assignment_no_alias(dst.expression(), src, func); + } + } + +} + +template long get_cost(const XprType& ) { return Eigen::internal::evaluator::CoeffReadCost; } + +using namespace std; + +#define VERIFY_IS_APPROX_EVALUATOR(DEST,EXPR) VERIFY_IS_APPROX(copy_using_evaluator(DEST,(EXPR)), (EXPR).eval()); +#define VERIFY_IS_APPROX_EVALUATOR2(DEST,EXPR,REF) VERIFY_IS_APPROX(copy_using_evaluator(DEST,(EXPR)), (REF).eval()); + +void test_evaluators() +{ + // Testing Matrix evaluator and Transpose + Vector2d v = Vector2d::Random(); + const Vector2d v_const(v); + Vector2d v2; + RowVector2d w; + + VERIFY_IS_APPROX_EVALUATOR(v2, v); + VERIFY_IS_APPROX_EVALUATOR(v2, v_const); + + // Testing Transpose + VERIFY_IS_APPROX_EVALUATOR(w, v.transpose()); // Transpose as rvalue + VERIFY_IS_APPROX_EVALUATOR(w, v_const.transpose()); + + copy_using_evaluator(w.transpose(), v); // Transpose as lvalue + VERIFY_IS_APPROX(w,v.transpose().eval()); + + copy_using_evaluator(w.transpose(), v_const); + VERIFY_IS_APPROX(w,v_const.transpose().eval()); + + // Testing Array evaluator + { + ArrayXXf a(2,3); + ArrayXXf b(3,2); + a << 1,2,3, 4,5,6; + const ArrayXXf a_const(a); + + VERIFY_IS_APPROX_EVALUATOR(b, a.transpose()); + + VERIFY_IS_APPROX_EVALUATOR(b, a_const.transpose()); + + // Testing CwiseNullaryOp evaluator + copy_using_evaluator(w, RowVector2d::Random()); + VERIFY((w.array() >= -1).all() && (w.array() <= 1).all()); // not easy to test ... + + VERIFY_IS_APPROX_EVALUATOR(w, RowVector2d::Zero()); + + VERIFY_IS_APPROX_EVALUATOR(w, RowVector2d::Constant(3)); + + // mix CwiseNullaryOp and transpose + VERIFY_IS_APPROX_EVALUATOR(w, Vector2d::Zero().transpose()); + } + + { + // test product expressions + int s = internal::random(1,100); + MatrixXf a(s,s), b(s,s), c(s,s), d(s,s); + a.setRandom(); + b.setRandom(); + c.setRandom(); + d.setRandom(); + VERIFY_IS_APPROX_EVALUATOR(d, (a + b)); + VERIFY_IS_APPROX_EVALUATOR(d, (a + b).transpose()); + VERIFY_IS_APPROX_EVALUATOR2(d, prod(a,b), a*b); + VERIFY_IS_APPROX_EVALUATOR2(d.noalias(), prod(a,b), a*b); + VERIFY_IS_APPROX_EVALUATOR2(d, prod(a,b) + c, a*b + c); + VERIFY_IS_APPROX_EVALUATOR2(d, s * prod(a,b), s * a*b); + VERIFY_IS_APPROX_EVALUATOR2(d, prod(a,b).transpose(), (a*b).transpose()); + VERIFY_IS_APPROX_EVALUATOR2(d, prod(a,b) + prod(b,c), a*b + b*c); + + // check that prod works even with aliasing present + c = a*a; + copy_using_evaluator(a, prod(a,a)); + VERIFY_IS_APPROX(a,c); + + // check compound assignment of products + d = c; + add_assign_using_evaluator(c.noalias(), prod(a,b)); + d.noalias() += a*b; + VERIFY_IS_APPROX(c, d); + + d = c; + subtract_assign_using_evaluator(c.noalias(), prod(a,b)); + d.noalias() -= a*b; + VERIFY_IS_APPROX(c, d); + } + + { + // test product with all possible sizes + int s = internal::random(1,100); + Matrix m11, res11; m11.setRandom(1,1); + Matrix m14, res14; m14.setRandom(1,4); + Matrix m1X, res1X; m1X.setRandom(1,s); + Matrix m41, res41; m41.setRandom(4,1); + Matrix m44, res44; m44.setRandom(4,4); + Matrix m4X, res4X; m4X.setRandom(4,s); + Matrix mX1, resX1; mX1.setRandom(s,1); + Matrix mX4, resX4; mX4.setRandom(s,4); + Matrix mXX, resXX; mXX.setRandom(s,s); + + VERIFY_IS_APPROX_EVALUATOR2(res11, prod(m11,m11), m11*m11); + VERIFY_IS_APPROX_EVALUATOR2(res11, prod(m14,m41), m14*m41); + VERIFY_IS_APPROX_EVALUATOR2(res11, prod(m1X,mX1), m1X*mX1); + VERIFY_IS_APPROX_EVALUATOR2(res14, prod(m11,m14), m11*m14); + VERIFY_IS_APPROX_EVALUATOR2(res14, prod(m14,m44), m14*m44); + VERIFY_IS_APPROX_EVALUATOR2(res14, prod(m1X,mX4), m1X*mX4); + VERIFY_IS_APPROX_EVALUATOR2(res1X, prod(m11,m1X), m11*m1X); + VERIFY_IS_APPROX_EVALUATOR2(res1X, prod(m14,m4X), m14*m4X); + VERIFY_IS_APPROX_EVALUATOR2(res1X, prod(m1X,mXX), m1X*mXX); + VERIFY_IS_APPROX_EVALUATOR2(res41, prod(m41,m11), m41*m11); + VERIFY_IS_APPROX_EVALUATOR2(res41, prod(m44,m41), m44*m41); + VERIFY_IS_APPROX_EVALUATOR2(res41, prod(m4X,mX1), m4X*mX1); + VERIFY_IS_APPROX_EVALUATOR2(res44, prod(m41,m14), m41*m14); + VERIFY_IS_APPROX_EVALUATOR2(res44, prod(m44,m44), m44*m44); + VERIFY_IS_APPROX_EVALUATOR2(res44, prod(m4X,mX4), m4X*mX4); + VERIFY_IS_APPROX_EVALUATOR2(res4X, prod(m41,m1X), m41*m1X); + VERIFY_IS_APPROX_EVALUATOR2(res4X, prod(m44,m4X), m44*m4X); + VERIFY_IS_APPROX_EVALUATOR2(res4X, prod(m4X,mXX), m4X*mXX); + VERIFY_IS_APPROX_EVALUATOR2(resX1, prod(mX1,m11), mX1*m11); + VERIFY_IS_APPROX_EVALUATOR2(resX1, prod(mX4,m41), mX4*m41); + VERIFY_IS_APPROX_EVALUATOR2(resX1, prod(mXX,mX1), mXX*mX1); + VERIFY_IS_APPROX_EVALUATOR2(resX4, prod(mX1,m14), mX1*m14); + VERIFY_IS_APPROX_EVALUATOR2(resX4, prod(mX4,m44), mX4*m44); + VERIFY_IS_APPROX_EVALUATOR2(resX4, prod(mXX,mX4), mXX*mX4); + VERIFY_IS_APPROX_EVALUATOR2(resXX, prod(mX1,m1X), mX1*m1X); + VERIFY_IS_APPROX_EVALUATOR2(resXX, prod(mX4,m4X), mX4*m4X); + VERIFY_IS_APPROX_EVALUATOR2(resXX, prod(mXX,mXX), mXX*mXX); + } + + { + ArrayXXf a(2,3); + ArrayXXf b(3,2); + a << 1,2,3, 4,5,6; + const ArrayXXf a_const(a); + + // this does not work because Random is eval-before-nested: + // copy_using_evaluator(w, Vector2d::Random().transpose()); + + // test CwiseUnaryOp + VERIFY_IS_APPROX_EVALUATOR(v2, 3 * v); + VERIFY_IS_APPROX_EVALUATOR(w, (3 * v).transpose()); + VERIFY_IS_APPROX_EVALUATOR(b, (a + 3).transpose()); + VERIFY_IS_APPROX_EVALUATOR(b, (2 * a_const + 3).transpose()); + + // test CwiseBinaryOp + VERIFY_IS_APPROX_EVALUATOR(v2, v + Vector2d::Ones()); + VERIFY_IS_APPROX_EVALUATOR(w, (v + Vector2d::Ones()).transpose().cwiseProduct(RowVector2d::Constant(3))); + + // dynamic matrices and arrays + MatrixXd mat1(6,6), mat2(6,6); + VERIFY_IS_APPROX_EVALUATOR(mat1, MatrixXd::Identity(6,6)); + VERIFY_IS_APPROX_EVALUATOR(mat2, mat1); + copy_using_evaluator(mat2.transpose(), mat1); + VERIFY_IS_APPROX(mat2.transpose(), mat1); + + ArrayXXd arr1(6,6), arr2(6,6); + VERIFY_IS_APPROX_EVALUATOR(arr1, ArrayXXd::Constant(6,6, 3.0)); + VERIFY_IS_APPROX_EVALUATOR(arr2, arr1); + + // test automatic resizing + mat2.resize(3,3); + VERIFY_IS_APPROX_EVALUATOR(mat2, mat1); + arr2.resize(9,9); + VERIFY_IS_APPROX_EVALUATOR(arr2, arr1); + + // test direct traversal + Matrix3f m3; + Array33f a3; + VERIFY_IS_APPROX_EVALUATOR(m3, Matrix3f::Identity()); // matrix, nullary + // TODO: find a way to test direct traversal with array + VERIFY_IS_APPROX_EVALUATOR(m3.transpose(), Matrix3f::Identity().transpose()); // transpose + VERIFY_IS_APPROX_EVALUATOR(m3, 2 * Matrix3f::Identity()); // unary + VERIFY_IS_APPROX_EVALUATOR(m3, Matrix3f::Identity() + Matrix3f::Zero()); // binary + VERIFY_IS_APPROX_EVALUATOR(m3.block(0,0,2,2), Matrix3f::Identity().block(1,1,2,2)); // block + + // test linear traversal + VERIFY_IS_APPROX_EVALUATOR(m3, Matrix3f::Zero()); // matrix, nullary + VERIFY_IS_APPROX_EVALUATOR(a3, Array33f::Zero()); // array + VERIFY_IS_APPROX_EVALUATOR(m3.transpose(), Matrix3f::Zero().transpose()); // transpose + VERIFY_IS_APPROX_EVALUATOR(m3, 2 * Matrix3f::Zero()); // unary + VERIFY_IS_APPROX_EVALUATOR(m3, Matrix3f::Zero() + m3); // binary + + // test inner vectorization + Matrix4f m4, m4src = Matrix4f::Random(); + Array44f a4, a4src = Matrix4f::Random(); + VERIFY_IS_APPROX_EVALUATOR(m4, m4src); // matrix + VERIFY_IS_APPROX_EVALUATOR(a4, a4src); // array + VERIFY_IS_APPROX_EVALUATOR(m4.transpose(), m4src.transpose()); // transpose + // TODO: find out why Matrix4f::Zero() does not allow inner vectorization + VERIFY_IS_APPROX_EVALUATOR(m4, 2 * m4src); // unary + VERIFY_IS_APPROX_EVALUATOR(m4, m4src + m4src); // binary + + // test linear vectorization + MatrixXf mX(6,6), mXsrc = MatrixXf::Random(6,6); + ArrayXXf aX(6,6), aXsrc = ArrayXXf::Random(6,6); + VERIFY_IS_APPROX_EVALUATOR(mX, mXsrc); // matrix + VERIFY_IS_APPROX_EVALUATOR(aX, aXsrc); // array + VERIFY_IS_APPROX_EVALUATOR(mX.transpose(), mXsrc.transpose()); // transpose + VERIFY_IS_APPROX_EVALUATOR(mX, MatrixXf::Zero(6,6)); // nullary + VERIFY_IS_APPROX_EVALUATOR(mX, 2 * mXsrc); // unary + VERIFY_IS_APPROX_EVALUATOR(mX, mXsrc + mXsrc); // binary + + // test blocks and slice vectorization + VERIFY_IS_APPROX_EVALUATOR(m4, (mXsrc.block<4,4>(1,0))); + VERIFY_IS_APPROX_EVALUATOR(aX, ArrayXXf::Constant(10, 10, 3.0).block(2, 3, 6, 6)); + + Matrix4f m4ref = m4; + copy_using_evaluator(m4.block(1, 1, 2, 3), m3.bottomRows(2)); + m4ref.block(1, 1, 2, 3) = m3.bottomRows(2); + VERIFY_IS_APPROX(m4, m4ref); + + mX.setIdentity(20,20); + MatrixXf mXref = MatrixXf::Identity(20,20); + mXsrc = MatrixXf::Random(9,12); + copy_using_evaluator(mX.block(4, 4, 9, 12), mXsrc); + mXref.block(4, 4, 9, 12) = mXsrc; + VERIFY_IS_APPROX(mX, mXref); + + // test Map + const float raw[3] = {1,2,3}; + float buffer[3] = {0,0,0}; + Vector3f v3; + Array3f a3f; + VERIFY_IS_APPROX_EVALUATOR(v3, Map(raw)); + VERIFY_IS_APPROX_EVALUATOR(a3f, Map(raw)); + Vector3f::Map(buffer) = 2*v3; + VERIFY(buffer[0] == 2); + VERIFY(buffer[1] == 4); + VERIFY(buffer[2] == 6); + + // test CwiseUnaryView + mat1.setRandom(); + mat2.setIdentity(); + MatrixXcd matXcd(6,6), matXcd_ref(6,6); + copy_using_evaluator(matXcd.real(), mat1); + copy_using_evaluator(matXcd.imag(), mat2); + matXcd_ref.real() = mat1; + matXcd_ref.imag() = mat2; + VERIFY_IS_APPROX(matXcd, matXcd_ref); + + // test Select + VERIFY_IS_APPROX_EVALUATOR(aX, (aXsrc > 0).select(aXsrc, -aXsrc)); + + // test Replicate + mXsrc = MatrixXf::Random(6, 6); + VectorXf vX = VectorXf::Random(6); + mX.resize(6, 6); + VERIFY_IS_APPROX_EVALUATOR(mX, mXsrc.colwise() + vX); + matXcd.resize(12, 12); + VERIFY_IS_APPROX_EVALUATOR(matXcd, matXcd_ref.replicate(2,2)); + VERIFY_IS_APPROX_EVALUATOR(matXcd, (matXcd_ref.replicate<2,2>())); + + // test partial reductions + VectorXd vec1(6); + VERIFY_IS_APPROX_EVALUATOR(vec1, mat1.rowwise().sum()); + VERIFY_IS_APPROX_EVALUATOR(vec1, mat1.colwise().sum().transpose()); + + // test MatrixWrapper and ArrayWrapper + mat1.setRandom(6,6); + arr1.setRandom(6,6); + VERIFY_IS_APPROX_EVALUATOR(mat2, arr1.matrix()); + VERIFY_IS_APPROX_EVALUATOR(arr2, mat1.array()); + VERIFY_IS_APPROX_EVALUATOR(mat2, (arr1 + 2).matrix()); + VERIFY_IS_APPROX_EVALUATOR(arr2, mat1.array() + 2); + mat2.array() = arr1 * arr1; + VERIFY_IS_APPROX(mat2, (arr1 * arr1).matrix()); + arr2.matrix() = MatrixXd::Identity(6,6); + VERIFY_IS_APPROX(arr2, MatrixXd::Identity(6,6).array()); + + // test Reverse + VERIFY_IS_APPROX_EVALUATOR(arr2, arr1.reverse()); + VERIFY_IS_APPROX_EVALUATOR(arr2, arr1.colwise().reverse()); + VERIFY_IS_APPROX_EVALUATOR(arr2, arr1.rowwise().reverse()); + arr2.reverse() = arr1; + VERIFY_IS_APPROX(arr2, arr1.reverse()); + mat2.array() = mat1.array().reverse(); + VERIFY_IS_APPROX(mat2.array(), mat1.array().reverse()); + + // test Diagonal + VERIFY_IS_APPROX_EVALUATOR(vec1, mat1.diagonal()); + vec1.resize(5); + VERIFY_IS_APPROX_EVALUATOR(vec1, mat1.diagonal(1)); + VERIFY_IS_APPROX_EVALUATOR(vec1, mat1.diagonal<-1>()); + vec1.setRandom(); + + mat2 = mat1; + copy_using_evaluator(mat1.diagonal(1), vec1); + mat2.diagonal(1) = vec1; + VERIFY_IS_APPROX(mat1, mat2); + + copy_using_evaluator(mat1.diagonal<-1>(), mat1.diagonal(1)); + mat2.diagonal<-1>() = mat2.diagonal(1); + VERIFY_IS_APPROX(mat1, mat2); + } + + { + // test swapping + MatrixXd mat1, mat2, mat1ref, mat2ref; + mat1ref = mat1 = MatrixXd::Random(6, 6); + mat2ref = mat2 = 2 * mat1 + MatrixXd::Identity(6, 6); + swap_using_evaluator(mat1, mat2); + mat1ref.swap(mat2ref); + VERIFY_IS_APPROX(mat1, mat1ref); + VERIFY_IS_APPROX(mat2, mat2ref); + + swap_using_evaluator(mat1.block(0, 0, 3, 3), mat2.block(3, 3, 3, 3)); + mat1ref.block(0, 0, 3, 3).swap(mat2ref.block(3, 3, 3, 3)); + VERIFY_IS_APPROX(mat1, mat1ref); + VERIFY_IS_APPROX(mat2, mat2ref); + + swap_using_evaluator(mat1.row(2), mat2.col(3).transpose()); + mat1.row(2).swap(mat2.col(3).transpose()); + VERIFY_IS_APPROX(mat1, mat1ref); + VERIFY_IS_APPROX(mat2, mat2ref); + } + + { + // test compound assignment + const Matrix4d mat_const = Matrix4d::Random(); + Matrix4d mat, mat_ref; + mat = mat_ref = Matrix4d::Identity(); + add_assign_using_evaluator(mat, mat_const); + mat_ref += mat_const; + VERIFY_IS_APPROX(mat, mat_ref); + + subtract_assign_using_evaluator(mat.row(1), 2*mat.row(2)); + mat_ref.row(1) -= 2*mat_ref.row(2); + VERIFY_IS_APPROX(mat, mat_ref); + + const ArrayXXf arr_const = ArrayXXf::Random(5,3); + ArrayXXf arr, arr_ref; + arr = arr_ref = ArrayXXf::Constant(5, 3, 0.5); + multiply_assign_using_evaluator(arr, arr_const); + arr_ref *= arr_const; + VERIFY_IS_APPROX(arr, arr_ref); + + divide_assign_using_evaluator(arr.row(1), arr.row(2) + 1); + arr_ref.row(1) /= (arr_ref.row(2) + 1); + VERIFY_IS_APPROX(arr, arr_ref); + } + + { + // test triangular shapes + MatrixXd A = MatrixXd::Random(6,6), B(6,6), C(6,6), D(6,6); + A.setRandom();B.setRandom(); + VERIFY_IS_APPROX_EVALUATOR2(B, A.triangularView(), MatrixXd(A.triangularView())); + + A.setRandom();B.setRandom(); + VERIFY_IS_APPROX_EVALUATOR2(B, A.triangularView(), MatrixXd(A.triangularView())); + + A.setRandom();B.setRandom(); + VERIFY_IS_APPROX_EVALUATOR2(B, A.triangularView(), MatrixXd(A.triangularView())); + + A.setRandom();B.setRandom(); + C = B; C.triangularView() = A; + copy_using_evaluator(B.triangularView(), A); + VERIFY(B.isApprox(C) && "copy_using_evaluator(B.triangularView(), A)"); + + A.setRandom();B.setRandom(); + C = B; C.triangularView() = A.triangularView(); + copy_using_evaluator(B.triangularView(), A.triangularView()); + VERIFY(B.isApprox(C) && "copy_using_evaluator(B.triangularView(), A.triangularView())"); + + + A.setRandom();B.setRandom(); + C = B; C.triangularView() = A.triangularView().transpose(); + copy_using_evaluator(B.triangularView(), A.triangularView().transpose()); + VERIFY(B.isApprox(C) && "copy_using_evaluator(B.triangularView(), A.triangularView().transpose())"); + + + A.setRandom();B.setRandom(); C = B; D = A; + C.triangularView().swap(D.triangularView()); + swap_using_evaluator(B.triangularView(), A.triangularView()); + VERIFY(B.isApprox(C) && "swap_using_evaluator(B.triangularView(), A.triangularView())"); + + + VERIFY_IS_APPROX_EVALUATOR2(B, prod(A.triangularView(),A), MatrixXd(A.triangularView()*A)); + + VERIFY_IS_APPROX_EVALUATOR2(B, prod(A.selfadjointView(),A), MatrixXd(A.selfadjointView()*A)); + } + + { + // test diagonal shapes + VectorXd d = VectorXd::Random(6); + MatrixXd A = MatrixXd::Random(6,6), B(6,6); + A.setRandom();B.setRandom(); + + VERIFY_IS_APPROX_EVALUATOR2(B, lazyprod(d.asDiagonal(),A), MatrixXd(d.asDiagonal()*A)); + VERIFY_IS_APPROX_EVALUATOR2(B, lazyprod(A,d.asDiagonal()), MatrixXd(A*d.asDiagonal())); + } + + { + // test CoeffReadCost + Matrix4d a, b; + VERIFY_IS_EQUAL( get_cost(a), 1 ); + VERIFY_IS_EQUAL( get_cost(a+b), 3); + VERIFY_IS_EQUAL( get_cost(2*a+b), 4); + VERIFY_IS_EQUAL( get_cost(a*b), 1); + VERIFY_IS_EQUAL( get_cost(a.lazyProduct(b)), 15); + VERIFY_IS_EQUAL( get_cost(a*(a*b)), 1); + VERIFY_IS_EQUAL( get_cost(a.lazyProduct(a*b)), 15); + VERIFY_IS_EQUAL( get_cost(a*(a+b)), 1); + VERIFY_IS_EQUAL( get_cost(a.lazyProduct(a+b)), 15); + } +} diff --git a/ground/gcs/src/libs/eigen/test/fastmath.cpp b/ground/gcs/src/libs/eigen/test/fastmath.cpp new file mode 100644 index 0000000000..cc5db07463 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/fastmath.cpp @@ -0,0 +1,99 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +void check(bool b, bool ref) +{ + std::cout << b; + if(b==ref) + std::cout << " OK "; + else + std::cout << " BAD "; +} + +#if EIGEN_COMP_MSVC && EIGEN_COMP_MSVC < 1800 +namespace std { + template bool (isfinite)(T x) { return _finite(x); } + template bool (isnan)(T x) { return _isnan(x); } + template bool (isinf)(T x) { return _fpclass(x)==_FPCLASS_NINF || _fpclass(x)==_FPCLASS_PINF; } +} +#endif + +template +void check_inf_nan(bool dryrun) { + Matrix m(10); + m.setRandom(); + m(3) = std::numeric_limits::quiet_NaN(); + + if(dryrun) + { + std::cout << "std::isfinite(" << m(3) << ") = "; check((std::isfinite)(m(3)),false); std::cout << " ; numext::isfinite = "; check((numext::isfinite)(m(3)), false); std::cout << "\n"; + std::cout << "std::isinf(" << m(3) << ") = "; check((std::isinf)(m(3)),false); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(3)), false); std::cout << "\n"; + std::cout << "std::isnan(" << m(3) << ") = "; check((std::isnan)(m(3)),true); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(3)), true); std::cout << "\n"; + std::cout << "allFinite: "; check(m.allFinite(), 0); std::cout << "\n"; + std::cout << "hasNaN: "; check(m.hasNaN(), 1); std::cout << "\n"; + std::cout << "\n"; + } + else + { + VERIFY( !(numext::isfinite)(m(3)) ); + VERIFY( !(numext::isinf)(m(3)) ); + VERIFY( (numext::isnan)(m(3)) ); + VERIFY( !m.allFinite() ); + VERIFY( m.hasNaN() ); + } + T hidden_zero = (std::numeric_limits::min)()*(std::numeric_limits::min)(); + m(4) /= hidden_zero; + if(dryrun) + { + std::cout << "std::isfinite(" << m(4) << ") = "; check((std::isfinite)(m(4)),false); std::cout << " ; numext::isfinite = "; check((numext::isfinite)(m(4)), false); std::cout << "\n"; + std::cout << "std::isinf(" << m(4) << ") = "; check((std::isinf)(m(4)),true); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(4)), true); std::cout << "\n"; + std::cout << "std::isnan(" << m(4) << ") = "; check((std::isnan)(m(4)),false); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(4)), false); std::cout << "\n"; + std::cout << "allFinite: "; check(m.allFinite(), 0); std::cout << "\n"; + std::cout << "hasNaN: "; check(m.hasNaN(), 1); std::cout << "\n"; + std::cout << "\n"; + } + else + { + VERIFY( !(numext::isfinite)(m(4)) ); + VERIFY( (numext::isinf)(m(4)) ); + VERIFY( !(numext::isnan)(m(4)) ); + VERIFY( !m.allFinite() ); + VERIFY( m.hasNaN() ); + } + m(3) = 0; + if(dryrun) + { + std::cout << "std::isfinite(" << m(3) << ") = "; check((std::isfinite)(m(3)),true); std::cout << " ; numext::isfinite = "; check((numext::isfinite)(m(3)), true); std::cout << "\n"; + std::cout << "std::isinf(" << m(3) << ") = "; check((std::isinf)(m(3)),false); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(3)), false); std::cout << "\n"; + std::cout << "std::isnan(" << m(3) << ") = "; check((std::isnan)(m(3)),false); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(3)), false); std::cout << "\n"; + std::cout << "allFinite: "; check(m.allFinite(), 0); std::cout << "\n"; + std::cout << "hasNaN: "; check(m.hasNaN(), 0); std::cout << "\n"; + std::cout << "\n\n"; + } + else + { + VERIFY( (numext::isfinite)(m(3)) ); + VERIFY( !(numext::isinf)(m(3)) ); + VERIFY( !(numext::isnan)(m(3)) ); + VERIFY( !m.allFinite() ); + VERIFY( !m.hasNaN() ); + } +} + +void test_fastmath() { + std::cout << "*** float *** \n\n"; check_inf_nan(true); + std::cout << "*** double ***\n\n"; check_inf_nan(true); + std::cout << "*** long double *** \n\n"; check_inf_nan(true); + + check_inf_nan(false); + check_inf_nan(false); + check_inf_nan(false); +} diff --git a/ground/gcs/src/libs/eigen/test/first_aligned.cpp b/ground/gcs/src/libs/eigen/test/first_aligned.cpp index 467f945100..ae2d4bc424 100644 --- a/ground/gcs/src/libs/eigen/test/first_aligned.cpp +++ b/ground/gcs/src/libs/eigen/test/first_aligned.cpp @@ -13,7 +13,7 @@ template void test_first_aligned_helper(Scalar *array, int size) { const int packet_size = sizeof(Scalar) * internal::packet_traits::size; - VERIFY(((size_t(array) + sizeof(Scalar) * internal::first_aligned(array, size)) % packet_size) == 0); + VERIFY(((size_t(array) + sizeof(Scalar) * internal::first_default_aligned(array, size)) % packet_size) == 0); } template @@ -21,7 +21,7 @@ void test_none_aligned_helper(Scalar *array, int size) { EIGEN_UNUSED_VARIABLE(array); EIGEN_UNUSED_VARIABLE(size); - VERIFY(internal::packet_traits::size == 1 || internal::first_aligned(array, size) == size); + VERIFY(internal::packet_traits::size == 1 || internal::first_default_aligned(array, size) == size); } struct some_non_vectorizable_type { float x; }; @@ -41,7 +41,7 @@ void test_first_aligned() test_first_aligned_helper(array_double+1, 50); test_first_aligned_helper(array_double+2, 50); - double *array_double_plus_4_bytes = (double*)(size_t(array_double)+4); + double *array_double_plus_4_bytes = (double*)(internal::UIntPtr(array_double)+4); test_none_aligned_helper(array_double_plus_4_bytes, 50); test_none_aligned_helper(array_double_plus_4_bytes+1, 50); diff --git a/ground/gcs/src/libs/eigen/test/geo_alignedbox.cpp b/ground/gcs/src/libs/eigen/test/geo_alignedbox.cpp index 84663ad1f4..d2339a6511 100644 --- a/ground/gcs/src/libs/eigen/test/geo_alignedbox.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_alignedbox.cpp @@ -16,7 +16,7 @@ using namespace std; template EIGEN_DONT_INLINE -void kill_extra_precision(T& x) { eigen_assert(&x != 0); } +void kill_extra_precision(T& x) { eigen_assert((void*)(&x) != (void*)0); } template void alignedbox(const BoxType& _box) @@ -48,12 +48,21 @@ template void alignedbox(const BoxType& _box) b0.extend(p0); b0.extend(p1); VERIFY(b0.contains(p0*s1+(Scalar(1)-s1)*p1)); + VERIFY(b0.contains(b0.center())); + VERIFY_IS_APPROX(b0.center(),(p0+p1)/Scalar(2)); (b2 = b0).extend(b1); VERIFY(b2.contains(b0)); VERIFY(b2.contains(b1)); VERIFY_IS_APPROX(b2.clamp(b0), b0); + // intersection + BoxType box1(VectorType::Random(dim)); + box1.extend(VectorType::Random(dim)); + BoxType box2(VectorType::Random(dim)); + box2.extend(VectorType::Random(dim)); + + VERIFY(box1.intersects(box2) == !box1.intersection(box2).isEmpty()); // alignment -- make sure there is no memory alignment assertion BoxType *bp0 = new BoxType(dim); diff --git a/ground/gcs/src/libs/eigen/test/geo_eulerangles.cpp b/ground/gcs/src/libs/eigen/test/geo_eulerangles.cpp index b4830bd41f..932ebe7732 100644 --- a/ground/gcs/src/libs/eigen/test/geo_eulerangles.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_eulerangles.cpp @@ -26,16 +26,16 @@ void verify_euler(const Matrix& ea, int i, int j, int k) VERIFY_IS_APPROX(m, mbis); /* If I==K, and ea[1]==0, then there no unique solution. */ /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ - if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) + if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision())) ) VERIFY((ea-eabis).norm() <= test_precision()); // approx_or_less_than does not work for 0 VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI)); - VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI)); - VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); } template void check_all_var(const Matrix& ea) @@ -64,7 +64,7 @@ template void eulerangles() typedef Quaternion Quaternionx; typedef AngleAxis AngleAxisx; - Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); Quaternionx q1; q1 = AngleAxisx(a, Vector3::Random().normalized()); Matrix3 m; @@ -84,13 +84,13 @@ template void eulerangles() check_all_var(ea); // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. - ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1); + ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1); check_all_var(ea); - ea[2] = ea[0] = internal::random(0,Scalar(M_PI)); + ea[2] = ea[0] = internal::random(0,Scalar(EIGEN_PI)); check_all_var(ea); - ea[0] = ea[1] = internal::random(0,Scalar(M_PI)); + ea[0] = ea[1] = internal::random(0,Scalar(EIGEN_PI)); check_all_var(ea); ea[1] = 0; diff --git a/ground/gcs/src/libs/eigen/test/geo_homogeneous.cpp b/ground/gcs/src/libs/eigen/test/geo_homogeneous.cpp index c91bde819c..2187c7bf98 100644 --- a/ground/gcs/src/libs/eigen/test/geo_homogeneous.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_homogeneous.cpp @@ -38,6 +38,10 @@ template void homogeneous(void) hv0 << v0, 1; VERIFY_IS_APPROX(v0.homogeneous(), hv0); VERIFY_IS_APPROX(v0, hv0.hnormalized()); + + VERIFY_IS_APPROX(v0.homogeneous().sum(), hv0.sum()); + VERIFY_IS_APPROX(v0.homogeneous().minCoeff(), hv0.minCoeff()); + VERIFY_IS_APPROX(v0.homogeneous().maxCoeff(), hv0.maxCoeff()); hm0 << m0, ones.transpose(); VERIFY_IS_APPROX(m0.colwise().homogeneous(), hm0); @@ -54,10 +58,11 @@ template void homogeneous(void) T2MatrixType t2 = T2MatrixType::Random(); VERIFY_IS_APPROX(t2 * (v0.homogeneous().eval()), t2 * v0.homogeneous()); VERIFY_IS_APPROX(t2 * (m0.colwise().homogeneous().eval()), t2 * m0.colwise().homogeneous()); + VERIFY_IS_APPROX(t2 * (v0.homogeneous().asDiagonal()), t2 * hv0.asDiagonal()); + VERIFY_IS_APPROX((v0.homogeneous().asDiagonal()) * t2, hv0.asDiagonal() * t2); VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t2, v0.transpose().rowwise().homogeneous() * t2); - m0.transpose().rowwise().homogeneous().eval(); VERIFY_IS_APPROX((m0.transpose().rowwise().homogeneous().eval()) * t2, m0.transpose().rowwise().homogeneous() * t2); @@ -82,7 +87,7 @@ template void homogeneous(void) VERIFY_IS_APPROX(aff * pts.colwise().homogeneous(), (aff * pts1).colwise().hnormalized()); VERIFY_IS_APPROX(caff * pts.colwise().homogeneous(), (caff * pts1).colwise().hnormalized()); VERIFY_IS_APPROX(proj * pts.colwise().homogeneous(), (proj * pts1)); - + VERIFY_IS_APPROX((aff * pts1).colwise().hnormalized(), aff * pts); VERIFY_IS_APPROX((caff * pts1).colwise().hnormalized(), caff * pts); @@ -91,6 +96,23 @@ template void homogeneous(void) VERIFY_IS_APPROX((aff * pts2).colwise().hnormalized(), aff * pts2.colwise().hnormalized()); VERIFY_IS_APPROX((caff * pts2).colwise().hnormalized(), caff * pts2.colwise().hnormalized()); VERIFY_IS_APPROX((proj * pts2).colwise().hnormalized(), (proj * pts2.colwise().hnormalized().colwise().homogeneous()).colwise().hnormalized()); + + // Test combination of homogeneous + + VERIFY_IS_APPROX( (t2 * v0.homogeneous()).hnormalized(), + (t2.template topLeftCorner() * v0 + t2.template topRightCorner()) + / ((t2.template bottomLeftCorner<1,Size>()*v0).value() + t2(Size,Size)) ); + + VERIFY_IS_APPROX( (t2 * pts.colwise().homogeneous()).colwise().hnormalized(), + (Matrix(t2 * pts1).colwise().hnormalized()) ); + + VERIFY_IS_APPROX( (t2 .lazyProduct( v0.homogeneous() )).hnormalized(), (t2 * v0.homogeneous()).hnormalized() ); + VERIFY_IS_APPROX( (t2 .lazyProduct ( pts.colwise().homogeneous() )).colwise().hnormalized(), (t2 * pts1).colwise().hnormalized() ); + + VERIFY_IS_APPROX( (v0.transpose().homogeneous() .lazyProduct( t2 )).hnormalized(), (v0.transpose().homogeneous()*t2).hnormalized() ); + VERIFY_IS_APPROX( (pts.transpose().rowwise().homogeneous() .lazyProduct( t2 )).rowwise().hnormalized(), (pts1.transpose()*t2).rowwise().hnormalized() ); + + VERIFY_IS_APPROX( (t2.template triangularView() * v0.homogeneous()).eval(), (t2.template triangularView()*hv0) ); } void test_geo_homogeneous() diff --git a/ground/gcs/src/libs/eigen/test/geo_hyperplane.cpp b/ground/gcs/src/libs/eigen/test/geo_hyperplane.cpp index 3275378011..27892850da 100644 --- a/ground/gcs/src/libs/eigen/test/geo_hyperplane.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_hyperplane.cpp @@ -18,10 +18,12 @@ template void hyperplane(const HyperplaneType& _plane) /* this test covers the following files: Hyperplane.h */ + using std::abs; typedef typename HyperplaneType::Index Index; const Index dim = _plane.dim(); enum { Options = HyperplaneType::Options }; typedef typename HyperplaneType::Scalar Scalar; + typedef typename HyperplaneType::RealScalar RealScalar; typedef Matrix VectorType; typedef Matrix MatrixType; @@ -42,7 +44,10 @@ template void hyperplane(const HyperplaneType& _plane) VERIFY_IS_APPROX( n1.dot(n1), Scalar(1) ); VERIFY_IS_MUCH_SMALLER_THAN( pl0.absDistance(p0), Scalar(1) ); - VERIFY_IS_APPROX( pl1.signedDistance(p1 + n1 * s0), s0 ); + if(numext::abs2(s0)>RealScalar(1e-6)) + VERIFY_IS_APPROX( pl1.signedDistance(p1 + n1 * s0), s0); + else + VERIFY_IS_MUCH_SMALLER_THAN( abs(pl1.signedDistance(p1 + n1 * s0) - s0), Scalar(1) ); VERIFY_IS_MUCH_SMALLER_THAN( pl1.signedDistance(pl1.projection(p0)), Scalar(1) ); VERIFY_IS_MUCH_SMALLER_THAN( pl1.absDistance(p1 + pl1.normal().unitOrthogonal() * s1), Scalar(1) ); @@ -52,6 +57,8 @@ template void hyperplane(const HyperplaneType& _plane) MatrixType rot = MatrixType::Random(dim,dim).householderQr().householderQ(); DiagonalMatrix scaling(VectorType::Random()); Translation translation(VectorType::Random()); + + while(scaling.diagonal().cwiseAbs().minCoeff() void hyperplane(const HyperplaneType& _plane) VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot,Isometry).absDistance(rot * p1), Scalar(1) ); pl2 = pl1; VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling).absDistance((rot*scaling) * p1), Scalar(1) ); + VERIFY_IS_APPROX( pl2.normal().norm(), RealScalar(1) ); pl2 = pl1; VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling*translation) - .absDistance((rot*scaling*translation) * p1), Scalar(1) ); + .absDistance((rot*scaling*translation) * p1), Scalar(1) ); + VERIFY_IS_APPROX( pl2.normal().norm(), RealScalar(1) ); pl2 = pl1; VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*translation,Isometry) .absDistance((rot*translation) * p1), Scalar(1) ); + VERIFY_IS_APPROX( pl2.normal().norm(), RealScalar(1) ); } // casting @@ -90,9 +100,9 @@ template void lines() Vector u = Vector::Random(); Vector v = Vector::Random(); Scalar a = internal::random(); - while (abs(a-1) < 1e-4) a = internal::random(); - while (u.norm() < 1e-4) u = Vector::Random(); - while (v.norm() < 1e-4) v = Vector::Random(); + while (abs(a-1) < Scalar(1e-4)) a = internal::random(); + while (u.norm() < Scalar(1e-4)) u = Vector::Random(); + while (v.norm() < Scalar(1e-4)) v = Vector::Random(); HLine line_u = HLine::Through(center + u, center + a*u); HLine line_v = HLine::Through(center + v, center + a*v); @@ -104,12 +114,15 @@ template void lines() Vector result = line_u.intersection(line_v); // the lines should intersect at the point we called "center" - VERIFY_IS_APPROX(result, center); + if(abs(a-1) > Scalar(1e-2) && abs(v.normalized().dot(u.normalized())) void hyperplane_alignment() typedef Hyperplane Plane3a; typedef Hyperplane Plane3u; - EIGEN_ALIGN16 Scalar array1[4]; - EIGEN_ALIGN16 Scalar array2[4]; - EIGEN_ALIGN16 Scalar array3[4+1]; + EIGEN_ALIGN_MAX Scalar array1[4]; + EIGEN_ALIGN_MAX Scalar array2[4]; + EIGEN_ALIGN_MAX Scalar array3[4+1]; Scalar* array3u = array3+1; Plane3a *p1 = ::new(reinterpret_cast(array1)) Plane3a; @@ -161,8 +174,8 @@ template void hyperplane_alignment() VERIFY_IS_APPROX(p1->coeffs(), p2->coeffs()); VERIFY_IS_APPROX(p1->coeffs(), p3->coeffs()); - #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY - if(internal::packet_traits::Vectorizable) + #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES > 0 + if(internal::packet_traits::Vectorizable && internal::packet_traits::size<=4) VERIFY_RAISES_ASSERT((::new(reinterpret_cast(array3u)) Plane3a)); #endif } diff --git a/ground/gcs/src/libs/eigen/test/geo_orthomethods.cpp b/ground/gcs/src/libs/eigen/test/geo_orthomethods.cpp index c836dae40c..e178df2575 100644 --- a/ground/gcs/src/libs/eigen/test/geo_orthomethods.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_orthomethods.cpp @@ -33,12 +33,16 @@ template void orthomethods_3() VERIFY_IS_MUCH_SMALLER_THAN(v1.dot(v1.cross(v2)), Scalar(1)); VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v2), Scalar(1)); VERIFY_IS_MUCH_SMALLER_THAN(v2.dot(v1.cross(v2)), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(Vector3::Random()).dot(v1), Scalar(1)); Matrix3 mat3; mat3 << v0.normalized(), (v0.cross(v1)).normalized(), (v0.cross(v1).cross(v0)).normalized(); VERIFY(mat3.isUnitary()); - + + mat3.setRandom(); + VERIFY_IS_APPROX(v0.cross(mat3*v1), -(mat3*v1).cross(v0)); + VERIFY_IS_APPROX(v0.cross(mat3.lazyProduct(v1)), -(mat3.lazyProduct(v1)).cross(v0)); // colwise/rowwise cross product mat3.setRandom(); @@ -47,6 +51,13 @@ template void orthomethods_3() int i = internal::random(0,2); mcross = mat3.colwise().cross(vec3); VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3)); + + VERIFY_IS_MUCH_SMALLER_THAN((mat3.adjoint() * mat3.colwise().cross(vec3)).diagonal().cwiseAbs().sum(), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN((mat3.adjoint() * mat3.colwise().cross(Vector3::Random())).diagonal().cwiseAbs().sum(), Scalar(1)); + + VERIFY_IS_MUCH_SMALLER_THAN((vec3.adjoint() * mat3.colwise().cross(vec3)).cwiseAbs().sum(), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN((vec3.adjoint() * Matrix3::Random().colwise().cross(vec3)).cwiseAbs().sum(), Scalar(1)); + mcross = mat3.rowwise().cross(vec3); VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3)); @@ -57,6 +68,7 @@ template void orthomethods_3() v40.w() = v41.w() = v42.w() = 0; v42.template head<3>() = v40.template head<3>().cross(v41.template head<3>()); VERIFY_IS_APPROX(v40.cross3(v41), v42); + VERIFY_IS_MUCH_SMALLER_THAN(v40.cross3(Vector4::Random()).dot(v40), Scalar(1)); // check mixed product typedef Matrix RealVector3; diff --git a/ground/gcs/src/libs/eigen/test/geo_parametrizedline.cpp b/ground/gcs/src/libs/eigen/test/geo_parametrizedline.cpp index f0462d40ad..9bf5f3c1d5 100644 --- a/ground/gcs/src/libs/eigen/test/geo_parametrizedline.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_parametrizedline.cpp @@ -66,9 +66,9 @@ template void parametrizedline_alignment() typedef ParametrizedLine Line4a; typedef ParametrizedLine Line4u; - EIGEN_ALIGN16 Scalar array1[8]; - EIGEN_ALIGN16 Scalar array2[8]; - EIGEN_ALIGN16 Scalar array3[8+1]; + EIGEN_ALIGN_MAX Scalar array1[16]; + EIGEN_ALIGN_MAX Scalar array2[16]; + EIGEN_ALIGN_MAX Scalar array3[16+1]; Scalar* array3u = array3+1; Line4a *p1 = ::new(reinterpret_cast(array1)) Line4a; @@ -85,8 +85,8 @@ template void parametrizedline_alignment() VERIFY_IS_APPROX(p1->direction(), p2->direction()); VERIFY_IS_APPROX(p1->direction(), p3->direction()); - #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY - if(internal::packet_traits::Vectorizable) + #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0 + if(internal::packet_traits::Vectorizable && internal::packet_traits::size<=4) VERIFY_RAISES_ASSERT((::new(reinterpret_cast(array3u)) Line4a)); #endif } diff --git a/ground/gcs/src/libs/eigen/test/geo_quaternion.cpp b/ground/gcs/src/libs/eigen/test/geo_quaternion.cpp index 1694b32c7b..96889e7220 100644 --- a/ground/gcs/src/libs/eigen/test/geo_quaternion.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_quaternion.cpp @@ -30,8 +30,8 @@ template void check_slerp(const QuatType& q0, const QuatType& Scalar largeEps = test_precision(); Scalar theta_tot = AA(q1*q0.inverse()).angle(); - if(theta_tot>M_PI) - theta_tot = Scalar(2.*M_PI)-theta_tot; + if(theta_tot>Scalar(EIGEN_PI)) + theta_tot = Scalar(2.)*Scalar(EIGEN_PI)-theta_tot; for(Scalar t=0; t<=Scalar(1.001); t+=Scalar(0.1)) { QuatType q = q0.slerp(t,q1); @@ -49,13 +49,13 @@ template void quaternion(void) */ using std::abs; typedef Matrix Vector3; - typedef Matrix Vector4; + typedef Matrix Matrix3; typedef Quaternion Quaternionx; typedef AngleAxis AngleAxisx; Scalar largeEps = test_precision(); if (internal::is_same::value) - largeEps = 1e-3f; + largeEps = Scalar(1e-3); Scalar eps = internal::random() * Scalar(1e-2); @@ -64,8 +64,8 @@ template void quaternion(void) v2 = Vector3::Random(), v3 = Vector3::Random(); - Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)), - b = internal::random(-Scalar(M_PI), Scalar(M_PI)); + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)), + b = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); // Quaternion: Identity(), setIdentity(); Quaternionx q1, q2; @@ -82,8 +82,8 @@ template void quaternion(void) // angular distance Scalar refangle = abs(AngleAxisx(q1.inverse()*q2).angle()); - if (refangle>Scalar(M_PI)) - refangle = Scalar(2)*Scalar(M_PI) - refangle; + if (refangle>Scalar(EIGEN_PI)) + refangle = Scalar(2)*Scalar(EIGEN_PI) - refangle; if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps) { @@ -101,6 +101,11 @@ template void quaternion(void) q2 = q1.toRotationMatrix(); VERIFY_IS_APPROX(q1*v1,q2*v1); + Matrix3 rot1(q1); + VERIFY_IS_APPROX(q1*v1,rot1*v1); + Quaternionx q3(rot1.transpose()*rot1); + VERIFY_IS_APPROX(q3*v1,v1); + // angle-axis conversion AngleAxisx aa = AngleAxisx(q1); @@ -109,8 +114,8 @@ template void quaternion(void) // Do not execute the test if the rotation angle is almost zero, or // the rotation axis and v1 are almost parallel. if (abs(aa.angle()) > 5*test_precision() - && (aa.axis() - v1.normalized()).norm() < 1.99 - && (aa.axis() + v1.normalized()).norm() < 1.99) + && (aa.axis() - v1.normalized()).norm() < Scalar(1.99) + && (aa.axis() + v1.normalized()).norm() < Scalar(1.99)) { VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); } @@ -151,19 +156,19 @@ template void quaternion(void) Quaternionx *q = new Quaternionx; delete q; - q1 = AngleAxisx(a, v0.normalized()); - q2 = AngleAxisx(b, v1.normalized()); + q1 = Quaternionx::UnitRandom(); + q2 = Quaternionx::UnitRandom(); check_slerp(q1,q2); q1 = AngleAxisx(b, v1.normalized()); - q2 = AngleAxisx(b+Scalar(M_PI), v1.normalized()); + q2 = AngleAxisx(b+Scalar(EIGEN_PI), v1.normalized()); check_slerp(q1,q2); q1 = AngleAxisx(b, v1.normalized()); q2 = AngleAxisx(-b, -v1.normalized()); check_slerp(q1,q2); - q1.coeffs() = Vector4::Random().normalized(); + q1 = Quaternionx::UnitRandom(); q2.coeffs() = -q1.coeffs(); check_slerp(q1,q2); } @@ -179,11 +184,11 @@ template void mapQuaternion(void){ Vector3 v0 = Vector3::Random(), v1 = Vector3::Random(); - Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); - EIGEN_ALIGN16 Scalar array1[4]; - EIGEN_ALIGN16 Scalar array2[4]; - EIGEN_ALIGN16 Scalar array3[4+1]; + EIGEN_ALIGN_MAX Scalar array1[4]; + EIGEN_ALIGN_MAX Scalar array2[4]; + EIGEN_ALIGN_MAX Scalar array3[4+1]; Scalar* array3unaligned = array3+1; MQuaternionA mq1(array1); @@ -232,9 +237,9 @@ template void quaternionAlignment(void){ typedef Quaternion QuaternionA; typedef Quaternion QuaternionUA; - EIGEN_ALIGN16 Scalar array1[4]; - EIGEN_ALIGN16 Scalar array2[4]; - EIGEN_ALIGN16 Scalar array3[4+1]; + EIGEN_ALIGN_MAX Scalar array1[4]; + EIGEN_ALIGN_MAX Scalar array2[4]; + EIGEN_ALIGN_MAX Scalar array3[4+1]; Scalar* arrayunaligned = array3+1; QuaternionA *q1 = ::new(reinterpret_cast(array1)) QuaternionA; @@ -247,8 +252,8 @@ template void quaternionAlignment(void){ VERIFY_IS_APPROX(q1->coeffs(), q2->coeffs()); VERIFY_IS_APPROX(q1->coeffs(), q3->coeffs()); - #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY - if(internal::packet_traits::Vectorizable) + #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0 + if(internal::packet_traits::Vectorizable && internal::packet_traits::size<=4) VERIFY_RAISES_ASSERT((::new(reinterpret_cast(arrayunaligned)) QuaternionA)); #endif } diff --git a/ground/gcs/src/libs/eigen/test/geo_transformations.cpp b/ground/gcs/src/libs/eigen/test/geo_transformations.cpp index 5477657145..278e527c25 100644 --- a/ground/gcs/src/libs/eigen/test/geo_transformations.cpp +++ b/ground/gcs/src/libs/eigen/test/geo_transformations.cpp @@ -12,6 +12,17 @@ #include #include +template +Matrix angleToVec(T a) +{ + return Matrix(std::cos(a), std::sin(a)); +} + +// This permits to workaround a bug in clang/llvm code generation. +template +EIGEN_DONT_INLINE +void dont_over_optimize(T& x) { volatile typename T::Scalar tmp = x(0); x(0) = tmp; } + template void non_projective_only() { /* this test covers the following files: @@ -29,7 +40,7 @@ template void non_projective_only() Transform3 t0, t1, t2; - Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); Quaternionx q1, q2; @@ -97,16 +108,14 @@ template void transformations() v1 = Vector3::Random(); Matrix3 matrot1, m; - Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); - Scalar s0 = internal::random(), - s1 = internal::random(); + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Scalar s0 = internal::random(), s1 = internal::random(); while(v0.norm() < test_precision()) v0 = Vector3::Random(); while(v1.norm() < test_precision()) v1 = Vector3::Random(); - VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); - VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0); + VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(EIGEN_PI), v0.unitOrthogonal()) * v0); if(abs(cos(a)) > test_precision()) { VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); @@ -132,14 +141,16 @@ template void transformations() AngleAxisx aa = AngleAxisx(q1); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - if(abs(aa.angle()) > NumTraits::dummy_precision()) + // The following test is stable only if 2*angle != angle and v1 is not colinear with axis + if( (abs(aa.angle()) > test_precision()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision())) ) { VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); } aa.fromRotationMatrix(aa.toRotationMatrix()); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - if(abs(aa.angle()) > NumTraits::dummy_precision()) + // The following test is stable only if 2*angle != angle and v1 is not colinear with axis + if( (abs(aa.angle()) > test_precision()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision())) ) { VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); } @@ -158,7 +169,7 @@ template void transformations() // TODO complete the tests ! a = 0; while (abs(a)(-Scalar(0.4)*Scalar(M_PI), Scalar(0.4)*Scalar(M_PI)); + a = internal::random(-Scalar(0.4)*Scalar(EIGEN_PI), Scalar(0.4)*Scalar(EIGEN_PI)); q1 = AngleAxisx(a, v0.normalized()); Transform3 t0, t1, t2; @@ -204,7 +215,7 @@ template void transformations() tmat4.matrix()(3,3) = Scalar(1); VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix()); - Scalar a3 = internal::random(-Scalar(M_PI), Scalar(M_PI)); + Scalar a3 = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); Vector3 v3 = Vector3::Random().normalized(); AngleAxisx aa3(a3, v3); Transform3 t3(aa3); @@ -216,12 +227,15 @@ template void transformations() t4 *= aa3; VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); - v3 = Vector3::Random(); + do { + v3 = Vector3::Random(); + dont_over_optimize(v3); + } while (v3.cwiseAbs().minCoeff()::epsilon()); Translation3 tv3(v3); Transform3 t5(tv3); t4 = tv3; VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); - t4.translate(-v3); + t4.translate((-v3).eval()); VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); t4 *= tv3; VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); @@ -320,6 +334,9 @@ template void transformations() t0.scale(v0); t1 *= AlignedScaling3(v0); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); + t1 = t1 * v0.asDiagonal(); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); // transformation * translation t0.translate(v0); t1 = t1 * Translation3(v0); @@ -410,12 +427,28 @@ template void transformations() VERIFY_IS_APPROX(r2d1f.template cast(),r2d1); Rotation2D r2d1d = r2d1.template cast(); VERIFY_IS_APPROX(r2d1d.template cast(),r2d1); - - t20 = Translation2(v20) * (Rotation2D(s0) * Eigen::Scaling(s0)); - t21 = Translation2(v20) * Rotation2D(s0) * Eigen::Scaling(s0); - VERIFY_IS_APPROX(t20,t21); + for(int k=0; k<100; ++k) + { + Scalar angle = internal::random(-100,100); + Rotation2D rot2(angle); + VERIFY( rot2.smallestPositiveAngle() >= 0 ); + VERIFY( rot2.smallestPositiveAngle() <= Scalar(2)*Scalar(EIGEN_PI) ); + VERIFY_IS_APPROX( angleToVec(rot2.smallestPositiveAngle()), angleToVec(rot2.angle()) ); + + VERIFY( rot2.smallestAngle() >= -Scalar(EIGEN_PI) ); + VERIFY( rot2.smallestAngle() <= Scalar(EIGEN_PI) ); + VERIFY_IS_APPROX( angleToVec(rot2.smallestAngle()), angleToVec(rot2.angle()) ); + + Matrix rot2_as_mat(rot2); + Rotation2D rot3(rot2_as_mat); + VERIFY_IS_APPROX( angleToVec(rot2.smallestAngle()), angleToVec(rot3.angle()) ); + } + + s0 = internal::random(-100,100); + s1 = internal::random(-100,100); Rotation2D R0(s0), R1(s1); + t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); t21 = Translation2(v20) * R0 * Eigen::Scaling(s0); VERIFY_IS_APPROX(t20,t21); @@ -425,9 +458,24 @@ template void transformations() VERIFY_IS_APPROX(t20,t21); VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); - VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle()); - VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle()); - VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle()); + VERIFY_IS_APPROX( angleToVec(R1.smallestPositiveAngle()), angleToVec((R0.slerp(1, R1)).smallestPositiveAngle()) ); + VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle()); + + if(std::cos(s0)>0) + VERIFY_IS_MUCH_SMALLER_THAN((R0.slerp(0.5, R0.inverse())).smallestAngle(), Scalar(1)); + else + VERIFY_IS_APPROX(Scalar(EIGEN_PI), (R0.slerp(0.5, R0.inverse())).smallestPositiveAngle()); + + // Check path length + Scalar l = 0; + int path_steps = 100; + for(int k=0; k::epsilon()*Scalar(path_steps/2))); // check basic features { @@ -437,6 +485,79 @@ template void transformations() Rotation2D r2(r1); // copy ctor VERIFY_IS_APPROX(r2.angle(),s0); } + + { + Transform3 t32(Matrix4::Random()), t33, t34; + t34 = t33 = t32; + t32.scale(v0); + t33*=AlignedScaling3(v0); + VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); + t33 = t34 * AlignedScaling3(v0); + VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); + } + +} + +template +void transform_associativity_left(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) +{ + VERIFY_IS_APPROX( q*(a1*v), (q*a1)*v ); + VERIFY_IS_APPROX( q*(a2*v), (q*a2)*v ); + VERIFY_IS_APPROX( q*(p*h).hnormalized(), ((q*p)*h).hnormalized() ); +} + +template +void transform_associativity2(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) +{ + VERIFY_IS_APPROX( a1*(q*v), (a1*q)*v ); + VERIFY_IS_APPROX( a2*(q*v), (a2*q)*v ); + VERIFY_IS_APPROX( p *(q*v).homogeneous(), (p *q)*v.homogeneous() ); + + transform_associativity_left(a1, a2,p, q, v, h); +} + +template +void transform_associativity(const RotationType& R) +{ + typedef Matrix VectorType; + typedef Matrix HVectorType; + typedef Matrix LinearType; + typedef Matrix MatrixType; + typedef Transform AffineCompactType; + typedef Transform AffineType; + typedef Transform ProjectiveType; + typedef DiagonalMatrix ScalingType; + typedef Translation TranslationType; + + AffineCompactType A1c; A1c.matrix().setRandom(); + AffineCompactType A2c; A2c.matrix().setRandom(); + AffineType A1(A1c); + AffineType A2(A2c); + ProjectiveType P1; P1.matrix().setRandom(); + VectorType v1 = VectorType::Random(); + VectorType v2 = VectorType::Random(); + HVectorType h1 = HVectorType::Random(); + Scalar s1 = internal::random(); + LinearType L = LinearType::Random(); + MatrixType M = MatrixType::Random(); + + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2c, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, v1.asDiagonal(), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, ScalingType(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(s1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, TranslationType(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity_left(A1c, A1, P1, L, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, R, v2, h1) ); + + VERIFY_IS_APPROX( A1*(M*h1), (A1*M)*h1 ); + VERIFY_IS_APPROX( A1c*(M*h1), (A1c*M)*h1 ); + VERIFY_IS_APPROX( P1*(M*h1), (P1*M)*h1 ); + + VERIFY_IS_APPROX( M*(A1*h1), (M*A1)*h1 ); + VERIFY_IS_APPROX( M*(A1c*h1), (M*A1c)*h1 ); + VERIFY_IS_APPROX( M*(P1*h1), ((M*P1)*h1) ); } template void transform_alignment() @@ -444,9 +565,9 @@ template void transform_alignment() typedef Transform Projective3a; typedef Transform Projective3u; - EIGEN_ALIGN16 Scalar array1[16]; - EIGEN_ALIGN16 Scalar array2[16]; - EIGEN_ALIGN16 Scalar array3[16+1]; + EIGEN_ALIGN_MAX Scalar array1[16]; + EIGEN_ALIGN_MAX Scalar array2[16]; + EIGEN_ALIGN_MAX Scalar array3[16+1]; Scalar* array3u = array3+1; Projective3a *p1 = ::new(reinterpret_cast(array1)) Projective3a; @@ -462,7 +583,7 @@ template void transform_alignment() VERIFY_IS_APPROX( (*p1) * (*p1), (*p2)*(*p3)); - #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY + #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0 if(internal::packet_traits::Vectorizable) VERIFY_RAISES_ASSERT((::new(reinterpret_cast(array3u)) Projective3a)); #endif @@ -517,5 +638,8 @@ void test_geo_transformations() CALL_SUBTEST_7(( transform_products() )); CALL_SUBTEST_7(( transform_products() )); + + CALL_SUBTEST_8(( transform_associativity(Rotation2D(internal::random()*double(EIGEN_PI))) )); + CALL_SUBTEST_8(( transform_associativity(Quaterniond::UnitRandom()) )); } } diff --git a/ground/gcs/src/libs/eigen/test/half_float.cpp b/ground/gcs/src/libs/eigen/test/half_float.cpp new file mode 100644 index 0000000000..f8d438e2fc --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/half_float.cpp @@ -0,0 +1,252 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include + +#include "main.h" + +#include + +// Make sure it's possible to forward declare Eigen::half +namespace Eigen { +struct half; +} + +using Eigen::half; + +void test_conversion() +{ + using Eigen::half_impl::__half; + + // Conversion from float. + VERIFY_IS_EQUAL(half(1.0f).x, 0x3c00); + VERIFY_IS_EQUAL(half(0.5f).x, 0x3800); + VERIFY_IS_EQUAL(half(0.33333f).x, 0x3555); + VERIFY_IS_EQUAL(half(0.0f).x, 0x0000); + VERIFY_IS_EQUAL(half(-0.0f).x, 0x8000); + VERIFY_IS_EQUAL(half(65504.0f).x, 0x7bff); + VERIFY_IS_EQUAL(half(65536.0f).x, 0x7c00); // Becomes infinity. + + // Denormals. + VERIFY_IS_EQUAL(half(-5.96046e-08f).x, 0x8001); + VERIFY_IS_EQUAL(half(5.96046e-08f).x, 0x0001); + VERIFY_IS_EQUAL(half(1.19209e-07f).x, 0x0002); + + // Verify round-to-nearest-even behavior. + float val1 = float(half(__half(0x3c00))); + float val2 = float(half(__half(0x3c01))); + float val3 = float(half(__half(0x3c02))); + VERIFY_IS_EQUAL(half(0.5f * (val1 + val2)).x, 0x3c00); + VERIFY_IS_EQUAL(half(0.5f * (val2 + val3)).x, 0x3c02); + + // Conversion from int. + VERIFY_IS_EQUAL(half(-1).x, 0xbc00); + VERIFY_IS_EQUAL(half(0).x, 0x0000); + VERIFY_IS_EQUAL(half(1).x, 0x3c00); + VERIFY_IS_EQUAL(half(2).x, 0x4000); + VERIFY_IS_EQUAL(half(3).x, 0x4200); + + // Conversion from bool. + VERIFY_IS_EQUAL(half(false).x, 0x0000); + VERIFY_IS_EQUAL(half(true).x, 0x3c00); + + // Conversion to float. + VERIFY_IS_EQUAL(float(half(__half(0x0000))), 0.0f); + VERIFY_IS_EQUAL(float(half(__half(0x3c00))), 1.0f); + + // Denormals. + VERIFY_IS_APPROX(float(half(__half(0x8001))), -5.96046e-08f); + VERIFY_IS_APPROX(float(half(__half(0x0001))), 5.96046e-08f); + VERIFY_IS_APPROX(float(half(__half(0x0002))), 1.19209e-07f); + + // NaNs and infinities. + VERIFY(!(numext::isinf)(float(half(65504.0f)))); // Largest finite number. + VERIFY(!(numext::isnan)(float(half(0.0f)))); + VERIFY((numext::isinf)(float(half(__half(0xfc00))))); + VERIFY((numext::isnan)(float(half(__half(0xfc01))))); + VERIFY((numext::isinf)(float(half(__half(0x7c00))))); + VERIFY((numext::isnan)(float(half(__half(0x7c01))))); + +#if !EIGEN_COMP_MSVC + // Visual Studio errors out on divisions by 0 + VERIFY((numext::isnan)(float(half(0.0 / 0.0)))); + VERIFY((numext::isinf)(float(half(1.0 / 0.0)))); + VERIFY((numext::isinf)(float(half(-1.0 / 0.0)))); +#endif + + // Exactly same checks as above, just directly on the half representation. + VERIFY(!(numext::isinf)(half(__half(0x7bff)))); + VERIFY(!(numext::isnan)(half(__half(0x0000)))); + VERIFY((numext::isinf)(half(__half(0xfc00)))); + VERIFY((numext::isnan)(half(__half(0xfc01)))); + VERIFY((numext::isinf)(half(__half(0x7c00)))); + VERIFY((numext::isnan)(half(__half(0x7c01)))); + +#if !EIGEN_COMP_MSVC + // Visual Studio errors out on divisions by 0 + VERIFY((numext::isnan)(half(0.0 / 0.0))); + VERIFY((numext::isinf)(half(1.0 / 0.0))); + VERIFY((numext::isinf)(half(-1.0 / 0.0))); +#endif +} + +void test_numtraits() +{ + std::cout << "epsilon = " << NumTraits::epsilon() << std::endl; + std::cout << "highest = " << NumTraits::highest() << std::endl; + std::cout << "lowest = " << NumTraits::lowest() << std::endl; + std::cout << "inifinty = " << NumTraits::infinity() << std::endl; + std::cout << "nan = " << NumTraits::quiet_NaN() << std::endl; + +} + +void test_arithmetic() +{ + VERIFY_IS_EQUAL(float(half(2) + half(2)), 4); + VERIFY_IS_EQUAL(float(half(2) + half(-2)), 0); + VERIFY_IS_APPROX(float(half(0.33333f) + half(0.66667f)), 1.0f); + VERIFY_IS_EQUAL(float(half(2.0f) * half(-5.5f)), -11.0f); + VERIFY_IS_APPROX(float(half(1.0f) / half(3.0f)), 0.33333f); + VERIFY_IS_EQUAL(float(-half(4096.0f)), -4096.0f); + VERIFY_IS_EQUAL(float(-half(-4096.0f)), 4096.0f); +} + +void test_comparison() +{ + VERIFY(half(1.0f) > half(0.5f)); + VERIFY(half(0.5f) < half(1.0f)); + VERIFY(!(half(1.0f) < half(0.5f))); + VERIFY(!(half(0.5f) > half(1.0f))); + + VERIFY(!(half(4.0f) > half(4.0f))); + VERIFY(!(half(4.0f) < half(4.0f))); + + VERIFY(!(half(0.0f) < half(-0.0f))); + VERIFY(!(half(-0.0f) < half(0.0f))); + VERIFY(!(half(0.0f) > half(-0.0f))); + VERIFY(!(half(-0.0f) > half(0.0f))); + + VERIFY(half(0.2f) > half(-1.0f)); + VERIFY(half(-1.0f) < half(0.2f)); + VERIFY(half(-16.0f) < half(-15.0f)); + + VERIFY(half(1.0f) == half(1.0f)); + VERIFY(half(1.0f) != half(2.0f)); + + // Comparisons with NaNs and infinities. +#if !EIGEN_COMP_MSVC + // Visual Studio errors out on divisions by 0 + VERIFY(!(half(0.0 / 0.0) == half(0.0 / 0.0))); + VERIFY(half(0.0 / 0.0) != half(0.0 / 0.0)); + + VERIFY(!(half(1.0) == half(0.0 / 0.0))); + VERIFY(!(half(1.0) < half(0.0 / 0.0))); + VERIFY(!(half(1.0) > half(0.0 / 0.0))); + VERIFY(half(1.0) != half(0.0 / 0.0)); + + VERIFY(half(1.0) < half(1.0 / 0.0)); + VERIFY(half(1.0) > half(-1.0 / 0.0)); +#endif +} + +void test_basic_functions() +{ + VERIFY_IS_EQUAL(float(numext::abs(half(3.5f))), 3.5f); + VERIFY_IS_EQUAL(float(abs(half(3.5f))), 3.5f); + VERIFY_IS_EQUAL(float(numext::abs(half(-3.5f))), 3.5f); + VERIFY_IS_EQUAL(float(abs(half(-3.5f))), 3.5f); + + VERIFY_IS_EQUAL(float(numext::floor(half(3.5f))), 3.0f); + VERIFY_IS_EQUAL(float(floor(half(3.5f))), 3.0f); + VERIFY_IS_EQUAL(float(numext::floor(half(-3.5f))), -4.0f); + VERIFY_IS_EQUAL(float(floor(half(-3.5f))), -4.0f); + + VERIFY_IS_EQUAL(float(numext::ceil(half(3.5f))), 4.0f); + VERIFY_IS_EQUAL(float(ceil(half(3.5f))), 4.0f); + VERIFY_IS_EQUAL(float(numext::ceil(half(-3.5f))), -3.0f); + VERIFY_IS_EQUAL(float(ceil(half(-3.5f))), -3.0f); + + VERIFY_IS_APPROX(float(numext::sqrt(half(0.0f))), 0.0f); + VERIFY_IS_APPROX(float(sqrt(half(0.0f))), 0.0f); + VERIFY_IS_APPROX(float(numext::sqrt(half(4.0f))), 2.0f); + VERIFY_IS_APPROX(float(sqrt(half(4.0f))), 2.0f); + + VERIFY_IS_APPROX(float(numext::pow(half(0.0f), half(1.0f))), 0.0f); + VERIFY_IS_APPROX(float(pow(half(0.0f), half(1.0f))), 0.0f); + VERIFY_IS_APPROX(float(numext::pow(half(2.0f), half(2.0f))), 4.0f); + VERIFY_IS_APPROX(float(pow(half(2.0f), half(2.0f))), 4.0f); + + VERIFY_IS_EQUAL(float(numext::exp(half(0.0f))), 1.0f); + VERIFY_IS_EQUAL(float(exp(half(0.0f))), 1.0f); + VERIFY_IS_APPROX(float(numext::exp(half(EIGEN_PI))), 20.f + float(EIGEN_PI)); + VERIFY_IS_APPROX(float(exp(half(EIGEN_PI))), 20.f + float(EIGEN_PI)); + + VERIFY_IS_EQUAL(float(numext::log(half(1.0f))), 0.0f); + VERIFY_IS_EQUAL(float(log(half(1.0f))), 0.0f); + VERIFY_IS_APPROX(float(numext::log(half(10.0f))), 2.30273f); + VERIFY_IS_APPROX(float(log(half(10.0f))), 2.30273f); + + VERIFY_IS_EQUAL(float(numext::log1p(half(0.0f))), 0.0f); + VERIFY_IS_EQUAL(float(log1p(half(0.0f))), 0.0f); + VERIFY_IS_APPROX(float(numext::log1p(half(10.0f))), 2.3978953f); + VERIFY_IS_APPROX(float(log1p(half(10.0f))), 2.3978953f); +} + +void test_trigonometric_functions() +{ + VERIFY_IS_APPROX(numext::cos(half(0.0f)), half(cosf(0.0f))); + VERIFY_IS_APPROX(cos(half(0.0f)), half(cosf(0.0f))); + VERIFY_IS_APPROX(numext::cos(half(EIGEN_PI)), half(cosf(EIGEN_PI))); + //VERIFY_IS_APPROX(numext::cos(half(EIGEN_PI/2)), half(cosf(EIGEN_PI/2))); + //VERIFY_IS_APPROX(numext::cos(half(3*EIGEN_PI/2)), half(cosf(3*EIGEN_PI/2))); + VERIFY_IS_APPROX(numext::cos(half(3.5f)), half(cosf(3.5f))); + + VERIFY_IS_APPROX(numext::sin(half(0.0f)), half(sinf(0.0f))); + VERIFY_IS_APPROX(sin(half(0.0f)), half(sinf(0.0f))); + // VERIFY_IS_APPROX(numext::sin(half(EIGEN_PI)), half(sinf(EIGEN_PI))); + VERIFY_IS_APPROX(numext::sin(half(EIGEN_PI/2)), half(sinf(EIGEN_PI/2))); + VERIFY_IS_APPROX(numext::sin(half(3*EIGEN_PI/2)), half(sinf(3*EIGEN_PI/2))); + VERIFY_IS_APPROX(numext::sin(half(3.5f)), half(sinf(3.5f))); + + VERIFY_IS_APPROX(numext::tan(half(0.0f)), half(tanf(0.0f))); + VERIFY_IS_APPROX(tan(half(0.0f)), half(tanf(0.0f))); + // VERIFY_IS_APPROX(numext::tan(half(EIGEN_PI)), half(tanf(EIGEN_PI))); + // VERIFY_IS_APPROX(numext::tan(half(EIGEN_PI/2)), half(tanf(EIGEN_PI/2))); + //VERIFY_IS_APPROX(numext::tan(half(3*EIGEN_PI/2)), half(tanf(3*EIGEN_PI/2))); + VERIFY_IS_APPROX(numext::tan(half(3.5f)), half(tanf(3.5f))); +} + +void test_array() +{ + typedef Array ArrayXh; + Index size = internal::random(1,10); + Index i = internal::random(0,size-1); + ArrayXh a1 = ArrayXh::Random(size), a2 = ArrayXh::Random(size); + VERIFY_IS_APPROX( a1+a1, half(2)*a1 ); + VERIFY( (a1.abs() >= half(0)).all() ); + VERIFY_IS_APPROX( (a1*a1).sqrt(), a1.abs() ); + + VERIFY( ((a1.min)(a2) <= (a1.max)(a2)).all() ); + a1(i) = half(-10.); + VERIFY_IS_EQUAL( a1.minCoeff(), half(-10.) ); + a1(i) = half(10.); + VERIFY_IS_EQUAL( a1.maxCoeff(), half(10.) ); + + std::stringstream ss; + ss << a1; +} + +void test_half_float() +{ + CALL_SUBTEST(test_conversion()); + CALL_SUBTEST(test_numtraits()); + CALL_SUBTEST(test_arithmetic()); + CALL_SUBTEST(test_comparison()); + CALL_SUBTEST(test_basic_functions()); + CALL_SUBTEST(test_trigonometric_functions()); + CALL_SUBTEST(test_array()); +} diff --git a/ground/gcs/src/libs/eigen/test/incomplete_cholesky.cpp b/ground/gcs/src/libs/eigen/test/incomplete_cholesky.cpp new file mode 100644 index 0000000000..59ffe92595 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/incomplete_cholesky.cpp @@ -0,0 +1,65 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015-2016 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +// #define EIGEN_DONT_VECTORIZE +// #define EIGEN_MAX_ALIGN_BYTES 0 +#include "sparse_solver.h" +#include +#include + +template void test_incomplete_cholesky_T() +{ + typedef SparseMatrix SparseMatrixType; + ConjugateGradient > > cg_illt_lower_amd; + ConjugateGradient > > cg_illt_lower_nat; + ConjugateGradient > > cg_illt_upper_amd; + ConjugateGradient > > cg_illt_upper_nat; + ConjugateGradient > > cg_illt_uplo_amd; + + + CALL_SUBTEST( check_sparse_spd_solving(cg_illt_lower_amd) ); + CALL_SUBTEST( check_sparse_spd_solving(cg_illt_lower_nat) ); + CALL_SUBTEST( check_sparse_spd_solving(cg_illt_upper_amd) ); + CALL_SUBTEST( check_sparse_spd_solving(cg_illt_upper_nat) ); + CALL_SUBTEST( check_sparse_spd_solving(cg_illt_uplo_amd) ); +} + +void test_incomplete_cholesky() +{ + CALL_SUBTEST_1(( test_incomplete_cholesky_T() )); + CALL_SUBTEST_2(( test_incomplete_cholesky_T, int>() )); + CALL_SUBTEST_3(( test_incomplete_cholesky_T() )); + +#ifdef EIGEN_TEST_PART_1 + // regression for bug 1150 + for(int N = 1; N<20; ++N) + { + Eigen::MatrixXd b( N, N ); + b.setOnes(); + + Eigen::SparseMatrix m( N, N ); + m.reserve(Eigen::VectorXi::Constant(N,4)); + for( int i = 0; i < N; ++i ) + { + m.insert( i, i ) = 1; + m.coeffRef( i, i / 2 ) = 2; + m.coeffRef( i, i / 3 ) = 2; + m.coeffRef( i, i / 4 ) = 2; + } + + Eigen::SparseMatrix A; + A = m * m.transpose(); + + Eigen::ConjugateGradient, + Eigen::Lower | Eigen::Upper, + Eigen::IncompleteCholesky > solver( A ); + VERIFY(solver.preconditioner().info() == Eigen::Success); + VERIFY(solver.info() == Eigen::Success); + } +#endif +} diff --git a/ground/gcs/src/libs/eigen/test/inplace_decomposition.cpp b/ground/gcs/src/libs/eigen/test/inplace_decomposition.cpp new file mode 100644 index 0000000000..92d0d91b6b --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/inplace_decomposition.cpp @@ -0,0 +1,110 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" +#include +#include +#include + +// This file test inplace decomposition through Ref<>, as supported by Cholesky, LU, and QR decompositions. + +template void inplace(bool square = false, bool SPD = false) +{ + typedef typename MatrixType::Scalar Scalar; + typedef Matrix RhsType; + typedef Matrix ResType; + + Index rows = MatrixType::RowsAtCompileTime==Dynamic ? internal::random(2,EIGEN_TEST_MAX_SIZE/2) : Index(MatrixType::RowsAtCompileTime); + Index cols = MatrixType::ColsAtCompileTime==Dynamic ? (square?rows:internal::random(2,rows)) : Index(MatrixType::ColsAtCompileTime); + + MatrixType A = MatrixType::Random(rows,cols); + RhsType b = RhsType::Random(rows); + ResType x(cols); + + if(SPD) + { + assert(square); + A.topRows(cols) = A.topRows(cols).adjoint() * A.topRows(cols); + A.diagonal().array() += 1e-3; + } + + MatrixType A0 = A; + MatrixType A1 = A; + + DecType dec(A); + + // Check that the content of A has been modified + VERIFY_IS_NOT_APPROX( A, A0 ); + + // Check that the decomposition is correct: + if(rows==cols) + { + VERIFY_IS_APPROX( A0 * (x = dec.solve(b)), b ); + } + else + { + VERIFY_IS_APPROX( A0.transpose() * A0 * (x = dec.solve(b)), A0.transpose() * b ); + } + + // Check that modifying A breaks the current dec: + A.setRandom(); + if(rows==cols) + { + VERIFY_IS_NOT_APPROX( A0 * (x = dec.solve(b)), b ); + } + else + { + VERIFY_IS_NOT_APPROX( A0.transpose() * A0 * (x = dec.solve(b)), A0.transpose() * b ); + } + + // Check that calling compute(A1) does not modify A1: + A = A0; + dec.compute(A1); + VERIFY_IS_EQUAL(A0,A1); + VERIFY_IS_NOT_APPROX( A, A0 ); + if(rows==cols) + { + VERIFY_IS_APPROX( A0 * (x = dec.solve(b)), b ); + } + else + { + VERIFY_IS_APPROX( A0.transpose() * A0 * (x = dec.solve(b)), A0.transpose() * b ); + } +} + + +void test_inplace_decomposition() +{ + EIGEN_UNUSED typedef Matrix Matrix43d; + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1(( inplace >, MatrixXd>(true,true) )); + CALL_SUBTEST_1(( inplace >, Matrix4d>(true,true) )); + + CALL_SUBTEST_2(( inplace >, MatrixXd>(true,true) )); + CALL_SUBTEST_2(( inplace >, Matrix4d>(true,true) )); + + CALL_SUBTEST_3(( inplace >, MatrixXd>(true,false) )); + CALL_SUBTEST_3(( inplace >, Matrix4d>(true,false) )); + + CALL_SUBTEST_4(( inplace >, MatrixXd>(true,false) )); + CALL_SUBTEST_4(( inplace >, Matrix4d>(true,false) )); + + CALL_SUBTEST_5(( inplace >, MatrixXd>(false,false) )); + CALL_SUBTEST_5(( inplace >, Matrix43d>(false,false) )); + + CALL_SUBTEST_6(( inplace >, MatrixXd>(false,false) )); + CALL_SUBTEST_6(( inplace >, Matrix43d>(false,false) )); + + CALL_SUBTEST_7(( inplace >, MatrixXd>(false,false) )); + CALL_SUBTEST_7(( inplace >, Matrix43d>(false,false) )); + + CALL_SUBTEST_8(( inplace >, MatrixXd>(false,false) )); + CALL_SUBTEST_8(( inplace >, Matrix43d>(false,false) )); + } +} diff --git a/ground/gcs/src/libs/eigen/test/integer_types.cpp b/ground/gcs/src/libs/eigen/test/integer_types.cpp index 950f8e9be8..a21f73a81b 100644 --- a/ground/gcs/src/libs/eigen/test/integer_types.cpp +++ b/ground/gcs/src/libs/eigen/test/integer_types.cpp @@ -158,4 +158,12 @@ void test_integer_types() CALL_SUBTEST_8( integer_type_tests(Matrix(1, 5)) ); } +#ifdef EIGEN_TEST_PART_9 + VERIFY_IS_EQUAL(internal::scalar_div_cost::value, 8); + VERIFY_IS_EQUAL(internal::scalar_div_cost::value, 8); + if(sizeof(long)>sizeof(int)) { + VERIFY(internal::scalar_div_cost::value > internal::scalar_div_cost::value); + VERIFY(internal::scalar_div_cost::value > internal::scalar_div_cost::value); + } +#endif } diff --git a/ground/gcs/src/libs/eigen/test/inverse.cpp b/ground/gcs/src/libs/eigen/test/inverse.cpp index 8187b088de..5c6777a18b 100644 --- a/ground/gcs/src/libs/eigen/test/inverse.cpp +++ b/ground/gcs/src/libs/eigen/test/inverse.cpp @@ -68,6 +68,15 @@ template void inverse(const MatrixType& m) VERIFY_IS_MUCH_SMALLER_THAN(abs(det-m3.determinant()), RealScalar(1)); m3.computeInverseWithCheck(m4, invertible); VERIFY( rows==1 ? invertible : !invertible ); + + // check with submatrices + { + Matrix m5; + m5.setRandom(); + m5.topLeftCorner(rows,rows) = m1; + m2 = m5.template topLeftCorner().inverse(); + VERIFY_IS_APPROX( (m5.template topLeftCorner()), m2.inverse() ); + } #endif // check in-place inversion @@ -93,12 +102,16 @@ void test_inverse() CALL_SUBTEST_3( inverse(Matrix3f()) ); CALL_SUBTEST_4( inverse(Matrix4f()) ); CALL_SUBTEST_4( inverse(Matrix()) ); + s = internal::random(50,320); CALL_SUBTEST_5( inverse(MatrixXf(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(25,100); CALL_SUBTEST_6( inverse(MatrixXcd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + CALL_SUBTEST_7( inverse(Matrix4d()) ); CALL_SUBTEST_7( inverse(Matrix()) ); } - TEST_SET_BUT_UNUSED_VARIABLE(s) } diff --git a/ground/gcs/src/libs/eigen/test/is_same_dense.cpp b/ground/gcs/src/libs/eigen/test/is_same_dense.cpp new file mode 100644 index 0000000000..2c7838ce96 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/is_same_dense.cpp @@ -0,0 +1,33 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +using internal::is_same_dense; + +void test_is_same_dense() +{ + typedef Matrix ColMatrixXd; + ColMatrixXd m1(10,10); + Ref ref_m1(m1); + Ref const_ref_m1(m1); + VERIFY(is_same_dense(m1,m1)); + VERIFY(is_same_dense(m1,ref_m1)); + VERIFY(is_same_dense(const_ref_m1,m1)); + VERIFY(is_same_dense(const_ref_m1,ref_m1)); + + VERIFY(is_same_dense(m1.block(0,0,m1.rows(),m1.cols()),m1)); + VERIFY(!is_same_dense(m1.row(0),m1.col(0))); + + Ref const_ref_m1_row(m1.row(1)); + VERIFY(!is_same_dense(m1.row(1),const_ref_m1_row)); + + Ref const_ref_m1_col(m1.col(1)); + VERIFY(is_same_dense(m1.col(1),const_ref_m1_col)); +} diff --git a/ground/gcs/src/libs/eigen/test/jacobisvd.cpp b/ground/gcs/src/libs/eigen/test/jacobisvd.cpp index 12c556e59a..7f5f715628 100644 --- a/ground/gcs/src/libs/eigen/test/jacobisvd.cpp +++ b/ground/gcs/src/libs/eigen/test/jacobisvd.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2008-2014 Gael Guennebaud // Copyright (C) 2009 Benoit Jacob // // This Source Code Form is subject to the terms of the Mozilla @@ -14,279 +14,47 @@ #include "main.h" #include -template -void jacobisvd_check_full(const MatrixType& m, const JacobiSVD& svd) -{ - typedef typename MatrixType::Index Index; - Index rows = m.rows(); - Index cols = m.cols(); - - enum { - RowsAtCompileTime = MatrixType::RowsAtCompileTime, - ColsAtCompileTime = MatrixType::ColsAtCompileTime - }; - - typedef typename MatrixType::Scalar Scalar; - typedef Matrix MatrixUType; - typedef Matrix MatrixVType; - - MatrixType sigma = MatrixType::Zero(rows,cols); - sigma.diagonal() = svd.singularValues().template cast(); - MatrixUType u = svd.matrixU(); - MatrixVType v = svd.matrixV(); - - VERIFY_IS_APPROX(m, u * sigma * v.adjoint()); - VERIFY_IS_UNITARY(u); - VERIFY_IS_UNITARY(v); -} - -template -void jacobisvd_compare_to_full(const MatrixType& m, - unsigned int computationOptions, - const JacobiSVD& referenceSvd) -{ - typedef typename MatrixType::Index Index; - Index rows = m.rows(); - Index cols = m.cols(); - Index diagSize = (std::min)(rows, cols); - - JacobiSVD svd(m, computationOptions); - - VERIFY_IS_APPROX(svd.singularValues(), referenceSvd.singularValues()); - if(computationOptions & ComputeFullU) - VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU()); - if(computationOptions & ComputeThinU) - VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU().leftCols(diagSize)); - if(computationOptions & ComputeFullV) - VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV()); - if(computationOptions & ComputeThinV) - VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV().leftCols(diagSize)); -} - -template -void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions) -{ - typedef typename MatrixType::Scalar Scalar; - typedef typename MatrixType::RealScalar RealScalar; - typedef typename MatrixType::Index Index; - Index rows = m.rows(); - Index cols = m.cols(); - - enum { - RowsAtCompileTime = MatrixType::RowsAtCompileTime, - ColsAtCompileTime = MatrixType::ColsAtCompileTime - }; - - typedef Matrix RhsType; - typedef Matrix SolutionType; - - RhsType rhs = RhsType::Random(rows, internal::random(1, cols)); - JacobiSVD svd(m, computationOptions); - - if(internal::is_same::value) svd.setThreshold(1e-8); - else if(internal::is_same::value) svd.setThreshold(1e-4); - - SolutionType x = svd.solve(rhs); - - RealScalar residual = (m*x-rhs).norm(); - // Check that there is no significantly better solution in the neighborhood of x - if(!test_isMuchSmallerThan(residual,rhs.norm())) - { - // If the residual is very small, then we have an exact solution, so we are already good. - for(int k=0;k::epsilon(); - RealScalar residual_y = (m*y-rhs).norm(); - VERIFY( test_isApprox(residual_y,residual) || residual < residual_y ); - - y.row(k) = x.row(k).array() - 2*NumTraits::epsilon(); - residual_y = (m*y-rhs).norm(); - VERIFY( test_isApprox(residual_y,residual) || residual < residual_y ); - } - } - - // evaluate normal equation which works also for least-squares solutions - if(internal::is_same::value) - { - // This test is not stable with single precision. - // This is probably because squaring m signicantly affects the precision. - VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs); - } - - // check minimal norm solutions - { - // generate a full-rank m x n problem with m MatrixType2; - typedef Matrix RhsType2; - typedef Matrix MatrixType2T; - Index rank = RankAtCompileTime2==Dynamic ? internal::random(1,cols) : Index(RankAtCompileTime2); - MatrixType2 m2(rank,cols); - int guard = 0; - do { - m2.setRandom(); - } while(m2.jacobiSvd().setThreshold(test_precision()).rank()!=rank && (++guard)<10); - VERIFY(guard<10); - RhsType2 rhs2 = RhsType2::Random(rank); - // use QR to find a reference minimal norm solution - HouseholderQR qr(m2.adjoint()); - Matrix tmp = qr.matrixQR().topLeftCorner(rank,rank).template triangularView().adjoint().solve(rhs2); - tmp.conservativeResize(cols); - tmp.tail(cols-rank).setZero(); - SolutionType x21 = qr.householderQ() * tmp; - // now check with SVD - JacobiSVD svd2(m2, computationOptions); - SolutionType x22 = svd2.solve(rhs2); - VERIFY_IS_APPROX(m2*x21, rhs2); - VERIFY_IS_APPROX(m2*x22, rhs2); - VERIFY_IS_APPROX(x21, x22); - - // Now check with a rank deficient matrix - typedef Matrix MatrixType3; - typedef Matrix RhsType3; - Index rows3 = RowsAtCompileTime3==Dynamic ? internal::random(rank+1,2*cols) : Index(RowsAtCompileTime3); - Matrix C = Matrix::Random(rows3,rank); - MatrixType3 m3 = C * m2; - RhsType3 rhs3 = C * rhs2; - JacobiSVD svd3(m3, computationOptions); - SolutionType x3 = svd3.solve(rhs3); - if(svd3.rank()!=rank) { - std::cout << m3 << "\n\n"; - std::cout << svd3.singularValues().transpose() << "\n"; - std::cout << svd3.rank() << " == " << rank << "\n"; - std::cout << x21.norm() << " == " << x3.norm() << "\n"; - } -// VERIFY_IS_APPROX(m3*x3, rhs3); - VERIFY_IS_APPROX(m3*x21, rhs3); - VERIFY_IS_APPROX(m2*x3, rhs2); - - VERIFY_IS_APPROX(x21, x3); - } -} - -template -void jacobisvd_test_all_computation_options(const MatrixType& m) -{ - if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols()) - return; - JacobiSVD fullSvd(m, ComputeFullU|ComputeFullV); - CALL_SUBTEST(( jacobisvd_check_full(m, fullSvd) )); - CALL_SUBTEST(( jacobisvd_solve(m, ComputeFullU | ComputeFullV) )); - - #if defined __INTEL_COMPILER - // remark #111: statement is unreachable - #pragma warning disable 111 - #endif - if(QRPreconditioner == FullPivHouseholderQRPreconditioner) - return; - - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU, fullSvd) )); - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullV, fullSvd) )); - CALL_SUBTEST(( jacobisvd_compare_to_full(m, 0, fullSvd) )); - - if (MatrixType::ColsAtCompileTime == Dynamic) { - // thin U/V are only available with dynamic number of columns - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd) )); - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinV, fullSvd) )); - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd) )); - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU , fullSvd) )); - CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd) )); - CALL_SUBTEST(( jacobisvd_solve(m, ComputeFullU | ComputeThinV) )); - CALL_SUBTEST(( jacobisvd_solve(m, ComputeThinU | ComputeFullV) )); - CALL_SUBTEST(( jacobisvd_solve(m, ComputeThinU | ComputeThinV) )); - - // test reconstruction - typedef typename MatrixType::Index Index; - Index diagSize = (std::min)(m.rows(), m.cols()); - JacobiSVD svd(m, ComputeThinU | ComputeThinV); - VERIFY_IS_APPROX(m, svd.matrixU().leftCols(diagSize) * svd.singularValues().asDiagonal() * svd.matrixV().leftCols(diagSize).adjoint()); - } -} +#define SVD_DEFAULT(M) JacobiSVD +#define SVD_FOR_MIN_NORM(M) JacobiSVD +#include "svd_common.h" +// Check all variants of JacobiSVD template void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true) { MatrixType m = a; if(pickrandom) - { - typedef typename MatrixType::Scalar Scalar; - typedef typename MatrixType::RealScalar RealScalar; - typedef typename MatrixType::Index Index; - Index diagSize = (std::min)(a.rows(), a.cols()); - RealScalar s = std::numeric_limits::max_exponent10/4; - s = internal::random(1,s); - Matrix d = Matrix::Random(diagSize); - for(Index k=0; k(-s,s)); - m = Matrix::Random(a.rows(),diagSize) * d.asDiagonal() * Matrix::Random(diagSize,a.cols()); - // cancel some coeffs - Index n = internal::random(0,m.size()-1); - for(Index i=0; i(0,m.rows()-1), internal::random(0,m.cols()-1)) = Scalar(0); - } + svd_fill_random(m); - CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); - CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); - CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); - CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); + CALL_SUBTEST(( svd_test_all_computation_options >(m, true) )); // check full only + CALL_SUBTEST(( svd_test_all_computation_options >(m, false) )); + CALL_SUBTEST(( svd_test_all_computation_options >(m, false) )); + if(m.rows()==m.cols()) + CALL_SUBTEST(( svd_test_all_computation_options >(m, false) )); } template void jacobisvd_verify_assert(const MatrixType& m) { - typedef typename MatrixType::Scalar Scalar; + svd_verify_assert >(m); typedef typename MatrixType::Index Index; Index rows = m.rows(); Index cols = m.cols(); enum { - RowsAtCompileTime = MatrixType::RowsAtCompileTime, ColsAtCompileTime = MatrixType::ColsAtCompileTime }; - typedef Matrix RhsType; - - RhsType rhs(rows); - - JacobiSVD svd; - VERIFY_RAISES_ASSERT(svd.matrixU()) - VERIFY_RAISES_ASSERT(svd.singularValues()) - VERIFY_RAISES_ASSERT(svd.matrixV()) - VERIFY_RAISES_ASSERT(svd.solve(rhs)) MatrixType a = MatrixType::Zero(rows, cols); a.setZero(); - svd.compute(a, 0); - VERIFY_RAISES_ASSERT(svd.matrixU()) - VERIFY_RAISES_ASSERT(svd.matrixV()) - svd.singularValues(); - VERIFY_RAISES_ASSERT(svd.solve(rhs)) if (ColsAtCompileTime == Dynamic) { - svd.compute(a, ComputeThinU); - svd.matrixU(); - VERIFY_RAISES_ASSERT(svd.matrixV()) - VERIFY_RAISES_ASSERT(svd.solve(rhs)) - - svd.compute(a, ComputeThinV); - svd.matrixV(); - VERIFY_RAISES_ASSERT(svd.matrixU()) - VERIFY_RAISES_ASSERT(svd.solve(rhs)) - JacobiSVD svd_fullqr; VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeFullU|ComputeThinV)) VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeThinV)) VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeFullV)) } - else - { - VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinU)) - VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinV)) - } } template @@ -302,126 +70,17 @@ void jacobisvd_method() VERIFY_IS_APPROX(m.jacobiSvd(ComputeFullU|ComputeFullV).solve(m), m); } -// work around stupid msvc error when constructing at compile time an expression that involves -// a division by zero, even if the numeric type has floating point -template -EIGEN_DONT_INLINE Scalar zero() { return Scalar(0); } - -// workaround aggressive optimization in ICC -template EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; } - -template -void jacobisvd_inf_nan() -{ - // all this function does is verify we don't iterate infinitely on nan/inf values - - JacobiSVD svd; - typedef typename MatrixType::Scalar Scalar; - Scalar some_inf = Scalar(1) / zero(); - VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf)); - svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV); - - Scalar nan = std::numeric_limits::quiet_NaN(); - VERIFY(nan != nan); - svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV); - - MatrixType m = MatrixType::Zero(10,10); - m(internal::random(0,9), internal::random(0,9)) = some_inf; - svd.compute(m, ComputeFullU | ComputeFullV); - - m = MatrixType::Zero(10,10); - m(internal::random(0,9), internal::random(0,9)) = nan; - svd.compute(m, ComputeFullU | ComputeFullV); - - // regression test for bug 791 - m.resize(3,3); - m << 0, 2*NumTraits::epsilon(), 0.5, - 0, -0.5, 0, - nan, 0, 0; - svd.compute(m, ComputeFullU | ComputeFullV); -} - -// Regression test for bug 286: JacobiSVD loops indefinitely with some -// matrices containing denormal numbers. -void jacobisvd_bug286() -{ -#if defined __INTEL_COMPILER -// shut up warning #239: floating point underflow -#pragma warning push -#pragma warning disable 239 -#endif - Matrix2d M; - M << -7.90884e-313, -4.94e-324, - 0, 5.60844e-313; -#if defined __INTEL_COMPILER -#pragma warning pop -#endif - JacobiSVD svd; - svd.compute(M); // just check we don't loop indefinitely -} - -void jacobisvd_preallocate() -{ - Vector3f v(3.f, 2.f, 1.f); - MatrixXf m = v.asDiagonal(); - - internal::set_is_malloc_allowed(false); - VERIFY_RAISES_ASSERT(VectorXf tmp(10);) - JacobiSVD svd; - internal::set_is_malloc_allowed(true); - svd.compute(m); - VERIFY_IS_APPROX(svd.singularValues(), v); - - JacobiSVD svd2(3,3); - internal::set_is_malloc_allowed(false); - svd2.compute(m); - internal::set_is_malloc_allowed(true); - VERIFY_IS_APPROX(svd2.singularValues(), v); - VERIFY_RAISES_ASSERT(svd2.matrixU()); - VERIFY_RAISES_ASSERT(svd2.matrixV()); - svd2.compute(m, ComputeFullU | ComputeFullV); - VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity()); - VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity()); - internal::set_is_malloc_allowed(false); - svd2.compute(m); - internal::set_is_malloc_allowed(true); - - JacobiSVD svd3(3,3,ComputeFullU|ComputeFullV); - internal::set_is_malloc_allowed(false); - svd2.compute(m); - internal::set_is_malloc_allowed(true); - VERIFY_IS_APPROX(svd2.singularValues(), v); - VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity()); - VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity()); - internal::set_is_malloc_allowed(false); - svd2.compute(m, ComputeFullU|ComputeFullV); - internal::set_is_malloc_allowed(true); -} - void test_jacobisvd() { CALL_SUBTEST_3(( jacobisvd_verify_assert(Matrix3f()) )); CALL_SUBTEST_4(( jacobisvd_verify_assert(Matrix4d()) )); CALL_SUBTEST_7(( jacobisvd_verify_assert(MatrixXf(10,12)) )); CALL_SUBTEST_8(( jacobisvd_verify_assert(MatrixXcd(7,5)) )); + + CALL_SUBTEST_11(svd_all_trivial_2x2(jacobisvd)); + CALL_SUBTEST_12(svd_all_trivial_2x2(jacobisvd)); for(int i = 0; i < g_repeat; i++) { - Matrix2cd m; - m << 0, 1, - 0, 1; - CALL_SUBTEST_1(( jacobisvd(m, false) )); - m << 1, 0, - 1, 0; - CALL_SUBTEST_1(( jacobisvd(m, false) )); - - Matrix2d n; - n << 0, 0, - 0, 0; - CALL_SUBTEST_2(( jacobisvd(n, false) )); - n << 0, 0, - 0, 1; - CALL_SUBTEST_2(( jacobisvd(n, false) )); - CALL_SUBTEST_3(( jacobisvd() )); CALL_SUBTEST_4(( jacobisvd() )); CALL_SUBTEST_5(( jacobisvd >() )); @@ -440,8 +99,14 @@ void test_jacobisvd() (void) c; // Test on inf/nan matrix - CALL_SUBTEST_7( jacobisvd_inf_nan() ); - CALL_SUBTEST_10( jacobisvd_inf_nan() ); + CALL_SUBTEST_7( (svd_inf_nan, MatrixXf>()) ); + CALL_SUBTEST_10( (svd_inf_nan, MatrixXd>()) ); + + // bug1395 test compile-time vectors as input + CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix()) )); + CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix()) )); + CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix(r)) )); + CALL_SUBTEST_13(( jacobisvd_verify_assert(Matrix(c)) )); } CALL_SUBTEST_7(( jacobisvd(MatrixXf(internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) )); @@ -455,8 +120,7 @@ void test_jacobisvd() CALL_SUBTEST_7( JacobiSVD(10,10) ); // Check that preallocation avoids subsequent mallocs - CALL_SUBTEST_9( jacobisvd_preallocate() ); + CALL_SUBTEST_9( svd_preallocate() ); - // Regression check for bug 286 - CALL_SUBTEST_2( jacobisvd_bug286() ); + CALL_SUBTEST_2( svd_underoverflow() ); } diff --git a/ground/gcs/src/libs/eigen/test/linearstructure.cpp b/ground/gcs/src/libs/eigen/test/linearstructure.cpp index 618984d5c2..17474af108 100644 --- a/ground/gcs/src/libs/eigen/test/linearstructure.cpp +++ b/ground/gcs/src/libs/eigen/test/linearstructure.cpp @@ -2,11 +2,15 @@ // for linear algebra. // // Copyright (C) 2006-2008 Benoit Jacob +// Copyright (C) 2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +static bool g_called; +#define EIGEN_SCALAR_BINARY_OP_PLUGIN { g_called |= (!internal::is_same::value); } + #include "main.h" template void linearStructure(const MatrixType& m) @@ -17,6 +21,7 @@ template void linearStructure(const MatrixType& m) */ typedef typename MatrixType::Index Index; typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; Index rows = m.rows(); Index cols = m.cols(); @@ -28,7 +33,7 @@ template void linearStructure(const MatrixType& m) m3(rows, cols); Scalar s1 = internal::random(); - while (abs(s1)<1e-3) s1 = internal::random(); + while (abs(s1)(); Index r = internal::random(0, rows-1), c = internal::random(0, cols-1); @@ -68,8 +73,48 @@ template void linearStructure(const MatrixType& m) VERIFY_IS_APPROX(m1.block(0,0,rows,cols) * s1, m1 * s1); } +// Make sure that complex * real and real * complex are properly optimized +template void real_complex(DenseIndex rows = MatrixType::RowsAtCompileTime, DenseIndex cols = MatrixType::ColsAtCompileTime) +{ + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + + RealScalar s = internal::random(); + MatrixType m1 = MatrixType::Random(rows, cols); + + g_called = false; + VERIFY_IS_APPROX(s*m1, Scalar(s)*m1); + VERIFY(g_called && "real * matrix not properly optimized"); + + g_called = false; + VERIFY_IS_APPROX(m1*s, m1*Scalar(s)); + VERIFY(g_called && "matrix * real not properly optimized"); + + g_called = false; + VERIFY_IS_APPROX(m1/s, m1/Scalar(s)); + VERIFY(g_called && "matrix / real not properly optimized"); + + g_called = false; + VERIFY_IS_APPROX(s+m1.array(), Scalar(s)+m1.array()); + VERIFY(g_called && "real + matrix not properly optimized"); + + g_called = false; + VERIFY_IS_APPROX(m1.array()+s, m1.array()+Scalar(s)); + VERIFY(g_called && "matrix + real not properly optimized"); + + g_called = false; + VERIFY_IS_APPROX(s-m1.array(), Scalar(s)-m1.array()); + VERIFY(g_called && "real - matrix not properly optimized"); + + g_called = false; + VERIFY_IS_APPROX(m1.array()-s, m1.array()-Scalar(s)); + VERIFY(g_called && "matrix - real not properly optimized"); +} + void test_linearstructure() { + g_called = true; + VERIFY(g_called); // avoid `unneeded-internal-declaration` warning. for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( linearStructure(Matrix()) ); CALL_SUBTEST_2( linearStructure(Matrix2f()) ); @@ -80,5 +125,25 @@ void test_linearstructure() CALL_SUBTEST_7( linearStructure(MatrixXi (internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_8( linearStructure(MatrixXcd(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_9( linearStructure(ArrayXXf (internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_10( linearStructure(ArrayXXcf (internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + + CALL_SUBTEST_11( real_complex() ); + CALL_SUBTEST_11( real_complex(10,10) ); + CALL_SUBTEST_11( real_complex(10,10) ); + } + +#ifdef EIGEN_TEST_PART_4 + { + // make sure that /=scalar and /scalar do not overflow + // rational: 1.0/4.94e-320 overflow, but m/4.94e-320 should not + Matrix4d m2, m3; + m3 = m2 = Matrix4d::Random()*1e-20; + m2 = m2 / 4.9e-320; + VERIFY_IS_APPROX(m2.cwiseQuotient(m2), Matrix4d::Ones()); + m3 /= 4.9e-320; + VERIFY_IS_APPROX(m3.cwiseQuotient(m3), Matrix4d::Ones()); + + } +#endif } diff --git a/ground/gcs/src/libs/eigen/test/lscg.cpp b/ground/gcs/src/libs/eigen/test/lscg.cpp new file mode 100644 index 0000000000..daa62a9540 --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/lscg.cpp @@ -0,0 +1,29 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "sparse_solver.h" +#include + +template void test_lscg_T() +{ + LeastSquaresConjugateGradient > lscg_colmajor_diag; + LeastSquaresConjugateGradient, IdentityPreconditioner> lscg_colmajor_I; + + CALL_SUBTEST( check_sparse_square_solving(lscg_colmajor_diag) ); + CALL_SUBTEST( check_sparse_square_solving(lscg_colmajor_I) ); + + CALL_SUBTEST( check_sparse_leastsquare_solving(lscg_colmajor_diag) ); + CALL_SUBTEST( check_sparse_leastsquare_solving(lscg_colmajor_I) ); +} + +void test_lscg() +{ + CALL_SUBTEST_1(test_lscg_T()); + CALL_SUBTEST_2(test_lscg_T >()); +} diff --git a/ground/gcs/src/libs/eigen/test/lu.cpp b/ground/gcs/src/libs/eigen/test/lu.cpp index 3746526946..9787f4d86f 100644 --- a/ground/gcs/src/libs/eigen/test/lu.cpp +++ b/ground/gcs/src/libs/eigen/test/lu.cpp @@ -11,6 +11,11 @@ #include using namespace std; +template +typename MatrixType::RealScalar matrix_l1_norm(const MatrixType& m) { + return m.cwiseAbs().colwise().sum().maxCoeff(); +} + template void lu_non_invertible() { typedef typename MatrixType::Index Index; @@ -92,6 +97,26 @@ template void lu_non_invertible() // test that the code, which does resize(), may be applied to an xpr m2.block(0,0,m2.rows(),m2.cols()) = lu.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); + + // test solve with transposed + m3 = MatrixType::Random(rows,cols2); + m2 = m1.transpose()*m3; + m3 = MatrixType::Random(rows,cols2); + lu.template _solve_impl_transposed(m2, m3); + VERIFY_IS_APPROX(m2, m1.transpose()*m3); + m3 = MatrixType::Random(rows,cols2); + m3 = lu.transpose().solve(m2); + VERIFY_IS_APPROX(m2, m1.transpose()*m3); + + // test solve with conjugate transposed + m3 = MatrixType::Random(rows,cols2); + m2 = m1.adjoint()*m3; + m3 = MatrixType::Random(rows,cols2); + lu.template _solve_impl_transposed(m2, m3); + VERIFY_IS_APPROX(m2, m1.adjoint()*m3); + m3 = MatrixType::Random(rows,cols2); + m3 = lu.adjoint().solve(m2); + VERIFY_IS_APPROX(m2, m1.adjoint()*m3); } template void lu_invertible() @@ -100,9 +125,9 @@ template void lu_invertible() LU.h */ typedef typename NumTraits::Real RealScalar; - DenseIndex size = MatrixType::RowsAtCompileTime; + Index size = MatrixType::RowsAtCompileTime; if( size==Dynamic) - size = internal::random(1,EIGEN_TEST_MAX_SIZE); + size = internal::random(1,EIGEN_TEST_MAX_SIZE); MatrixType m1(size, size), m2(size, size), m3(size, size); FullPivLU lu; @@ -123,7 +148,28 @@ template void lu_invertible() m3 = MatrixType::Random(size,size); m2 = lu.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); - VERIFY_IS_APPROX(m2, lu.inverse()*m3); + MatrixType m1_inverse = lu.inverse(); + VERIFY_IS_APPROX(m2, m1_inverse*m3); + + RealScalar rcond = (RealScalar(1) / matrix_l1_norm(m1)) / matrix_l1_norm(m1_inverse); + const RealScalar rcond_est = lu.rcond(); + // Verify that the estimated condition number is within a factor of 10 of the + // truth. + VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10); + + // test solve with transposed + lu.template _solve_impl_transposed(m3, m2); + VERIFY_IS_APPROX(m3, m1.transpose()*m2); + m3 = MatrixType::Random(size,size); + m3 = lu.transpose().solve(m2); + VERIFY_IS_APPROX(m2, m1.transpose()*m3); + + // test solve with conjugate transposed + lu.template _solve_impl_transposed(m3, m2); + VERIFY_IS_APPROX(m3, m1.adjoint()*m2); + m3 = MatrixType::Random(size,size); + m3 = lu.adjoint().solve(m2); + VERIFY_IS_APPROX(m2, m1.adjoint()*m3); // Regression test for Bug 302 MatrixType m4 = MatrixType::Random(size,size); @@ -136,14 +182,39 @@ template void lu_partial_piv() PartialPivLU.h */ typedef typename MatrixType::Index Index; - Index rows = internal::random(1,4); - Index cols = rows; + typedef typename NumTraits::Real RealScalar; + Index size = internal::random(1,4); - MatrixType m1(cols, rows); + MatrixType m1(size, size), m2(size, size), m3(size, size); m1.setRandom(); PartialPivLU plu(m1); VERIFY_IS_APPROX(m1, plu.reconstructedMatrix()); + + m3 = MatrixType::Random(size,size); + m2 = plu.solve(m3); + VERIFY_IS_APPROX(m3, m1*m2); + MatrixType m1_inverse = plu.inverse(); + VERIFY_IS_APPROX(m2, m1_inverse*m3); + + RealScalar rcond = (RealScalar(1) / matrix_l1_norm(m1)) / matrix_l1_norm(m1_inverse); + const RealScalar rcond_est = plu.rcond(); + // Verify that the estimate is within a factor of 10 of the truth. + VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10); + + // test solve with transposed + plu.template _solve_impl_transposed(m3, m2); + VERIFY_IS_APPROX(m3, m1.transpose()*m2); + m3 = MatrixType::Random(size,size); + m3 = plu.transpose().solve(m2); + VERIFY_IS_APPROX(m2, m1.transpose()*m3); + + // test solve with conjugate transposed + plu.template _solve_impl_transposed(m3, m2); + VERIFY_IS_APPROX(m3, m1.adjoint()*m2); + m3 = MatrixType::Random(size,size); + m3 = plu.adjoint().solve(m2); + VERIFY_IS_APPROX(m2, m1.adjoint()*m3); } template void lu_verify_assert() diff --git a/ground/gcs/src/libs/eigen/test/main.h b/ground/gcs/src/libs/eigen/test/main.h index 6642048662..25d2dcf43e 100644 --- a/ground/gcs/src/libs/eigen/test/main.h +++ b/ground/gcs/src/libs/eigen/test/main.h @@ -41,7 +41,14 @@ #include #include #include +#include #include +#if __cplusplus >= 201103L +#include +#ifdef EIGEN_USE_THREADS +#include +#endif +#endif // To test that all calls from Eigen code to std::min() and std::max() are // protected by parenthesis against macro expansion, the min()/max() macros @@ -49,14 +56,48 @@ // compiler error. #define min(A,B) please_protect_your_min_with_parentheses #define max(A,B) please_protect_your_max_with_parentheses +#define isnan(X) please_protect_your_isnan_with_parentheses +#define isinf(X) please_protect_your_isinf_with_parentheses +#define isfinite(X) please_protect_your_isfinite_with_parentheses +#ifdef M_PI +#undef M_PI +#endif +#define M_PI please_use_EIGEN_PI_instead_of_M_PI #define FORBIDDEN_IDENTIFIER (this_identifier_is_forbidden_to_avoid_clashes) this_identifier_is_forbidden_to_avoid_clashes // B0 is defined in POSIX header termios.h #define B0 FORBIDDEN_IDENTIFIER +// Unit tests calling Eigen's blas library must preserve the default blocking size +// to avoid troubles. +#ifndef EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS +#define EIGEN_DEBUG_SMALL_PRODUCT_BLOCKS +#endif // shuts down ICC's remark #593: variable "XXX" was set but never used -#define TEST_SET_BUT_UNUSED_VARIABLE(X) X = X + 0; +#define TEST_SET_BUT_UNUSED_VARIABLE(X) EIGEN_UNUSED_VARIABLE(X) + +#ifdef TEST_ENABLE_TEMPORARY_TRACKING + +static long int nb_temporaries; +static long int nb_temporaries_on_assert = -1; + +inline void on_temporary_creation(long int size) { + // here's a great place to set a breakpoint when debugging failures in this test! + if(size!=0) nb_temporaries++; + if(nb_temporaries_on_assert>0) assert(nb_temporaries g_test_stack; + // level == 0 <=> abort if test fail + // level >= 1 <=> warning message to std::cerr if test fail + static int g_test_level = 0; static int g_repeat; static unsigned int g_seed; static bool g_has_set_repeat, g_has_set_seed; } +#define TRACK std::cerr << __FILE__ << " " << __LINE__ << std::endl +// #define TRACK while() + #define EI_PP_MAKE_STRING2(S) #S #define EI_PP_MAKE_STRING(S) EI_PP_MAKE_STRING2(S) #define EIGEN_DEFAULT_IO_FORMAT IOFormat(4, 0, " ", "\n", "", "", "", "") +#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(__CUDA_ARCH__) + #define EIGEN_EXCEPTIONS +#endif + #ifndef EIGEN_NO_ASSERTION_CHECKING namespace Eigen @@ -135,33 +186,35 @@ namespace Eigen if(report_on_cerr_on_assert_failure) \ std::cerr << #a << " " __FILE__ << "(" << __LINE__ << ")\n"; \ Eigen::no_more_assert = true; \ - throw Eigen::eigen_assert_exception(); \ + EIGEN_THROW_X(Eigen::eigen_assert_exception()); \ } \ else if (Eigen::internal::push_assert) \ { \ eigen_assert_list.push_back(std::string(EI_PP_MAKE_STRING(__FILE__) " (" EI_PP_MAKE_STRING(__LINE__) ") : " #a) ); \ } + #ifdef EIGEN_EXCEPTIONS #define VERIFY_RAISES_ASSERT(a) \ { \ Eigen::no_more_assert = false; \ - Eigen::eigen_assert_list.clear(); \ - Eigen::internal::push_assert = true; \ + Eigen::eigen_assert_list.clear(); \ + Eigen::internal::push_assert = true; \ Eigen::report_on_cerr_on_assert_failure = false; \ try { \ a; \ std::cerr << "One of the following asserts should have been triggered:\n"; \ - for (uint ai=0 ; ai0) + std::cerr << "WARNING: "; std::cerr << "Test " << testname << " failed in " << file << " (" << line << ")" << std::endl << " " << condition_as_string << std::endl; std::cerr << "Stack:\n"; @@ -208,14 +271,20 @@ inline void verify_impl(bool condition, const char *testname, const char *file, for(int i=test_stack_size-1; i>=0; --i) std::cerr << " - " << Eigen::g_test_stack[i] << "\n"; std::cerr << "\n"; - abort(); + if(Eigen::g_test_level==0) + abort(); } } #define VERIFY(a) ::verify_impl(a, g_test_stack.back().c_str(), __FILE__, __LINE__, EI_PP_MAKE_STRING(a)) -#define VERIFY_IS_EQUAL(a, b) VERIFY(test_is_equal(a, b)) -#define VERIFY_IS_APPROX(a, b) VERIFY(test_isApprox(a, b)) +#define VERIFY_GE(a, b) ::verify_impl(a >= b, g_test_stack.back().c_str(), __FILE__, __LINE__, EI_PP_MAKE_STRING(a >= b)) +#define VERIFY_LE(a, b) ::verify_impl(a <= b, g_test_stack.back().c_str(), __FILE__, __LINE__, EI_PP_MAKE_STRING(a <= b)) + + +#define VERIFY_IS_EQUAL(a, b) VERIFY(test_is_equal(a, b, true)) +#define VERIFY_IS_NOT_EQUAL(a, b) VERIFY(test_is_equal(a, b, false)) +#define VERIFY_IS_APPROX(a, b) VERIFY(verifyIsApprox(a, b)) #define VERIFY_IS_NOT_APPROX(a, b) VERIFY(!test_isApprox(a, b)) #define VERIFY_IS_MUCH_SMALLER_THAN(a, b) VERIFY(test_isMuchSmallerThan(a, b)) #define VERIFY_IS_NOT_MUCH_SMALLER_THAN(a, b) VERIFY(!test_isMuchSmallerThan(a, b)) @@ -236,9 +305,10 @@ namespace Eigen { template inline typename NumTraits::Real test_precision() { return NumTraits::dummy_precision(); } template<> inline float test_precision() { return 1e-3f; } template<> inline double test_precision() { return 1e-6; } +template<> inline long double test_precision() { return 1e-6l; } template<> inline float test_precision >() { return test_precision(); } template<> inline double test_precision >() { return test_precision(); } -template<> inline long double test_precision() { return 1e-6; } +template<> inline long double test_precision >() { return test_precision(); } inline bool test_isApprox(const int& a, const int& b) { return internal::isApprox(a, b, test_precision()); } @@ -253,14 +323,15 @@ inline bool test_isMuchSmallerThan(const float& a, const float& b) { return internal::isMuchSmallerThan(a, b, test_precision()); } inline bool test_isApproxOrLessThan(const float& a, const float& b) { return internal::isApproxOrLessThan(a, b, test_precision()); } + inline bool test_isApprox(const double& a, const double& b) { return internal::isApprox(a, b, test_precision()); } - inline bool test_isMuchSmallerThan(const double& a, const double& b) { return internal::isMuchSmallerThan(a, b, test_precision()); } inline bool test_isApproxOrLessThan(const double& a, const double& b) { return internal::isApproxOrLessThan(a, b, test_precision()); } +#ifndef EIGEN_TEST_NO_COMPLEX inline bool test_isApprox(const std::complex& a, const std::complex& b) { return internal::isApprox(a, b, test_precision >()); } inline bool test_isMuchSmallerThan(const std::complex& a, const std::complex& b) @@ -271,6 +342,15 @@ inline bool test_isApprox(const std::complex& a, const std::complex& a, const std::complex& b) { return internal::isMuchSmallerThan(a, b, test_precision >()); } +#ifndef EIGEN_TEST_NO_LONGDOUBLE +inline bool test_isApprox(const std::complex& a, const std::complex& b) +{ return internal::isApprox(a, b, test_precision >()); } +inline bool test_isMuchSmallerThan(const std::complex& a, const std::complex& b) +{ return internal::isMuchSmallerThan(a, b, test_precision >()); } +#endif +#endif + +#ifndef EIGEN_TEST_NO_LONGDOUBLE inline bool test_isApprox(const long double& a, const long double& b) { bool ret = internal::isApprox(a, b, test_precision()); @@ -284,13 +364,127 @@ inline bool test_isMuchSmallerThan(const long double& a, const long double& b) { return internal::isMuchSmallerThan(a, b, test_precision()); } inline bool test_isApproxOrLessThan(const long double& a, const long double& b) { return internal::isApproxOrLessThan(a, b, test_precision()); } +#endif // EIGEN_TEST_NO_LONGDOUBLE + +inline bool test_isApprox(const half& a, const half& b) +{ return internal::isApprox(a, b, test_precision()); } +inline bool test_isMuchSmallerThan(const half& a, const half& b) +{ return internal::isMuchSmallerThan(a, b, test_precision()); } +inline bool test_isApproxOrLessThan(const half& a, const half& b) +{ return internal::isApproxOrLessThan(a, b, test_precision()); } + +// test_relative_error returns the relative difference between a and b as a real scalar as used in isApprox. +template +typename NumTraits::NonInteger test_relative_error(const EigenBase &a, const EigenBase &b) +{ + using std::sqrt; + typedef typename NumTraits::NonInteger RealScalar; + typename internal::nested_eval::type ea(a.derived()); + typename internal::nested_eval::type eb(b.derived()); + return sqrt(RealScalar((ea-eb).cwiseAbs2().sum()) / RealScalar((std::min)(eb.cwiseAbs2().sum(),ea.cwiseAbs2().sum()))); +} + +template +typename T1::RealScalar test_relative_error(const T1 &a, const T2 &b, const typename T1::Coefficients* = 0) +{ + return test_relative_error(a.coeffs(), b.coeffs()); +} + +template +typename T1::Scalar test_relative_error(const T1 &a, const T2 &b, const typename T1::MatrixType* = 0) +{ + return test_relative_error(a.matrix(), b.matrix()); +} + +template +S test_relative_error(const Translation &a, const Translation &b) +{ + return test_relative_error(a.vector(), b.vector()); +} + +template +S test_relative_error(const ParametrizedLine &a, const ParametrizedLine &b) +{ + return (std::max)(test_relative_error(a.origin(), b.origin()), test_relative_error(a.origin(), b.origin())); +} + +template +S test_relative_error(const AlignedBox &a, const AlignedBox &b) +{ + return (std::max)(test_relative_error((a.min)(), (b.min)()), test_relative_error((a.max)(), (b.max)())); +} + +template class SparseMatrixBase; +template +typename T1::RealScalar test_relative_error(const MatrixBase &a, const SparseMatrixBase &b) +{ + return test_relative_error(a,b.toDense()); +} + +template class SparseMatrixBase; +template +typename T1::RealScalar test_relative_error(const SparseMatrixBase &a, const MatrixBase &b) +{ + return test_relative_error(a.toDense(),b); +} + +template class SparseMatrixBase; +template +typename T1::RealScalar test_relative_error(const SparseMatrixBase &a, const SparseMatrixBase &b) +{ + return test_relative_error(a.toDense(),b.toDense()); +} + +template +typename NumTraits::Real>::NonInteger test_relative_error(const T1 &a, const T2 &b, typename internal::enable_if::Real>::value, T1>::type* = 0) +{ + typedef typename NumTraits::Real>::NonInteger RealScalar; + return numext::sqrt(RealScalar(numext::abs2(a-b))/RealScalar((numext::mini)(numext::abs2(a),numext::abs2(b)))); +} + +template +T test_relative_error(const Rotation2D &a, const Rotation2D &b) +{ + return test_relative_error(a.angle(), b.angle()); +} + +template +T test_relative_error(const AngleAxis &a, const AngleAxis &b) +{ + return (std::max)(test_relative_error(a.angle(), b.angle()), test_relative_error(a.axis(), b.axis())); +} template -inline bool test_isApprox(const Type1& a, const Type2& b) +inline bool test_isApprox(const Type1& a, const Type2& b, typename Type1::Scalar* = 0) // Enabled for Eigen's type only { return a.isApprox(b, test_precision()); } +// get_test_precision is a small wrapper to test_precision allowing to return the scalar precision for either scalars or expressions +template +typename NumTraits::Real get_test_precision(const T&, const typename T::Scalar* = 0) +{ + return test_precision::Real>(); +} + +template +typename NumTraits::Real get_test_precision(const T&,typename internal::enable_if::Real>::value, T>::type* = 0) +{ + return test_precision::Real>(); +} + +// verifyIsApprox is a wrapper to test_isApprox that outputs the relative difference magnitude if the test fails. +template +inline bool verifyIsApprox(const Type1& a, const Type2& b) +{ + bool ret = test_isApprox(a,b); + if(!ret) + { + std::cerr << "Difference too large wrt tolerance " << get_test_precision(a) << ", relative error is: " << test_relative_error(a,b) << std::endl; + } + return ret; +} + // The idea behind this function is to compare the two scalars a and b where // the scalar ref is a hint about the expected order of magnitude of a and b. // WARNING: the scalar a and b must be positive @@ -326,17 +520,17 @@ inline bool test_isUnitary(const MatrixBase& m) // Forward declaration to avoid ICC warning template -bool test_is_equal(const T& actual, const U& expected); +bool test_is_equal(const T& actual, const U& expected, bool expect_equal=true); template -bool test_is_equal(const T& actual, const U& expected) +bool test_is_equal(const T& actual, const U& expected, bool expect_equal) { - if (actual==expected) + if ((actual==expected) == expect_equal) return true; // false: std::cerr - << std::endl << " actual = " << actual - << std::endl << " expected = " << expected << std::endl << std::endl; + << "\n actual = " << actual + << "\n expected " << (expect_equal ? "= " : "!=") << expected << "\n\n"; return false; } @@ -347,11 +541,10 @@ bool test_is_equal(const T& actual, const U& expected) */ // Forward declaration to avoid ICC warning template -void createRandomPIMatrixOfRank(typename MatrixType::Index desired_rank, typename MatrixType::Index rows, typename MatrixType::Index cols, MatrixType& m); +void createRandomPIMatrixOfRank(Index desired_rank, Index rows, Index cols, MatrixType& m); template -void createRandomPIMatrixOfRank(typename MatrixType::Index desired_rank, typename MatrixType::Index rows, typename MatrixType::Index cols, MatrixType& m) +void createRandomPIMatrixOfRank(Index desired_rank, Index rows, Index cols, MatrixType& m) { - typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; enum { Rows = MatrixType::RowsAtCompileTime, Cols = MatrixType::ColsAtCompileTime }; @@ -388,11 +581,10 @@ void createRandomPIMatrixOfRank(typename MatrixType::Index desired_rank, typenam // Forward declaration to avoid ICC warning template -void randomPermutationVector(PermutationVectorType& v, typename PermutationVectorType::Index size); +void randomPermutationVector(PermutationVectorType& v, Index size); template -void randomPermutationVector(PermutationVectorType& v, typename PermutationVectorType::Index size) +void randomPermutationVector(PermutationVectorType& v, Index size) { - typedef typename PermutationVectorType::Index Index; typedef typename PermutationVectorType::Scalar Scalar; v.resize(size); for(Index i = 0; i < size; ++i) v(i) = Scalar(i); @@ -411,12 +603,7 @@ template bool isNotNaN(const T& x) return x==x; } -template bool isNaN(const T& x) -{ - return x!=x; -} - -template bool isInf(const T& x) +template bool isPlusInf(const T& x) { return x > NumTraits::highest(); } @@ -437,13 +624,15 @@ template struct GetDifferentType > // Forward declaration to avoid ICC warning template std::string type_name(); -template std::string type_name() { return "other"; } -template<> std::string type_name() { return "float"; } -template<> std::string type_name() { return "double"; } -template<> std::string type_name() { return "int"; } -template<> std::string type_name >() { return "complex"; } -template<> std::string type_name >() { return "complex"; } -template<> std::string type_name >() { return "complex"; } +template std::string type_name() { return "other"; } +template<> std::string type_name() { return "float"; } +template<> std::string type_name() { return "double"; } +template<> std::string type_name() { return "long double"; } +template<> std::string type_name() { return "int"; } +template<> std::string type_name >() { return "complex"; } +template<> std::string type_name >() { return "complex"; } +template<> std::string type_name >() { return "complex"; } +template<> std::string type_name >() { return "complex"; } // forward declaration of the main test function void EIGEN_CAT(test_,EIGEN_TEST_FUNC)(); @@ -550,3 +739,8 @@ int main(int argc, char *argv[]) // remark #1572: floating-point equality and inequality comparisons are unreliable #pragma warning disable 279 383 1418 1572 #endif + +#ifdef _MSC_VER + // 4503 - decorated name length exceeded, name was truncated + #pragma warning( disable : 4503) +#endif diff --git a/ground/gcs/src/libs/eigen/test/mapped_matrix.cpp b/ground/gcs/src/libs/eigen/test/mapped_matrix.cpp index 58904fa376..6a84c58975 100644 --- a/ground/gcs/src/libs/eigen/test/mapped_matrix.cpp +++ b/ground/gcs/src/libs/eigen/test/mapped_matrix.cpp @@ -13,6 +13,8 @@ #include "main.h" +#define EIGEN_TESTMAP_MAX_SIZE 256 + template void map_class_vector(const VectorType& m) { typedef typename VectorType::Index Index; @@ -20,23 +22,26 @@ template void map_class_vector(const VectorType& m) Index size = m.size(); - // test Map.h Scalar* array1 = internal::aligned_new(size); Scalar* array2 = internal::aligned_new(size); Scalar* array3 = new Scalar[size+1]; - Scalar* array3unaligned = size_t(array3)%16 == 0 ? array3+1 : array3; + Scalar* array3unaligned = (internal::UIntPtr(array3)%EIGEN_MAX_ALIGN_BYTES) == 0 ? array3+1 : array3; + Scalar array4[EIGEN_TESTMAP_MAX_SIZE]; - Map(array1, size) = VectorType::Random(size); - Map(array2, size) = Map(array1, size); + Map(array1, size) = VectorType::Random(size); + Map(array2, size) = Map(array1, size); Map(array3unaligned, size) = Map(array1, size); - VectorType ma1 = Map(array1, size); - VectorType ma2 = Map(array2, size); + Map(array4, size) = Map(array1, size); + VectorType ma1 = Map(array1, size); + VectorType ma2 = Map(array2, size); VectorType ma3 = Map(array3unaligned, size); + VectorType ma4 = Map(array4, size); VERIFY_IS_EQUAL(ma1, ma2); VERIFY_IS_EQUAL(ma1, ma3); + VERIFY_IS_EQUAL(ma1, ma4); #ifdef EIGEN_VECTORIZE - if(internal::packet_traits::Vectorizable) - VERIFY_RAISES_ASSERT((Map(array3unaligned, size))) + if(internal::packet_traits::Vectorizable && size>=AlignedMax) + VERIFY_RAISES_ASSERT((Map(array3unaligned, size))) #endif internal::aligned_delete(array1, size); @@ -50,23 +55,64 @@ template void map_class_matrix(const MatrixType& m) typedef typename MatrixType::Scalar Scalar; Index rows = m.rows(), cols = m.cols(), size = rows*cols; + Scalar s1 = internal::random(); - // test Map.h + // array1 and array2 -> aligned heap allocation Scalar* array1 = internal::aligned_new(size); for(int i = 0; i < size; i++) array1[i] = Scalar(1); Scalar* array2 = internal::aligned_new(size); for(int i = 0; i < size; i++) array2[i] = Scalar(1); + // array3unaligned -> unaligned pointer to heap Scalar* array3 = new Scalar[size+1]; for(int i = 0; i < size+1; i++) array3[i] = Scalar(1); - Scalar* array3unaligned = size_t(array3)%16 == 0 ? array3+1 : array3; - Map(array1, rows, cols) = MatrixType::Ones(rows,cols); - Map(array2, rows, cols) = Map(array1, rows, cols); - Map(array3unaligned, rows, cols) = Map(array1, rows, cols); - MatrixType ma1 = Map(array1, rows, cols); - MatrixType ma2 = Map(array2, rows, cols); + Scalar* array3unaligned = internal::UIntPtr(array3)%EIGEN_MAX_ALIGN_BYTES == 0 ? array3+1 : array3; + Scalar array4[256]; + if(size<=256) + for(int i = 0; i < size; i++) array4[i] = Scalar(1); + + Map map1(array1, rows, cols); + Map map2(array2, rows, cols); + Map map3(array3unaligned, rows, cols); + Map map4(array4, rows, cols); + + VERIFY_IS_EQUAL(map1, MatrixType::Ones(rows,cols)); + VERIFY_IS_EQUAL(map2, MatrixType::Ones(rows,cols)); + VERIFY_IS_EQUAL(map3, MatrixType::Ones(rows,cols)); + map1 = MatrixType::Random(rows,cols); + map2 = map1; + map3 = map1; + MatrixType ma1 = map1; + MatrixType ma2 = map2; + MatrixType ma3 = map3; + VERIFY_IS_EQUAL(map1, map2); + VERIFY_IS_EQUAL(map1, map3); VERIFY_IS_EQUAL(ma1, ma2); - MatrixType ma3 = Map(array3unaligned, rows, cols); VERIFY_IS_EQUAL(ma1, ma3); + VERIFY_IS_EQUAL(ma1, map3); + + VERIFY_IS_APPROX(s1*map1, s1*map2); + VERIFY_IS_APPROX(s1*ma1, s1*ma2); + VERIFY_IS_EQUAL(s1*ma1, s1*ma3); + VERIFY_IS_APPROX(s1*map1, s1*map3); + + map2 *= s1; + map3 *= s1; + VERIFY_IS_APPROX(s1*map1, map2); + VERIFY_IS_APPROX(s1*map1, map3); + + if(size<=256) + { + VERIFY_IS_EQUAL(map4, MatrixType::Ones(rows,cols)); + map4 = map1; + MatrixType ma4 = map4; + VERIFY_IS_EQUAL(map1, map4); + VERIFY_IS_EQUAL(ma1, map4); + VERIFY_IS_EQUAL(ma1, ma4); + VERIFY_IS_APPROX(s1*map1, s1*map4); + + map4 *= s1; + VERIFY_IS_APPROX(s1*map1, map4); + } internal::aligned_delete(array1, size); internal::aligned_delete(array2, size); @@ -80,11 +126,10 @@ template void map_static_methods(const VectorType& m) Index size = m.size(); - // test Map.h Scalar* array1 = internal::aligned_new(size); Scalar* array2 = internal::aligned_new(size); Scalar* array3 = new Scalar[size+1]; - Scalar* array3unaligned = size_t(array3)%16 == 0 ? array3+1 : array3; + Scalar* array3unaligned = internal::UIntPtr(array3)%EIGEN_MAX_ALIGN_BYTES == 0 ? array3+1 : array3; VectorType::MapAligned(array1, size) = VectorType::Random(size); VectorType::Map(array2, size) = VectorType::Map(array1, size); @@ -109,9 +154,9 @@ template void check_const_correctness(const PlainObjec // verify that map-to-const don't have LvalueBit typedef typename internal::add_const::type ConstPlainObjectType; VERIFY( !(internal::traits >::Flags & LvalueBit) ); - VERIFY( !(internal::traits >::Flags & LvalueBit) ); + VERIFY( !(internal::traits >::Flags & LvalueBit) ); VERIFY( !(Map::Flags & LvalueBit) ); - VERIFY( !(Map::Flags & LvalueBit) ); + VERIFY( !(Map::Flags & LvalueBit) ); } template @@ -142,6 +187,7 @@ void test_mapped_matrix() CALL_SUBTEST_1( map_class_vector(Matrix()) ); CALL_SUBTEST_1( check_const_correctness(Matrix()) ); CALL_SUBTEST_2( map_class_vector(Vector4d()) ); + CALL_SUBTEST_2( map_class_vector(VectorXd(13)) ); CALL_SUBTEST_2( check_const_correctness(Matrix4d()) ); CALL_SUBTEST_3( map_class_vector(RowVector4f()) ); CALL_SUBTEST_4( map_class_vector(VectorXcf(8)) ); diff --git a/ground/gcs/src/libs/eigen/test/mapstaticmethods.cpp b/ground/gcs/src/libs/eigen/test/mapstaticmethods.cpp index 5b512bde48..06272d1062 100644 --- a/ground/gcs/src/libs/eigen/test/mapstaticmethods.cpp +++ b/ground/gcs/src/libs/eigen/test/mapstaticmethods.cpp @@ -69,7 +69,8 @@ struct mapstaticmethods_impl { static void run(const PlainObjectType& m) { - int rows = m.rows(), cols = m.cols(); + typedef typename PlainObjectType::Index Index; + Index rows = m.rows(), cols = m.cols(); int i = internal::random(2,5), j = internal::random(2,5); @@ -115,7 +116,8 @@ struct mapstaticmethods_impl { static void run(const PlainObjectType& v) { - int size = v.size(); + typedef typename PlainObjectType::Index Index; + Index size = v.size(); int i = internal::random(2,5); diff --git a/ground/gcs/src/libs/eigen/test/mapstride.cpp b/ground/gcs/src/libs/eigen/test/mapstride.cpp index b1dc9de2ab..4858f8fea2 100644 --- a/ground/gcs/src/libs/eigen/test/mapstride.cpp +++ b/ground/gcs/src/libs/eigen/test/mapstride.cpp @@ -23,7 +23,7 @@ template void map_class_vector(const VectorTy Scalar* a_array = internal::aligned_new(arraysize+1); Scalar* array = a_array; if(Alignment!=Aligned) - array = (Scalar*)(ptrdiff_t(a_array) + (internal::packet_traits::AlignedOnScalar?sizeof(Scalar):sizeof(typename NumTraits::Real))); + array = (Scalar*)(internal::IntPtr(a_array) + (internal::packet_traits::AlignedOnScalar?sizeof(Scalar):sizeof(typename NumTraits::Real))); { Map > map(array, size); @@ -56,16 +56,30 @@ template void map_class_matrix(const MatrixTy Index rows = _m.rows(), cols = _m.cols(); MatrixType m = MatrixType::Random(rows,cols); + Scalar s1 = internal::random(); Index arraysize = 2*(rows+4)*(cols+4); - Scalar* a_array = internal::aligned_new(arraysize+1); - Scalar* array = a_array; + Scalar* a_array1 = internal::aligned_new(arraysize+1); + Scalar* array1 = a_array1; if(Alignment!=Aligned) - array = (Scalar*)(ptrdiff_t(a_array) + (internal::packet_traits::AlignedOnScalar?sizeof(Scalar):sizeof(typename NumTraits::Real))); + array1 = (Scalar*)(internal::IntPtr(a_array1) + (internal::packet_traits::AlignedOnScalar?sizeof(Scalar):sizeof(typename NumTraits::Real))); + Scalar a_array2[256]; + Scalar* array2 = a_array2; + if(Alignment!=Aligned) + array2 = (Scalar*)(internal::IntPtr(a_array2) + (internal::packet_traits::AlignedOnScalar?sizeof(Scalar):sizeof(typename NumTraits::Real))); + else + array2 = (Scalar*)(((internal::UIntPtr(a_array2)+EIGEN_MAX_ALIGN_BYTES-1)/EIGEN_MAX_ALIGN_BYTES)*EIGEN_MAX_ALIGN_BYTES); + Index maxsize2 = a_array2 - array2 + 256; + // test no inner stride and some dynamic outer stride + for(int k=0; k<2; ++k) { + if(k==1 && (m.innerSize()+1)*m.outerSize() > maxsize2) + break; + Scalar* array = (k==0 ? array1 : array2); + Map > map(array, rows, cols, OuterStride(m.innerSize()+1)); map = m; VERIFY(map.outerStride() == map.innerSize()+1); @@ -75,11 +89,19 @@ template void map_class_matrix(const MatrixTy VERIFY(array[map.outerStride()*i+j] == m.coeffByOuterInner(i,j)); VERIFY(map.coeffByOuterInner(i,j) == m.coeffByOuterInner(i,j)); } + VERIFY_IS_APPROX(s1*map,s1*m); + map *= s1; + VERIFY_IS_APPROX(map,s1*m); } // test no inner stride and an outer stride of +4. This is quite important as for fixed-size matrices, // this allows to hit the special case where it's vectorizable. + for(int k=0; k<2; ++k) { + if(k==1 && (m.innerSize()+4)*m.outerSize() > maxsize2) + break; + Scalar* array = (k==0 ? array1 : array2); + enum { InnerSize = MatrixType::InnerSizeAtCompileTime, OuterStrideAtCompileTime = InnerSize==Dynamic ? Dynamic : InnerSize+4 @@ -94,10 +116,18 @@ template void map_class_matrix(const MatrixTy VERIFY(array[map.outerStride()*i+j] == m.coeffByOuterInner(i,j)); VERIFY(map.coeffByOuterInner(i,j) == m.coeffByOuterInner(i,j)); } + VERIFY_IS_APPROX(s1*map,s1*m); + map *= s1; + VERIFY_IS_APPROX(map,s1*m); } // test both inner stride and outer stride + for(int k=0; k<2; ++k) { + if(k==1 && (2*m.innerSize()+1)*(m.outerSize()*2) > maxsize2) + break; + Scalar* array = (k==0 ? array1 : array2); + Map > map(array, rows, cols, Stride(2*m.innerSize()+1, 2)); map = m; VERIFY(map.outerStride() == 2*map.innerSize()+1); @@ -108,9 +138,12 @@ template void map_class_matrix(const MatrixTy VERIFY(array[map.outerStride()*i+map.innerStride()*j] == m.coeffByOuterInner(i,j)); VERIFY(map.coeffByOuterInner(i,j) == m.coeffByOuterInner(i,j)); } + VERIFY_IS_APPROX(s1*map,s1*m); + map *= s1; + VERIFY_IS_APPROX(map,s1*m); } - internal::aligned_delete(a_array, arraysize+1); + internal::aligned_delete(a_array1, arraysize+1); } void test_mapstride() diff --git a/ground/gcs/src/libs/eigen/test/meta.cpp b/ground/gcs/src/libs/eigen/test/meta.cpp index 3302c5887f..b8dea68e80 100644 --- a/ground/gcs/src/libs/eigen/test/meta.cpp +++ b/ground/gcs/src/libs/eigen/test/meta.cpp @@ -9,6 +9,12 @@ #include "main.h" +template +bool check_is_convertible(const From&, const To&) +{ + return internal::is_convertible::value; +} + void test_meta() { VERIFY((internal::conditional<(3<4),internal::true_type, internal::false_type>::type::value)); @@ -52,6 +58,24 @@ void test_meta() VERIFY(( internal::is_same::type >::value)); VERIFY(( internal::is_same::type >::value)); + VERIFY(( internal::is_convertible::value )); + VERIFY(( internal::is_convertible::value )); + VERIFY(( internal::is_convertible::value )); + VERIFY((!internal::is_convertible,double>::value )); + VERIFY(( internal::is_convertible::value )); +// VERIFY((!internal::is_convertible::value )); //does not work because the conversion is prevented by a static assertion + VERIFY((!internal::is_convertible::value )); + VERIFY((!internal::is_convertible::value )); + { + float f; + MatrixXf A, B; + VectorXf a, b; + VERIFY(( check_is_convertible(a.dot(b), f) )); + VERIFY(( check_is_convertible(a.transpose()*b, f) )); + VERIFY((!check_is_convertible(A*B, f) )); + VERIFY(( check_is_convertible(A*B, A) )); + } + VERIFY(internal::meta_sqrt<1>::ret == 1); #define VERIFY_META_SQRT(X) VERIFY(internal::meta_sqrt::ret == int(std::sqrt(double(X)))) VERIFY_META_SQRT(2); diff --git a/ground/gcs/src/libs/eigen/test/metis_support.cpp b/ground/gcs/src/libs/eigen/test/metis_support.cpp index 932b040748..d87c56a130 100644 --- a/ground/gcs/src/libs/eigen/test/metis_support.cpp +++ b/ground/gcs/src/libs/eigen/test/metis_support.cpp @@ -3,24 +3,10 @@ // // Copyright (C) 2012 Désiré Nuentsa-Wakam // -// Eigen is free software; you can redistribute it and/or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 3 of the License, or (at your option) any later version. -// -// Alternatively, 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 2 of -// the License, or (at your option) any later version. -// -// Eigen 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 Lesser General Public License or the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public -// License and a copy of the GNU General Public License along with -// Eigen. If not, see . +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #include "sparse_solver.h" #include #include diff --git a/ground/gcs/src/libs/eigen/test/mixingtypes.cpp b/ground/gcs/src/libs/eigen/test/mixingtypes.cpp index 6c2f748755..ad9c2c6525 100644 --- a/ground/gcs/src/libs/eigen/test/mixingtypes.cpp +++ b/ground/gcs/src/libs/eigen/test/mixingtypes.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2008-2015 Gael Guennebaud // Copyright (C) 2008 Benoit Jacob // // This Source Code Form is subject to the terms of the Mozilla @@ -15,14 +15,26 @@ #define EIGEN_NO_STATIC_ASSERT // turn static asserts into runtime asserts in order to check them #endif -// #ifndef EIGEN_DONT_VECTORIZE -// #define EIGEN_DONT_VECTORIZE // SSE intrinsics aren't designed to allow mixing types -// #endif +#if defined(EIGEN_TEST_PART_1) || defined(EIGEN_TEST_PART_2) || defined(EIGEN_TEST_PART_3) + +#ifndef EIGEN_DONT_VECTORIZE +#define EIGEN_DONT_VECTORIZE +#endif + +#endif + +static bool g_called; +#define EIGEN_SCALAR_BINARY_OP_PLUGIN { g_called |= (!internal::is_same::value); } #include "main.h" using namespace std; +#define VERIFY_MIX_SCALAR(XPR,REF) \ + g_called = false; \ + VERIFY_IS_APPROX(XPR,REF); \ + VERIFY( g_called && #XPR" not properly optimized"); + template void mixingtypes(int size = SizeAtCompileType) { typedef std::complex CF; @@ -38,8 +50,10 @@ template void mixingtypes(int size = SizeAtCompileType) Mat_f mf = Mat_f::Random(size,size); Mat_d md = mf.template cast(); + //Mat_d rd = md; Mat_cf mcf = Mat_cf::Random(size,size); Mat_cd mcd = mcf.template cast >(); + Mat_cd rcd = mcd; Vec_f vf = Vec_f::Random(size,1); Vec_d vd = vf.template cast(); Vec_cf vcf = Vec_cf::Random(size,1); @@ -49,19 +63,59 @@ template void mixingtypes(int size = SizeAtCompileType) complex scf = internal::random >(); complex scd = internal::random >(); - mf+mf; - VERIFY_RAISES_ASSERT(mf+md); - VERIFY_RAISES_ASSERT(mf+mcf); + + float epsf = std::sqrt(std::numeric_limits ::min EIGEN_EMPTY ()); + double epsd = std::sqrt(std::numeric_limits::min EIGEN_EMPTY ()); + + while(std::abs(sf )(); + while(std::abs(sd )(); + while(std::abs(scf)(); + while(std::abs(scd)(); + +// VERIFY_RAISES_ASSERT(mf+md); // does not even compile + +#ifdef EIGEN_DONT_VECTORIZE VERIFY_RAISES_ASSERT(vf=vd); VERIFY_RAISES_ASSERT(vf+=vd); - VERIFY_RAISES_ASSERT(mcd=md); - +#endif + // check scalar products - VERIFY_IS_APPROX(vcf * sf , vcf * complex(sf)); - VERIFY_IS_APPROX(sd * vcd, complex(sd) * vcd); - VERIFY_IS_APPROX(vf * scf , vf.template cast >() * scf); - VERIFY_IS_APPROX(scd * vd, scd * vd.template cast >()); + VERIFY_MIX_SCALAR(vcf * sf , vcf * complex(sf)); + VERIFY_MIX_SCALAR(sd * vcd , complex(sd) * vcd); + VERIFY_MIX_SCALAR(vf * scf , vf.template cast >() * scf); + VERIFY_MIX_SCALAR(scd * vd , scd * vd.template cast >()); + + VERIFY_MIX_SCALAR(vcf * 2 , vcf * complex(2)); + VERIFY_MIX_SCALAR(vcf * 2.1 , vcf * complex(2.1)); + VERIFY_MIX_SCALAR(2 * vcf, vcf * complex(2)); + VERIFY_MIX_SCALAR(2.1 * vcf , vcf * complex(2.1)); + + // check scalar quotients + VERIFY_MIX_SCALAR(vcf / sf , vcf / complex(sf)); + VERIFY_MIX_SCALAR(vf / scf , vf.template cast >() / scf); + VERIFY_MIX_SCALAR(vf.array() / scf, vf.template cast >().array() / scf); + VERIFY_MIX_SCALAR(scd / vd.array() , scd / vd.template cast >().array()); + + // check scalar increment + VERIFY_MIX_SCALAR(vcf.array() + sf , vcf.array() + complex(sf)); + VERIFY_MIX_SCALAR(sd + vcd.array(), complex(sd) + vcd.array()); + VERIFY_MIX_SCALAR(vf.array() + scf, vf.template cast >().array() + scf); + VERIFY_MIX_SCALAR(scd + vd.array() , scd + vd.template cast >().array()); + + // check scalar subtractions + VERIFY_MIX_SCALAR(vcf.array() - sf , vcf.array() - complex(sf)); + VERIFY_MIX_SCALAR(sd - vcd.array(), complex(sd) - vcd.array()); + VERIFY_MIX_SCALAR(vf.array() - scf, vf.template cast >().array() - scf); + VERIFY_MIX_SCALAR(scd - vd.array() , scd - vd.template cast >().array()); + + // check scalar powers + VERIFY_MIX_SCALAR( pow(vcf.array(), sf), Eigen::pow(vcf.array(), complex(sf)) ); + VERIFY_MIX_SCALAR( vcf.array().pow(sf) , Eigen::pow(vcf.array(), complex(sf)) ); + VERIFY_MIX_SCALAR( pow(sd, vcd.array()), Eigen::pow(complex(sd), vcd.array()) ); + VERIFY_MIX_SCALAR( Eigen::pow(vf.array(), scf), Eigen::pow(vf.template cast >().array(), scf) ); + VERIFY_MIX_SCALAR( vf.array().pow(scf) , Eigen::pow(vf.template cast >().array(), scf) ); + VERIFY_MIX_SCALAR( Eigen::pow(scd, vd.array()), Eigen::pow(scd, vd.template cast >().array()) ); // check dot product vf.dot(vf); @@ -75,6 +129,7 @@ template void mixingtypes(int size = SizeAtCompileType) VERIFY_IS_APPROX(vcd.asDiagonal() * md, vcd.asDiagonal() * md.template cast >()); VERIFY_IS_APPROX(mcf * vf.asDiagonal(), mcf * vf.template cast >().asDiagonal()); VERIFY_IS_APPROX(md * vcd.asDiagonal(), md.template cast >() * vcd.asDiagonal()); + // vd.asDiagonal() * mf; // does not even compile // vcd.asDiagonal() * mf; // does not even compile @@ -92,7 +147,6 @@ template void mixingtypes(int size = SizeAtCompileType) VERIFY_IS_APPROX(mcd.array() *= md.array(), mcd2.array() *= md.array().template cast >()); // check matrix-matrix products - VERIFY_IS_APPROX(sd*md*mcd, (sd*md).template cast().eval()*mcd); VERIFY_IS_APPROX(sd*mcd*md, sd*mcd*md.template cast()); VERIFY_IS_APPROX(scd*md*mcd, scd*md.template cast().eval()*mcd); @@ -103,6 +157,20 @@ template void mixingtypes(int size = SizeAtCompileType) VERIFY_IS_APPROX(scf*mf*mcf, scf*mf.template cast()*mcf); VERIFY_IS_APPROX(scf*mcf*mf, scf*mcf*mf.template cast()); + VERIFY_IS_APPROX(sd*md.adjoint()*mcd, (sd*md).template cast().eval().adjoint()*mcd); + VERIFY_IS_APPROX(sd*mcd.adjoint()*md, sd*mcd.adjoint()*md.template cast()); + VERIFY_IS_APPROX(sd*md.adjoint()*mcd.adjoint(), (sd*md).template cast().eval().adjoint()*mcd.adjoint()); + VERIFY_IS_APPROX(sd*mcd.adjoint()*md.adjoint(), sd*mcd.adjoint()*md.template cast().adjoint()); + VERIFY_IS_APPROX(sd*md*mcd.adjoint(), (sd*md).template cast().eval()*mcd.adjoint()); + VERIFY_IS_APPROX(sd*mcd*md.adjoint(), sd*mcd*md.template cast().adjoint()); + + VERIFY_IS_APPROX(sf*mf.adjoint()*mcf, (sf*mf).template cast().eval().adjoint()*mcf); + VERIFY_IS_APPROX(sf*mcf.adjoint()*mf, sf*mcf.adjoint()*mf.template cast()); + VERIFY_IS_APPROX(sf*mf.adjoint()*mcf.adjoint(), (sf*mf).template cast().eval().adjoint()*mcf.adjoint()); + VERIFY_IS_APPROX(sf*mcf.adjoint()*mf.adjoint(), sf*mcf.adjoint()*mf.template cast().adjoint()); + VERIFY_IS_APPROX(sf*mf*mcf.adjoint(), (sf*mf).template cast().eval()*mcf.adjoint()); + VERIFY_IS_APPROX(sf*mcf*mf.adjoint(), sf*mcf*mf.template cast().adjoint()); + VERIFY_IS_APPROX(sf*mf*vcf, (sf*mf).template cast().eval()*vcf); VERIFY_IS_APPROX(scf*mf*vcf,(scf*mf.template cast()).eval()*vcf); VERIFY_IS_APPROX(sf*mcf*vf, sf*mcf*vf.template cast()); @@ -122,11 +190,111 @@ template void mixingtypes(int size = SizeAtCompileType) VERIFY_IS_APPROX(scd*vcd.adjoint()*md, scd*vcd.adjoint()*md.template cast().eval()); VERIFY_IS_APPROX(sd*vd.adjoint()*mcd, sd*vd.adjoint().template cast().eval()*mcd); VERIFY_IS_APPROX(scd*vd.adjoint()*mcd, scd*vd.adjoint().template cast().eval()*mcd); + + VERIFY_IS_APPROX( sd*vcd.adjoint()*md.template triangularView(), sd*vcd.adjoint()*md.template cast().eval().template triangularView()); + VERIFY_IS_APPROX(scd*vcd.adjoint()*md.template triangularView(), scd*vcd.adjoint()*md.template cast().eval().template triangularView()); + VERIFY_IS_APPROX( sd*vcd.adjoint()*md.transpose().template triangularView(), sd*vcd.adjoint()*md.transpose().template cast().eval().template triangularView()); + VERIFY_IS_APPROX(scd*vcd.adjoint()*md.transpose().template triangularView(), scd*vcd.adjoint()*md.transpose().template cast().eval().template triangularView()); + VERIFY_IS_APPROX( sd*vd.adjoint()*mcd.template triangularView(), sd*vd.adjoint().template cast().eval()*mcd.template triangularView()); + VERIFY_IS_APPROX(scd*vd.adjoint()*mcd.template triangularView(), scd*vd.adjoint().template cast().eval()*mcd.template triangularView()); + VERIFY_IS_APPROX( sd*vd.adjoint()*mcd.transpose().template triangularView(), sd*vd.adjoint().template cast().eval()*mcd.transpose().template triangularView()); + VERIFY_IS_APPROX(scd*vd.adjoint()*mcd.transpose().template triangularView(), scd*vd.adjoint().template cast().eval()*mcd.transpose().template triangularView()); + + // Not supported yet: trmm +// VERIFY_IS_APPROX(sd*mcd*md.template triangularView(), sd*mcd*md.template cast().eval().template triangularView()); +// VERIFY_IS_APPROX(scd*mcd*md.template triangularView(), scd*mcd*md.template cast().eval().template triangularView()); +// VERIFY_IS_APPROX(sd*md*mcd.template triangularView(), sd*md.template cast().eval()*mcd.template triangularView()); +// VERIFY_IS_APPROX(scd*md*mcd.template triangularView(), scd*md.template cast().eval()*mcd.template triangularView()); + + // Not supported yet: symv +// VERIFY_IS_APPROX(sd*vcd.adjoint()*md.template selfadjointView(), sd*vcd.adjoint()*md.template cast().eval().template selfadjointView()); +// VERIFY_IS_APPROX(scd*vcd.adjoint()*md.template selfadjointView(), scd*vcd.adjoint()*md.template cast().eval().template selfadjointView()); +// VERIFY_IS_APPROX(sd*vd.adjoint()*mcd.template selfadjointView(), sd*vd.adjoint().template cast().eval()*mcd.template selfadjointView()); +// VERIFY_IS_APPROX(scd*vd.adjoint()*mcd.template selfadjointView(), scd*vd.adjoint().template cast().eval()*mcd.template selfadjointView()); + + // Not supported yet: symm +// VERIFY_IS_APPROX(sd*vcd.adjoint()*md.template selfadjointView(), sd*vcd.adjoint()*md.template cast().eval().template selfadjointView()); +// VERIFY_IS_APPROX(scd*vcd.adjoint()*md.template selfadjointView(), scd*vcd.adjoint()*md.template cast().eval().template selfadjointView()); +// VERIFY_IS_APPROX(sd*vd.adjoint()*mcd.template selfadjointView(), sd*vd.adjoint().template cast().eval()*mcd.template selfadjointView()); +// VERIFY_IS_APPROX(scd*vd.adjoint()*mcd.template selfadjointView(), scd*vd.adjoint().template cast().eval()*mcd.template selfadjointView()); + + rcd.setZero(); + VERIFY_IS_APPROX(Mat_cd(rcd.template triangularView() = sd * mcd * md), + Mat_cd((sd * mcd * md.template cast().eval()).template triangularView())); + VERIFY_IS_APPROX(Mat_cd(rcd.template triangularView() = sd * md * mcd), + Mat_cd((sd * md.template cast().eval() * mcd).template triangularView())); + VERIFY_IS_APPROX(Mat_cd(rcd.template triangularView() = scd * mcd * md), + Mat_cd((scd * mcd * md.template cast().eval()).template triangularView())); + VERIFY_IS_APPROX(Mat_cd(rcd.template triangularView() = scd * md * mcd), + Mat_cd((scd * md.template cast().eval() * mcd).template triangularView())); + + + VERIFY_IS_APPROX( md.array() * mcd.array(), md.template cast().eval().array() * mcd.array() ); + VERIFY_IS_APPROX( mcd.array() * md.array(), mcd.array() * md.template cast().eval().array() ); + + VERIFY_IS_APPROX( md.array() + mcd.array(), md.template cast().eval().array() + mcd.array() ); + VERIFY_IS_APPROX( mcd.array() + md.array(), mcd.array() + md.template cast().eval().array() ); + + VERIFY_IS_APPROX( md.array() - mcd.array(), md.template cast().eval().array() - mcd.array() ); + VERIFY_IS_APPROX( mcd.array() - md.array(), mcd.array() - md.template cast().eval().array() ); + + if(mcd.array().abs().minCoeff()>epsd) + { + VERIFY_IS_APPROX( md.array() / mcd.array(), md.template cast().eval().array() / mcd.array() ); + } + if(md.array().abs().minCoeff()>epsd) + { + VERIFY_IS_APPROX( mcd.array() / md.array(), mcd.array() / md.template cast().eval().array() ); + } + + if(md.array().abs().minCoeff()>epsd || mcd.array().abs().minCoeff()>epsd) + { + VERIFY_IS_APPROX( md.array().pow(mcd.array()), md.template cast().eval().array().pow(mcd.array()) ); + VERIFY_IS_APPROX( mcd.array().pow(md.array()), mcd.array().pow(md.template cast().eval().array()) ); + + VERIFY_IS_APPROX( pow(md.array(),mcd.array()), md.template cast().eval().array().pow(mcd.array()) ); + VERIFY_IS_APPROX( pow(mcd.array(),md.array()), mcd.array().pow(md.template cast().eval().array()) ); + } + + rcd = mcd; + VERIFY_IS_APPROX( rcd = md, md.template cast().eval() ); + rcd = mcd; + VERIFY_IS_APPROX( rcd += md, mcd + md.template cast().eval() ); + rcd = mcd; + VERIFY_IS_APPROX( rcd -= md, mcd - md.template cast().eval() ); + rcd = mcd; + VERIFY_IS_APPROX( rcd.array() *= md.array(), mcd.array() * md.template cast().eval().array() ); + rcd = mcd; + if(md.array().abs().minCoeff()>epsd) + { + VERIFY_IS_APPROX( rcd.array() /= md.array(), mcd.array() / md.template cast().eval().array() ); + } + + rcd = mcd; + VERIFY_IS_APPROX( rcd.noalias() += md + mcd*md, mcd + (md.template cast().eval()) + mcd*(md.template cast().eval())); + + VERIFY_IS_APPROX( rcd.noalias() = md*md, ((md*md).eval().template cast()) ); + rcd = mcd; + VERIFY_IS_APPROX( rcd.noalias() += md*md, mcd + ((md*md).eval().template cast()) ); + rcd = mcd; + VERIFY_IS_APPROX( rcd.noalias() -= md*md, mcd - ((md*md).eval().template cast()) ); + + VERIFY_IS_APPROX( rcd.noalias() = mcd + md*md, mcd + ((md*md).eval().template cast()) ); + rcd = mcd; + VERIFY_IS_APPROX( rcd.noalias() += mcd + md*md, mcd + mcd + ((md*md).eval().template cast()) ); + rcd = mcd; + VERIFY_IS_APPROX( rcd.noalias() -= mcd + md*md, - ((md*md).eval().template cast()) ); } void test_mixingtypes() { - CALL_SUBTEST_1(mixingtypes<3>()); - CALL_SUBTEST_2(mixingtypes<4>()); - CALL_SUBTEST_3(mixingtypes(internal::random(1,EIGEN_TEST_MAX_SIZE))); + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1(mixingtypes<3>()); + CALL_SUBTEST_2(mixingtypes<4>()); + CALL_SUBTEST_3(mixingtypes(internal::random(1,EIGEN_TEST_MAX_SIZE))); + + CALL_SUBTEST_4(mixingtypes<3>()); + CALL_SUBTEST_5(mixingtypes<4>()); + CALL_SUBTEST_6(mixingtypes(internal::random(1,EIGEN_TEST_MAX_SIZE))); + } } diff --git a/ground/gcs/src/libs/eigen/test/mpl2only.cpp b/ground/gcs/src/libs/eigen/test/mpl2only.cpp index 5ef0d2b2e8..7d04d6bba6 100644 --- a/ground/gcs/src/libs/eigen/test/mpl2only.cpp +++ b/ground/gcs/src/libs/eigen/test/mpl2only.cpp @@ -12,7 +12,9 @@ #include #include #include +#include #include +#include int main() { diff --git a/ground/gcs/src/libs/eigen/test/nesting_ops.cpp b/ground/gcs/src/libs/eigen/test/nesting_ops.cpp index 1e85232833..a419b0e44a 100644 --- a/ground/gcs/src/libs/eigen/test/nesting_ops.cpp +++ b/ground/gcs/src/libs/eigen/test/nesting_ops.cpp @@ -2,16 +2,37 @@ // for linear algebra. // // Copyright (C) 2010 Hauke Heibel +// Copyright (C) 2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define TEST_ENABLE_TEMPORARY_TRACKING + #include "main.h" -template void run_nesting_ops(const MatrixType& _m) +template +void use_n_times(const XprType &xpr) { - typename MatrixType::Nested m(_m); + typename internal::nested_eval::type mat(xpr); + typename XprType::PlainObject res(mat.rows(), mat.cols()); + nb_temporaries--; // remove res + res.setZero(); + for(int i=0; i +bool verify_eval_type(const XprType &, const ReferenceType&) +{ + typedef typename internal::nested_eval::type EvalType; + return internal::is_same::type, typename internal::remove_all::type>::value; +} + +template void run_nesting_ops_1(const MatrixType& _m) +{ + typename internal::nested_eval::type m(_m); // Make really sure that we are in debug mode! VERIFY_RAISES_ASSERT(eigen_assert(false)); @@ -24,10 +45,63 @@ template void run_nesting_ops(const MatrixType& _m) VERIFY_IS_APPROX( (m.transpose() * m).array().abs().sum(), (m.transpose() * m).array().abs().sum() ); } +template void run_nesting_ops_2(const MatrixType& _m) +{ + typedef typename MatrixType::Scalar Scalar; + Index rows = _m.rows(); + Index cols = _m.cols(); + MatrixType m1 = MatrixType::Random(rows,cols); + Matrix m2; + + if((MatrixType::SizeAtCompileTime==Dynamic)) + { + VERIFY_EVALUATION_COUNT( use_n_times<1>(m1 + m1*m1), 1 ); + VERIFY_EVALUATION_COUNT( use_n_times<10>(m1 + m1*m1), 1 ); + + VERIFY_EVALUATION_COUNT( use_n_times<1>(m1.template triangularView().solve(m1.col(0))), 1 ); + VERIFY_EVALUATION_COUNT( use_n_times<10>(m1.template triangularView().solve(m1.col(0))), 1 ); + + VERIFY_EVALUATION_COUNT( use_n_times<1>(Scalar(2)*m1.template triangularView().solve(m1.col(0))), 2 ); // FIXME could be one by applying the scaling in-place on the solve result + VERIFY_EVALUATION_COUNT( use_n_times<1>(m1.col(0)+m1.template triangularView().solve(m1.col(0))), 2 ); // FIXME could be one by adding m1.col() inplace + VERIFY_EVALUATION_COUNT( use_n_times<10>(m1.col(0)+m1.template triangularView().solve(m1.col(0))), 2 ); + } + + { + VERIFY( verify_eval_type<10>(m1, m1) ); + if(!NumTraits::IsComplex) + { + VERIFY( verify_eval_type<3>(2*m1, 2*m1) ); + VERIFY( verify_eval_type<4>(2*m1, m1) ); + } + else + { + VERIFY( verify_eval_type<2>(2*m1, 2*m1) ); + VERIFY( verify_eval_type<3>(2*m1, m1) ); + } + VERIFY( verify_eval_type<2>(m1+m1, m1+m1) ); + VERIFY( verify_eval_type<3>(m1+m1, m1) ); + VERIFY( verify_eval_type<1>(m1*m1.transpose(), m2) ); + VERIFY( verify_eval_type<1>(m1*(m1+m1).transpose(), m2) ); + VERIFY( verify_eval_type<2>(m1*m1.transpose(), m2) ); + VERIFY( verify_eval_type<1>(m1+m1*m1, m1) ); + + VERIFY( verify_eval_type<1>(m1.template triangularView().solve(m1), m1) ); + VERIFY( verify_eval_type<1>(m1+m1.template triangularView().solve(m1), m1) ); + } +} + + void test_nesting_ops() { - CALL_SUBTEST_1(run_nesting_ops(MatrixXf::Random(25,25))); - CALL_SUBTEST_2(run_nesting_ops(MatrixXd::Random(25,25))); - CALL_SUBTEST_3(run_nesting_ops(Matrix4f::Random())); - CALL_SUBTEST_4(run_nesting_ops(Matrix4d::Random())); + CALL_SUBTEST_1(run_nesting_ops_1(MatrixXf::Random(25,25))); + CALL_SUBTEST_2(run_nesting_ops_1(MatrixXcd::Random(25,25))); + CALL_SUBTEST_3(run_nesting_ops_1(Matrix4f::Random())); + CALL_SUBTEST_4(run_nesting_ops_1(Matrix2d::Random())); + + Index s = internal::random(1,EIGEN_TEST_MAX_SIZE); + CALL_SUBTEST_1( run_nesting_ops_2(MatrixXf(s,s)) ); + CALL_SUBTEST_2( run_nesting_ops_2(MatrixXcd(s,s)) ); + CALL_SUBTEST_3( run_nesting_ops_2(Matrix4f()) ); + CALL_SUBTEST_4( run_nesting_ops_2(Matrix2d()) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } diff --git a/ground/gcs/src/libs/eigen/test/nomalloc.cpp b/ground/gcs/src/libs/eigen/test/nomalloc.cpp index 8e0402358e..50756c2fb4 100644 --- a/ground/gcs/src/libs/eigen/test/nomalloc.cpp +++ b/ground/gcs/src/libs/eigen/test/nomalloc.cpp @@ -8,20 +8,10 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. -// this hack is needed to make this file compiles with -pedantic (gcc) -#ifdef __GNUC__ -#define throw(X) -#endif - -#ifdef __INTEL_COMPILER - // disable "warning #76: argument to macro is empty" produced by the above hack - #pragma warning disable 76 -#endif - // discard stack allocation as that too bypasses malloc #define EIGEN_STACK_ALLOCATION_LIMIT 0 -// any heap allocation will raise an assert -#define EIGEN_NO_MALLOC +// heap allocation will raise an assert if enabled at runtime +#define EIGEN_RUNTIME_NO_MALLOC #include "main.h" #include @@ -88,14 +78,15 @@ template void nomalloc(const MatrixType& m) VERIFY_IS_APPROX(m2,m2); m2.template selfadjointView().rankUpdate(m1.col(0),-1); - m2.template selfadjointView().rankUpdate(m1.row(0),-1); + m2.template selfadjointView().rankUpdate(m1.row(0),-1); + m2.template selfadjointView().rankUpdate(m1.col(0), m1.col(0)); // rank-2 // The following fancy matrix-matrix products are not safe yet regarding static allocation -// m1 += m1.template triangularView() * m2.col(; -// m1.template selfadjointView().rankUpdate(m2); -// m1 += m1.template triangularView() * m2; -// m1 += m1.template selfadjointView() * m2; -// VERIFY_IS_APPROX(m1,m1); + m2.template selfadjointView().rankUpdate(m1); + m2 += m2.template triangularView() * m1; + m2.template triangularView() = m2 * m2; + m1 += m1.template selfadjointView() * m2; + VERIFY_IS_APPROX(m2,m2); } template @@ -171,7 +162,7 @@ void test_zerosized() { Eigen::VectorXd v; // explicit zero-sized: Eigen::ArrayXXd A0(0,0); - Eigen::ArrayXd v0(std::ptrdiff_t(0)); // FIXME ArrayXd(0) is ambiguous + Eigen::ArrayXd v0(0); // assigning empty objects to each other: A=A0; @@ -183,9 +174,11 @@ template void test_reference(const MatrixType& m) { enum { Flag = MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; enum { TransposeFlag = !MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; typename MatrixType::Index rows = m.rows(), cols=m.cols(); + typedef Eigen::Matrix MatrixX; + typedef Eigen::Matrix MatrixXT; // Dynamic reference: - typedef Eigen::Ref > Ref; - typedef Eigen::Ref > RefT; + typedef Eigen::Ref Ref; + typedef Eigen::Ref RefT; Ref r1(m); Ref r2(m.block(rows/3, cols/4, rows/2, cols/2)); @@ -195,10 +188,30 @@ template void test_reference(const MatrixType& m) { VERIFY_RAISES_ASSERT(RefT r5(m)); VERIFY_RAISES_ASSERT(Ref r6(m.transpose())); VERIFY_RAISES_ASSERT(Ref r7(Scalar(2) * m)); + + // Copy constructors shall also never malloc + Ref r8 = r1; + RefT r9 = r3; + + // Initializing from a compatible Ref shall also never malloc + Eigen::Ref > r10=r8, r11=m; + + // Initializing from an incompatible Ref will malloc: + typedef Eigen::Ref RefAligned; + VERIFY_RAISES_ASSERT(RefAligned r12=r10); + VERIFY_RAISES_ASSERT(Ref r13=r10); // r10 has more dynamic strides + } void test_nomalloc() { + // create some dynamic objects + Eigen::MatrixXd M1 = MatrixXd::Random(3,3); + Ref R1 = 2.0*M1; // Ref requires temporary + + // from here on prohibit malloc: + Eigen::internal::set_is_malloc_allowed(false); + // check that our operator new is indeed called: VERIFY_RAISES_ASSERT(MatrixXd dummy(MatrixXd::Random(3,3))); CALL_SUBTEST_1(nomalloc(Matrix()) ); @@ -207,6 +220,10 @@ void test_nomalloc() // Check decomposition modules with dynamic matrices that have a known compile-time max size (ctms) CALL_SUBTEST_4(ctms_decompositions()); + CALL_SUBTEST_5(test_zerosized()); + CALL_SUBTEST_6(test_reference(Matrix())); + CALL_SUBTEST_7(test_reference(R1)); + CALL_SUBTEST_8(Ref R2 = M1.topRows<2>(); test_reference(R2)); } diff --git a/ground/gcs/src/libs/eigen/test/nullary.cpp b/ground/gcs/src/libs/eigen/test/nullary.cpp index fbc721a1a1..acd55506e9 100644 --- a/ground/gcs/src/libs/eigen/test/nullary.cpp +++ b/ground/gcs/src/libs/eigen/test/nullary.cpp @@ -2,6 +2,7 @@ // for linear algebra. // // Copyright (C) 2010-2011 Jitse Niesen +// Copyright (C) 2016 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -12,7 +13,6 @@ template bool equalsIdentity(const MatrixType& A) { - typedef typename MatrixType::Index Index; typedef typename MatrixType::Scalar Scalar; Scalar zero = static_cast(0); @@ -30,13 +30,41 @@ bool equalsIdentity(const MatrixType& A) bool diagOK = (A.diagonal().array() == 1).all(); return offDiagOK && diagOK; + +} + +template +void check_extremity_accuracy(const VectorType &v, const typename VectorType::Scalar &low, const typename VectorType::Scalar &high) +{ + typedef typename VectorType::Scalar Scalar; + typedef typename VectorType::RealScalar RealScalar; + + RealScalar prec = internal::is_same::value ? NumTraits::dummy_precision()*10 : NumTraits::dummy_precision()/10; + Index size = v.size(); + + if(size<20) + return; + + for (int i=0; isize-6) + { + Scalar ref = (low*RealScalar(size-i-1))/RealScalar(size-1) + (high*RealScalar(i))/RealScalar(size-1); + if(std::abs(ref)>1) + { + if(!internal::isApprox(v(i), ref, prec)) + std::cout << v(i) << " != " << ref << " ; relative error: " << std::abs((v(i)-ref)/ref) << " ; required precision: " << prec << " ; range: " << low << "," << high << " ; i: " << i << "\n"; + VERIFY(internal::isApprox(v(i), (low*RealScalar(size-i-1))/RealScalar(size-1) + (high*RealScalar(i))/RealScalar(size-1), prec)); + } + } + } } template void testVectorType(const VectorType& base) { - typedef typename internal::traits::Index Index; - typedef typename internal::traits::Scalar Scalar; + typedef typename VectorType::Scalar Scalar; + typedef typename VectorType::RealScalar RealScalar; const Index size = base.size(); @@ -44,36 +72,61 @@ void testVectorType(const VectorType& base) Scalar low = (size == 1 ? high : internal::random(-500,500)); if (low>high) std::swap(low,high); + // check low==high + if(internal::random(0.f,1.f)<0.05f) + low = high; + // check abs(low) >> abs(high) + else if(size>2 && std::numeric_limits::max_exponent10>0 && internal::random(0.f,1.f)<0.1f) + low = -internal::random(1,2) * RealScalar(std::pow(RealScalar(10),std::numeric_limits::max_exponent10/2)); + const Scalar step = ((size == 1) ? 1 : (high-low)/(size-1)); // check whether the result yields what we expect it to do VectorType m(base); m.setLinSpaced(size,low,high); - VectorType n(size); - for (int i=0; i::IsInteger) + { + VectorType n(size); + for (int i=0; i::IsInteger) || ((high-low)>=size && (Index(high-low)%(size-1))==0) || (Index(high-low+1)::IsInteger) || (high-low>=size)) + for (int i=0; i::epsilon() ); + // random access version + m = VectorType::LinSpaced(size,low,high); + VERIFY_IS_APPROX(m,n); + VERIFY( internal::isApprox(m(m.size()-1),high) ); + VERIFY( size==1 || internal::isApprox(m(0),low) ); + VERIFY_IS_EQUAL(m(m.size()-1) , high); + if(!NumTraits::IsInteger) + CALL_SUBTEST( check_extremity_accuracy(m, low, high) ); + } - // These guys sometimes fail! This is not good. Any ideas how to fix them!? - //VERIFY( m(m.size()-1) == high ); - //VERIFY( m(0) == low ); + VERIFY( m(m.size()-1) <= high ); + VERIFY( (m.array() <= high).all() ); + VERIFY( (m.array() >= low).all() ); - // sequential access version - m = VectorType::LinSpaced(Sequential,size,low,high); - VERIFY_IS_APPROX(m,n); - // These guys sometimes fail! This is not good. Any ideas how to fix them!? - //VERIFY( m(m.size()-1) == high ); - //VERIFY( m(0) == low ); + VERIFY( m(m.size()-1) >= low ); + if(size>=1) + { + VERIFY( internal::isApprox(m(0),low) ); + VERIFY_IS_EQUAL(m(0) , low); + } // check whether everything works with row and col major vectors Matrix row_vector(size); @@ -95,23 +148,77 @@ void testVectorType(const VectorType& base) VERIFY_IS_APPROX( ScalarMatrix::LinSpaced(1,low,high), ScalarMatrix::Constant(high) ); // regression test for bug 526 (linear vectorized transversal) - if (size > 1) { + if (size > 1 && (!NumTraits::IsInteger)) { m.tail(size-1).setLinSpaced(low, high); VERIFY_IS_APPROX(m(size-1), high); } + + // regression test for bug 1383 (LinSpaced with empty size/range) + { + Index n0 = VectorType::SizeAtCompileTime==Dynamic ? 0 : VectorType::SizeAtCompileTime; + low = internal::random(); + m = VectorType::LinSpaced(n0,low,low-1); + VERIFY(m.size()==n0); + + if(VectorType::SizeAtCompileTime==Dynamic) + { + VERIFY_IS_EQUAL(VectorType::LinSpaced(n0,0,Scalar(n0-1)).sum(),Scalar(0)); + VERIFY_IS_EQUAL(VectorType::LinSpaced(n0,low,low-1).sum(),Scalar(0)); + } + + m.setLinSpaced(n0,0,Scalar(n0-1)); + VERIFY(m.size()==n0); + m.setLinSpaced(n0,low,low-1); + VERIFY(m.size()==n0); + + // empty range only: + VERIFY_IS_APPROX(VectorType::LinSpaced(size,low,low),VectorType::Constant(size,low)); + m.setLinSpaced(size,low,low); + VERIFY_IS_APPROX(m,VectorType::Constant(size,low)); + + if(NumTraits::IsInteger) + { + VERIFY_IS_APPROX( VectorType::LinSpaced(size,low,Scalar(low+size-1)), VectorType::LinSpaced(size,Scalar(low+size-1),low).reverse() ); + + if(VectorType::SizeAtCompileTime==Dynamic) + { + // Check negative multiplicator path: + for(Index k=1; k<5; ++k) + VERIFY_IS_APPROX( VectorType::LinSpaced(size,low,Scalar(low+(size-1)*k)), VectorType::LinSpaced(size,Scalar(low+(size-1)*k),low).reverse() ); + // Check negative divisor path: + for(Index k=1; k<5; ++k) + VERIFY_IS_APPROX( VectorType::LinSpaced(size*k,low,Scalar(low+size-1)), VectorType::LinSpaced(size*k,Scalar(low+size-1),low).reverse() ); + } + } + } } template void testMatrixType(const MatrixType& m) { - typedef typename MatrixType::Index Index; + using std::abs; const Index rows = m.rows(); const Index cols = m.cols(); + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + + Scalar s1; + do { + s1 = internal::random(); + } while(abs(s1)::IsInteger)); MatrixType A; A.setIdentity(rows, cols); VERIFY(equalsIdentity(A)); VERIFY(equalsIdentity(MatrixType::Identity(rows, cols))); + + + A = MatrixType::Constant(rows,cols,s1); + Index i = internal::random(0,rows-1); + Index j = internal::random(0,cols-1); + VERIFY_IS_APPROX( MatrixType::Constant(rows,cols,s1)(i,j), s1 ); + VERIFY_IS_APPROX( MatrixType::Constant(rows,cols,s1).coeff(i,j), s1 ); + VERIFY_IS_APPROX( A(i,j), s1 ); } void test_nullary() @@ -120,12 +227,78 @@ void test_nullary() CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random(1,300),internal::random(1,300))) ); CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random(1,300),internal::random(1,300))) ); - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_4( testVectorType(VectorXd(internal::random(1,300))) ); + for(int i = 0; i < g_repeat*10; i++) { + CALL_SUBTEST_4( testVectorType(VectorXd(internal::random(1,30000))) ); CALL_SUBTEST_5( testVectorType(Vector4d()) ); // regression test for bug 232 CALL_SUBTEST_6( testVectorType(Vector3d()) ); - CALL_SUBTEST_7( testVectorType(VectorXf(internal::random(1,300))) ); + CALL_SUBTEST_7( testVectorType(VectorXf(internal::random(1,30000))) ); CALL_SUBTEST_8( testVectorType(Vector3f()) ); + CALL_SUBTEST_8( testVectorType(Vector4f()) ); + CALL_SUBTEST_8( testVectorType(Matrix()) ); CALL_SUBTEST_8( testVectorType(Matrix()) ); + + CALL_SUBTEST_9( testVectorType(VectorXi(internal::random(1,10))) ); + CALL_SUBTEST_9( testVectorType(VectorXi(internal::random(9,300))) ); + CALL_SUBTEST_9( testVectorType(Matrix()) ); + } + +#ifdef EIGEN_TEST_PART_6 + // Assignment of a RowVectorXd to a MatrixXd (regression test for bug #79). + VERIFY( (MatrixXd(RowVectorXd::LinSpaced(3, 0, 1)) - RowVector3d(0, 0.5, 1)).norm() < std::numeric_limits::epsilon() ); +#endif + +#ifdef EIGEN_TEST_PART_9 + // Check possible overflow issue + { + int n = 60000; + ArrayXi a1(n), a2(n); + a1.setLinSpaced(n, 0, n-1); + for(int i=0; i >::value )); + VERIFY(( !internal::has_unary_operator >::value )); + VERIFY(( !internal::has_binary_operator >::value )); + VERIFY(( internal::functor_has_linear_access >::ret )); + + VERIFY(( !internal::has_nullary_operator >::value )); + VERIFY(( !internal::has_unary_operator >::value )); + VERIFY(( internal::has_binary_operator >::value )); + VERIFY(( !internal::functor_has_linear_access >::ret )); + + VERIFY(( !internal::has_nullary_operator >::value )); + VERIFY(( internal::has_unary_operator >::value )); + VERIFY(( !internal::has_binary_operator >::value )); + VERIFY(( internal::functor_has_linear_access >::ret )); + + // Regression unit test for a weird MSVC bug. + // Search "nullary_wrapper_workaround_msvc" in CoreEvaluators.h for the details. + // See also traits::match. + { + MatrixXf A = MatrixXf::Random(3,3); + Ref R = 2.0*A; + VERIFY_IS_APPROX(R, A+A); + + Ref R1 = MatrixXf::Random(3,3)+A; + + VectorXi V = VectorXi::Random(3); + Ref R2 = VectorXi::LinSpaced(3,1,3)+V; + VERIFY_IS_APPROX(R2, V+Vector3i(1,2,3)); + + VERIFY(( internal::has_nullary_operator >::value )); + VERIFY(( !internal::has_unary_operator >::value )); + VERIFY(( !internal::has_binary_operator >::value )); + VERIFY(( internal::functor_has_linear_access >::ret )); + + VERIFY(( !internal::has_nullary_operator >::value )); + VERIFY(( internal::has_unary_operator >::value )); + VERIFY(( !internal::has_binary_operator >::value )); + VERIFY(( internal::functor_has_linear_access >::ret )); } +#endif } diff --git a/ground/gcs/src/libs/eigen/test/packetmath.cpp b/ground/gcs/src/libs/eigen/test/packetmath.cpp index 38aa256ce8..7821a1738d 100644 --- a/ground/gcs/src/libs/eigen/test/packetmath.cpp +++ b/ground/gcs/src/libs/eigen/test/packetmath.cpp @@ -9,16 +9,28 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "main.h" +#include "unsupported/Eigen/SpecialFunctions" +#if defined __GNUC__ && __GNUC__>=6 + #pragma GCC diagnostic ignored "-Wignored-attributes" +#endif // using namespace Eigen; +#ifdef EIGEN_VECTORIZE_SSE +const bool g_vectorize_sse = true; +#else +const bool g_vectorize_sse = false; +#endif + namespace Eigen { namespace internal { template T negate(const T& x) { return -x; } } } -template bool isApproxAbs(const Scalar& a, const Scalar& b, const typename NumTraits::Real& refvalue) +// NOTE: we disbale inlining for this function to workaround a GCC issue when using -O3 and the i387 FPU. +template EIGEN_DONT_INLINE +bool isApproxAbs(const Scalar& a, const Scalar& b, const typename NumTraits::Real& refvalue) { return internal::isMuchSmallerThan(a-b, refvalue); } @@ -29,7 +41,7 @@ template bool areApproxAbs(const Scalar* a, const Scalar* b, in { if (!isApproxAbs(a[i],b[i],refvalue)) { - std::cout << "[" << Map >(a,size) << "]" << " != " << Map >(b,size) << "\n"; + std::cout << "ref: [" << Map >(a,size) << "]" << " != vec: [" << Map >(b,size) << "]\n"; return false; } } @@ -42,21 +54,13 @@ template bool areApprox(const Scalar* a, const Scalar* b, int s { if (a[i]!=b[i] && !internal::isApprox(a[i],b[i])) { - std::cout << "[" << Map >(a,size) << "]" << " != " << Map >(b,size) << "\n"; + std::cout << "ref: [" << Map >(a,size) << "]" << " != vec: [" << Map >(b,size) << "]\n"; return false; } } return true; } - -#define CHECK_CWISE2(REFOP, POP) { \ - for (int i=0; i(data1), internal::pload(data1+PacketSize))); \ - VERIFY(areApprox(ref, data2, PacketSize) && #POP); \ -} - #define CHECK_CWISE1(REFOP, POP) { \ for (int i=0; i VERIFY(areApprox(ref, data2, PacketSize) && #POP); \ } +#define CHECK_CWISE2_IF(COND, REFOP, POP) if(COND) { \ + packet_helper h; \ + for (int i=0; i template void packetmath() { using std::abs; - typedef typename internal::packet_traits::type Packet; - const int PacketSize = internal::packet_traits::size; + typedef internal::packet_traits PacketTraits; + typedef typename PacketTraits::type Packet; + const int PacketSize = PacketTraits::size; typedef typename NumTraits::Real RealScalar; - const int size = PacketSize*4; - EIGEN_ALIGN16 Scalar data1[internal::packet_traits::size*4]; - EIGEN_ALIGN16 Scalar data2[internal::packet_traits::size*4]; - EIGEN_ALIGN16 Packet packets[PacketSize*2]; - EIGEN_ALIGN16 Scalar ref[internal::packet_traits::size*4]; + const int max_size = PacketSize > 4 ? PacketSize : 4; + const int size = PacketSize*max_size; + EIGEN_ALIGN_MAX Scalar data1[size]; + EIGEN_ALIGN_MAX Scalar data2[size]; + EIGEN_ALIGN_MAX Packet packets[PacketSize*2]; + EIGEN_ALIGN_MAX Scalar ref[size]; RealScalar refvalue = 0; for (int i=0; i void packetmath() else if (offset==1) internal::palign<1>(packets[0], packets[1]); else if (offset==2) internal::palign<2>(packets[0], packets[1]); else if (offset==3) internal::palign<3>(packets[0], packets[1]); + else if (offset==4) internal::palign<4>(packets[0], packets[1]); + else if (offset==5) internal::palign<5>(packets[0], packets[1]); + else if (offset==6) internal::palign<6>(packets[0], packets[1]); + else if (offset==7) internal::palign<7>(packets[0], packets[1]); + else if (offset==8) internal::palign<8>(packets[0], packets[1]); + else if (offset==9) internal::palign<9>(packets[0], packets[1]); + else if (offset==10) internal::palign<10>(packets[0], packets[1]); + else if (offset==11) internal::palign<11>(packets[0], packets[1]); + else if (offset==12) internal::palign<12>(packets[0], packets[1]); + else if (offset==13) internal::palign<13>(packets[0], packets[1]); + else if (offset==14) internal::palign<14>(packets[0], packets[1]); + else if (offset==15) internal::palign<15>(packets[0], packets[1]); internal::pstore(data2, packets[0]); for (int i=0; i void packetmath() VERIFY(areApprox(ref, data2, PacketSize) && "internal::palign"); } - CHECK_CWISE2(REF_ADD, internal::padd); - CHECK_CWISE2(REF_SUB, internal::psub); - CHECK_CWISE2(REF_MUL, internal::pmul); - #ifndef EIGEN_VECTORIZE_ALTIVEC - if (!internal::is_same::value) - CHECK_CWISE2(REF_DIV, internal::pdiv); - #endif + VERIFY((!PacketTraits::Vectorizable) || PacketTraits::HasAdd); + VERIFY((!PacketTraits::Vectorizable) || PacketTraits::HasSub); + VERIFY((!PacketTraits::Vectorizable) || PacketTraits::HasMul); + VERIFY((!PacketTraits::Vectorizable) || PacketTraits::HasNegate); + VERIFY((internal::is_same::value) || (!PacketTraits::Vectorizable) || PacketTraits::HasDiv); + + CHECK_CWISE2_IF(PacketTraits::HasAdd, REF_ADD, internal::padd); + CHECK_CWISE2_IF(PacketTraits::HasSub, REF_SUB, internal::psub); + CHECK_CWISE2_IF(PacketTraits::HasMul, REF_MUL, internal::pmul); + CHECK_CWISE2_IF(PacketTraits::HasDiv, REF_DIV, internal::pdiv); + CHECK_CWISE1(internal::negate, internal::pnegate); CHECK_CWISE1(numext::conj, internal::pconj); @@ -165,9 +195,31 @@ template void packetmath() internal::pstore(data2, internal::pset1(data1[offset])); VERIFY(areApprox(ref, data2, PacketSize) && "internal::pset1"); } - + + { + for (int i=0; i(data1, A0, A1, A2, A3); + internal::pstore(data2+0*PacketSize, A0); + internal::pstore(data2+1*PacketSize, A1); + internal::pstore(data2+2*PacketSize, A2); + internal::pstore(data2+3*PacketSize, A3); + VERIFY(areApprox(ref, data2, 4*PacketSize) && "internal::pbroadcast4"); + } + + { + for (int i=0; i(data1, A0, A1); + internal::pstore(data2+0*PacketSize, A0); + internal::pstore(data2+1*PacketSize, A1); + VERIFY(areApprox(ref, data2, 2*PacketSize) && "internal::pbroadcast2"); + } + VERIFY(internal::isApprox(data1[0], internal::pfirst(internal::pload(data1))) && "internal::pfirst"); - + if(PacketSize>1) { for(int offset=0;offset<4;++offset) @@ -179,11 +231,31 @@ template void packetmath() } } + if(PacketSize>2) + { + for(int offset=0;offset<4;++offset) + { + for(int i=0;i(data1+offset)); + VERIFY(areApprox(ref, data2, PacketSize) && "ploadquad"); + } + } + ref[0] = 0; for (int i=0; i(data1)), refvalue) && "internal::predux"); + { + for (int i=0; i<4; ++i) + ref[i] = 0; + for (int i=0; i(data1))); + VERIFY(areApprox(ref, data2, PacketSize>4?PacketSize/2:PacketSize) && "internal::predux_downto4"); + } + ref[0] = 1; for (int i=0; i void packetmath() ref[i] = data1[PacketSize-i-1]; internal::pstore(data2, internal::preverse(internal::pload(data1))); VERIFY(areApprox(ref, data2, PacketSize) && "internal::preverse"); + + internal::PacketBlock kernel; + for (int i=0; i(data1+i*PacketSize); + } + ptranspose(kernel); + for (int i=0; i(data1); + Packet elsePacket = internal::pload(data2); + EIGEN_ALIGN_MAX internal::Selector selector; + for (int i = 0; i < PacketSize; ++i) { + selector.select[i] = i; + } + + Packet blend = internal::pblend(selector, thenPacket, elsePacket); + EIGEN_ALIGN_MAX Scalar result[size]; + internal::pstore(result, blend); + for (int i = 0; i < PacketSize; ++i) { + VERIFY(isApproxAbs(result[i], (selector.select[i] ? data1[i] : data2[i]), refvalue)); + } + } + + if (PacketTraits::HasBlend || g_vectorize_sse) { + // pinsertfirst + for (int i=0; i(); + ref[0] = s; + internal::pstore(data2, internal::pinsertfirst(internal::pload(data1),s)); + VERIFY(areApprox(ref, data2, PacketSize) && "internal::pinsertfirst"); + } + + if (PacketTraits::HasBlend || g_vectorize_sse) { + // pinsertlast + for (int i=0; i(); + ref[PacketSize-1] = s; + internal::pstore(data2, internal::pinsertlast(internal::pload(data1),s)); + VERIFY(areApprox(ref, data2, PacketSize) && "internal::pinsertlast"); + } } template void packetmath_real() { using std::abs; - typedef typename internal::packet_traits::type Packet; - const int PacketSize = internal::packet_traits::size; + typedef internal::packet_traits PacketTraits; + typedef typename PacketTraits::type Packet; + const int PacketSize = PacketTraits::size; const int size = PacketSize*4; - EIGEN_ALIGN16 Scalar data1[internal::packet_traits::size*4]; - EIGEN_ALIGN16 Scalar data2[internal::packet_traits::size*4]; - EIGEN_ALIGN16 Scalar ref[internal::packet_traits::size*4]; + EIGEN_ALIGN_MAX Scalar data1[PacketTraits::size*4]; + EIGEN_ALIGN_MAX Scalar data2[PacketTraits::size*4]; + EIGEN_ALIGN_MAX Scalar ref[PacketTraits::size*4]; for (int i=0; i(-1,1) * std::pow(Scalar(10), internal::random(-3,3)); data2[i] = internal::random(-1,1) * std::pow(Scalar(10), internal::random(-3,3)); } - CHECK_CWISE1_IF(internal::packet_traits::HasSin, std::sin, internal::psin); - CHECK_CWISE1_IF(internal::packet_traits::HasCos, std::cos, internal::pcos); - CHECK_CWISE1_IF(internal::packet_traits::HasTan, std::tan, internal::ptan); - + CHECK_CWISE1_IF(PacketTraits::HasSin, std::sin, internal::psin); + CHECK_CWISE1_IF(PacketTraits::HasCos, std::cos, internal::pcos); + CHECK_CWISE1_IF(PacketTraits::HasTan, std::tan, internal::ptan); + + CHECK_CWISE1_IF(PacketTraits::HasRound, numext::round, internal::pround); + CHECK_CWISE1_IF(PacketTraits::HasCeil, numext::ceil, internal::pceil); + CHECK_CWISE1_IF(PacketTraits::HasFloor, numext::floor, internal::pfloor); + for (int i=0; i(-1,1); data2[i] = internal::random(-1,1); } - CHECK_CWISE1_IF(internal::packet_traits::HasASin, std::asin, internal::pasin); - CHECK_CWISE1_IF(internal::packet_traits::HasACos, std::acos, internal::pacos); + CHECK_CWISE1_IF(PacketTraits::HasASin, std::asin, internal::pasin); + CHECK_CWISE1_IF(PacketTraits::HasACos, std::acos, internal::pacos); for (int i=0; i(-87,88); data2[i] = internal::random(-87,88); } - CHECK_CWISE1_IF(internal::packet_traits::HasExp, std::exp, internal::pexp); + CHECK_CWISE1_IF(PacketTraits::HasExp, std::exp, internal::pexp); + for (int i=0; i(-1,1) * std::pow(Scalar(10), internal::random(-6,6)); + data2[i] = internal::random(-1,1) * std::pow(Scalar(10), internal::random(-6,6)); + } + CHECK_CWISE1_IF(PacketTraits::HasTanh, std::tanh, internal::ptanh); + if(PacketTraits::HasExp && PacketTraits::size>=2) + { + data1[0] = std::numeric_limits::quiet_NaN(); + data1[1] = std::numeric_limits::epsilon(); + packet_helper h; + h.store(data2, internal::pexp(h.load(data1))); + VERIFY((numext::isnan)(data2[0])); + VERIFY_IS_EQUAL(std::exp(std::numeric_limits::epsilon()), data2[1]); + + data1[0] = -std::numeric_limits::epsilon(); + data1[1] = 0; + h.store(data2, internal::pexp(h.load(data1))); + VERIFY_IS_EQUAL(std::exp(-std::numeric_limits::epsilon()), data2[0]); + VERIFY_IS_EQUAL(std::exp(Scalar(0)), data2[1]); + + data1[0] = (std::numeric_limits::min)(); + data1[1] = -(std::numeric_limits::min)(); + h.store(data2, internal::pexp(h.load(data1))); + VERIFY_IS_EQUAL(std::exp((std::numeric_limits::min)()), data2[0]); + VERIFY_IS_EQUAL(std::exp(-(std::numeric_limits::min)()), data2[1]); + + data1[0] = std::numeric_limits::denorm_min(); + data1[1] = -std::numeric_limits::denorm_min(); + h.store(data2, internal::pexp(h.load(data1))); + VERIFY_IS_EQUAL(std::exp(std::numeric_limits::denorm_min()), data2[0]); + VERIFY_IS_EQUAL(std::exp(-std::numeric_limits::denorm_min()), data2[1]); + } + + if (PacketTraits::HasTanh) { + // NOTE this test migh fail with GCC prior to 6.3, see MathFunctionsImpl.h for details. + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasTanh,Packet> h; + h.store(data2, internal::ptanh(h.load(data1))); + VERIFY((numext::isnan)(data2[0])); + } + +#if EIGEN_HAS_C99_MATH + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasLGamma,Packet> h; + h.store(data2, internal::plgamma(h.load(data1))); + VERIFY((numext::isnan)(data2[0])); + } { data1[0] = std::numeric_limits::quiet_NaN(); - packet_helper::HasExp,Packet> h; - h.store(data2, internal::pexp(h.load(data1))); - VERIFY(isNaN(data2[0])); + packet_helper::HasErf,Packet> h; + h.store(data2, internal::perf(h.load(data1))); + VERIFY((numext::isnan)(data2[0])); } + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasErfc,Packet> h; + h.store(data2, internal::perfc(h.load(data1))); + VERIFY((numext::isnan)(data2[0])); + } +#endif // EIGEN_HAS_C99_MATH for (int i=0; i(0,1) * std::pow(Scalar(10), internal::random(-6,6)); data2[i] = internal::random(0,1) * std::pow(Scalar(10), internal::random(-6,6)); } - if(internal::random(0,1)<0.1) + + if(internal::random(0,1)<0.1f) data1[internal::random(0, PacketSize)] = 0; - CHECK_CWISE1_IF(internal::packet_traits::HasSqrt, std::sqrt, internal::psqrt); - CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); + CHECK_CWISE1_IF(PacketTraits::HasSqrt, std::sqrt, internal::psqrt); + CHECK_CWISE1_IF(PacketTraits::HasLog, std::log, internal::plog); +#if EIGEN_HAS_C99_MATH && (__cplusplus > 199711L) + CHECK_CWISE1_IF(PacketTraits::HasLog1p, std::log1p, internal::plog1p); + CHECK_CWISE1_IF(internal::packet_traits::HasLGamma, std::lgamma, internal::plgamma); + CHECK_CWISE1_IF(internal::packet_traits::HasErf, std::erf, internal::perf); + CHECK_CWISE1_IF(internal::packet_traits::HasErfc, std::erfc, internal::perfc); +#endif + + if(PacketTraits::HasLog && PacketTraits::size>=2) { data1[0] = std::numeric_limits::quiet_NaN(); - packet_helper::HasLog,Packet> h; + data1[1] = std::numeric_limits::epsilon(); + packet_helper h; h.store(data2, internal::plog(h.load(data1))); - VERIFY(isNaN(data2[0])); - data1[0] = -1.0f; + VERIFY((numext::isnan)(data2[0])); + VERIFY_IS_EQUAL(std::log(std::numeric_limits::epsilon()), data2[1]); + + data1[0] = -std::numeric_limits::epsilon(); + data1[1] = 0; + h.store(data2, internal::plog(h.load(data1))); + VERIFY((numext::isnan)(data2[0])); + VERIFY_IS_EQUAL(std::log(Scalar(0)), data2[1]); + + data1[0] = (std::numeric_limits::min)(); + data1[1] = -(std::numeric_limits::min)(); + h.store(data2, internal::plog(h.load(data1))); + VERIFY_IS_EQUAL(std::log((std::numeric_limits::min)()), data2[0]); + VERIFY((numext::isnan)(data2[1])); + + data1[0] = std::numeric_limits::denorm_min(); + data1[1] = -std::numeric_limits::denorm_min(); + h.store(data2, internal::plog(h.load(data1))); + // VERIFY_IS_EQUAL(std::log(std::numeric_limits::denorm_min()), data2[0]); + VERIFY((numext::isnan)(data2[1])); + + data1[0] = Scalar(-1.0f); h.store(data2, internal::plog(h.load(data1))); - VERIFY(isNaN(data2[0])); -#if !EIGEN_FAST_MATH + VERIFY((numext::isnan)(data2[0])); h.store(data2, internal::psqrt(h.load(data1))); - VERIFY(isNaN(data2[0])); - VERIFY(isNaN(data2[1])); -#endif + VERIFY((numext::isnan)(data2[0])); + VERIFY((numext::isnan)(data2[1])); } } template void packetmath_notcomplex() { using std::abs; - typedef typename internal::packet_traits::type Packet; - const int PacketSize = internal::packet_traits::size; + typedef internal::packet_traits PacketTraits; + typedef typename PacketTraits::type Packet; + const int PacketSize = PacketTraits::size; + + EIGEN_ALIGN_MAX Scalar data1[PacketTraits::size*4]; + EIGEN_ALIGN_MAX Scalar data2[PacketTraits::size*4]; + EIGEN_ALIGN_MAX Scalar ref[PacketTraits::size*4]; - EIGEN_ALIGN16 Scalar data1[internal::packet_traits::size*4]; - EIGEN_ALIGN16 Scalar data2[internal::packet_traits::size*4]; - EIGEN_ALIGN16 Scalar ref[internal::packet_traits::size*4]; - - Array::Map(data1, internal::packet_traits::size*4).setRandom(); + Array::Map(data1, PacketTraits::size*4).setRandom(); ref[0] = data1[0]; for (int i=0; i(data1))) && "internal::predux_min"); - CHECK_CWISE2((std::min), internal::pmin); - CHECK_CWISE2((std::max), internal::pmax); + VERIFY((!PacketTraits::Vectorizable) || PacketTraits::HasMin); + VERIFY((!PacketTraits::Vectorizable) || PacketTraits::HasMax); + + CHECK_CWISE2_IF(PacketTraits::HasMin, (std::min), internal::pmin); + CHECK_CWISE2_IF(PacketTraits::HasMax, (std::max), internal::pmax); CHECK_CWISE1(abs, internal::pabs); ref[0] = data1[0]; for (int i=0; i(data1))) && "internal::predux_max"); - + for (int i=0; i(data1[0])); VERIFY(areApprox(ref, data2, PacketSize) && "internal::plset"); } template void test_conj_helper(Scalar* data1, Scalar* data2, Scalar* ref, Scalar* pval) { - typedef typename internal::packet_traits::type Packet; - const int PacketSize = internal::packet_traits::size; - + typedef internal::packet_traits PacketTraits; + typedef typename PacketTraits::type Packet; + const int PacketSize = PacketTraits::size; + internal::conj_if cj0; internal::conj_if cj1; internal::conj_helper cj; internal::conj_helper pcj; - + for(int i=0;i void test_conj_helper(Scalar } internal::pstore(pval,pcj.pmul(internal::pload(data1),internal::pload(data2))); VERIFY(areApprox(ref, pval, PacketSize) && "conj_helper pmul"); - + for(int i=0;i void test_conj_helper(Scalar template void packetmath_complex() { - typedef typename internal::packet_traits::type Packet; - const int PacketSize = internal::packet_traits::size; + typedef internal::packet_traits PacketTraits; + typedef typename PacketTraits::type Packet; + const int PacketSize = PacketTraits::size; const int size = PacketSize*4; - EIGEN_ALIGN16 Scalar data1[PacketSize*4]; - EIGEN_ALIGN16 Scalar data2[PacketSize*4]; - EIGEN_ALIGN16 Scalar ref[PacketSize*4]; - EIGEN_ALIGN16 Scalar pval[PacketSize*4]; + EIGEN_ALIGN_MAX Scalar data1[PacketSize*4]; + EIGEN_ALIGN_MAX Scalar data2[PacketSize*4]; + EIGEN_ALIGN_MAX Scalar ref[PacketSize*4]; + EIGEN_ALIGN_MAX Scalar pval[PacketSize*4]; for (int i=0; i() * Scalar(1e2); data2[i] = internal::random() * Scalar(1e2); } - + test_conj_helper (data1,data2,ref,pval); test_conj_helper (data1,data2,ref,pval); test_conj_helper (data1,data2,ref,pval); test_conj_helper (data1,data2,ref,pval); - + { for(int i=0;i(data1))); VERIFY(areApprox(ref, pval, PacketSize) && "pcplxflip"); } - - +} + +template void packetmath_scatter_gather() +{ + typedef internal::packet_traits PacketTraits; + typedef typename PacketTraits::type Packet; + typedef typename NumTraits::Real RealScalar; + const int PacketSize = PacketTraits::size; + EIGEN_ALIGN_MAX Scalar data1[PacketSize]; + RealScalar refvalue = 0; + for (int i=0; i()/RealScalar(PacketSize); + } + + int stride = internal::random(1,20); + + EIGEN_ALIGN_MAX Scalar buffer[PacketSize*20]; + memset(buffer, 0, 20*PacketSize*sizeof(Scalar)); + Packet packet = internal::pload(data1); + internal::pscatter(buffer, packet, stride); + + for (int i = 0; i < PacketSize*20; ++i) { + if ((i%stride) == 0 && i()/RealScalar(PacketSize); + } + packet = internal::pgather(buffer, 7); + internal::pstore(data1, packet); + for (int i = 0; i < PacketSize; ++i) { + VERIFY(isApproxAbs(data1[i], buffer[i*7], refvalue) && "pgather"); + } } void test_packetmath() @@ -369,17 +619,23 @@ void test_packetmath() CALL_SUBTEST_1( packetmath() ); CALL_SUBTEST_2( packetmath() ); CALL_SUBTEST_3( packetmath() ); - CALL_SUBTEST_1( packetmath >() ); - CALL_SUBTEST_2( packetmath >() ); + CALL_SUBTEST_4( packetmath >() ); + CALL_SUBTEST_5( packetmath >() ); CALL_SUBTEST_1( packetmath_notcomplex() ); CALL_SUBTEST_2( packetmath_notcomplex() ); CALL_SUBTEST_3( packetmath_notcomplex() ); - + CALL_SUBTEST_1( packetmath_real() ); CALL_SUBTEST_2( packetmath_real() ); - CALL_SUBTEST_1( packetmath_complex >() ); - CALL_SUBTEST_2( packetmath_complex >() ); + CALL_SUBTEST_4( packetmath_complex >() ); + CALL_SUBTEST_5( packetmath_complex >() ); + + CALL_SUBTEST_1( packetmath_scatter_gather() ); + CALL_SUBTEST_2( packetmath_scatter_gather() ); + CALL_SUBTEST_3( packetmath_scatter_gather() ); + CALL_SUBTEST_4( packetmath_scatter_gather >() ); + CALL_SUBTEST_5( packetmath_scatter_gather >() ); } } diff --git a/ground/gcs/src/libs/eigen/test/pastix_support.cpp b/ground/gcs/src/libs/eigen/test/pastix_support.cpp index 14da0944be..b62f857394 100644 --- a/ground/gcs/src/libs/eigen/test/pastix_support.cpp +++ b/ground/gcs/src/libs/eigen/test/pastix_support.cpp @@ -7,6 +7,8 @@ // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS #include "sparse_solver.h" #include #include @@ -25,6 +27,14 @@ template void test_pastix_T() check_sparse_spd_solving(pastix_llt_upper); check_sparse_spd_solving(pastix_ldlt_upper); check_sparse_square_solving(pastix_lu); + + // Some compilation check: + pastix_llt_lower.iparm(); + pastix_llt_lower.dparm(); + pastix_ldlt_lower.iparm(); + pastix_ldlt_lower.dparm(); + pastix_lu.iparm(); + pastix_lu.dparm(); } // There is no support for selfadjoint matrices with PaStiX. diff --git a/ground/gcs/src/libs/eigen/test/permutationmatrices.cpp b/ground/gcs/src/libs/eigen/test/permutationmatrices.cpp index 7b0dbc7638..db12665797 100644 --- a/ground/gcs/src/libs/eigen/test/permutationmatrices.cpp +++ b/ground/gcs/src/libs/eigen/test/permutationmatrices.cpp @@ -7,6 +7,8 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define TEST_ENABLE_TEMPORARY_TRACKING + #include "main.h" using namespace std; @@ -33,7 +35,9 @@ template void permutationmatrices(const MatrixType& m) RightPermutationVectorType rv; randomPermutationVector(rv, cols); RightPermutationType rp(rv); - MatrixType m_permuted = lp * m_original * rp; + MatrixType m_permuted = MatrixType::Random(rows,cols); + + VERIFY_EVALUATION_COUNT(m_permuted = lp * m_original * rp, 1); // 1 temp for sub expression "lp * m_original" for (int i=0; i void permutationmatrices(const MatrixType& m) Matrix rm(rp); VERIFY_IS_APPROX(m_permuted, lm*m_original*rm); - + + m_permuted = m_original; + VERIFY_EVALUATION_COUNT(m_permuted = lp * m_permuted * rp, 1); + VERIFY_IS_APPROX(m_permuted, lm*m_original*rm); + VERIFY_IS_APPROX(lp.inverse()*m_permuted*rp.inverse(), m_original); VERIFY_IS_APPROX(lv.asPermutation().inverse()*m_permuted*rv.asPermutation().inverse(), m_original); VERIFY_IS_APPROX(MapLeftPerm(lv.data(),lv.size()).inverse()*m_permuted*MapRightPerm(rv.data(),rv.size()).inverse(), m_original); @@ -63,22 +71,22 @@ template void permutationmatrices(const MatrixType& m) LeftPermutationType identityp; identityp.setIdentity(rows); VERIFY_IS_APPROX(m_original, identityp*m_original); - + // check inplace permutations m_permuted = m_original; - m_permuted = lp.inverse() * m_permuted; + VERIFY_EVALUATION_COUNT(m_permuted.noalias()= lp.inverse() * m_permuted, 1); // 1 temp to allocate the mask VERIFY_IS_APPROX(m_permuted, lp.inverse()*m_original); - + m_permuted = m_original; - m_permuted = m_permuted * rp.inverse(); + VERIFY_EVALUATION_COUNT(m_permuted.noalias() = m_permuted * rp.inverse(), 1); // 1 temp to allocate the mask VERIFY_IS_APPROX(m_permuted, m_original*rp.inverse()); - + m_permuted = m_original; - m_permuted = lp * m_permuted; + VERIFY_EVALUATION_COUNT(m_permuted.noalias() = lp * m_permuted, 1); // 1 temp to allocate the mask VERIFY_IS_APPROX(m_permuted, lp*m_original); - + m_permuted = m_original; - m_permuted = m_permuted * rp; + VERIFY_EVALUATION_COUNT(m_permuted.noalias() = m_permuted * rp, 1); // 1 temp to allocate the mask VERIFY_IS_APPROX(m_permuted, m_original*rp); if(rows>1 && cols>1) @@ -99,7 +107,38 @@ template void permutationmatrices(const MatrixType& m) rm = rp; rm.col(i).swap(rm.col(j)); VERIFY_IS_APPROX(rm, rp2.toDenseMatrix().template cast()); - } + } + + { + // simple compilation check + Matrix A = rp; + Matrix B = rp.transpose(); + VERIFY_IS_APPROX(A, B.transpose()); + } +} + +template +void bug890() +{ + typedef Matrix MatrixType; + typedef Matrix VectorType; + typedef Stride S; + typedef Map MapType; + typedef PermutationMatrix Perm; + + VectorType v1(2), v2(2), op(4), rhs(2); + v1 << 666,667; + op << 1,0,0,1; + rhs << 42,42; + + Perm P(2); + P.indices() << 1, 0; + + MapType(v1.data(),2,1,S(1,1)) = P * MapType(rhs.data(),2,1,S(1,1)); + VERIFY_IS_APPROX(v1, (P * rhs).eval()); + + MapType(v1.data(),2,1,S(1,1)) = P.inverse() * MapType(rhs.data(),2,1,S(1,1)); + VERIFY_IS_APPROX(v1, (P.inverse() * rhs).eval()); } void test_permutationmatrices() @@ -113,4 +152,5 @@ void test_permutationmatrices() CALL_SUBTEST_6( permutationmatrices(Matrix(20, 30)) ); CALL_SUBTEST_7( permutationmatrices(MatrixXcf(15, 10)) ); } + CALL_SUBTEST_5( bug890() ); } diff --git a/ground/gcs/src/libs/eigen/test/prec_inverse_4x4.cpp b/ground/gcs/src/libs/eigen/test/prec_inverse_4x4.cpp index c4ef2d4bdf..eb6ad18c94 100644 --- a/ground/gcs/src/libs/eigen/test/prec_inverse_4x4.cpp +++ b/ground/gcs/src/libs/eigen/test/prec_inverse_4x4.cpp @@ -53,14 +53,29 @@ template void inverse_general_4x4(int repeat) // FIXME that 1.25 used to be 1.2 until we tested gcc 4.1 on 30 June 2010 and got 1.21. VERIFY(error_avg < (NumTraits::IsComplex ? 8.0 : 1.25)); VERIFY(error_max < (NumTraits::IsComplex ? 64.0 : 20.0)); + + { + int s = 5;//internal::random(4,10); + int i = 0;//internal::random(0,s-4); + int j = 0;//internal::random(0,s-4); + Matrix mat(s,s); + mat.setRandom(); + MatrixType submat = mat.template block<4,4>(i,j); + MatrixType mat_inv = mat.template block<4,4>(i,j).inverse(); + VERIFY_IS_APPROX(mat_inv, submat.inverse()); + mat.template block<4,4>(i,j) = submat.inverse(); + VERIFY_IS_APPROX(mat_inv, (mat.template block<4,4>(i,j))); + } } void test_prec_inverse_4x4() { CALL_SUBTEST_1((inverse_permutation_4x4())); CALL_SUBTEST_1(( inverse_general_4x4(200000 * g_repeat) )); + CALL_SUBTEST_1(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_2((inverse_permutation_4x4 >())); + CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_3((inverse_permutation_4x4())); diff --git a/ground/gcs/src/libs/eigen/test/product.h b/ground/gcs/src/libs/eigen/test/product.h index 0d054ff469..3b65112706 100644 --- a/ground/gcs/src/libs/eigen/test/product.h +++ b/ground/gcs/src/libs/eigen/test/product.h @@ -22,7 +22,6 @@ template void product(const MatrixType& m) /* this test covers the following files: Identity.h Product.h */ - typedef typename MatrixType::Index Index; typedef typename MatrixType::Scalar Scalar; typedef Matrix RowVectorType; typedef Matrix ColVectorType; @@ -112,6 +111,23 @@ template void product(const MatrixType& m) vcres.noalias() -= m1.transpose() * v1; VERIFY_IS_APPROX(vcres, vc2 - m1.transpose() * v1); + // test d ?= a+b*c rules + res.noalias() = square + m1 * m2.transpose(); + VERIFY_IS_APPROX(res, square + m1 * m2.transpose()); + res.noalias() += square + m1 * m2.transpose(); + VERIFY_IS_APPROX(res, 2*(square + m1 * m2.transpose())); + res.noalias() -= square + m1 * m2.transpose(); + VERIFY_IS_APPROX(res, square + m1 * m2.transpose()); + + // test d ?= a-b*c rules + res.noalias() = square - m1 * m2.transpose(); + VERIFY_IS_APPROX(res, square - m1 * m2.transpose()); + res.noalias() += square - m1 * m2.transpose(); + VERIFY_IS_APPROX(res, 2*(square - m1 * m2.transpose())); + res.noalias() -= square - m1 * m2.transpose(); + VERIFY_IS_APPROX(res, square - m1 * m2.transpose()); + + tm1 = m1; VERIFY_IS_APPROX(tm1.transpose() * v1, m1.transpose() * v1); VERIFY_IS_APPROX(v1.transpose() * tm1, v1.transpose() * m1); @@ -152,19 +168,44 @@ template void product(const MatrixType& m) VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2, (ref2.row(0) = m1.row(0) * square2)); } + // vector.block() (see bug 1283) + { + RowVectorType w1(rows); + VERIFY_IS_APPROX(square * v1.block(0,0,rows,1), square * v1); + VERIFY_IS_APPROX(w1.noalias() = square * v1.block(0,0,rows,1), square * v1); + VERIFY_IS_APPROX(w1.block(0,0,rows,1).noalias() = square * v1.block(0,0,rows,1), square * v1); + + Matrix w2(cols); + VERIFY_IS_APPROX(vc2.block(0,0,cols,1).transpose() * square2, vc2.transpose() * square2); + VERIFY_IS_APPROX(w2.noalias() = vc2.block(0,0,cols,1).transpose() * square2, vc2.transpose() * square2); + VERIFY_IS_APPROX(w2.block(0,0,1,cols).noalias() = vc2.block(0,0,cols,1).transpose() * square2, vc2.transpose() * square2); + + vc2 = square2.block(0,0,1,cols).transpose(); + VERIFY_IS_APPROX(square2.block(0,0,1,cols) * square2, vc2.transpose() * square2); + VERIFY_IS_APPROX(w2.noalias() = square2.block(0,0,1,cols) * square2, vc2.transpose() * square2); + VERIFY_IS_APPROX(w2.block(0,0,1,cols).noalias() = square2.block(0,0,1,cols) * square2, vc2.transpose() * square2); + + vc2 = square2.block(0,0,cols,1); + VERIFY_IS_APPROX(square2.block(0,0,cols,1).transpose() * square2, vc2.transpose() * square2); + VERIFY_IS_APPROX(w2.noalias() = square2.block(0,0,cols,1).transpose() * square2, vc2.transpose() * square2); + VERIFY_IS_APPROX(w2.block(0,0,1,cols).noalias() = square2.block(0,0,cols,1).transpose() * square2, vc2.transpose() * square2); + } + // inner product { Scalar x = square2.row(c) * square2.col(c2); VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum()); } - + // outer product - VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); - VERIFY_IS_APPROX(m1.row(r).transpose() * m1.col(c).transpose(), m1.block(r,0,1,cols).transpose() * m1.block(0,c,rows,1).transpose()); - VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); - VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); - VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols)); - VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); + { + VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.row(r).transpose() * m1.col(c).transpose(), m1.block(r,0,1,cols).transpose() * m1.block(0,c,rows,1).transpose()); + VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); + } // Aliasing { @@ -178,4 +219,13 @@ template void product(const MatrixType& m) // CwiseUnaryOp VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z); } + + // regression for blas_trais + { + VERIFY_IS_APPROX(square * (square*square).transpose(), square * square.transpose() * square.transpose()); + VERIFY_IS_APPROX(square * (-(square*square)), -square * square * square); + VERIFY_IS_APPROX(square * (s1*(square*square)), s1 * square * square * square); + VERIFY_IS_APPROX(square * (square*square).conjugate(), square * square.conjugate() * square.conjugate()); + } + } diff --git a/ground/gcs/src/libs/eigen/test/product_extra.cpp b/ground/gcs/src/libs/eigen/test/product_extra.cpp index ea24869371..e2b855bff0 100644 --- a/ground/gcs/src/libs/eigen/test/product_extra.cpp +++ b/ground/gcs/src/libs/eigen/test/product_extra.cpp @@ -98,6 +98,16 @@ template void product_extra(const MatrixType& m) // regression test MatrixType tmp = m1 * m1.adjoint() * s1; VERIFY_IS_APPROX(tmp, m1 * m1.adjoint() * s1); + + // regression test for bug 1343, assignment to arrays + Array a1 = m1 * vc2; + VERIFY_IS_APPROX(a1.matrix(),m1*vc2); + Array a2 = s1 * (m1 * vc2); + VERIFY_IS_APPROX(a2.matrix(),s1*m1*vc2); + Array a3 = v1 * m1; + VERIFY_IS_APPROX(a3.matrix(),v1*m1); + Array a4 = m1 * m2.adjoint(); + VERIFY_IS_APPROX(a4.matrix(),m1*m2.adjoint()); } // Regression test for bug reported at http://forum.kde.org/viewtopic.php?f=74&t=96947 @@ -116,8 +126,8 @@ void zero_sized_objects(const MatrixType& m) typedef typename MatrixType::Scalar Scalar; const int PacketSize = internal::packet_traits::size; const int PacketSize1 = PacketSize>1 ? PacketSize-1 : 1; - DenseIndex rows = m.rows(); - DenseIndex cols = m.cols(); + Index rows = m.rows(); + Index cols = m.cols(); { MatrixType res, a(rows,0), b(0,cols); @@ -169,6 +179,7 @@ void zero_sized_objects(const MatrixType& m) } } +template void bug_127() { // Bug 127 @@ -193,6 +204,16 @@ void bug_127() a*b; } +template void bug_817() +{ + ArrayXXf B = ArrayXXf::Random(10,10), C; + VectorXf x = VectorXf::Random(10); + C = (x.transpose()*B.matrix()); + B = (x.transpose()*B.matrix()); + VERIFY_IS_APPROX(B,C); +} + +template void unaligned_objects() { // Regression test for the bug reported here: @@ -222,6 +243,116 @@ void unaligned_objects() } } +template +EIGEN_DONT_INLINE +Index test_compute_block_size(Index m, Index n, Index k) +{ + Index mc(m), nc(n), kc(k); + internal::computeProductBlockingSizes(kc, mc, nc); + return kc+mc+nc; +} + +template +Index compute_block_size() +{ + Index ret = 0; + ret += test_compute_block_size(0,1,1); + ret += test_compute_block_size(1,0,1); + ret += test_compute_block_size(1,1,0); + ret += test_compute_block_size(0,0,1); + ret += test_compute_block_size(0,1,0); + ret += test_compute_block_size(1,0,0); + ret += test_compute_block_size(0,0,0); + return ret; +} + +template +void aliasing_with_resize() +{ + Index m = internal::random(10,50); + Index n = internal::random(10,50); + MatrixXd A, B, C(m,n), D(m,m); + VectorXd a, b, c(n); + C.setRandom(); + D.setRandom(); + c.setRandom(); + double s = internal::random(1,10); + + A = C; + B = A * A.transpose(); + A = A * A.transpose(); + VERIFY_IS_APPROX(A,B); + + A = C; + B = (A * A.transpose())/s; + A = (A * A.transpose())/s; + VERIFY_IS_APPROX(A,B); + + A = C; + B = (A * A.transpose()) + D; + A = (A * A.transpose()) + D; + VERIFY_IS_APPROX(A,B); + + A = C; + B = D + (A * A.transpose()); + A = D + (A * A.transpose()); + VERIFY_IS_APPROX(A,B); + + A = C; + B = s * (A * A.transpose()); + A = s * (A * A.transpose()); + VERIFY_IS_APPROX(A,B); + + A = C; + a = c; + b = (A * a)/s; + a = (A * a)/s; + VERIFY_IS_APPROX(a,b); +} + +template +void bug_1308() +{ + int n = 10; + MatrixXd r(n,n); + VectorXd v = VectorXd::Random(n); + r = v * RowVectorXd::Ones(n); + VERIFY_IS_APPROX(r, v.rowwise().replicate(n)); + r = VectorXd::Ones(n) * v.transpose(); + VERIFY_IS_APPROX(r, v.rowwise().replicate(n).transpose()); + + Matrix4d ones44 = Matrix4d::Ones(); + Matrix4d m44 = Matrix4d::Ones() * Matrix4d::Ones(); + VERIFY_IS_APPROX(m44,Matrix4d::Constant(4)); + VERIFY_IS_APPROX(m44.noalias()=ones44*Matrix4d::Ones(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(m44.noalias()=ones44.transpose()*Matrix4d::Ones(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(m44.noalias()=Matrix4d::Ones()*ones44, Matrix4d::Constant(4)); + VERIFY_IS_APPROX(m44.noalias()=Matrix4d::Ones()*ones44.transpose(), Matrix4d::Constant(4)); + + typedef Matrix RMatrix4d; + RMatrix4d r44 = Matrix4d::Ones() * Matrix4d::Ones(); + VERIFY_IS_APPROX(r44,Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=ones44*Matrix4d::Ones(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=ones44.transpose()*Matrix4d::Ones(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=Matrix4d::Ones()*ones44, Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=Matrix4d::Ones()*ones44.transpose(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=ones44*RMatrix4d::Ones(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=ones44.transpose()*RMatrix4d::Ones(), Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=RMatrix4d::Ones()*ones44, Matrix4d::Constant(4)); + VERIFY_IS_APPROX(r44.noalias()=RMatrix4d::Ones()*ones44.transpose(), Matrix4d::Constant(4)); + +// RowVector4d r4; + m44.setOnes(); + r44.setZero(); + VERIFY_IS_APPROX(r44.noalias() += m44.row(0).transpose() * RowVector4d::Ones(), ones44); + r44.setZero(); + VERIFY_IS_APPROX(r44.noalias() += m44.col(0) * RowVector4d::Ones(), ones44); + r44.setZero(); + VERIFY_IS_APPROX(r44.noalias() += Vector4d::Ones() * m44.row(0), ones44); + r44.setZero(); + VERIFY_IS_APPROX(r44.noalias() += Vector4d::Ones() * m44.col(0).transpose(), ones44); +} + void test_product_extra() { for(int i = 0; i < g_repeat; i++) { @@ -232,6 +363,13 @@ void test_product_extra() CALL_SUBTEST_4( product_extra(MatrixXcd(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_1( zero_sized_objects(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } - CALL_SUBTEST_5( bug_127() ); - CALL_SUBTEST_6( unaligned_objects() ); + CALL_SUBTEST_5( bug_127<0>() ); + CALL_SUBTEST_5( bug_817<0>() ); + CALL_SUBTEST_5( bug_1308<0>() ); + CALL_SUBTEST_6( unaligned_objects<0>() ); + CALL_SUBTEST_7( compute_block_size() ); + CALL_SUBTEST_7( compute_block_size() ); + CALL_SUBTEST_7( compute_block_size >() ); + CALL_SUBTEST_8( aliasing_with_resize() ); + } diff --git a/ground/gcs/src/libs/eigen/test/product_large.cpp b/ground/gcs/src/libs/eigen/test/product_large.cpp index 6bb4d1ad1c..845cd40ca1 100644 --- a/ground/gcs/src/libs/eigen/test/product_large.cpp +++ b/ground/gcs/src/libs/eigen/test/product_large.cpp @@ -21,12 +21,12 @@ void test_aliasing() VectorType y(rows); y.setZero(); MatrixType A(rows,cols); A.setRandom(); // CwiseBinaryOp - VERIFY_IS_APPROX(x = y + A*x, A*z); + VERIFY_IS_APPROX(x = y + A*x, A*z); // OK because "y + A*x" is marked as "assume-aliasing" x = z; // CwiseUnaryOp - VERIFY_IS_APPROX(x = T(1.)*(A*x), A*z); + VERIFY_IS_APPROX(x = T(1.)*(A*x), A*z); // OK because 1*(A*x) is replaced by (1*A*x) which is a Product<> expression x = z; - VERIFY_IS_APPROX(x = y+(-(A*x)), -A*z); + // VERIFY_IS_APPROX(x = y-A*x, -A*z); // Not OK in 3.3 because x is resized before A*x gets evaluated x = z; } @@ -62,15 +62,16 @@ void test_product_large() // check the functions to setup blocking sizes compile and do not segfault // FIXME check they do what they are supposed to do !! std::ptrdiff_t l1 = internal::random(10000,20000); - std::ptrdiff_t l2 = internal::random(1000000,2000000); - setCpuCacheSizes(l1,l2); + std::ptrdiff_t l2 = internal::random(100000,200000); + std::ptrdiff_t l3 = internal::random(1000000,2000000); + setCpuCacheSizes(l1,l2,l3); VERIFY(l1==l1CacheSize()); VERIFY(l2==l2CacheSize()); std::ptrdiff_t k1 = internal::random(10,100)*16; std::ptrdiff_t m1 = internal::random(10,100)*16; std::ptrdiff_t n1 = internal::random(10,100)*16; // only makes sure it compiles fine - internal::computeProductBlockingSizes(k1,m1,n1); + internal::computeProductBlockingSizes(k1,m1,n1,1); } { @@ -83,5 +84,24 @@ void test_product_large() MatrixXf r2 = mat1.row(2)*mat2; VERIFY_IS_APPROX(r2, (mat1.row(2)*mat2).eval()); } + + { + Eigen::MatrixXd A(10,10), B, C; + A.setRandom(); + C = A; + for(int k=0; k<79; ++k) + C = C * A; + B.noalias() = (((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)) * ((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))) + * (((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)) * ((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))); + VERIFY_IS_APPROX(B,C); + } +#endif + + // Regression test for bug 714: +#if defined EIGEN_HAS_OPENMP + omp_set_dynamic(1); + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_6( product(Matrix(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + } #endif } diff --git a/ground/gcs/src/libs/eigen/test/product_mmtr.cpp b/ground/gcs/src/libs/eigen/test/product_mmtr.cpp index aeba009f4f..f6e4bb1ae7 100644 --- a/ground/gcs/src/libs/eigen/test/product_mmtr.cpp +++ b/ground/gcs/src/libs/eigen/test/product_mmtr.cpp @@ -13,7 +13,8 @@ ref2 = ref1 = DEST; \ DEST.template triangularView() OP; \ ref1 OP; \ - ref2.template triangularView() = ref1; \ + ref2.template triangularView() \ + = ref1.template triangularView(); \ VERIFY_IS_APPROX(DEST,ref2); \ } @@ -32,6 +33,8 @@ template void mmtr(int size) MatrixColMaj osc(othersize,size); osc.setRandom(); MatrixRowMaj sor(size,othersize); sor.setRandom(); MatrixRowMaj osr(othersize,size); osr.setRandom(); + MatrixColMaj sqc(size,size); sqc.setRandom(); + MatrixRowMaj sqr(size,size); sqr.setRandom(); Scalar s = internal::random(); @@ -49,6 +52,29 @@ template void mmtr(int size) CHECK_MMTR(matc, Upper, -= s*(osc.transpose()*osc.conjugate())); CHECK_MMTR(matr, Lower, -= s*soc*soc.adjoint()); CHECK_MMTR(matr, Upper, -= soc*(s*soc.adjoint())); + + CHECK_MMTR(matc, Lower, -= s*sqr*sqc.template triangularView()); + CHECK_MMTR(matc, Upper, = s*sqc*sqr.template triangularView()); + CHECK_MMTR(matc, Lower, += s*sqr*sqc.template triangularView()); + CHECK_MMTR(matc, Upper, = s*sqc*sqc.template triangularView()); + + CHECK_MMTR(matc, Lower, = (s*sqr).template triangularView()*sqc); + CHECK_MMTR(matc, Upper, -= (s*sqc).template triangularView()*sqc); + CHECK_MMTR(matc, Lower, = (s*sqr).template triangularView()*sqc); + CHECK_MMTR(matc, Upper, += (s*sqc).template triangularView()*sqc); + + // check aliasing + ref2 = ref1 = matc; + ref1 = sqc.adjoint() * matc * sqc; + ref2.template triangularView() = ref1.template triangularView(); + matc.template triangularView() = sqc.adjoint() * matc * sqc; + VERIFY_IS_APPROX(matc, ref2); + + ref2 = ref1 = matc; + ref1 = sqc * matc * sqc.adjoint(); + ref2.template triangularView() = ref1.template triangularView(); + matc.template triangularView() = sqc * matc * sqc.adjoint(); + VERIFY_IS_APPROX(matc, ref2); } void test_product_mmtr() diff --git a/ground/gcs/src/libs/eigen/test/product_notemporary.cpp b/ground/gcs/src/libs/eigen/test/product_notemporary.cpp index 5599d268d8..8bf71b4f2e 100644 --- a/ground/gcs/src/libs/eigen/test/product_notemporary.cpp +++ b/ground/gcs/src/libs/eigen/test/product_notemporary.cpp @@ -7,25 +7,10 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. -static int nb_temporaries; - -inline void on_temporary_creation(int size) { - // here's a great place to set a breakpoint when debugging failures in this test! - if(size!=0) nb_temporaries++; -} - - -#define EIGEN_DENSE_STORAGE_CTOR_PLUGIN { on_temporary_creation(size); } +#define TEST_ENABLE_TEMPORARY_TRACKING #include "main.h" -#define VERIFY_EVALUATION_COUNT(XPR,N) {\ - nb_temporaries = 0; \ - XPR; \ - if(nb_temporaries!=N) std::cerr << "nb_temporaries == " << nb_temporaries << "\n"; \ - VERIFY( (#XPR) && nb_temporaries==N ); \ - } - template void product_notemporary(const MatrixType& m) { /* This test checks the number of temporaries created @@ -62,14 +47,18 @@ template void product_notemporary(const MatrixType& m) VERIFY_EVALUATION_COUNT( m3.noalias() = m1 * m2.adjoint(), 0); VERIFY_EVALUATION_COUNT( m3 = s1 * (m1 * m2.transpose()), 1); - VERIFY_EVALUATION_COUNT( m3 = m3 + s1 * (m1 * m2.transpose()), 1); +// VERIFY_EVALUATION_COUNT( m3 = m3 + s1 * (m1 * m2.transpose()), 1); VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * (m1 * m2.transpose()), 0); VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()), 1); + VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()).transpose(), 1); - VERIFY_EVALUATION_COUNT( m3.noalias() = m3 + m1 * m2.transpose(), 1); // 0 in 3.3 - VERIFY_EVALUATION_COUNT( m3.noalias() += m3 + m1 * m2.transpose(), 1); // 0 in 3.3 - VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 + m1 * m2.transpose(), 1); // 0 in 3.3 + VERIFY_EVALUATION_COUNT( m3.noalias() = m3 + m1 * m2.transpose(), 0); + VERIFY_EVALUATION_COUNT( m3.noalias() += m3 + m1 * m2.transpose(), 0); + VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 + m1 * m2.transpose(), 0); + VERIFY_EVALUATION_COUNT( m3.noalias() = m3 - m1 * m2.transpose(), 0); + VERIFY_EVALUATION_COUNT( m3.noalias() += m3 - m1 * m2.transpose(), 0); + VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 - m1 * m2.transpose(), 0); VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * m2.adjoint(), 0); VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * (m1*s3+m2*s2).adjoint(), 1); @@ -86,7 +75,7 @@ template void product_notemporary(const MatrixType& m) VERIFY_EVALUATION_COUNT( m3.noalias() -= (s1 * m1).template triangularView() * m2, 0); VERIFY_EVALUATION_COUNT( rm3.noalias() = (s1 * m1.adjoint()).template triangularView() * (m2+m2), 1); VERIFY_EVALUATION_COUNT( rm3.noalias() = (s1 * m1.adjoint()).template triangularView() * m2.adjoint(), 0); - + VERIFY_EVALUATION_COUNT( m3.template triangularView() = (m1 * m2.adjoint()), 0); VERIFY_EVALUATION_COUNT( m3.template triangularView() -= (m1 * m2.adjoint()), 0); @@ -123,8 +112,7 @@ template void product_notemporary(const MatrixType& m) VERIFY_EVALUATION_COUNT( Scalar tmp = 0; tmp += Scalar(RealScalar(1)) / (m3.transpose() * m3).diagonal().array().abs().sum(), 0 ); // Zero temporaries for ... CoeffBasedProductMode - // - does not work with GCC because of the <..>, we'ld need variadic macros ... - //VERIFY_EVALUATION_COUNT( m3.col(0).head<5>() * m3.col(0).transpose() + m3.col(0).head<5>() * m3.col(0).transpose(), 0 ); + VERIFY_EVALUATION_COUNT( m3.col(0).template head<5>() * m3.col(0).transpose() + m3.col(0).template head<5>() * m3.col(0).transpose(), 0 ); // Check matrix * vectors VERIFY_EVALUATION_COUNT( cvres.noalias() = m1 * cv1, 0 ); @@ -132,6 +120,26 @@ template void product_notemporary(const MatrixType& m) VERIFY_EVALUATION_COUNT( cvres.noalias() -= m1 * m2.col(0), 0 ); VERIFY_EVALUATION_COUNT( cvres.noalias() -= m1 * rv1.adjoint(), 0 ); VERIFY_EVALUATION_COUNT( cvres.noalias() -= m1 * m2.row(0).transpose(), 0 ); + + VERIFY_EVALUATION_COUNT( cvres.noalias() = (m1+m1) * cv1, 0 ); + VERIFY_EVALUATION_COUNT( cvres.noalias() = (rm3+rm3) * cv1, 0 ); + VERIFY_EVALUATION_COUNT( cvres.noalias() = (m1+m1) * (m1*cv1), 1 ); + VERIFY_EVALUATION_COUNT( cvres.noalias() = (rm3+rm3) * (m1*cv1), 1 ); + + // Check outer products + m3 = cv1 * rv1; + VERIFY_EVALUATION_COUNT( m3.noalias() = cv1 * rv1, 0 ); + VERIFY_EVALUATION_COUNT( m3.noalias() = (cv1+cv1) * (rv1+rv1), 1 ); + VERIFY_EVALUATION_COUNT( m3.noalias() = (m1*cv1) * (rv1), 1 ); + VERIFY_EVALUATION_COUNT( m3.noalias() += (m1*cv1) * (rv1), 1 ); + VERIFY_EVALUATION_COUNT( rm3.noalias() = (cv1) * (rv1 * m1), 1 ); + VERIFY_EVALUATION_COUNT( rm3.noalias() -= (cv1) * (rv1 * m1), 1 ); + VERIFY_EVALUATION_COUNT( rm3.noalias() = (m1*cv1) * (rv1 * m1), 2 ); + VERIFY_EVALUATION_COUNT( rm3.noalias() += (m1*cv1) * (rv1 * m1), 2 ); + + // Check nested products + VERIFY_EVALUATION_COUNT( cvres.noalias() = m1.adjoint() * m1 * cv1, 1 ); + VERIFY_EVALUATION_COUNT( rvres.noalias() = rv1 * (m1 * m2.adjoint()), 1 ); } void test_product_notemporary() @@ -140,11 +148,12 @@ void test_product_notemporary() for(int i = 0; i < g_repeat; i++) { s = internal::random(16,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_1( product_notemporary(MatrixXf(s, s)) ); - s = internal::random(16,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_2( product_notemporary(MatrixXd(s, s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(16,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_3( product_notemporary(MatrixXcf(s,s)) ); - s = internal::random(16,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_4( product_notemporary(MatrixXcd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } } diff --git a/ground/gcs/src/libs/eigen/test/product_selfadjoint.cpp b/ground/gcs/src/libs/eigen/test/product_selfadjoint.cpp index 374e2393b7..3d768aa7e2 100644 --- a/ground/gcs/src/libs/eigen/test/product_selfadjoint.cpp +++ b/ground/gcs/src/libs/eigen/test/product_selfadjoint.cpp @@ -67,14 +67,21 @@ void test_product_selfadjoint() CALL_SUBTEST_1( product_selfadjoint(Matrix()) ); CALL_SUBTEST_2( product_selfadjoint(Matrix()) ); CALL_SUBTEST_3( product_selfadjoint(Matrix3d()) ); + s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_4( product_selfadjoint(MatrixXcf(s, s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_5( product_selfadjoint(MatrixXcd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_6( product_selfadjoint(MatrixXd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_7( product_selfadjoint(Matrix(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } - TEST_SET_BUT_UNUSED_VARIABLE(s) } diff --git a/ground/gcs/src/libs/eigen/test/product_small.cpp b/ground/gcs/src/libs/eigen/test/product_small.cpp index 8b132abb67..fdfdd9f6c5 100644 --- a/ground/gcs/src/libs/eigen/test/product_small.cpp +++ b/ground/gcs/src/libs/eigen/test/product_small.cpp @@ -9,8 +9,10 @@ #define EIGEN_NO_STATIC_ASSERT #include "product.h" +#include // regression test for bug 447 +template void product1x1() { Matrix matAstatic; @@ -28,16 +30,237 @@ void product1x1() matAdynamic.cwiseProduct(matBdynamic.transpose()).sum() ); } +template +const TC& ref_prod(TC &C, const TA &A, const TB &B) +{ + for(Index i=0;i +typename internal::enable_if::type +test_lazy_single(int rows, int cols, int depth) +{ + Matrix A(rows,depth); A.setRandom(); + Matrix B(depth,cols); B.setRandom(); + Matrix C(rows,cols); C.setRandom(); + Matrix D(C); + VERIFY_IS_APPROX(C+=A.lazyProduct(B), ref_prod(D,A,B)); +} + +template +typename internal::enable_if< ( (Rows ==1&&Depth!=1&&OA==ColMajor) + || (Depth==1&&Rows !=1&&OA==RowMajor) + || (Cols ==1&&Depth!=1&&OB==RowMajor) + || (Depth==1&&Cols !=1&&OB==ColMajor) + || (Rows ==1&&Cols !=1&&OC==ColMajor) + || (Cols ==1&&Rows !=1&&OC==RowMajor)),void>::type +test_lazy_single(int, int, int) +{ +} + +template +void test_lazy_all_layout(int rows=Rows, int cols=Cols, int depth=Depth) +{ + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); + CALL_SUBTEST(( test_lazy_single(rows,cols,depth) )); +} + +template +void test_lazy_l1() +{ + int rows = internal::random(1,12); + int cols = internal::random(1,12); + int depth = internal::random(1,12); + + // Inner + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout(1,1,depth) )); + + // Outer + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout(4,cols) )); + CALL_SUBTEST(( test_lazy_all_layout(7,cols) )); + CALL_SUBTEST(( test_lazy_all_layout(rows) )); + CALL_SUBTEST(( test_lazy_all_layout(rows) )); + CALL_SUBTEST(( test_lazy_all_layout(rows,cols) )); +} + +template +void test_lazy_l2() +{ + int rows = internal::random(1,12); + int cols = internal::random(1,12); + int depth = internal::random(1,12); + + // mat-vec + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout(rows) )); + CALL_SUBTEST(( test_lazy_all_layout(4,1,depth) )); + CALL_SUBTEST(( test_lazy_all_layout(rows,1,depth) )); + + // vec-mat + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout(1,cols) )); + CALL_SUBTEST(( test_lazy_all_layout(1,4,depth) )); + CALL_SUBTEST(( test_lazy_all_layout(1,cols,depth) )); +} + +template +void test_lazy_l3() +{ + int rows = internal::random(1,12); + int cols = internal::random(1,12); + int depth = internal::random(1,12); + // mat-mat + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout(rows) )); + CALL_SUBTEST(( test_lazy_all_layout(4,3,depth) )); + CALL_SUBTEST(( test_lazy_all_layout(rows,6,depth) )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout() )); + CALL_SUBTEST(( test_lazy_all_layout(8,cols) )); + CALL_SUBTEST(( test_lazy_all_layout(3,4,depth) )); + CALL_SUBTEST(( test_lazy_all_layout(4,cols,depth) )); +} + +template +void test_linear_but_not_vectorizable() +{ + // Check tricky cases for which the result of the product is a vector and thus must exhibit the LinearBit flag, + // but is not vectorizable along the linear dimension. + Index n = N==Dynamic ? internal::random(1,32) : N; + Index m = M==Dynamic ? internal::random(1,32) : M; + Index k = K==Dynamic ? internal::random(1,32) : K; + + { + Matrix A; A.setRandom(n,m+1); + Matrix B; B.setRandom(m*2,k); + Matrix C; + Matrix R; + + C.noalias() = A.template topLeftCorner<1,M>() * (B.template topRows()+B.template bottomRows()); + R.noalias() = A.template topLeftCorner<1,M>() * (B.template topRows()+B.template bottomRows()).eval(); + VERIFY_IS_APPROX(C,R); + } + + { + Matrix A; A.setRandom(m+1,n); + Matrix B; B.setRandom(k,m*2); + Matrix C; + Matrix R; + + C.noalias() = (B.template leftCols()+B.template rightCols()) * A.template topLeftCorner(); + R.noalias() = (B.template leftCols()+B.template rightCols()).eval() * A.template topLeftCorner(); + VERIFY_IS_APPROX(C,R); + } +} + +template +void bug_1311() +{ + Matrix< double, Rows, 2 > A; A.setRandom(); + Vector2d b = Vector2d::Random() ; + Matrix res; + res.noalias() = 1. * (A * b); + VERIFY_IS_APPROX(res, A*b); + res.noalias() = 1.*A * b; + VERIFY_IS_APPROX(res, A*b); + res.noalias() = (1.*A).lazyProduct(b); + VERIFY_IS_APPROX(res, A*b); + res.noalias() = (1.*A).lazyProduct(1.*b); + VERIFY_IS_APPROX(res, A*b); + res.noalias() = (A).lazyProduct(1.*b); + VERIFY_IS_APPROX(res, A*b); +} void test_product_small() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( product(Matrix()) ); - CALL_SUBTEST_2( product(Matrix()) ); + CALL_SUBTEST_2( product(Matrix()) ); + CALL_SUBTEST_8( product(Matrix()) ); CALL_SUBTEST_3( product(Matrix3d()) ); CALL_SUBTEST_4( product(Matrix4d()) ); CALL_SUBTEST_5( product(Matrix4f()) ); - CALL_SUBTEST_6( product1x1() ); + CALL_SUBTEST_6( product1x1<0>() ); + + CALL_SUBTEST_11( test_lazy_l1() ); + CALL_SUBTEST_12( test_lazy_l2() ); + CALL_SUBTEST_13( test_lazy_l3() ); + + CALL_SUBTEST_21( test_lazy_l1() ); + CALL_SUBTEST_22( test_lazy_l2() ); + CALL_SUBTEST_23( test_lazy_l3() ); + + CALL_SUBTEST_31( test_lazy_l1 >() ); + CALL_SUBTEST_32( test_lazy_l2 >() ); + CALL_SUBTEST_33( test_lazy_l3 >() ); + + CALL_SUBTEST_41( test_lazy_l1 >() ); + CALL_SUBTEST_42( test_lazy_l2 >() ); + CALL_SUBTEST_43( test_lazy_l3 >() ); + + CALL_SUBTEST_7(( test_linear_but_not_vectorizable() )); + CALL_SUBTEST_7(( test_linear_but_not_vectorizable() )); + CALL_SUBTEST_7(( test_linear_but_not_vectorizable() )); + + CALL_SUBTEST_6( bug_1311<3>() ); + CALL_SUBTEST_6( bug_1311<5>() ); } #ifdef EIGEN_TEST_PART_6 @@ -46,5 +269,25 @@ void test_product_small() Vector3f v = Vector3f::Random(); VERIFY_IS_APPROX( (v * v.transpose()) * v, (v * v.transpose()).eval() * v); } + + { + // regression test for pull-request #93 + Eigen::Matrix A; A.setRandom(); + Eigen::Matrix B; B.setRandom(); + Eigen::Matrix C; C.setRandom(); + VERIFY_IS_APPROX(B * A.inverse(), B * A.inverse()[0]); + VERIFY_IS_APPROX(A.inverse() * C, A.inverse()[0] * C); + } + + { + Eigen::Matrix A, B, C; + A.setRandom(); + C = A; + for(int k=0; k<79; ++k) + C = C * A; + B.noalias() = (((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)) * ((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))) + * (((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)) * ((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))); + VERIFY_IS_APPROX(B,C); + } #endif } diff --git a/ground/gcs/src/libs/eigen/test/product_symm.cpp b/ground/gcs/src/libs/eigen/test/product_symm.cpp index 74d7329b11..8c44383f9e 100644 --- a/ground/gcs/src/libs/eigen/test/product_symm.cpp +++ b/ground/gcs/src/libs/eigen/test/product_symm.cpp @@ -39,6 +39,24 @@ template void symm(int size = Size, in VERIFY_IS_APPROX(rhs12 = (s1*m2).template selfadjointView() * (s2*rhs1), rhs13 = (s1*m1) * (s2*rhs1)); + VERIFY_IS_APPROX(rhs12 = (s1*m2).transpose().template selfadjointView() * (s2*rhs1), + rhs13 = (s1*m1.transpose()) * (s2*rhs1)); + + VERIFY_IS_APPROX(rhs12 = (s1*m2).template selfadjointView().transpose() * (s2*rhs1), + rhs13 = (s1*m1.transpose()) * (s2*rhs1)); + + VERIFY_IS_APPROX(rhs12 = (s1*m2).conjugate().template selfadjointView() * (s2*rhs1), + rhs13 = (s1*m1).conjugate() * (s2*rhs1)); + + VERIFY_IS_APPROX(rhs12 = (s1*m2).template selfadjointView().conjugate() * (s2*rhs1), + rhs13 = (s1*m1).conjugate() * (s2*rhs1)); + + VERIFY_IS_APPROX(rhs12 = (s1*m2).adjoint().template selfadjointView() * (s2*rhs1), + rhs13 = (s1*m1).adjoint() * (s2*rhs1)); + + VERIFY_IS_APPROX(rhs12 = (s1*m2).template selfadjointView().adjoint() * (s2*rhs1), + rhs13 = (s1*m1).adjoint() * (s2*rhs1)); + m2 = m1.template triangularView(); rhs12.setRandom(); rhs13 = rhs12; m3 = m2.template selfadjointView(); VERIFY_IS_EQUAL(m1, m3); diff --git a/ground/gcs/src/libs/eigen/test/product_syrk.cpp b/ground/gcs/src/libs/eigen/test/product_syrk.cpp index 73c95000ce..e10f0f2f23 100644 --- a/ground/gcs/src/libs/eigen/test/product_syrk.cpp +++ b/ground/gcs/src/libs/eigen/test/product_syrk.cpp @@ -125,11 +125,12 @@ void test_product_syrk() int s; s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_1( syrk(MatrixXf(s, s)) ); - s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_2( syrk(MatrixXd(s, s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_3( syrk(MatrixXcf(s, s)) ); - s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_4( syrk(MatrixXcd(s, s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } } diff --git a/ground/gcs/src/libs/eigen/test/product_trmm.cpp b/ground/gcs/src/libs/eigen/test/product_trmm.cpp index d715b9a363..12e554410a 100644 --- a/ground/gcs/src/libs/eigen/test/product_trmm.cpp +++ b/ground/gcs/src/libs/eigen/test/product_trmm.cpp @@ -9,10 +9,18 @@ #include "main.h" +template +int get_random_size() +{ + const int factor = NumTraits::ReadCost; + const int max_test_size = EIGEN_TEST_MAX_SIZE>2*factor ? EIGEN_TEST_MAX_SIZE/factor : EIGEN_TEST_MAX_SIZE; + return internal::random(1,max_test_size); +} + template -void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), - int cols=internal::random(1,EIGEN_TEST_MAX_SIZE), - int otherCols = OtherCols==Dynamic?internal::random(1,EIGEN_TEST_MAX_SIZE):OtherCols) +void trmm(int rows=get_random_size(), + int cols=get_random_size(), + int otherCols = OtherCols==Dynamic?get_random_size():OtherCols) { typedef Matrix TriMatrix; typedef Matrix OnTheRight; @@ -42,13 +50,13 @@ void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), VERIFY_IS_APPROX( ge_xs.noalias() = mat.template triangularView() * ge_right, tri * ge_right); VERIFY_IS_APPROX( ge_sx.noalias() = ge_left * mat.template triangularView(), ge_left * tri); - + VERIFY_IS_APPROX( ge_xs.noalias() = (s1*mat.adjoint()).template triangularView() * (s2*ge_left.transpose()), s1*triTr.conjugate() * (s2*ge_left.transpose())); VERIFY_IS_APPROX( ge_sx.noalias() = ge_right.transpose() * mat.adjoint().template triangularView(), ge_right.transpose() * triTr.conjugate()); VERIFY_IS_APPROX( ge_xs.noalias() = (s1*mat.adjoint()).template triangularView() * (s2*ge_left.adjoint()), s1*triTr.conjugate() * (s2*ge_left.adjoint())); VERIFY_IS_APPROX( ge_sx.noalias() = ge_right.adjoint() * mat.adjoint().template triangularView(), ge_right.adjoint() * triTr.conjugate()); - + ge_xs_save = ge_xs; VERIFY_IS_APPROX( (ge_xs_save + s1*triTr.conjugate() * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.adjoint()).template triangularView() * (s2*ge_left.adjoint()) ); ge_sx.setRandom(); @@ -61,13 +69,13 @@ void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), } template -void trmv(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), int cols=internal::random(1,EIGEN_TEST_MAX_SIZE)) +void trmv(int rows=get_random_size(), int cols=get_random_size()) { trmm(rows,cols,1); } template -void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), int cols=internal::random(1,EIGEN_TEST_MAX_SIZE), int otherCols = internal::random(1,EIGEN_TEST_MAX_SIZE)) +void trmm(int rows=get_random_size(), int cols=get_random_size(), int otherCols = get_random_size()) { trmm(rows,cols,otherCols); } diff --git a/ground/gcs/src/libs/eigen/test/product_trmv.cpp b/ground/gcs/src/libs/eigen/test/product_trmv.cpp index 4c3c435c25..57a202afc6 100644 --- a/ground/gcs/src/libs/eigen/test/product_trmv.cpp +++ b/ground/gcs/src/libs/eigen/test/product_trmv.cpp @@ -78,12 +78,14 @@ void test_product_trmv() CALL_SUBTEST_1( trmv(Matrix()) ); CALL_SUBTEST_2( trmv(Matrix()) ); CALL_SUBTEST_3( trmv(Matrix3d()) ); + s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_4( trmv(MatrixXcf(s,s)) ); - s = internal::random(1,EIGEN_TEST_MAX_SIZE/2); CALL_SUBTEST_5( trmv(MatrixXcd(s,s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) + s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_6( trmv(Matrix(s, s)) ); + TEST_SET_BUT_UNUSED_VARIABLE(s) } - TEST_SET_BUT_UNUSED_VARIABLE(s); } diff --git a/ground/gcs/src/libs/eigen/test/product_trsolve.cpp b/ground/gcs/src/libs/eigen/test/product_trsolve.cpp index 69892b3a82..4b97fa9d64 100644 --- a/ground/gcs/src/libs/eigen/test/product_trsolve.cpp +++ b/ground/gcs/src/libs/eigen/test/product_trsolve.cpp @@ -84,10 +84,18 @@ void test_product_trsolve() CALL_SUBTEST_4((trsolve,Dynamic,Dynamic>(internal::random(1,EIGEN_TEST_MAX_SIZE/2),internal::random(1,EIGEN_TEST_MAX_SIZE/2)))); // vectors - CALL_SUBTEST_1((trsolve(internal::random(1,EIGEN_TEST_MAX_SIZE)))); - CALL_SUBTEST_5((trsolve,Dynamic,1>(internal::random(1,EIGEN_TEST_MAX_SIZE)))); - CALL_SUBTEST_6((trsolve())); - CALL_SUBTEST_7((trsolve())); - CALL_SUBTEST_8((trsolve,4,1>())); + CALL_SUBTEST_5((trsolve(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_6((trsolve(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_7((trsolve,Dynamic,1>(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_8((trsolve,Dynamic,1>(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + + // meta-unrollers + CALL_SUBTEST_9((trsolve())); + CALL_SUBTEST_10((trsolve())); + CALL_SUBTEST_11((trsolve,4,1>())); + CALL_SUBTEST_12((trsolve())); + CALL_SUBTEST_13((trsolve())); + CALL_SUBTEST_14((trsolve())); + } } diff --git a/ground/gcs/src/libs/eigen/test/qr.cpp b/ground/gcs/src/libs/eigen/test/qr.cpp index a79e0dd34f..dfcc1e8f97 100644 --- a/ground/gcs/src/libs/eigen/test/qr.cpp +++ b/ground/gcs/src/libs/eigen/test/qr.cpp @@ -54,6 +54,8 @@ template void qr_invertible() { using std::log; using std::abs; + using std::pow; + using std::max; typedef typename NumTraits::Real RealScalar; typedef typename MatrixType::Scalar Scalar; @@ -65,7 +67,7 @@ template void qr_invertible() if (internal::is_same::value) { // let's build a matrix more stable to inverse - MatrixType a = MatrixType::Random(size,size*2); + MatrixType a = MatrixType::Random(size,size*4); m1 += a * a.adjoint(); } @@ -81,8 +83,11 @@ template void qr_invertible() m3 = qr.householderQ(); // get a unitary m1 = m3 * m1 * m3; qr.compute(m1); - VERIFY_IS_APPROX(absdet, qr.absDeterminant()); VERIFY_IS_APPROX(log(absdet), qr.logAbsDeterminant()); + // This test is tricky if the determinant becomes too small. + // Since we generate random numbers with magnitude rrange [0,1], the average determinant is 0.5^size + VERIFY_IS_MUCH_SMALLER_THAN( abs(absdet-qr.absDeterminant()), numext::maxi(RealScalar(pow(0.5,size)),numext::maxi(abs(absdet),abs(qr.absDeterminant()))) ); + } template void qr_verify_assert() diff --git a/ground/gcs/src/libs/eigen/test/qr_colpivoting.cpp b/ground/gcs/src/libs/eigen/test/qr_colpivoting.cpp index eb3feac01a..26ed27f5cd 100644 --- a/ground/gcs/src/libs/eigen/test/qr_colpivoting.cpp +++ b/ground/gcs/src/libs/eigen/test/qr_colpivoting.cpp @@ -10,21 +10,103 @@ #include "main.h" #include +#include + +template +void cod() { + typedef typename MatrixType::Index Index; + + Index rows = internal::random(2, EIGEN_TEST_MAX_SIZE); + Index cols = internal::random(2, EIGEN_TEST_MAX_SIZE); + Index cols2 = internal::random(2, EIGEN_TEST_MAX_SIZE); + Index rank = internal::random(1, (std::min)(rows, cols) - 1); + + typedef typename MatrixType::Scalar Scalar; + typedef Matrix + MatrixQType; + MatrixType matrix; + createRandomPIMatrixOfRank(rank, rows, cols, matrix); + CompleteOrthogonalDecomposition cod(matrix); + VERIFY(rank == cod.rank()); + VERIFY(cols - cod.rank() == cod.dimensionOfKernel()); + VERIFY(!cod.isInjective()); + VERIFY(!cod.isInvertible()); + VERIFY(!cod.isSurjective()); + + MatrixQType q = cod.householderQ(); + VERIFY_IS_UNITARY(q); + + MatrixType z = cod.matrixZ(); + VERIFY_IS_UNITARY(z); + + MatrixType t; + t.setZero(rows, cols); + t.topLeftCorner(rank, rank) = + cod.matrixT().topLeftCorner(rank, rank).template triangularView(); + + MatrixType c = q * t * z * cod.colsPermutation().inverse(); + VERIFY_IS_APPROX(matrix, c); + + MatrixType exact_solution = MatrixType::Random(cols, cols2); + MatrixType rhs = matrix * exact_solution; + MatrixType cod_solution = cod.solve(rhs); + VERIFY_IS_APPROX(rhs, matrix * cod_solution); + + // Verify that we get the same minimum-norm solution as the SVD. + JacobiSVD svd(matrix, ComputeThinU | ComputeThinV); + MatrixType svd_solution = svd.solve(rhs); + VERIFY_IS_APPROX(cod_solution, svd_solution); + + MatrixType pinv = cod.pseudoInverse(); + VERIFY_IS_APPROX(cod_solution, pinv * rhs); +} + +template +void cod_fixedsize() { + enum { + Rows = MatrixType::RowsAtCompileTime, + Cols = MatrixType::ColsAtCompileTime + }; + typedef typename MatrixType::Scalar Scalar; + int rank = internal::random(1, (std::min)(int(Rows), int(Cols)) - 1); + Matrix matrix; + createRandomPIMatrixOfRank(rank, Rows, Cols, matrix); + CompleteOrthogonalDecomposition > cod(matrix); + VERIFY(rank == cod.rank()); + VERIFY(Cols - cod.rank() == cod.dimensionOfKernel()); + VERIFY(cod.isInjective() == (rank == Rows)); + VERIFY(cod.isSurjective() == (rank == Cols)); + VERIFY(cod.isInvertible() == (cod.isInjective() && cod.isSurjective())); + + Matrix exact_solution; + exact_solution.setRandom(Cols, Cols2); + Matrix rhs = matrix * exact_solution; + Matrix cod_solution = cod.solve(rhs); + VERIFY_IS_APPROX(rhs, matrix * cod_solution); + + // Verify that we get the same minimum-norm solution as the SVD. + JacobiSVD svd(matrix, ComputeFullU | ComputeFullV); + Matrix svd_solution = svd.solve(rhs); + VERIFY_IS_APPROX(cod_solution, svd_solution); +} template void qr() { + using std::sqrt; typedef typename MatrixType::Index Index; Index rows = internal::random(2,EIGEN_TEST_MAX_SIZE), cols = internal::random(2,EIGEN_TEST_MAX_SIZE), cols2 = internal::random(2,EIGEN_TEST_MAX_SIZE); Index rank = internal::random(1, (std::min)(rows, cols)-1); typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; typedef Matrix MatrixQType; MatrixType m1; createRandomPIMatrixOfRank(rank,rows,cols,m1); ColPivHouseholderQR qr(m1); - VERIFY(rank == qr.rank()); - VERIFY(cols - qr.rank() == qr.dimensionOfKernel()); + VERIFY_IS_EQUAL(rank, qr.rank()); + VERIFY_IS_EQUAL(cols - qr.rank(), qr.dimensionOfKernel()); VERIFY(!qr.isInjective()); VERIFY(!qr.isInvertible()); VERIFY(!qr.isSurjective()); @@ -36,26 +118,59 @@ template void qr() MatrixType c = q * r * qr.colsPermutation().inverse(); VERIFY_IS_APPROX(m1, c); + // Verify that the absolute value of the diagonal elements in R are + // non-increasing until they reach the singularity threshold. + RealScalar threshold = + sqrt(RealScalar(rows)) * numext::abs(r(0, 0)) * NumTraits::epsilon(); + for (Index i = 0; i < (std::min)(rows, cols) - 1; ++i) { + RealScalar x = numext::abs(r(i, i)); + RealScalar y = numext::abs(r(i + 1, i + 1)); + if (x < threshold && y < threshold) continue; + if (!test_isApproxOrLessThan(y, x)) { + for (Index j = 0; j < (std::min)(rows, cols); ++j) { + std::cout << "i = " << j << ", |r_ii| = " << numext::abs(r(j, j)) << std::endl; + } + std::cout << "Failure at i=" << i << ", rank=" << rank + << ", threshold=" << threshold << std::endl; + } + VERIFY_IS_APPROX_OR_LESS_THAN(y, x); + } + MatrixType m2 = MatrixType::Random(cols,cols2); MatrixType m3 = m1*m2; m2 = MatrixType::Random(cols,cols2); m2 = qr.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); + + { + Index size = rows; + do { + m1 = MatrixType::Random(size,size); + qr.compute(m1); + } while(!qr.isInvertible()); + MatrixType m1_inv = qr.inverse(); + m3 = m1 * MatrixType::Random(size,cols2); + m2 = qr.solve(m3); + VERIFY_IS_APPROX(m2, m1_inv*m3); + } } template void qr_fixedsize() { + using std::sqrt; + using std::abs; enum { Rows = MatrixType::RowsAtCompileTime, Cols = MatrixType::ColsAtCompileTime }; typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; int rank = internal::random(1, (std::min)(int(Rows), int(Cols))-1); Matrix m1; createRandomPIMatrixOfRank(rank,Rows,Cols,m1); ColPivHouseholderQR > qr(m1); - VERIFY(rank == qr.rank()); - VERIFY(Cols - qr.rank() == qr.dimensionOfKernel()); - VERIFY(qr.isInjective() == (rank == Rows)); - VERIFY(qr.isSurjective() == (rank == Cols)); - VERIFY(qr.isInvertible() == (qr.isInjective() && qr.isSurjective())); + VERIFY_IS_EQUAL(rank, qr.rank()); + VERIFY_IS_EQUAL(Cols - qr.rank(), qr.dimensionOfKernel()); + VERIFY_IS_EQUAL(qr.isInjective(), (rank == Rows)); + VERIFY_IS_EQUAL(qr.isSurjective(), (rank == Cols)); + VERIFY_IS_EQUAL(qr.isInvertible(), (qr.isInjective() && qr.isSurjective())); Matrix r = qr.matrixQR().template triangularView(); Matrix c = qr.householderQ() * r * qr.colsPermutation().inverse(); @@ -66,6 +181,71 @@ template void qr_fixedsize() m2 = Matrix::Random(Cols,Cols2); m2 = qr.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); + // Verify that the absolute value of the diagonal elements in R are + // non-increasing until they reache the singularity threshold. + RealScalar threshold = + sqrt(RealScalar(Rows)) * (std::abs)(r(0, 0)) * NumTraits::epsilon(); + for (Index i = 0; i < (std::min)(int(Rows), int(Cols)) - 1; ++i) { + RealScalar x = numext::abs(r(i, i)); + RealScalar y = numext::abs(r(i + 1, i + 1)); + if (x < threshold && y < threshold) continue; + if (!test_isApproxOrLessThan(y, x)) { + for (Index j = 0; j < (std::min)(int(Rows), int(Cols)); ++j) { + std::cout << "i = " << j << ", |r_ii| = " << numext::abs(r(j, j)) << std::endl; + } + std::cout << "Failure at i=" << i << ", rank=" << rank + << ", threshold=" << threshold << std::endl; + } + VERIFY_IS_APPROX_OR_LESS_THAN(y, x); + } +} + +// This test is meant to verify that pivots are chosen such that +// even for a graded matrix, the diagonal of R falls of roughly +// monotonically until it reaches the threshold for singularity. +// We use the so-called Kahan matrix, which is a famous counter-example +// for rank-revealing QR. See +// http://www.netlib.org/lapack/lawnspdf/lawn176.pdf +// page 3 for more detail. +template void qr_kahan_matrix() +{ + using std::sqrt; + using std::abs; + typedef typename MatrixType::Index Index; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + + Index rows = 300, cols = rows; + + MatrixType m1; + m1.setZero(rows,cols); + RealScalar s = std::pow(NumTraits::epsilon(), 1.0 / rows); + RealScalar c = std::sqrt(1 - s*s); + RealScalar pow_s_i(1.0); // pow(s,i) + for (Index i = 0; i < rows; ++i) { + m1(i, i) = pow_s_i; + m1.row(i).tail(rows - i - 1) = -pow_s_i * c * MatrixType::Ones(1, rows - i - 1); + pow_s_i *= s; + } + m1 = (m1 + m1.transpose()).eval(); + ColPivHouseholderQR qr(m1); + MatrixType r = qr.matrixQR().template triangularView(); + + RealScalar threshold = + std::sqrt(RealScalar(rows)) * numext::abs(r(0, 0)) * NumTraits::epsilon(); + for (Index i = 0; i < (std::min)(rows, cols) - 1; ++i) { + RealScalar x = numext::abs(r(i, i)); + RealScalar y = numext::abs(r(i + 1, i + 1)); + if (x < threshold && y < threshold) continue; + if (!test_isApproxOrLessThan(y, x)) { + for (Index j = 0; j < (std::min)(rows, cols); ++j) { + std::cout << "i = " << j << ", |r_ii| = " << numext::abs(r(j, j)) << std::endl; + } + std::cout << "Failure at i=" << i << ", rank=" << qr.rank() + << ", threshold=" << threshold << std::endl; + } + VERIFY_IS_APPROX_OR_LESS_THAN(y, x); + } } template void qr_invertible() @@ -131,6 +311,15 @@ void test_qr_colpivoting() CALL_SUBTEST_5(( qr_fixedsize, 1 >() )); } + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1( cod() ); + CALL_SUBTEST_2( cod() ); + CALL_SUBTEST_3( cod() ); + CALL_SUBTEST_4(( cod_fixedsize, 4 >() )); + CALL_SUBTEST_5(( cod_fixedsize, 3 >() )); + CALL_SUBTEST_5(( cod_fixedsize, 1 >() )); + } + for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( qr_invertible() ); CALL_SUBTEST_2( qr_invertible() ); @@ -147,4 +336,7 @@ void test_qr_colpivoting() // Test problem size constructors CALL_SUBTEST_9(ColPivHouseholderQR(10, 20)); + + CALL_SUBTEST_1( qr_kahan_matrix() ); + CALL_SUBTEST_2( qr_kahan_matrix() ); } diff --git a/ground/gcs/src/libs/eigen/test/qr_fullpivoting.cpp b/ground/gcs/src/libs/eigen/test/qr_fullpivoting.cpp index 511f2473f7..70e89c198b 100644 --- a/ground/gcs/src/libs/eigen/test/qr_fullpivoting.cpp +++ b/ground/gcs/src/libs/eigen/test/qr_fullpivoting.cpp @@ -15,16 +15,20 @@ template void qr() { typedef typename MatrixType::Index Index; - Index rows = internal::random(20,200), cols = internal::random(20,200), cols2 = internal::random(20,200); - Index rank = internal::random(1, (std::min)(rows, cols)-1); + Index max_size = EIGEN_TEST_MAX_SIZE; + Index min_size = numext::maxi(1,EIGEN_TEST_MAX_SIZE/10); + Index rows = internal::random(min_size,max_size), + cols = internal::random(min_size,max_size), + cols2 = internal::random(min_size,max_size), + rank = internal::random(1, (std::min)(rows, cols)-1); typedef typename MatrixType::Scalar Scalar; typedef Matrix MatrixQType; MatrixType m1; createRandomPIMatrixOfRank(rank,rows,cols,m1); FullPivHouseholderQR qr(m1); - VERIFY(rank == qr.rank()); - VERIFY(cols - qr.rank() == qr.dimensionOfKernel()); + VERIFY_IS_EQUAL(rank, qr.rank()); + VERIFY_IS_EQUAL(cols - qr.rank(), qr.dimensionOfKernel()); VERIFY(!qr.isInjective()); VERIFY(!qr.isInvertible()); VERIFY(!qr.isSurjective()); @@ -40,12 +44,28 @@ template void qr() MatrixType c = qr.matrixQ() * r * qr.colsPermutation().inverse(); VERIFY_IS_APPROX(m1, c); - + + // stress the ReturnByValue mechanism + MatrixType tmp; + VERIFY_IS_APPROX(tmp.noalias() = qr.matrixQ() * r, (qr.matrixQ() * r).eval()); + MatrixType m2 = MatrixType::Random(cols,cols2); MatrixType m3 = m1*m2; m2 = MatrixType::Random(cols,cols2); m2 = qr.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); + + { + Index size = rows; + do { + m1 = MatrixType::Random(size,size); + qr.compute(m1); + } while(!qr.isInvertible()); + MatrixType m1_inv = qr.inverse(); + m3 = m1 * MatrixType::Random(size,cols2); + m2 = qr.solve(m3); + VERIFY_IS_APPROX(m2, m1_inv*m3); + } } template void qr_invertible() @@ -55,7 +75,9 @@ template void qr_invertible() typedef typename NumTraits::Real RealScalar; typedef typename MatrixType::Scalar Scalar; - int size = internal::random(10,50); + Index max_size = numext::mini(50,EIGEN_TEST_MAX_SIZE); + Index min_size = numext::maxi(1,EIGEN_TEST_MAX_SIZE/10); + Index size = internal::random(min_size,max_size); MatrixType m1(size, size), m2(size, size), m3(size, size); m1 = MatrixType::Random(size,size); diff --git a/ground/gcs/src/libs/eigen/test/rand.cpp b/ground/gcs/src/libs/eigen/test/rand.cpp new file mode 100644 index 0000000000..51cf01773f --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/rand.cpp @@ -0,0 +1,118 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +typedef long long int64; + +template Scalar check_in_range(Scalar x, Scalar y) +{ + Scalar r = internal::random(x,y); + VERIFY(r>=x); + if(y>=x) + { + VERIFY(r<=y); + } + return r; +} + +template void check_all_in_range(Scalar x, Scalar y) +{ + Array mask(y-x+1); + mask.fill(0); + long n = (y-x+1)*32; + for(long k=0; k0).all() ); +} + +template void check_histogram(Scalar x, Scalar y, int bins) +{ + Array hist(bins); + hist.fill(0); + int f = 100000; + int n = bins*f; + int64 range = int64(y)-int64(x); + int divisor = int((range+1)/bins); + assert(((range+1)%bins)==0); + for(int k=0; k()/double(f))-1.0).abs()<0.02).all() ); +} + +void test_rand() +{ + long long_ref = NumTraits::highest()/10; + signed char char_offset = (std::min)(g_repeat,64); + signed char short_offset = (std::min)(g_repeat,16000); + + for(int i = 0; i < g_repeat*10000; i++) { + CALL_SUBTEST(check_in_range(10,11)); + CALL_SUBTEST(check_in_range(1.24234523,1.24234523)); + CALL_SUBTEST(check_in_range(-1,1)); + CALL_SUBTEST(check_in_range(-1432.2352,-1432.2352)); + + CALL_SUBTEST(check_in_range(10,11)); + CALL_SUBTEST(check_in_range(1.24234523,1.24234523)); + CALL_SUBTEST(check_in_range(-1,1)); + CALL_SUBTEST(check_in_range(-1432.2352,-1432.2352)); + + CALL_SUBTEST(check_in_range(0,-1)); + CALL_SUBTEST(check_in_range(0,-1)); + CALL_SUBTEST(check_in_range(0,-1)); + CALL_SUBTEST(check_in_range(-673456,673456)); + CALL_SUBTEST(check_in_range(-RAND_MAX+10,RAND_MAX-10)); + CALL_SUBTEST(check_in_range(-24345,24345)); + CALL_SUBTEST(check_in_range(-long_ref,long_ref)); + } + + CALL_SUBTEST(check_all_in_range(11,11)); + CALL_SUBTEST(check_all_in_range(11,11+char_offset)); + CALL_SUBTEST(check_all_in_range(-5,5)); + CALL_SUBTEST(check_all_in_range(-11-char_offset,-11)); + CALL_SUBTEST(check_all_in_range(-126,-126+char_offset)); + CALL_SUBTEST(check_all_in_range(126-char_offset,126)); + CALL_SUBTEST(check_all_in_range(-126,126)); + + CALL_SUBTEST(check_all_in_range(11,11)); + CALL_SUBTEST(check_all_in_range(11,11+short_offset)); + CALL_SUBTEST(check_all_in_range(-5,5)); + CALL_SUBTEST(check_all_in_range(-11-short_offset,-11)); + CALL_SUBTEST(check_all_in_range(-24345,-24345+short_offset)); + CALL_SUBTEST(check_all_in_range(24345,24345+short_offset)); + + CALL_SUBTEST(check_all_in_range(11,11)); + CALL_SUBTEST(check_all_in_range(11,11+g_repeat)); + CALL_SUBTEST(check_all_in_range(-5,5)); + CALL_SUBTEST(check_all_in_range(-11-g_repeat,-11)); + CALL_SUBTEST(check_all_in_range(-673456,-673456+g_repeat)); + CALL_SUBTEST(check_all_in_range(673456,673456+g_repeat)); + + CALL_SUBTEST(check_all_in_range(11,11)); + CALL_SUBTEST(check_all_in_range(11,11+g_repeat)); + CALL_SUBTEST(check_all_in_range(-5,5)); + CALL_SUBTEST(check_all_in_range(-11-g_repeat,-11)); + CALL_SUBTEST(check_all_in_range(-long_ref,-long_ref+g_repeat)); + CALL_SUBTEST(check_all_in_range( long_ref, long_ref+g_repeat)); + + CALL_SUBTEST(check_histogram(-5,5,11)); + int bins = 100; + CALL_SUBTEST(check_histogram(-3333,-3333+bins*(3333/bins)-1,bins)); + bins = 1000; + CALL_SUBTEST(check_histogram(-RAND_MAX+10,-RAND_MAX+10+bins*(RAND_MAX/bins)-1,bins)); + CALL_SUBTEST(check_histogram(-RAND_MAX+10,-int64(RAND_MAX)+10+bins*(2*int64(RAND_MAX)/bins)-1,bins)); +} diff --git a/ground/gcs/src/libs/eigen/test/real_qz.cpp b/ground/gcs/src/libs/eigen/test/real_qz.cpp index a1766c6d95..99ac31235f 100644 --- a/ground/gcs/src/libs/eigen/test/real_qz.cpp +++ b/ground/gcs/src/libs/eigen/test/real_qz.cpp @@ -7,6 +7,7 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define EIGEN_RUNTIME_NO_MALLOC #include "main.h" #include #include @@ -41,7 +42,11 @@ template void real_qz(const MatrixType& m) break; } - RealQZ qz(A,B); + RealQZ qz(dim); + // TODO enable full-prealocation of required memory, this probably requires an in-place mode for HessenbergDecomposition + //Eigen::internal::set_is_malloc_allowed(false); + qz.compute(A,B); + //Eigen::internal::set_is_malloc_allowed(true); VERIFY_IS_EQUAL(qz.info(), Success); // check for zeros @@ -49,11 +54,20 @@ template void real_qz(const MatrixType& m) for (Index i=0; i void matrixRedux(const MatrixType& m) @@ -21,7 +24,7 @@ template void matrixRedux(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols); // The entries of m1 are uniformly distributed in [0,1], so m1.prod() is very small. This may lead to test - // failures if we underflow into denormals. Thus, we scale so that entires are close to 1. + // failures if we underflow into denormals. Thus, we scale so that entries are close to 1. MatrixType m1_for_prod = MatrixType::Ones(rows, cols) + RealScalar(0.2) * m1; VERIFY_IS_MUCH_SMALLER_THAN(MatrixType::Zero(rows, cols).sum(), Scalar(1)); @@ -65,6 +68,12 @@ template void matrixRedux(const MatrixType& m) // test empty objects VERIFY_IS_APPROX(m1.block(r0,c0,0,0).sum(), Scalar(0)); VERIFY_IS_APPROX(m1.block(r0,c0,0,0).prod(), Scalar(1)); + + // test nesting complex expression + VERIFY_EVALUATION_COUNT( (m1.matrix()*m1.matrix().transpose()).sum(), (MatrixType::IsVectorAtCompileTime && MatrixType::SizeAtCompileTime!=1 ? 0 : 1) ); + Matrix m2(rows,rows); + m2.setRandom(); + VERIFY_EVALUATION_COUNT( ((m1.matrix()*m1.matrix().transpose())+m2).sum(),(MatrixType::IsVectorAtCompileTime && MatrixType::SizeAtCompileTime!=1 ? 0 : 1)); } template void vectorRedux(const VectorType& w) @@ -147,8 +156,10 @@ void test_redux() CALL_SUBTEST_1( matrixRedux(Array()) ); CALL_SUBTEST_2( matrixRedux(Matrix2f()) ); CALL_SUBTEST_2( matrixRedux(Array2f()) ); + CALL_SUBTEST_2( matrixRedux(Array22f()) ); CALL_SUBTEST_3( matrixRedux(Matrix4d()) ); CALL_SUBTEST_3( matrixRedux(Array4d()) ); + CALL_SUBTEST_3( matrixRedux(Array44d()) ); CALL_SUBTEST_4( matrixRedux(MatrixXcf(internal::random(1,maxsize), internal::random(1,maxsize))) ); CALL_SUBTEST_4( matrixRedux(ArrayXXcf(internal::random(1,maxsize), internal::random(1,maxsize))) ); CALL_SUBTEST_5( matrixRedux(MatrixXd (internal::random(1,maxsize), internal::random(1,maxsize))) ); diff --git a/ground/gcs/src/libs/eigen/test/ref.cpp b/ground/gcs/src/libs/eigen/test/ref.cpp index 8297e263a9..769db0414b 100644 --- a/ground/gcs/src/libs/eigen/test/ref.cpp +++ b/ground/gcs/src/libs/eigen/test/ref.cpp @@ -12,26 +12,10 @@ #undef EIGEN_DEFAULT_TO_ROW_MAJOR #endif -static int nb_temporaries; - -inline void on_temporary_creation(int) { - // here's a great place to set a breakpoint when debugging failures in this test! - nb_temporaries++; -} - - -#define EIGEN_DENSE_STORAGE_CTOR_PLUGIN { on_temporary_creation(size); } +#define TEST_ENABLE_TEMPORARY_TRACKING #include "main.h" -#define VERIFY_EVALUATION_COUNT(XPR,N) {\ - nb_temporaries = 0; \ - XPR; \ - if(nb_temporaries!=N) std::cerr << "nb_temporaries == " << nb_temporaries << "\n"; \ - VERIFY( (#XPR) && nb_temporaries==N ); \ - } - - // test Ref.h // Deal with i387 extended precision @@ -248,6 +232,12 @@ int test_ref_overload_fun1(Ref ) { return 3; } int test_ref_overload_fun2(Ref ) { return 4; } int test_ref_overload_fun2(Ref ) { return 5; } +void test_ref_ambiguous(const Ref &A, Ref B) +{ + B = A; + B = A - A; +} + // See also bug 969 void test_ref_overloads() { @@ -260,6 +250,9 @@ void test_ref_overloads() VERIFY( test_ref_overload_fun2(Ad)==4 ); VERIFY( test_ref_overload_fun2(Ad+Bd)==4 ); VERIFY( test_ref_overload_fun2(Af+Bf)==5 ); + + ArrayXd A, B; + test_ref_ambiguous(A, B); } void test_ref() diff --git a/ground/gcs/src/libs/eigen/test/runtest.sh b/ground/gcs/src/libs/eigen/test/runtest.sh deleted file mode 100755 index 2be250819f..0000000000 --- a/ground/gcs/src/libs/eigen/test/runtest.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/bin/bash - -black='\E[30m' -red='\E[31m' -green='\E[32m' -yellow='\E[33m' -blue='\E[34m' -magenta='\E[35m' -cyan='\E[36m' -white='\E[37m' - -if ! ./$1 > /dev/null 2> .runtest.log ; then - echo -e $red Test $1 failed: $black - echo -e $blue - cat .runtest.log - echo -e $black - exit 1 -else -echo -e $green Test $1 passed$black -fi diff --git a/ground/gcs/src/libs/eigen/test/rvalue_types.cpp b/ground/gcs/src/libs/eigen/test/rvalue_types.cpp index b3c85652cb..8887f1b1b9 100644 --- a/ground/gcs/src/libs/eigen/test/rvalue_types.cpp +++ b/ground/gcs/src/libs/eigen/test/rvalue_types.cpp @@ -11,20 +11,22 @@ #include -#ifdef EIGEN_HAVE_RVALUE_REFERENCES +using internal::UIntPtr; + +#if EIGEN_HAS_RVALUE_REFERENCES template void rvalue_copyassign(const MatrixType& m) { typedef typename internal::traits::Scalar Scalar; - + // create a temporary which we are about to destroy by moving MatrixType tmp = m; - long src_address = reinterpret_cast(tmp.data()); - + UIntPtr src_address = reinterpret_cast(tmp.data()); + // move the temporary to n MatrixType n = std::move(tmp); - long dst_address = reinterpret_cast(n.data()); + UIntPtr dst_address = reinterpret_cast(n.data()); if (MatrixType::RowsAtCompileTime==Dynamic|| MatrixType::ColsAtCompileTime==Dynamic) { @@ -51,7 +53,7 @@ void test_rvalue_types() CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); - + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); diff --git a/ground/gcs/src/libs/eigen/test/schur_complex.cpp b/ground/gcs/src/libs/eigen/test/schur_complex.cpp index 5e869790f4..deb78e44e6 100644 --- a/ground/gcs/src/libs/eigen/test/schur_complex.cpp +++ b/ground/gcs/src/libs/eigen/test/schur_complex.cpp @@ -25,7 +25,7 @@ template void schur(int size = MatrixType::ColsAtCompileTim ComplexMatrixType T = schurOfA.matrixT(); for(int row = 1; row < size; ++row) { for(int col = 0; col < row; ++col) { - VERIFY(T(row,col) == (typename MatrixType::Scalar)0); + VERIFY(T(row,col) == (typename MatrixType::Scalar)0); } } VERIFY_IS_APPROX(A.template cast(), U * T * U.adjoint()); @@ -70,7 +70,7 @@ template void schur(int size = MatrixType::ColsAtCompileTim VERIFY_IS_EQUAL(cs1.matrixT(), csOnlyT.matrixT()); VERIFY_RAISES_ASSERT(csOnlyT.matrixU()); - if (size > 1) + if (size > 1 && size < 20) { // Test matrix with NaN A(0,0) = std::numeric_limits::quiet_NaN(); diff --git a/ground/gcs/src/libs/eigen/test/schur_real.cpp b/ground/gcs/src/libs/eigen/test/schur_real.cpp index 36b9c24d1d..4aede87df6 100644 --- a/ground/gcs/src/libs/eigen/test/schur_real.cpp +++ b/ground/gcs/src/libs/eigen/test/schur_real.cpp @@ -82,7 +82,7 @@ template void schur(int size = MatrixType::ColsAtCompileTim Atriangular.template triangularView().setZero(); rs3.setMaxIterations(1).compute(Atriangular); // triangular matrices do not need any iterations VERIFY_IS_EQUAL(rs3.info(), Success); - VERIFY_IS_EQUAL(rs3.matrixT(), Atriangular); + VERIFY_IS_APPROX(rs3.matrixT(), Atriangular); // approx because of scaling... VERIFY_IS_EQUAL(rs3.matrixU(), MatrixType::Identity(size, size)); // Test computation of only T, not U @@ -91,7 +91,7 @@ template void schur(int size = MatrixType::ColsAtCompileTim VERIFY_IS_EQUAL(rs1.matrixT(), rsOnlyT.matrixT()); VERIFY_RAISES_ASSERT(rsOnlyT.matrixU()); - if (size > 2) + if (size > 2 && size < 20) { // Test matrix with NaN A(0,0) = std::numeric_limits::quiet_NaN(); diff --git a/ground/gcs/src/libs/eigen/test/selfadjoint.cpp b/ground/gcs/src/libs/eigen/test/selfadjoint.cpp index 76dab6d64a..92401e5065 100644 --- a/ground/gcs/src/libs/eigen/test/selfadjoint.cpp +++ b/ground/gcs/src/libs/eigen/test/selfadjoint.cpp @@ -21,7 +21,9 @@ template void selfadjoint(const MatrixType& m) Index cols = m.cols(); MatrixType m1 = MatrixType::Random(rows, cols), - m3(rows, cols); + m2 = MatrixType::Random(rows, cols), + m3(rows, cols), + m4(rows, cols); m1.diagonal() = m1.diagonal().real().template cast(); @@ -30,10 +32,19 @@ template void selfadjoint(const MatrixType& m) VERIFY_IS_APPROX(MatrixType(m3.template triangularView()), MatrixType(m1.template triangularView())); VERIFY_IS_APPROX(m3, m3.adjoint()); - m3 = m1.template selfadjointView(); VERIFY_IS_APPROX(MatrixType(m3.template triangularView()), MatrixType(m1.template triangularView())); VERIFY_IS_APPROX(m3, m3.adjoint()); + + m3 = m1.template selfadjointView(); + m4 = m2; + m4 += m1.template selfadjointView(); + VERIFY_IS_APPROX(m4, m2+m3); + + m3 = m1.template selfadjointView(); + m4 = m2; + m4 -= m1.template selfadjointView(); + VERIFY_IS_APPROX(m4, m2-m3); } void bug_159() diff --git a/ground/gcs/src/libs/eigen/test/simplicial_cholesky.cpp b/ground/gcs/src/libs/eigen/test/simplicial_cholesky.cpp index 7864684210..649c817b4b 100644 --- a/ground/gcs/src/libs/eigen/test/simplicial_cholesky.cpp +++ b/ground/gcs/src/libs/eigen/test/simplicial_cholesky.cpp @@ -9,16 +9,17 @@ #include "sparse_solver.h" -template void test_simplicial_cholesky_T() +template void test_simplicial_cholesky_T() { - SimplicialCholesky, Lower> chol_colmajor_lower_amd; - SimplicialCholesky, Upper> chol_colmajor_upper_amd; - SimplicialLLT, Lower> llt_colmajor_lower_amd; - SimplicialLLT, Upper> llt_colmajor_upper_amd; - SimplicialLDLT, Lower> ldlt_colmajor_lower_amd; - SimplicialLDLT, Upper> ldlt_colmajor_upper_amd; - SimplicialLDLT, Lower, NaturalOrdering > ldlt_colmajor_lower_nat; - SimplicialLDLT, Upper, NaturalOrdering > ldlt_colmajor_upper_nat; + typedef SparseMatrix SparseMatrixType; + SimplicialCholesky chol_colmajor_lower_amd; + SimplicialCholesky chol_colmajor_upper_amd; + SimplicialLLT< SparseMatrixType, Lower> llt_colmajor_lower_amd; + SimplicialLLT< SparseMatrixType, Upper> llt_colmajor_upper_amd; + SimplicialLDLT< SparseMatrixType, Lower> ldlt_colmajor_lower_amd; + SimplicialLDLT< SparseMatrixType, Upper> ldlt_colmajor_upper_amd; + SimplicialLDLT< SparseMatrixType, Lower, NaturalOrdering > ldlt_colmajor_lower_nat; + SimplicialLDLT< SparseMatrixType, Upper, NaturalOrdering > ldlt_colmajor_upper_nat; check_sparse_spd_solving(chol_colmajor_lower_amd); check_sparse_spd_solving(chol_colmajor_upper_amd); @@ -34,12 +35,13 @@ template void test_simplicial_cholesky_T() check_sparse_spd_determinant(ldlt_colmajor_lower_amd); check_sparse_spd_determinant(ldlt_colmajor_upper_amd); - check_sparse_spd_solving(ldlt_colmajor_lower_nat); - check_sparse_spd_solving(ldlt_colmajor_upper_nat); + check_sparse_spd_solving(ldlt_colmajor_lower_nat, 300, 1000); + check_sparse_spd_solving(ldlt_colmajor_upper_nat, 300, 1000); } void test_simplicial_cholesky() { - CALL_SUBTEST_1(test_simplicial_cholesky_T()); - CALL_SUBTEST_2(test_simplicial_cholesky_T >()); + CALL_SUBTEST_1(( test_simplicial_cholesky_T() )); + CALL_SUBTEST_2(( test_simplicial_cholesky_T, int>() )); + CALL_SUBTEST_3(( test_simplicial_cholesky_T() )); } diff --git a/ground/gcs/src/libs/eigen/test/sizeof.cpp b/ground/gcs/src/libs/eigen/test/sizeof.cpp index d9ad356205..03ad204538 100644 --- a/ground/gcs/src/libs/eigen/test/sizeof.cpp +++ b/ground/gcs/src/libs/eigen/test/sizeof.cpp @@ -13,14 +13,27 @@ template void verifySizeOf(const MatrixType&) { typedef typename MatrixType::Scalar Scalar; if (MatrixType::RowsAtCompileTime!=Dynamic && MatrixType::ColsAtCompileTime!=Dynamic) - VERIFY(std::ptrdiff_t(sizeof(MatrixType))==std::ptrdiff_t(sizeof(Scalar))*std::ptrdiff_t(MatrixType::SizeAtCompileTime)); + VERIFY_IS_EQUAL(std::ptrdiff_t(sizeof(MatrixType)),std::ptrdiff_t(sizeof(Scalar))*std::ptrdiff_t(MatrixType::SizeAtCompileTime)); else - VERIFY(sizeof(MatrixType)==sizeof(Scalar*) + 2 * sizeof(typename MatrixType::Index)); + VERIFY_IS_EQUAL(sizeof(MatrixType),sizeof(Scalar*) + 2 * sizeof(typename MatrixType::Index)); } void test_sizeof() { CALL_SUBTEST(verifySizeOf(Matrix()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Array()) ); + CALL_SUBTEST(verifySizeOf(Vector2d()) ); + CALL_SUBTEST(verifySizeOf(Vector4f()) ); CALL_SUBTEST(verifySizeOf(Matrix4d()) ); CALL_SUBTEST(verifySizeOf(Matrix()) ); CALL_SUBTEST(verifySizeOf(Matrix()) ); diff --git a/ground/gcs/src/libs/eigen/test/sizeoverflow.cpp b/ground/gcs/src/libs/eigen/test/sizeoverflow.cpp index 16d6f8d040..240d22294c 100644 --- a/ground/gcs/src/libs/eigen/test/sizeoverflow.cpp +++ b/ground/gcs/src/libs/eigen/test/sizeoverflow.cpp @@ -18,8 +18,6 @@ VERIFY(threw && "should have thrown bad_alloc: " #a); \ } -typedef DenseIndex Index; - template void triggerMatrixBadAlloc(Index rows, Index cols) { diff --git a/ground/gcs/src/libs/eigen/test/sparse.h b/ground/gcs/src/libs/eigen/test/sparse.h index e19a76316e..9912e1e246 100644 --- a/ground/gcs/src/libs/eigen/test/sparse.h +++ b/ground/gcs/src/libs/eigen/test/sparse.h @@ -53,15 +53,15 @@ enum { * \param zeroCoords and nonzeroCoords allows to get the coordinate lists of the non zero, * and zero coefficients respectively. */ -template void +template void initSparse(double density, Matrix& refMat, - SparseMatrix& sparseMat, + SparseMatrix& sparseMat, int flags = 0, - std::vector >* zeroCoords = 0, - std::vector >* nonzeroCoords = 0) + std::vector >* zeroCoords = 0, + std::vector >* nonzeroCoords = 0) { - enum { IsRowMajor = SparseMatrix::IsRowMajor }; + enum { IsRowMajor = SparseMatrix::IsRowMajor }; sparseMat.setZero(); //sparseMat.reserve(int(refMat.rows()*refMat.cols()*density)); sparseMat.reserve(VectorXi::Constant(IsRowMajor ? refMat.rows() : refMat.cols(), int((1.5*density)*(IsRowMajor?refMat.cols():refMat.rows())))); @@ -71,14 +71,17 @@ initSparse(double density, //sparseMat.startVec(j); for(Index i=0; i(0,1) < density) ? internal::random() : Scalar(0); if ((flags&ForceNonZeroDiag) && (i==j)) { + // FIXME: the following is too conservative v = internal::random()*Scalar(3.); - v = v*v + Scalar(5.); + v = v*v; + if(numext::real(v)>0) v += Scalar(5); + else v -= Scalar(5); } if ((flags & MakeLowerTriangular) && aj>ai) v = Scalar(0); @@ -93,11 +96,11 @@ initSparse(double density, //sparseMat.insertBackByOuterInner(j,i) = v; sparseMat.insertByOuterInner(j,i) = v; if (nonzeroCoords) - nonzeroCoords->push_back(Matrix (ai,aj)); + nonzeroCoords->push_back(Matrix (ai,aj)); } else if (zeroCoords) { - zeroCoords->push_back(Matrix (ai,aj)); + zeroCoords->push_back(Matrix (ai,aj)); } refMat(ai,aj) = v; } @@ -163,7 +166,7 @@ initSparse(double density, { sparseVec.reserve(int(refVec.size()*density)); sparseVec.setZero(); - for(Index i=0; i(0,1) < density) ? internal::random() : Scalar(0); if (v!=Scalar(0)) diff --git a/ground/gcs/src/libs/eigen/test/sparse_basic.cpp b/ground/gcs/src/libs/eigen/test/sparse_basic.cpp index abe6a9d143..384985028f 100644 --- a/ground/gcs/src/libs/eigen/test/sparse_basic.cpp +++ b/ground/gcs/src/libs/eigen/test/sparse_basic.cpp @@ -9,22 +9,28 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +static long g_realloc_count = 0; +#define EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN g_realloc_count++; + #include "sparse.h" template void sparse_basic(const SparseMatrixType& ref) { - typedef typename SparseMatrixType::Index Index; - typedef Matrix Vector2; + typedef typename SparseMatrixType::StorageIndex StorageIndex; + typedef Matrix Vector2; const Index rows = ref.rows(); const Index cols = ref.cols(); + //const Index inner = ref.innerSize(); + //const Index outer = ref.outerSize(); + typedef typename SparseMatrixType::Scalar Scalar; + typedef typename SparseMatrixType::RealScalar RealScalar; enum { Flags = SparseMatrixType::Flags }; double density = (std::max)(8./(rows*cols), 0.01); typedef Matrix DenseMatrix; typedef Matrix DenseVector; - typedef Matrix RowDenseVector; Scalar eps = 1e-6; Scalar s1 = internal::random(); @@ -37,94 +43,22 @@ template void sparse_basic(const SparseMatrixType& re std::vector nonzeroCoords; initSparse(density, refMat, m, 0, &zeroCoords, &nonzeroCoords); - if (zeroCoords.size()==0 || nonzeroCoords.size()==0) - return; - // test coeff and coeffRef - for (int i=0; i<(int)zeroCoords.size(); ++i) + for (std::size_t i=0; i >::value) - VERIFY_RAISES_ASSERT( m.coeffRef(zeroCoords[0].x(),zeroCoords[0].y()) = 5 ); + VERIFY_RAISES_ASSERT( m.coeffRef(zeroCoords[i].x(),zeroCoords[i].y()) = 5 ); } VERIFY_IS_APPROX(m, refMat); - m.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); - refMat.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); + if(!nonzeroCoords.empty()) { + m.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); + refMat.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); + } VERIFY_IS_APPROX(m, refMat); - - // test InnerIterators and Block expressions - for (int t=0; t<10; ++t) - { - int j = internal::random(0,cols-1); - int i = internal::random(0,rows-1); - int w = internal::random(1,cols-j-1); - int h = internal::random(1,rows-i-1); - VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); - for(int c=0; c void sparse_basic(const SparseMatrixType& re DenseMatrix m1(rows,cols); m1.setZero(); SparseMatrixType m2(rows,cols); - if(internal::random()%2) - m2.reserve(VectorXi::Constant(m2.outerSize(), 2)); + bool call_reserve = internal::random()%2; + Index nnz = internal::random(1,int(rows)/2); + if(call_reserve) + { + if(internal::random()%2) + m2.reserve(VectorXi::Constant(m2.outerSize(), int(nnz))); + else + m2.reserve(m2.outerSize() * nnz); + } + g_realloc_count = 0; for (Index j=0; j(0,rows-1); if (m1.coeff(i,j)==Scalar(0)) m2.insert(i,j) = m1(i,j) = internal::random(); } } + + if(call_reserve && !SparseMatrixType::IsRowMajor) + { + VERIFY(g_realloc_count==0); + } + m2.finalize(); VERIFY_IS_APPROX(m2,m1); } @@ -179,9 +127,9 @@ template void sparse_basic(const SparseMatrixType& re DenseMatrix m1(rows,cols); m1.setZero(); SparseMatrixType m2(rows,cols); - VectorXi r(VectorXi::Constant(m2.outerSize(), ((mode%2)==0) ? m2.innerSize() : std::max(1,m2.innerSize()/8))); + VectorXi r(VectorXi::Constant(m2.outerSize(), ((mode%2)==0) ? int(m2.innerSize()) : std::max(1,int(m2.innerSize())/8))); m2.reserve(r); - for (int k=0; k(0,rows-1); Index j = internal::random(0,cols-1); @@ -195,110 +143,46 @@ template void sparse_basic(const SparseMatrixType& re VERIFY_IS_APPROX(m2,m1); } - // test innerVector() - { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); - initSparse(density, refMat2, m2); - Index j0 = internal::random(0,rows-1); - Index j1 = internal::random(0,rows-1); - if(SparseMatrixType::IsRowMajor) - VERIFY_IS_APPROX(m2.innerVector(j0), refMat2.row(j0)); - else - VERIFY_IS_APPROX(m2.innerVector(j0), refMat2.col(j0)); - - if(SparseMatrixType::IsRowMajor) - VERIFY_IS_APPROX(m2.innerVector(j0)+m2.innerVector(j1), refMat2.row(j0)+refMat2.row(j1)); - else - VERIFY_IS_APPROX(m2.innerVector(j0)+m2.innerVector(j1), refMat2.col(j0)+refMat2.col(j1)); - - SparseMatrixType m3(rows,rows); - m3.reserve(VectorXi::Constant(rows,rows/2)); - for(Index j=0; j0) - VERIFY(j==numext::real(m3.innerVector(j).lastCoeff())); - } - m3.makeCompressed(); - for(Index j=0; j0) - VERIFY(j==numext::real(m3.innerVector(j).lastCoeff())); - } - - //m2.innerVector(j0) = 2*m2.innerVector(j1); - //refMat2.col(j0) = 2*refMat2.col(j1); - //VERIFY_IS_APPROX(m2, refMat2); - } - - // test innerVectors() - { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); - initSparse(density, refMat2, m2); - if(internal::random(0,1)>0.5) m2.makeCompressed(); - - Index j0 = internal::random(0,rows-2); - Index j1 = internal::random(0,rows-2); - Index n0 = internal::random(1,rows-(std::max)(j0,j1)); - if(SparseMatrixType::IsRowMajor) - VERIFY_IS_APPROX(m2.innerVectors(j0,n0), refMat2.block(j0,0,n0,cols)); - else - VERIFY_IS_APPROX(m2.innerVectors(j0,n0), refMat2.block(0,j0,rows,n0)); - if(SparseMatrixType::IsRowMajor) - VERIFY_IS_APPROX(m2.innerVectors(j0,n0)+m2.innerVectors(j1,n0), - refMat2.middleRows(j0,n0)+refMat2.middleRows(j1,n0)); - else - VERIFY_IS_APPROX(m2.innerVectors(j0,n0)+m2.innerVectors(j1,n0), - refMat2.block(0,j0,rows,n0)+refMat2.block(0,j1,rows,n0)); - - VERIFY_IS_APPROX(m2, refMat2); - - m2.innerVectors(j0,n0) = m2.innerVectors(j0,n0) + m2.innerVectors(j1,n0); - if(SparseMatrixType::IsRowMajor) - refMat2.middleRows(j0,n0) = (refMat2.middleRows(j0,n0) + refMat2.middleRows(j1,n0)).eval(); - else - refMat2.middleCols(j0,n0) = (refMat2.middleCols(j0,n0) + refMat2.middleCols(j1,n0)).eval(); - - VERIFY_IS_APPROX(m2, refMat2); - - } - // test basic computations { - DenseMatrix refM1 = DenseMatrix::Zero(rows, rows); - DenseMatrix refM2 = DenseMatrix::Zero(rows, rows); - DenseMatrix refM3 = DenseMatrix::Zero(rows, rows); - DenseMatrix refM4 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m1(rows, rows); - SparseMatrixType m2(rows, rows); - SparseMatrixType m3(rows, rows); - SparseMatrixType m4(rows, rows); + DenseMatrix refM1 = DenseMatrix::Zero(rows, cols); + DenseMatrix refM2 = DenseMatrix::Zero(rows, cols); + DenseMatrix refM3 = DenseMatrix::Zero(rows, cols); + DenseMatrix refM4 = DenseMatrix::Zero(rows, cols); + SparseMatrixType m1(rows, cols); + SparseMatrixType m2(rows, cols); + SparseMatrixType m3(rows, cols); + SparseMatrixType m4(rows, cols); initSparse(density, refM1, m1); initSparse(density, refM2, m2); initSparse(density, refM3, m3); initSparse(density, refM4, m4); + if(internal::random()) + m1.makeCompressed(); + + Index m1_nnz = m1.nonZeros(); + + VERIFY_IS_APPROX(m1*s1, refM1*s1); VERIFY_IS_APPROX(m1+m2, refM1+refM2); VERIFY_IS_APPROX(m1+m2+m3, refM1+refM2+refM3); VERIFY_IS_APPROX(m3.cwiseProduct(m1+m2), refM3.cwiseProduct(refM1+refM2)); VERIFY_IS_APPROX(m1*s1-m2, refM1*s1-refM2); - - VERIFY_IS_APPROX(m1*=s1, refM1*=s1); - VERIFY_IS_APPROX(m1/=s1, refM1/=s1); - - VERIFY_IS_APPROX(m1+=m2, refM1+=refM2); - VERIFY_IS_APPROX(m1-=m2, refM1-=refM2); + VERIFY_IS_APPROX(m4=m1/s1, refM1/s1); + VERIFY_IS_EQUAL(m4.nonZeros(), m1_nnz); if(SparseMatrixType::IsRowMajor) VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.row(0)), refM1.row(0).dot(refM2.row(0))); else - VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.row(0)), refM1.col(0).dot(refM2.row(0))); + VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.col(0)), refM1.col(0).dot(refM2.col(0))); + + DenseVector rv = DenseVector::Random(m1.cols()); + DenseVector cv = DenseVector::Random(m1.rows()); + Index r = internal::random(0,m1.rows()-2); + Index c = internal::random(0,m1.cols()-1); + VERIFY_IS_APPROX(( m1.template block<1,Dynamic>(r,0,1,m1.cols()).dot(rv)) , refM1.row(r).dot(rv)); + VERIFY_IS_APPROX(m1.row(r).dot(rv), refM1.row(r).dot(rv)); + VERIFY_IS_APPROX(m1.col(c).dot(cv), refM1.col(c).dot(cv)); VERIFY_IS_APPROX(m1.conjugate(), refM1.conjugate()); VERIFY_IS_APPROX(m1.real(), refM1.real()); @@ -310,103 +194,163 @@ template void sparse_basic(const SparseMatrixType& re VERIFY_IS_APPROX(refM4.cwiseProduct(m3), refM4.cwiseProduct(refM3)); // VERIFY_IS_APPROX(m3.cwise()/refM4, refM3.cwise()/refM4); + VERIFY_IS_APPROX(refM4 + m3, refM4 + refM3); + VERIFY_IS_APPROX(m3 + refM4, refM3 + refM4); + VERIFY_IS_APPROX(refM4 - m3, refM4 - refM3); + VERIFY_IS_APPROX(m3 - refM4, refM3 - refM4); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + RealScalar(0.5)*m3).eval(), RealScalar(0.5)*refM4 + RealScalar(0.5)*refM3); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + m3*RealScalar(0.5)).eval(), RealScalar(0.5)*refM4 + RealScalar(0.5)*refM3); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + m3.cwiseProduct(m3)).eval(), RealScalar(0.5)*refM4 + refM3.cwiseProduct(refM3)); + + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + RealScalar(0.5)*m3).eval(), RealScalar(0.5)*refM4 + RealScalar(0.5)*refM3); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + m3*RealScalar(0.5)).eval(), RealScalar(0.5)*refM4 + RealScalar(0.5)*refM3); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + (m3+m3)).eval(), RealScalar(0.5)*refM4 + (refM3+refM3)); + VERIFY_IS_APPROX(((refM3+m3)+RealScalar(0.5)*m3).eval(), RealScalar(0.5)*refM3 + (refM3+refM3)); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + (refM3+m3)).eval(), RealScalar(0.5)*refM4 + (refM3+refM3)); + VERIFY_IS_APPROX((RealScalar(0.5)*refM4 + (m3+refM3)).eval(), RealScalar(0.5)*refM4 + (refM3+refM3)); + + + VERIFY_IS_APPROX(m1.sum(), refM1.sum()); + + m4 = m1; refM4 = m4; + + VERIFY_IS_APPROX(m1*=s1, refM1*=s1); + VERIFY_IS_EQUAL(m1.nonZeros(), m1_nnz); + VERIFY_IS_APPROX(m1/=s1, refM1/=s1); + VERIFY_IS_EQUAL(m1.nonZeros(), m1_nnz); + + VERIFY_IS_APPROX(m1+=m2, refM1+=refM2); + VERIFY_IS_APPROX(m1-=m2, refM1-=refM2); + + if (rows>=2 && cols>=2) + { + VERIFY_RAISES_ASSERT( m1 += m1.innerVector(0) ); + VERIFY_RAISES_ASSERT( m1 -= m1.innerVector(0) ); + VERIFY_RAISES_ASSERT( refM1 -= m1.innerVector(0) ); + VERIFY_RAISES_ASSERT( refM1 += m1.innerVector(0) ); + m1 = m4; refM1 = refM4; + } + // test aliasing VERIFY_IS_APPROX((m1 = -m1), (refM1 = -refM1)); + VERIFY_IS_EQUAL(m1.nonZeros(), m1_nnz); + m1 = m4; refM1 = refM4; VERIFY_IS_APPROX((m1 = m1.transpose()), (refM1 = refM1.transpose().eval())); + VERIFY_IS_EQUAL(m1.nonZeros(), m1_nnz); + m1 = m4; refM1 = refM4; VERIFY_IS_APPROX((m1 = -m1.transpose()), (refM1 = -refM1.transpose().eval())); + VERIFY_IS_EQUAL(m1.nonZeros(), m1_nnz); + m1 = m4; refM1 = refM4; VERIFY_IS_APPROX((m1 += -m1), (refM1 += -refM1)); + VERIFY_IS_EQUAL(m1.nonZeros(), m1_nnz); + m1 = m4; refM1 = refM4; + + if(m1.isCompressed()) + { + VERIFY_IS_APPROX(m1.coeffs().sum(), m1.sum()); + m1.coeffs() += s1; + for(Index j = 0; j SpBool; + SpBool mb1 = m1.real().template cast(); + SpBool mb2 = m2.real().template cast(); + VERIFY_IS_EQUAL(mb1.template cast().sum(), refM1.real().template cast().count()); + VERIFY_IS_EQUAL((mb1 && mb2).template cast().sum(), (refM1.real().template cast() && refM2.real().template cast()).count()); + VERIFY_IS_EQUAL((mb1 || mb2).template cast().sum(), (refM1.real().template cast() || refM2.real().template cast()).count()); + SpBool mb3 = mb1 && mb2; + if(mb1.coeffs().all() && mb2.coeffs().all()) + { + VERIFY_IS_EQUAL(mb3.nonZeros(), (refM1.real().template cast() && refM2.real().template cast()).count()); + } + } } - // test transpose + // test reverse iterators { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); + DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols); + SparseMatrixType m2(rows, cols); initSparse(density, refMat2, m2); - VERIFY_IS_APPROX(m2.transpose().eval(), refMat2.transpose().eval()); - VERIFY_IS_APPROX(m2.transpose(), refMat2.transpose()); + std::vector ref_value(m2.innerSize()); + std::vector ref_index(m2.innerSize()); + if(internal::random()) + m2.makeCompressed(); + for(Index j = 0; j(density, refMat2, m2); - Index j0 = internal::random(0,rows-2); - Index j1 = internal::random(0,rows-2); - Index n0 = internal::random(1,rows-(std::max)(j0,j1)); - if(SparseMatrixType::IsRowMajor) - VERIFY_IS_APPROX(m2.block(j0,0,n0,cols), refMat2.block(j0,0,n0,cols)); - else - VERIFY_IS_APPROX(m2.block(0,j0,rows,n0), refMat2.block(0,j0,rows,n0)); - - if(SparseMatrixType::IsRowMajor) - VERIFY_IS_APPROX(m2.block(j0,0,n0,cols)+m2.block(j1,0,n0,cols), - refMat2.block(j0,0,n0,cols)+refMat2.block(j1,0,n0,cols)); - else - VERIFY_IS_APPROX(m2.block(0,j0,rows,n0)+m2.block(0,j1,rows,n0), - refMat2.block(0,j0,rows,n0)+refMat2.block(0,j1,rows,n0)); - - Index i = internal::random(0,m2.outerSize()-1); - if(SparseMatrixType::IsRowMajor) { - m2.innerVector(i) = m2.innerVector(i) * s1; - refMat2.row(i) = refMat2.row(i) * s1; - VERIFY_IS_APPROX(m2,refMat2); - } else { - m2.innerVector(i) = m2.innerVector(i) * s1; - refMat2.col(i) = refMat2.col(i) * s1; - VERIFY_IS_APPROX(m2,refMat2); - } - - VERIFY_IS_APPROX(DenseVector(m2.col(j0)), refMat2.col(j0)); - VERIFY_IS_APPROX(m2.col(j0), refMat2.col(j0)); - - VERIFY_IS_APPROX(RowDenseVector(m2.row(j0)), refMat2.row(j0)); - VERIFY_IS_APPROX(m2.row(j0), refMat2.row(j0)); + VERIFY_IS_APPROX(m2.transpose().eval(), refMat2.transpose().eval()); + VERIFY_IS_APPROX(m2.transpose(), refMat2.transpose()); + + VERIFY_IS_APPROX(SparseMatrixType(m2.adjoint()), refMat2.adjoint()); - VERIFY_IS_APPROX(m2.block(j0,j1,n0,n0), refMat2.block(j0,j1,n0,n0)); - VERIFY_IS_APPROX((2*m2).block(j0,j1,n0,n0), (2*refMat2).block(j0,j1,n0,n0)); + // check isApprox handles opposite storage order + typename Transpose::PlainObject m3(m2); + VERIFY(m2.isApprox(m3)); } // test prune { - SparseMatrixType m2(rows, rows); - DenseMatrix refM2(rows, rows); + SparseMatrixType m2(rows, cols); + DenseMatrix refM2(rows, cols); refM2.setZero(); int countFalseNonZero = 0; int countTrueNonZero = 0; - for (Index j=0; j(0,1); - if (x<0.1) + if (x<0.1f) { // do nothing } - else if (x<0.5) + else if (x<0.5f) { countFalseNonZero++; - m2.insertBackByOuterInner(j,i) = Scalar(0); + m2.insert(i,j) = Scalar(0); } else { countTrueNonZero++; - m2.insertBackByOuterInner(j,i) = Scalar(1); - if(SparseMatrixType::IsRowMajor) - refM2(j,i) = Scalar(1); - else - refM2(i,j) = Scalar(1); + m2.insert(i,j) = Scalar(1); + refM2(i,j) = Scalar(1); } } } - m2.finalize(); + if(internal::random()) + m2.makeCompressed(); VERIFY(countFalseNonZero+countTrueNonZero == m2.nonZeros()); - VERIFY_IS_APPROX(m2, refM2); + if(countTrueNonZero>0) + VERIFY_IS_APPROX(m2, refM2); m2.prune(Scalar(1)); VERIFY(countTrueNonZero==m2.nonZeros()); VERIFY_IS_APPROX(m2, refM2); @@ -414,29 +358,74 @@ template void sparse_basic(const SparseMatrixType& re // test setFromTriplets { - typedef Triplet TripletType; + typedef Triplet TripletType; std::vector triplets; - int ntriplets = rows*cols; + Index ntriplets = rows*cols; triplets.reserve(ntriplets); - DenseMatrix refMat(rows,cols); - refMat.setZero(); - for(int i=0;i(0,rows-1); - Index c = internal::random(0,cols-1); + StorageIndex r = internal::random(0,StorageIndex(rows-1)); + StorageIndex c = internal::random(0,StorageIndex(cols-1)); Scalar v = internal::random(); triplets.push_back(TripletType(r,c,v)); - refMat(r,c) += v; + refMat_sum(r,c) += v; + if(std::abs(refMat_prod(r,c))==0) + refMat_prod(r,c) = v; + else + refMat_prod(r,c) *= v; + refMat_last(r,c) = v; } SparseMatrixType m(rows,cols); m.setFromTriplets(triplets.begin(), triplets.end()); - VERIFY_IS_APPROX(m, refMat); + VERIFY_IS_APPROX(m, refMat_sum); + + m.setFromTriplets(triplets.begin(), triplets.end(), std::multiplies()); + VERIFY_IS_APPROX(m, refMat_prod); +#if (defined(__cplusplus) && __cplusplus >= 201103L) + m.setFromTriplets(triplets.begin(), triplets.end(), [] (Scalar,Scalar b) { return b; }); + VERIFY_IS_APPROX(m, refMat_last); +#endif + } + + // test Map + { + DenseMatrix refMat2(rows, cols), refMat3(rows, cols); + SparseMatrixType m2(rows, cols), m3(rows, cols); + initSparse(density, refMat2, m2); + initSparse(density, refMat3, m3); + { + Map mapMat2(m2.rows(), m2.cols(), m2.nonZeros(), m2.outerIndexPtr(), m2.innerIndexPtr(), m2.valuePtr(), m2.innerNonZeroPtr()); + Map mapMat3(m3.rows(), m3.cols(), m3.nonZeros(), m3.outerIndexPtr(), m3.innerIndexPtr(), m3.valuePtr(), m3.innerNonZeroPtr()); + VERIFY_IS_APPROX(mapMat2+mapMat3, refMat2+refMat3); + VERIFY_IS_APPROX(mapMat2+mapMat3, refMat2+refMat3); + } + { + MappedSparseMatrix mapMat2(m2.rows(), m2.cols(), m2.nonZeros(), m2.outerIndexPtr(), m2.innerIndexPtr(), m2.valuePtr(), m2.innerNonZeroPtr()); + MappedSparseMatrix mapMat3(m3.rows(), m3.cols(), m3.nonZeros(), m3.outerIndexPtr(), m3.innerIndexPtr(), m3.valuePtr(), m3.innerNonZeroPtr()); + VERIFY_IS_APPROX(mapMat2+mapMat3, refMat2+refMat3); + VERIFY_IS_APPROX(mapMat2+mapMat3, refMat2+refMat3); + } + + Index i = internal::random(0,rows-1); + Index j = internal::random(0,cols-1); + m2.coeffRef(i,j) = 123; + if(internal::random()) + m2.makeCompressed(); + Map mapMat2(rows, cols, m2.nonZeros(), m2.outerIndexPtr(), m2.innerIndexPtr(), m2.valuePtr(), m2.innerNonZeroPtr()); + VERIFY_IS_EQUAL(m2.coeff(i,j),Scalar(123)); + VERIFY_IS_EQUAL(mapMat2.coeff(i,j),Scalar(123)); + mapMat2.coeffRef(i,j) = -123; + VERIFY_IS_EQUAL(m2.coeff(i,j),Scalar(-123)); } // test triangularView { - DenseMatrix refMat2(rows, rows), refMat3(rows, rows); - SparseMatrixType m2(rows, rows), m3(rows, rows); + DenseMatrix refMat2(rows, cols), refMat3(rows, cols); + SparseMatrixType m2(rows, cols), m3(rows, cols); initSparse(density, refMat2, m2); refMat3 = refMat2.template triangularView(); m3 = m2.template triangularView(); @@ -446,13 +435,15 @@ template void sparse_basic(const SparseMatrixType& re m3 = m2.template triangularView(); VERIFY_IS_APPROX(m3, refMat3); - refMat3 = refMat2.template triangularView(); - m3 = m2.template triangularView(); - VERIFY_IS_APPROX(m3, refMat3); + { + refMat3 = refMat2.template triangularView(); + m3 = m2.template triangularView(); + VERIFY_IS_APPROX(m3, refMat3); - refMat3 = refMat2.template triangularView(); - m3 = m2.template triangularView(); - VERIFY_IS_APPROX(m3, refMat3); + refMat3 = refMat2.template triangularView(); + m3 = m2.template triangularView(); + VERIFY_IS_APPROX(m3, refMat3); + } refMat3 = refMat2.template triangularView(); m3 = m2.template triangularView(); @@ -461,6 +452,10 @@ template void sparse_basic(const SparseMatrixType& re refMat3 = refMat2.template triangularView(); m3 = m2.template triangularView(); VERIFY_IS_APPROX(m3, refMat3); + + // check sparse-triangular to dense + refMat3 = m2.template triangularView(); + VERIFY_IS_APPROX(refMat3, DenseMatrix(refMat2.template triangularView())); } // test selfadjointView @@ -472,6 +467,19 @@ template void sparse_basic(const SparseMatrixType& re refMat3 = refMat2.template selfadjointView(); m3 = m2.template selfadjointView(); VERIFY_IS_APPROX(m3, refMat3); + + refMat3 += refMat2.template selfadjointView(); + m3 += m2.template selfadjointView(); + VERIFY_IS_APPROX(m3, refMat3); + + refMat3 -= refMat2.template selfadjointView(); + m3 -= m2.template selfadjointView(); + VERIFY_IS_APPROX(m3, refMat3); + + // selfadjointView only works for square matrices: + SparseMatrixType m4(rows, rows+1); + VERIFY_RAISES_ASSERT(m4.template selfadjointView()); + VERIFY_RAISES_ASSERT(m4.template selfadjointView()); } // test sparseView @@ -480,28 +488,59 @@ template void sparse_basic(const SparseMatrixType& re SparseMatrixType m2(rows, rows); initSparse(density, refMat2, m2); VERIFY_IS_APPROX(m2.eval(), refMat2.sparseView().eval()); + + // sparse view on expressions: + VERIFY_IS_APPROX((s1*m2).eval(), (s1*refMat2).sparseView().eval()); + VERIFY_IS_APPROX((m2+m2).eval(), (refMat2+refMat2).sparseView().eval()); + VERIFY_IS_APPROX((m2*m2).eval(), (refMat2.lazyProduct(refMat2)).sparseView().eval()); + VERIFY_IS_APPROX((m2*m2).eval(), (refMat2*refMat2).sparseView().eval()); } // test diagonal { - DenseMatrix refMat2 = DenseMatrix::Zero(rows, rows); - SparseMatrixType m2(rows, rows); + DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols); + SparseMatrixType m2(rows, cols); initSparse(density, refMat2, m2); VERIFY_IS_APPROX(m2.diagonal(), refMat2.diagonal().eval()); + DenseVector d = m2.diagonal(); + VERIFY_IS_APPROX(d, refMat2.diagonal().eval()); + d = m2.diagonal().array(); + VERIFY_IS_APPROX(d, refMat2.diagonal().eval()); + VERIFY_IS_APPROX(const_cast(m2).diagonal(), refMat2.diagonal().eval()); + + initSparse(density, refMat2, m2, ForceNonZeroDiag); + m2.diagonal() += refMat2.diagonal(); + refMat2.diagonal() += refMat2.diagonal(); + VERIFY_IS_APPROX(m2, refMat2); + } + + // test diagonal to sparse + { + DenseVector d = DenseVector::Random(rows); + DenseMatrix refMat2 = d.asDiagonal(); + SparseMatrixType m2(rows, rows); + m2 = d.asDiagonal(); + VERIFY_IS_APPROX(m2, refMat2); + SparseMatrixType m3(d.asDiagonal()); + VERIFY_IS_APPROX(m3, refMat2); + refMat2 += d.asDiagonal(); + m2 += d.asDiagonal(); + VERIFY_IS_APPROX(m2, refMat2); } // test conservative resize { - std::vector< std::pair > inc; - inc.push_back(std::pair(-3,-2)); - inc.push_back(std::pair(0,0)); - inc.push_back(std::pair(3,2)); - inc.push_back(std::pair(3,0)); - inc.push_back(std::pair(0,3)); + std::vector< std::pair > inc; + if(rows > 3 && cols > 2) + inc.push_back(std::pair(-3,-2)); + inc.push_back(std::pair(0,0)); + inc.push_back(std::pair(3,2)); + inc.push_back(std::pair(3,0)); + inc.push_back(std::pair(0,3)); for(size_t i = 0; i< inc.size(); i++) { - Index incRows = inc[i].first; - Index incCols = inc[i].second; + StorageIndex incRows = inc[i].first; + StorageIndex incCols = inc[i].second; SparseMatrixType m1(rows, cols); DenseMatrix refMat1 = DenseMatrix::Zero(rows, cols); initSparse(density, refMat1, m1); @@ -546,21 +585,104 @@ template void sparse_basic(const SparseMatrixType& re refMat1.setIdentity(); VERIFY_IS_APPROX(m1, refMat1); } + + // test array/vector of InnerIterator + { + typedef typename SparseMatrixType::InnerIterator IteratorType; + + DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols); + SparseMatrixType m2(rows, cols); + initSparse(density, refMat2, m2); + IteratorType static_array[2]; + static_array[0] = IteratorType(m2,0); + static_array[1] = IteratorType(m2,m2.outerSize()-1); + VERIFY( static_array[0] || m2.innerVector(static_array[0].outer()).nonZeros() == 0 ); + VERIFY( static_array[1] || m2.innerVector(static_array[1].outer()).nonZeros() == 0 ); + if(static_array[0] && static_array[1]) + { + ++(static_array[1]); + static_array[1] = IteratorType(m2,0); + VERIFY( static_array[1] ); + VERIFY( static_array[1].index() == static_array[0].index() ); + VERIFY( static_array[1].outer() == static_array[0].outer() ); + VERIFY( static_array[1].value() == static_array[0].value() ); + } + + std::vector iters(2); + iters[0] = IteratorType(m2,0); + iters[1] = IteratorType(m2,m2.outerSize()-1); + } +} + + +template +void big_sparse_triplet(Index rows, Index cols, double density) { + typedef typename SparseMatrixType::StorageIndex StorageIndex; + typedef typename SparseMatrixType::Scalar Scalar; + typedef Triplet TripletType; + std::vector triplets; + double nelements = density * rows*cols; + VERIFY(nelements>=0 && nelements < NumTraits::highest()); + Index ntriplets = Index(nelements); + triplets.reserve(ntriplets); + Scalar sum = Scalar(0); + for(Index i=0;i(0,rows-1); + Index c = internal::random(0,cols-1); + Scalar v = internal::random(); + triplets.push_back(TripletType(r,c,v)); + sum += v; + } + SparseMatrixType m(rows,cols); + m.setFromTriplets(triplets.begin(), triplets.end()); + VERIFY(m.nonZeros() <= ntriplets); + VERIFY_IS_APPROX(sum, m.sum()); } + void test_sparse_basic() { for(int i = 0; i < g_repeat; i++) { - int s = Eigen::internal::random(1,50); - EIGEN_UNUSED_VARIABLE(s); + int r = Eigen::internal::random(1,200), c = Eigen::internal::random(1,200); + if(Eigen::internal::random(0,4) == 0) { + r = c; // check square matrices in 25% of tries + } + EIGEN_UNUSED_VARIABLE(r+c); + CALL_SUBTEST_1(( sparse_basic(SparseMatrix(1, 1)) )); CALL_SUBTEST_1(( sparse_basic(SparseMatrix(8, 8)) )); - CALL_SUBTEST_2(( sparse_basic(SparseMatrix, ColMajor>(s, s)) )); - CALL_SUBTEST_2(( sparse_basic(SparseMatrix, RowMajor>(s, s)) )); - CALL_SUBTEST_1(( sparse_basic(SparseMatrix(s, s)) )); - CALL_SUBTEST_1(( sparse_basic(SparseMatrix(s, s)) )); - CALL_SUBTEST_1(( sparse_basic(SparseMatrix(s, s)) )); + CALL_SUBTEST_2(( sparse_basic(SparseMatrix, ColMajor>(r, c)) )); + CALL_SUBTEST_2(( sparse_basic(SparseMatrix, RowMajor>(r, c)) )); + CALL_SUBTEST_1(( sparse_basic(SparseMatrix(r, c)) )); + CALL_SUBTEST_5(( sparse_basic(SparseMatrix(r, c)) )); + CALL_SUBTEST_5(( sparse_basic(SparseMatrix(r, c)) )); - CALL_SUBTEST_1(( sparse_basic(SparseMatrix(short(s), short(s))) )); - CALL_SUBTEST_1(( sparse_basic(SparseMatrix(short(s), short(s))) )); + r = Eigen::internal::random(1,100); + c = Eigen::internal::random(1,100); + if(Eigen::internal::random(0,4) == 0) { + r = c; // check square matrices in 25% of tries + } + + CALL_SUBTEST_6(( sparse_basic(SparseMatrix(short(r), short(c))) )); + CALL_SUBTEST_6(( sparse_basic(SparseMatrix(short(r), short(c))) )); + } + + // Regression test for bug 900: (manually insert higher values here, if you have enough RAM): + CALL_SUBTEST_3((big_sparse_triplet >(10000, 10000, 0.125))); + CALL_SUBTEST_4((big_sparse_triplet >(10000, 10000, 0.125))); + + // Regression test for bug 1105 +#ifdef EIGEN_TEST_PART_7 + { + int n = Eigen::internal::random(200,600); + SparseMatrix,0, long> mat(n, n); + std::complex val; + + for(int i=0; i +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "sparse.h" + +template +typename Eigen::internal::enable_if<(T::Flags&RowMajorBit)==RowMajorBit, typename T::RowXpr>::type +innervec(T& A, Index i) +{ + return A.row(i); +} + +template +typename Eigen::internal::enable_if<(T::Flags&RowMajorBit)==0, typename T::ColXpr>::type +innervec(T& A, Index i) +{ + return A.col(i); +} + +template void sparse_block(const SparseMatrixType& ref) +{ + const Index rows = ref.rows(); + const Index cols = ref.cols(); + const Index inner = ref.innerSize(); + const Index outer = ref.outerSize(); + + typedef typename SparseMatrixType::Scalar Scalar; + typedef typename SparseMatrixType::StorageIndex StorageIndex; + + double density = (std::max)(8./(rows*cols), 0.01); + typedef Matrix DenseMatrix; + typedef Matrix DenseVector; + typedef Matrix RowDenseVector; + typedef SparseVector SparseVectorType; + + Scalar s1 = internal::random(); + { + SparseMatrixType m(rows, cols); + DenseMatrix refMat = DenseMatrix::Zero(rows, cols); + initSparse(density, refMat, m); + + VERIFY_IS_APPROX(m, refMat); + + // test InnerIterators and Block expressions + for (int t=0; t<10; ++t) + { + Index j = internal::random(0,cols-2); + Index i = internal::random(0,rows-2); + Index w = internal::random(1,cols-j); + Index h = internal::random(1,rows-i); + + VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); + for(Index c=0; c(density, refMat2, m2); + Index j0 = internal::random(0,outer-1); + Index j1 = internal::random(0,outer-1); + Index r0 = internal::random(0,rows-1); + Index c0 = internal::random(0,cols-1); + + VERIFY_IS_APPROX(m2.innerVector(j0), innervec(refMat2,j0)); + VERIFY_IS_APPROX(m2.innerVector(j0)+m2.innerVector(j1), innervec(refMat2,j0)+innervec(refMat2,j1)); + + m2.innerVector(j0) *= Scalar(2); + innervec(refMat2,j0) *= Scalar(2); + VERIFY_IS_APPROX(m2, refMat2); + + m2.row(r0) *= Scalar(3); + refMat2.row(r0) *= Scalar(3); + VERIFY_IS_APPROX(m2, refMat2); + + m2.col(c0) *= Scalar(4); + refMat2.col(c0) *= Scalar(4); + VERIFY_IS_APPROX(m2, refMat2); + + m2.row(r0) /= Scalar(3); + refMat2.row(r0) /= Scalar(3); + VERIFY_IS_APPROX(m2, refMat2); + + m2.col(c0) /= Scalar(4); + refMat2.col(c0) /= Scalar(4); + VERIFY_IS_APPROX(m2, refMat2); + + SparseVectorType v1; + VERIFY_IS_APPROX(v1 = m2.col(c0) * 4, refMat2.col(c0)*4); + VERIFY_IS_APPROX(v1 = m2.row(r0) * 4, refMat2.row(r0).transpose()*4); + + SparseMatrixType m3(rows,cols); + m3.reserve(VectorXi::Constant(outer,int(inner/2))); + for(Index j=0; j(k+1); + for(Index j=0; j<(std::min)(outer, inner); ++j) + { + VERIFY(j==numext::real(m3.innerVector(j).nonZeros())); + if(j>0) + VERIFY(j==numext::real(m3.innerVector(j).lastCoeff())); + } + m3.makeCompressed(); + for(Index j=0; j<(std::min)(outer, inner); ++j) + { + VERIFY(j==numext::real(m3.innerVector(j).nonZeros())); + if(j>0) + VERIFY(j==numext::real(m3.innerVector(j).lastCoeff())); + } + + VERIFY(m3.innerVector(j0).nonZeros() == m3.transpose().innerVector(j0).nonZeros()); + +// m2.innerVector(j0) = 2*m2.innerVector(j1); +// refMat2.col(j0) = 2*refMat2.col(j1); +// VERIFY_IS_APPROX(m2, refMat2); + } + + // test innerVectors() + { + DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols); + SparseMatrixType m2(rows, cols); + initSparse(density, refMat2, m2); + if(internal::random(0,1)>0.5f) m2.makeCompressed(); + Index j0 = internal::random(0,outer-2); + Index j1 = internal::random(0,outer-2); + Index n0 = internal::random(1,outer-(std::max)(j0,j1)); + if(SparseMatrixType::IsRowMajor) + VERIFY_IS_APPROX(m2.innerVectors(j0,n0), refMat2.block(j0,0,n0,cols)); + else + VERIFY_IS_APPROX(m2.innerVectors(j0,n0), refMat2.block(0,j0,rows,n0)); + if(SparseMatrixType::IsRowMajor) + VERIFY_IS_APPROX(m2.innerVectors(j0,n0)+m2.innerVectors(j1,n0), + refMat2.middleRows(j0,n0)+refMat2.middleRows(j1,n0)); + else + VERIFY_IS_APPROX(m2.innerVectors(j0,n0)+m2.innerVectors(j1,n0), + refMat2.block(0,j0,rows,n0)+refMat2.block(0,j1,rows,n0)); + + VERIFY_IS_APPROX(m2, refMat2); + + VERIFY(m2.innerVectors(j0,n0).nonZeros() == m2.transpose().innerVectors(j0,n0).nonZeros()); + + m2.innerVectors(j0,n0) = m2.innerVectors(j0,n0) + m2.innerVectors(j1,n0); + if(SparseMatrixType::IsRowMajor) + refMat2.middleRows(j0,n0) = (refMat2.middleRows(j0,n0) + refMat2.middleRows(j1,n0)).eval(); + else + refMat2.middleCols(j0,n0) = (refMat2.middleCols(j0,n0) + refMat2.middleCols(j1,n0)).eval(); + + VERIFY_IS_APPROX(m2, refMat2); + } + + // test generic blocks + { + DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols); + SparseMatrixType m2(rows, cols); + initSparse(density, refMat2, m2); + Index j0 = internal::random(0,outer-2); + Index j1 = internal::random(0,outer-2); + Index n0 = internal::random(1,outer-(std::max)(j0,j1)); + if(SparseMatrixType::IsRowMajor) + VERIFY_IS_APPROX(m2.block(j0,0,n0,cols), refMat2.block(j0,0,n0,cols)); + else + VERIFY_IS_APPROX(m2.block(0,j0,rows,n0), refMat2.block(0,j0,rows,n0)); + + if(SparseMatrixType::IsRowMajor) + VERIFY_IS_APPROX(m2.block(j0,0,n0,cols)+m2.block(j1,0,n0,cols), + refMat2.block(j0,0,n0,cols)+refMat2.block(j1,0,n0,cols)); + else + VERIFY_IS_APPROX(m2.block(0,j0,rows,n0)+m2.block(0,j1,rows,n0), + refMat2.block(0,j0,rows,n0)+refMat2.block(0,j1,rows,n0)); + + Index i = internal::random(0,m2.outerSize()-1); + if(SparseMatrixType::IsRowMajor) { + m2.innerVector(i) = m2.innerVector(i) * s1; + refMat2.row(i) = refMat2.row(i) * s1; + VERIFY_IS_APPROX(m2,refMat2); + } else { + m2.innerVector(i) = m2.innerVector(i) * s1; + refMat2.col(i) = refMat2.col(i) * s1; + VERIFY_IS_APPROX(m2,refMat2); + } + + Index r0 = internal::random(0,rows-2); + Index c0 = internal::random(0,cols-2); + Index r1 = internal::random(1,rows-r0); + Index c1 = internal::random(1,cols-c0); + + VERIFY_IS_APPROX(DenseVector(m2.col(c0)), refMat2.col(c0)); + VERIFY_IS_APPROX(m2.col(c0), refMat2.col(c0)); + + VERIFY_IS_APPROX(RowDenseVector(m2.row(r0)), refMat2.row(r0)); + VERIFY_IS_APPROX(m2.row(r0), refMat2.row(r0)); + + VERIFY_IS_APPROX(m2.block(r0,c0,r1,c1), refMat2.block(r0,c0,r1,c1)); + VERIFY_IS_APPROX((2*m2).block(r0,c0,r1,c1), (2*refMat2).block(r0,c0,r1,c1)); + + if(m2.nonZeros()>0) + { + VERIFY_IS_APPROX(m2, refMat2); + SparseMatrixType m3(rows, cols); + DenseMatrix refMat3(rows, cols); refMat3.setZero(); + Index n = internal::random(1,10); + for(Index k=0; k(0,outer-1); + Index o2 = internal::random(0,outer-1); + if(SparseMatrixType::IsRowMajor) + { + m3.innerVector(o1) = m2.row(o2); + refMat3.row(o1) = refMat2.row(o2); + } + else + { + m3.innerVector(o1) = m2.col(o2); + refMat3.col(o1) = refMat2.col(o2); + } + if(internal::random()) + m3.makeCompressed(); + } + if(m3.nonZeros()>0) + VERIFY_IS_APPROX(m3, refMat3); + } + } +} + +void test_sparse_block() +{ + for(int i = 0; i < g_repeat; i++) { + int r = Eigen::internal::random(1,200), c = Eigen::internal::random(1,200); + if(Eigen::internal::random(0,4) == 0) { + r = c; // check square matrices in 25% of tries + } + EIGEN_UNUSED_VARIABLE(r+c); + CALL_SUBTEST_1(( sparse_block(SparseMatrix(1, 1)) )); + CALL_SUBTEST_1(( sparse_block(SparseMatrix(8, 8)) )); + CALL_SUBTEST_1(( sparse_block(SparseMatrix(r, c)) )); + CALL_SUBTEST_2(( sparse_block(SparseMatrix, ColMajor>(r, c)) )); + CALL_SUBTEST_2(( sparse_block(SparseMatrix, RowMajor>(r, c)) )); + + CALL_SUBTEST_3(( sparse_block(SparseMatrix(r, c)) )); + CALL_SUBTEST_3(( sparse_block(SparseMatrix(r, c)) )); + + r = Eigen::internal::random(1,100); + c = Eigen::internal::random(1,100); + if(Eigen::internal::random(0,4) == 0) { + r = c; // check square matrices in 25% of tries + } + + CALL_SUBTEST_4(( sparse_block(SparseMatrix(short(r), short(c))) )); + CALL_SUBTEST_4(( sparse_block(SparseMatrix(short(r), short(c))) )); + } +} diff --git a/ground/gcs/src/libs/eigen/test/sparse_permutations.cpp b/ground/gcs/src/libs/eigen/test/sparse_permutations.cpp index e4ce1d6796..b82cceff80 100644 --- a/ground/gcs/src/libs/eigen/test/sparse_permutations.cpp +++ b/ground/gcs/src/libs/eigen/test/sparse_permutations.cpp @@ -1,25 +1,57 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2011 Gael Guennebaud +// Copyright (C) 2011-2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +static long int nb_transposed_copies; +#define EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN {nb_transposed_copies++;} +#define VERIFY_TRANSPOSITION_COUNT(XPR,N) {\ + nb_transposed_copies = 0; \ + XPR; \ + if(nb_transposed_copies!=N) std::cerr << "nb_transposed_copies == " << nb_transposed_copies << "\n"; \ + VERIFY( (#XPR) && nb_transposed_copies==N ); \ + } + #include "sparse.h" -template void sparse_permutations(const SparseMatrixType& ref) +template +bool is_sorted(const T& mat) { + for(Index k = 0; k=it.index()) + return false; + prev = it.index(); + } + } + return true; +} + +template +typename internal::nested_eval::type eval(const T &xpr) { - typedef typename SparseMatrixType::Index Index; + VERIFY( int(internal::nested_eval::type::Flags&RowMajorBit) == int(internal::evaluator::Flags&RowMajorBit) ); + return xpr; +} +template void sparse_permutations(const SparseMatrixType& ref) +{ const Index rows = ref.rows(); const Index cols = ref.cols(); typedef typename SparseMatrixType::Scalar Scalar; - typedef typename SparseMatrixType::Index Index; - typedef SparseMatrix OtherSparseMatrixType; + typedef typename SparseMatrixType::StorageIndex StorageIndex; + typedef SparseMatrix OtherSparseMatrixType; typedef Matrix DenseMatrix; - typedef Matrix VectorI; + typedef Matrix VectorI; +// bool IsRowMajor1 = SparseMatrixType::IsRowMajor; +// bool IsRowMajor2 = OtherSparseMatrixType::IsRowMajor; double density = (std::max)(8./(rows*cols), 0.01); @@ -44,58 +76,69 @@ template void sparse_permutations(c randomPermutationVector(pi, cols); p.indices() = pi; - res = mat*p; + VERIFY( is_sorted( ::eval(mat*p) )); + VERIFY( is_sorted( res = mat*p )); + VERIFY_TRANSPOSITION_COUNT( ::eval(mat*p), 0); + //VERIFY_TRANSPOSITION_COUNT( res = mat*p, IsRowMajor ? 1 : 0 ); res_d = mat_d*p; VERIFY(res.isApprox(res_d) && "mat*p"); - res = p*mat; + VERIFY( is_sorted( ::eval(p*mat) )); + VERIFY( is_sorted( res = p*mat )); + VERIFY_TRANSPOSITION_COUNT( ::eval(p*mat), 0); res_d = p*mat_d; VERIFY(res.isApprox(res_d) && "p*mat"); - res = mat*p.inverse(); + VERIFY( is_sorted( (mat*p).eval() )); + VERIFY( is_sorted( res = mat*p.inverse() )); + VERIFY_TRANSPOSITION_COUNT( ::eval(mat*p.inverse()), 0); res_d = mat*p.inverse(); VERIFY(res.isApprox(res_d) && "mat*inv(p)"); - res = p.inverse()*mat; + VERIFY( is_sorted( (p*mat+p*mat).eval() )); + VERIFY( is_sorted( res = p.inverse()*mat )); + VERIFY_TRANSPOSITION_COUNT( ::eval(p.inverse()*mat), 0); res_d = p.inverse()*mat_d; VERIFY(res.isApprox(res_d) && "inv(p)*mat"); - res = mat.twistedBy(p); + VERIFY( is_sorted( (p * mat * p.inverse()).eval() )); + VERIFY( is_sorted( res = mat.twistedBy(p) )); + VERIFY_TRANSPOSITION_COUNT( ::eval(p * mat * p.inverse()), 0); res_d = (p * mat_d) * p.inverse(); VERIFY(res.isApprox(res_d) && "p*mat*inv(p)"); - res = mat.template selfadjointView().twistedBy(p_null); + VERIFY( is_sorted( res = mat.template selfadjointView().twistedBy(p_null) )); res_d = up_sym_d; VERIFY(res.isApprox(res_d) && "full selfadjoint upper to full"); - res = mat.template selfadjointView().twistedBy(p_null); + VERIFY( is_sorted( res = mat.template selfadjointView().twistedBy(p_null) )); res_d = lo_sym_d; VERIFY(res.isApprox(res_d) && "full selfadjoint lower to full"); - res = up.template selfadjointView().twistedBy(p_null); + VERIFY( is_sorted( res = up.template selfadjointView().twistedBy(p_null) )); res_d = up_sym_d; VERIFY(res.isApprox(res_d) && "upper selfadjoint to full"); - res = lo.template selfadjointView().twistedBy(p_null); + VERIFY( is_sorted( res = lo.template selfadjointView().twistedBy(p_null) )); res_d = lo_sym_d; VERIFY(res.isApprox(res_d) && "lower selfadjoint full"); - res = mat.template selfadjointView(); + VERIFY( is_sorted( res = mat.template selfadjointView() )); res_d = up_sym_d; VERIFY(res.isApprox(res_d) && "full selfadjoint upper to full"); - res = mat.template selfadjointView(); + VERIFY( is_sorted( res = mat.template selfadjointView() )); res_d = lo_sym_d; VERIFY(res.isApprox(res_d) && "full selfadjoint lower to full"); - res = up.template selfadjointView(); + VERIFY( is_sorted( res = up.template selfadjointView() )); res_d = up_sym_d; VERIFY(res.isApprox(res_d) && "upper selfadjoint to full"); - res = lo.template selfadjointView(); + VERIFY( is_sorted( res = lo.template selfadjointView() )); res_d = lo_sym_d; VERIFY(res.isApprox(res_d) && "lower selfadjoint full"); @@ -152,19 +195,19 @@ template void sparse_permutations(c VERIFY(res.isApprox(res_d) && "upper selfadjoint twisted to lower"); - res = mat.template selfadjointView().twistedBy(p); + VERIFY( is_sorted( res = mat.template selfadjointView().twistedBy(p) )); res_d = (p * up_sym_d) * p.inverse(); VERIFY(res.isApprox(res_d) && "full selfadjoint upper twisted to full"); - res = mat.template selfadjointView().twistedBy(p); + VERIFY( is_sorted( res = mat.template selfadjointView().twistedBy(p) )); res_d = (p * lo_sym_d) * p.inverse(); VERIFY(res.isApprox(res_d) && "full selfadjoint lower twisted to full"); - res = up.template selfadjointView().twistedBy(p); + VERIFY( is_sorted( res = up.template selfadjointView().twistedBy(p) )); res_d = (p * up_sym_d) * p.inverse(); VERIFY(res.isApprox(res_d) && "upper selfadjoint twisted to full"); - res = lo.template selfadjointView().twistedBy(p); + VERIFY( is_sorted( res = lo.template selfadjointView().twistedBy(p) )); res_d = (p * lo_sym_d) * p.inverse(); VERIFY(res.isApprox(res_d) && "lower selfadjoint twisted to full"); } @@ -184,4 +227,10 @@ void test_sparse_permutations() CALL_SUBTEST_1(( sparse_permutations_all(s) )); CALL_SUBTEST_2(( sparse_permutations_all >(s) )); } + + VERIFY((internal::is_same,OnTheRight,false,SparseShape>::ReturnType, + internal::nested_eval,PermutationMatrix,AliasFreeProduct>,1>::type>::value)); + + VERIFY((internal::is_same,OnTheLeft,false,SparseShape>::ReturnType, + internal::nested_eval,SparseMatrix,AliasFreeProduct>,1>::type>::value)); } diff --git a/ground/gcs/src/libs/eigen/test/sparse_product.cpp b/ground/gcs/src/libs/eigen/test/sparse_product.cpp index a2ea9d5b79..c1edd26e3a 100644 --- a/ground/gcs/src/libs/eigen/test/sparse_product.cpp +++ b/ground/gcs/src/libs/eigen/test/sparse_product.cpp @@ -7,37 +7,29 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. -#include "sparse.h" +static long int nb_temporaries; -template struct test_outer; +inline void on_temporary_creation() { + // here's a great place to set a breakpoint when debugging failures in this test! + nb_temporaries++; +} -template struct test_outer { - static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - typedef typename SparseMatrixType::Index Index; - Index c = internal::random(0,m2.cols()-1); - Index c1 = internal::random(0,m2.cols()-1); - VERIFY_IS_APPROX(m4=m2.col(c)*refMat2.col(c1).transpose(), refMat4=refMat2.col(c)*refMat2.col(c1).transpose()); - VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.col(c).transpose(), refMat4=refMat2.col(c1)*refMat2.col(c).transpose()); - } -}; - -template struct test_outer { - static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - typedef typename SparseMatrixType::Index Index; - Index r = internal::random(0,m2.rows()-1); - Index c1 = internal::random(0,m2.cols()-1); - VERIFY_IS_APPROX(m4=m2.row(r).transpose()*refMat2.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*refMat2.col(c1).transpose()); - VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.row(r), refMat4=refMat2.col(c1)*refMat2.row(r)); +#define EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN { on_temporary_creation(); } + +#include "sparse.h" + +#define VERIFY_EVALUATION_COUNT(XPR,N) {\ + nb_temporaries = 0; \ + CALL_SUBTEST( XPR ); \ + if(nb_temporaries!=N) std::cerr << "nb_temporaries == " << nb_temporaries << "\n"; \ + VERIFY( (#XPR) && nb_temporaries==N ); \ } -}; -// (m2,m4,refMat2,refMat4,dv1); -// VERIFY_IS_APPROX(m4=m2.innerVector(c)*dv1.transpose(), refMat4=refMat2.colVector(c)*dv1.transpose()); -// VERIFY_IS_APPROX(m4=dv1*mcm.col(c).transpose(), refMat4=dv1*refMat2.col(c).transpose()); + template void sparse_product() { - typedef typename SparseMatrixType::Index Index; + typedef typename SparseMatrixType::StorageIndex StorageIndex; Index n = 100; const Index rows = internal::random(1,n); const Index cols = internal::random(1,n); @@ -45,12 +37,12 @@ template void sparse_product() typedef typename SparseMatrixType::Scalar Scalar; enum { Flags = SparseMatrixType::Flags }; - double density = (std::max)(8./(rows*cols), 0.1); + double density = (std::max)(8./(rows*cols), 0.2); typedef Matrix DenseMatrix; typedef Matrix DenseVector; typedef Matrix RowDenseVector; - typedef SparseVector ColSpVector; - typedef SparseVector RowSpVector; + typedef SparseVector ColSpVector; + typedef SparseVector RowSpVector; Scalar s1 = internal::random(); Scalar s2 = internal::random(); @@ -93,33 +85,124 @@ template void sparse_product() VERIFY_IS_APPROX(m4 = m2*m3/s1, refMat4 = refMat2*refMat3/s1); VERIFY_IS_APPROX(m4 = m2*m3*s1, refMat4 = refMat2*refMat3*s1); VERIFY_IS_APPROX(m4 = s2*m2*m3*s1, refMat4 = s2*refMat2*refMat3*s1); + VERIFY_IS_APPROX(m4 = (m2+m2)*m3, refMat4 = (refMat2+refMat2)*refMat3); + VERIFY_IS_APPROX(m4 = m2*m3.leftCols(cols/2), refMat4 = refMat2*refMat3.leftCols(cols/2)); + VERIFY_IS_APPROX(m4 = m2*(m3+m3).leftCols(cols/2), refMat4 = refMat2*(refMat3+refMat3).leftCols(cols/2)); VERIFY_IS_APPROX(m4=(m2*m3).pruned(0), refMat4=refMat2*refMat3); VERIFY_IS_APPROX(m4=(m2t.transpose()*m3).pruned(0), refMat4=refMat2t.transpose()*refMat3); VERIFY_IS_APPROX(m4=(m2t.transpose()*m3t.transpose()).pruned(0), refMat4=refMat2t.transpose()*refMat3t.transpose()); VERIFY_IS_APPROX(m4=(m2*m3t.transpose()).pruned(0), refMat4=refMat2*refMat3t.transpose()); + // make sure the right product implementation is called: + if((!SparseMatrixType::IsRowMajor) && m2.rows()<=m3.cols()) + { + VERIFY_EVALUATION_COUNT(m4 = m2*m3, 3); // 1 temp for the result + 2 for transposing and get a sorted result. + VERIFY_EVALUATION_COUNT(m4 = (m2*m3).pruned(0), 1); + VERIFY_EVALUATION_COUNT(m4 = (m2*m3).eval().pruned(0), 4); + } + + // and that pruning is effective: + { + DenseMatrix Ad(2,2); + Ad << -1, 1, 1, 1; + SparseMatrixType As(Ad.sparseView()), B(2,2); + VERIFY_IS_EQUAL( (As*As.transpose()).eval().nonZeros(), 4); + VERIFY_IS_EQUAL( (Ad*Ad.transpose()).eval().sparseView().eval().nonZeros(), 2); + VERIFY_IS_EQUAL( (As*As.transpose()).pruned(1e-6).eval().nonZeros(), 2); + } + + // dense ?= sparse * sparse + VERIFY_IS_APPROX(dm4 =m2*m3, refMat4 =refMat2*refMat3); + VERIFY_IS_APPROX(dm4+=m2*m3, refMat4+=refMat2*refMat3); + VERIFY_IS_APPROX(dm4-=m2*m3, refMat4-=refMat2*refMat3); + VERIFY_IS_APPROX(dm4 =m2t.transpose()*m3, refMat4 =refMat2t.transpose()*refMat3); + VERIFY_IS_APPROX(dm4+=m2t.transpose()*m3, refMat4+=refMat2t.transpose()*refMat3); + VERIFY_IS_APPROX(dm4-=m2t.transpose()*m3, refMat4-=refMat2t.transpose()*refMat3); + VERIFY_IS_APPROX(dm4 =m2t.transpose()*m3t.transpose(), refMat4 =refMat2t.transpose()*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4+=m2t.transpose()*m3t.transpose(), refMat4+=refMat2t.transpose()*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4-=m2t.transpose()*m3t.transpose(), refMat4-=refMat2t.transpose()*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4 =m2*m3t.transpose(), refMat4 =refMat2*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4+=m2*m3t.transpose(), refMat4+=refMat2*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4-=m2*m3t.transpose(), refMat4-=refMat2*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4 = m2*m3*s1, refMat4 = refMat2*refMat3*s1); + // test aliasing m4 = m2; refMat4 = refMat2; VERIFY_IS_APPROX(m4=m4*m3, refMat4=refMat4*refMat3); - // sparse * dense + // sparse * dense matrix VERIFY_IS_APPROX(dm4=m2*refMat3, refMat4=refMat2*refMat3); VERIFY_IS_APPROX(dm4=m2*refMat3t.transpose(), refMat4=refMat2*refMat3t.transpose()); VERIFY_IS_APPROX(dm4=m2t.transpose()*refMat3, refMat4=refMat2t.transpose()*refMat3); VERIFY_IS_APPROX(dm4=m2t.transpose()*refMat3t.transpose(), refMat4=refMat2t.transpose()*refMat3t.transpose()); + VERIFY_IS_APPROX(dm4=m2*refMat3, refMat4=refMat2*refMat3); + VERIFY_IS_APPROX(dm4=dm4+m2*refMat3, refMat4=refMat4+refMat2*refMat3); + VERIFY_IS_APPROX(dm4+=m2*refMat3, refMat4+=refMat2*refMat3); + VERIFY_IS_APPROX(dm4-=m2*refMat3, refMat4-=refMat2*refMat3); + VERIFY_IS_APPROX(dm4.noalias()+=m2*refMat3, refMat4+=refMat2*refMat3); + VERIFY_IS_APPROX(dm4.noalias()-=m2*refMat3, refMat4-=refMat2*refMat3); VERIFY_IS_APPROX(dm4=m2*(refMat3+refMat3), refMat4=refMat2*(refMat3+refMat3)); VERIFY_IS_APPROX(dm4=m2t.transpose()*(refMat3+refMat5)*0.5, refMat4=refMat2t.transpose()*(refMat3+refMat5)*0.5); + + // sparse * dense vector + VERIFY_IS_APPROX(dm4.col(0)=m2*refMat3.col(0), refMat4.col(0)=refMat2*refMat3.col(0)); + VERIFY_IS_APPROX(dm4.col(0)=m2*refMat3t.transpose().col(0), refMat4.col(0)=refMat2*refMat3t.transpose().col(0)); + VERIFY_IS_APPROX(dm4.col(0)=m2t.transpose()*refMat3.col(0), refMat4.col(0)=refMat2t.transpose()*refMat3.col(0)); + VERIFY_IS_APPROX(dm4.col(0)=m2t.transpose()*refMat3t.transpose().col(0), refMat4.col(0)=refMat2t.transpose()*refMat3t.transpose().col(0)); // dense * sparse VERIFY_IS_APPROX(dm4=refMat2*m3, refMat4=refMat2*refMat3); + VERIFY_IS_APPROX(dm4=dm4+refMat2*m3, refMat4=refMat4+refMat2*refMat3); + VERIFY_IS_APPROX(dm4+=refMat2*m3, refMat4+=refMat2*refMat3); + VERIFY_IS_APPROX(dm4-=refMat2*m3, refMat4-=refMat2*refMat3); + VERIFY_IS_APPROX(dm4.noalias()+=refMat2*m3, refMat4+=refMat2*refMat3); + VERIFY_IS_APPROX(dm4.noalias()-=refMat2*m3, refMat4-=refMat2*refMat3); VERIFY_IS_APPROX(dm4=refMat2*m3t.transpose(), refMat4=refMat2*refMat3t.transpose()); VERIFY_IS_APPROX(dm4=refMat2t.transpose()*m3, refMat4=refMat2t.transpose()*refMat3); VERIFY_IS_APPROX(dm4=refMat2t.transpose()*m3t.transpose(), refMat4=refMat2t.transpose()*refMat3t.transpose()); // sparse * dense and dense * sparse outer product - test_outer::run(m2,m4,refMat2,refMat4); + { + Index c = internal::random(0,depth-1); + Index r = internal::random(0,rows-1); + Index c1 = internal::random(0,cols-1); + Index r1 = internal::random(0,depth-1); + DenseMatrix dm5 = DenseMatrix::Random(depth, cols); + + VERIFY_IS_APPROX( m4=m2.col(c)*dm5.col(c1).transpose(), refMat4=refMat2.col(c)*dm5.col(c1).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX( m4=m2.middleCols(c,1)*dm5.col(c1).transpose(), refMat4=refMat2.col(c)*dm5.col(c1).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(dm4=m2.col(c)*dm5.col(c1).transpose(), refMat4=refMat2.col(c)*dm5.col(c1).transpose()); + + VERIFY_IS_APPROX(m4=dm5.col(c1)*m2.col(c).transpose(), refMat4=dm5.col(c1)*refMat2.col(c).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(m4=dm5.col(c1)*m2.middleCols(c,1).transpose(), refMat4=dm5.col(c1)*refMat2.col(c).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(dm4=dm5.col(c1)*m2.col(c).transpose(), refMat4=dm5.col(c1)*refMat2.col(c).transpose()); + + VERIFY_IS_APPROX( m4=dm5.row(r1).transpose()*m2.col(c).transpose(), refMat4=dm5.row(r1).transpose()*refMat2.col(c).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(dm4=dm5.row(r1).transpose()*m2.col(c).transpose(), refMat4=dm5.row(r1).transpose()*refMat2.col(c).transpose()); + + VERIFY_IS_APPROX( m4=m2.row(r).transpose()*dm5.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*dm5.col(c1).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX( m4=m2.middleRows(r,1).transpose()*dm5.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*dm5.col(c1).transpose()); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(dm4=m2.row(r).transpose()*dm5.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*dm5.col(c1).transpose()); + + VERIFY_IS_APPROX( m4=dm5.col(c1)*m2.row(r), refMat4=dm5.col(c1)*refMat2.row(r)); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX( m4=dm5.col(c1)*m2.middleRows(r,1), refMat4=dm5.col(c1)*refMat2.row(r)); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(dm4=dm5.col(c1)*m2.row(r), refMat4=dm5.col(c1)*refMat2.row(r)); + + VERIFY_IS_APPROX( m4=dm5.row(r1).transpose()*m2.row(r), refMat4=dm5.row(r1).transpose()*refMat2.row(r)); + VERIFY_IS_EQUAL(m4.nonZeros(), (refMat4.array()!=0).count()); + VERIFY_IS_APPROX(dm4=dm5.row(r1).transpose()*m2.row(r), refMat4=dm5.row(r1).transpose()*refMat2.row(r)); + } VERIFY_IS_APPROX(m6=m6*m6, refMat6=refMat6*refMat6); @@ -131,11 +214,11 @@ template void sparse_product() RowSpVector rv0(depth), rv1; RowDenseVector drv0(depth), drv1(rv1); initSparse(2*density,drv0, rv0); - - VERIFY_IS_APPROX(cv1=rv0*m3, dcv1=drv0*refMat3); + + VERIFY_IS_APPROX(cv1=m3*cv0, dcv1=refMat3*dcv0); VERIFY_IS_APPROX(rv1=rv0*m3, drv1=drv0*refMat3); - VERIFY_IS_APPROX(cv1=m3*cv0, dcv1=refMat3*dcv0); VERIFY_IS_APPROX(cv1=m3t.adjoint()*cv0, dcv1=refMat3t.adjoint()*dcv0); + VERIFY_IS_APPROX(cv1=rv0*m3, dcv1=drv0*refMat3); VERIFY_IS_APPROX(rv1=m3*cv0, drv1=refMat3*dcv0); } @@ -158,12 +241,16 @@ template void sparse_product() // also check with a SparseWrapper: DenseVector v1 = DenseVector::Random(cols); DenseVector v2 = DenseVector::Random(rows); + DenseVector v3 = DenseVector::Random(rows); VERIFY_IS_APPROX(m3=m2*v1.asDiagonal(), refM3=refM2*v1.asDiagonal()); VERIFY_IS_APPROX(m3=m2.transpose()*v2.asDiagonal(), refM3=refM2.transpose()*v2.asDiagonal()); VERIFY_IS_APPROX(m3=v2.asDiagonal()*m2, refM3=v2.asDiagonal()*refM2); VERIFY_IS_APPROX(m3=v1.asDiagonal()*m2.transpose(), refM3=v1.asDiagonal()*refM2.transpose()); VERIFY_IS_APPROX(m3=v2.asDiagonal()*m2*v1.asDiagonal(), refM3=v2.asDiagonal()*refM2*v1.asDiagonal()); + + VERIFY_IS_APPROX(v2=m2*v1.asDiagonal()*v1, refM2*v1.asDiagonal()*v1); + VERIFY_IS_APPROX(v3=v2.asDiagonal()*m2*v1, v2.asDiagonal()*refM2*v1); // evaluate to a dense matrix to check the .row() and .col() iterator functions VERIFY_IS_APPROX(d3=m2*d1, refM3=refM2*d1); @@ -172,7 +259,7 @@ template void sparse_product() VERIFY_IS_APPROX(d3=d1*m2.transpose(), refM3=d1*refM2.transpose()); } - // test self adjoint products + // test self-adjoint and triangular-view products { DenseMatrix b = DenseMatrix::Random(rows, rows); DenseMatrix x = DenseMatrix::Random(rows, rows); @@ -180,9 +267,12 @@ template void sparse_product() DenseMatrix refUp = DenseMatrix::Zero(rows, rows); DenseMatrix refLo = DenseMatrix::Zero(rows, rows); DenseMatrix refS = DenseMatrix::Zero(rows, rows); + DenseMatrix refA = DenseMatrix::Zero(rows, rows); SparseMatrixType mUp(rows, rows); SparseMatrixType mLo(rows, rows); SparseMatrixType mS(rows, rows); + SparseMatrixType mA(rows, rows); + initSparse(density, refA, mA); do { initSparse(density, refUp, mUp, ForceRealDiag|/*ForceNonZeroDiag|*/MakeUpperTriangular); } while (refUp.isZero()); @@ -195,26 +285,41 @@ template void sparse_product() for (int k=0; k()*b, refX=refS*b); VERIFY_IS_APPROX(x=mLo.template selfadjointView()*b, refX=refS*b); VERIFY_IS_APPROX(x=mS.template selfadjointView()*b, refX=refS*b); + + VERIFY_IS_APPROX(x.noalias()+=mUp.template selfadjointView()*b, refX+=refS*b); + VERIFY_IS_APPROX(x.noalias()-=mLo.template selfadjointView()*b, refX-=refS*b); + VERIFY_IS_APPROX(x.noalias()+=mS.template selfadjointView()*b, refX+=refS*b); - // sparse selfadjointView * sparse + // sparse selfadjointView with sparse matrices SparseMatrixType mSres(rows,rows); VERIFY_IS_APPROX(mSres = mLo.template selfadjointView()*mS, refX = refLo.template selfadjointView()*refS); - // sparse * sparse selfadjointview VERIFY_IS_APPROX(mSres = mS * mLo.template selfadjointView(), refX = refS * refLo.template selfadjointView()); + + // sparse triangularView with dense matrices + VERIFY_IS_APPROX(x=mA.template triangularView()*b, refX=refA.template triangularView()*b); + VERIFY_IS_APPROX(x=mA.template triangularView()*b, refX=refA.template triangularView()*b); + VERIFY_IS_APPROX(x=b*mA.template triangularView(), refX=b*refA.template triangularView()); + VERIFY_IS_APPROX(x=b*mA.template triangularView(), refX=b*refA.template triangularView()); + + // sparse triangularView with sparse matrices + VERIFY_IS_APPROX(mSres = mA.template triangularView()*mS, refX = refA.template triangularView()*refS); + VERIFY_IS_APPROX(mSres = mS * mA.template triangularView(), refX = refS * refA.template triangularView()); + VERIFY_IS_APPROX(mSres = mA.template triangularView()*mS, refX = refA.template triangularView()*refS); + VERIFY_IS_APPROX(mSres = mS * mA.template triangularView(), refX = refS * refA.template triangularView()); } - } // New test for Bug in SparseTimeDenseProduct @@ -239,11 +344,35 @@ template void sparse_produc VERIFY_IS_APPROX( m4(0,0), 0.0 ); } +template +void bug_942() +{ + typedef Matrix Vector; + typedef SparseMatrix ColSpMat; + typedef SparseMatrix RowSpMat; + ColSpMat cmA(1,1); + cmA.insert(0,0) = 1; + + RowSpMat rmA(1,1); + rmA.insert(0,0) = 1; + + Vector d(1); + d[0] = 2; + + double res = 2; + + VERIFY_IS_APPROX( ( cmA*d.asDiagonal() ).eval().coeff(0,0), res ); + VERIFY_IS_APPROX( ( d.asDiagonal()*rmA ).eval().coeff(0,0), res ); + VERIFY_IS_APPROX( ( rmA*d.asDiagonal() ).eval().coeff(0,0), res ); + VERIFY_IS_APPROX( ( d.asDiagonal()*cmA ).eval().coeff(0,0), res ); +} + void test_sparse_product() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( (sparse_product >()) ); CALL_SUBTEST_1( (sparse_product >()) ); + CALL_SUBTEST_1( (bug_942()) ); CALL_SUBTEST_2( (sparse_product, ColMajor > >()) ); CALL_SUBTEST_2( (sparse_product, RowMajor > >()) ); CALL_SUBTEST_3( (sparse_product >()) ); diff --git a/ground/gcs/src/libs/eigen/test/sparse_ref.cpp b/ground/gcs/src/libs/eigen/test/sparse_ref.cpp new file mode 100644 index 0000000000..5e9607234d --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/sparse_ref.cpp @@ -0,0 +1,139 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 20015 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +// This unit test cannot be easily written to work with EIGEN_DEFAULT_TO_ROW_MAJOR +#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR +#undef EIGEN_DEFAULT_TO_ROW_MAJOR +#endif + +static long int nb_temporaries; + +inline void on_temporary_creation() { + // here's a great place to set a breakpoint when debugging failures in this test! + nb_temporaries++; +} + +#define EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN { on_temporary_creation(); } + +#include "main.h" +#include + +#define VERIFY_EVALUATION_COUNT(XPR,N) {\ + nb_temporaries = 0; \ + CALL_SUBTEST( XPR ); \ + if(nb_temporaries!=N) std::cerr << "nb_temporaries == " << nb_temporaries << "\n"; \ + VERIFY( (#XPR) && nb_temporaries==N ); \ + } + +template void check_const_correctness(const PlainObjectType&) +{ + // verify that ref-to-const don't have LvalueBit + typedef typename internal::add_const::type ConstPlainObjectType; + VERIFY( !(internal::traits >::Flags & LvalueBit) ); + VERIFY( !(internal::traits >::Flags & LvalueBit) ); + VERIFY( !(Ref::Flags & LvalueBit) ); + VERIFY( !(Ref::Flags & LvalueBit) ); +} + +template +EIGEN_DONT_INLINE void call_ref_1(Ref > a, const B &b) { VERIFY_IS_EQUAL(a.toDense(),b.toDense()); } + +template +EIGEN_DONT_INLINE void call_ref_2(const Ref >& a, const B &b) { VERIFY_IS_EQUAL(a.toDense(),b.toDense()); } + +template +EIGEN_DONT_INLINE void call_ref_3(const Ref, StandardCompressedFormat>& a, const B &b) { + VERIFY(a.isCompressed()); + VERIFY_IS_EQUAL(a.toDense(),b.toDense()); +} + +template +EIGEN_DONT_INLINE void call_ref_4(Ref > a, const B &b) { VERIFY_IS_EQUAL(a.toDense(),b.toDense()); } + +template +EIGEN_DONT_INLINE void call_ref_5(const Ref >& a, const B &b) { VERIFY_IS_EQUAL(a.toDense(),b.toDense()); } + +void call_ref() +{ + SparseMatrix A = MatrixXf::Random(10,10).sparseView(0.5,1); + SparseMatrix B = MatrixXf::Random(10,10).sparseView(0.5,1); + SparseMatrix C = MatrixXf::Random(10,10).sparseView(0.5,1); + C.reserve(VectorXi::Constant(C.outerSize(), 2)); + const SparseMatrix& Ac(A); + Block > Ab(A,0,1, 3,3); + const Block > Abc(A,0,1,3,3); + SparseVector vc = VectorXf::Random(10).sparseView(0.5,1); + SparseVector vr = VectorXf::Random(10).sparseView(0.5,1); + SparseMatrix AA = A*A; + + + VERIFY_EVALUATION_COUNT( call_ref_1(A, A), 0); +// VERIFY_EVALUATION_COUNT( call_ref_1(Ac, Ac), 0); // does not compile on purpose + VERIFY_EVALUATION_COUNT( call_ref_2(A, A), 0); + VERIFY_EVALUATION_COUNT( call_ref_3(A, A), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(A.transpose(), A.transpose()), 1); + VERIFY_EVALUATION_COUNT( call_ref_3(A.transpose(), A.transpose()), 1); + VERIFY_EVALUATION_COUNT( call_ref_2(Ac,Ac), 0); + VERIFY_EVALUATION_COUNT( call_ref_3(Ac,Ac), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(A+A,2*Ac), 1); + VERIFY_EVALUATION_COUNT( call_ref_3(A+A,2*Ac), 1); + VERIFY_EVALUATION_COUNT( call_ref_2(B, B), 1); + VERIFY_EVALUATION_COUNT( call_ref_3(B, B), 1); + VERIFY_EVALUATION_COUNT( call_ref_2(B.transpose(), B.transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_3(B.transpose(), B.transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(A*A, AA), 3); + VERIFY_EVALUATION_COUNT( call_ref_3(A*A, AA), 3); + + VERIFY(!C.isCompressed()); + VERIFY_EVALUATION_COUNT( call_ref_3(C, C), 1); + + Ref > Ar(A); + VERIFY_IS_APPROX(Ar+Ar, A+A); + VERIFY_EVALUATION_COUNT( call_ref_1(Ar, A), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(Ar, A), 0); + + Ref > Br(B); + VERIFY_EVALUATION_COUNT( call_ref_1(Br.transpose(), Br.transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(Br, Br), 1); + VERIFY_EVALUATION_COUNT( call_ref_2(Br.transpose(), Br.transpose()), 0); + + Ref > Arc(A); +// VERIFY_EVALUATION_COUNT( call_ref_1(Arc, Arc), 0); // does not compile on purpose + VERIFY_EVALUATION_COUNT( call_ref_2(Arc, Arc), 0); + + VERIFY_EVALUATION_COUNT( call_ref_2(A.middleCols(1,3), A.middleCols(1,3)), 0); + + VERIFY_EVALUATION_COUNT( call_ref_2(A.col(2), A.col(2)), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(vc, vc), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(vr.transpose(), vr.transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(vr, vr.transpose()), 0); + + VERIFY_EVALUATION_COUNT( call_ref_2(A.block(1,1,3,3), A.block(1,1,3,3)), 1); // should be 0 (allocate starts/nnz only) + + VERIFY_EVALUATION_COUNT( call_ref_4(vc, vc), 0); + VERIFY_EVALUATION_COUNT( call_ref_4(vr, vr.transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(vc, vc), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(vr, vr.transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_4(A.col(2), A.col(2)), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(A.col(2), A.col(2)), 0); + // VERIFY_EVALUATION_COUNT( call_ref_4(A.row(2), A.row(2).transpose()), 1); // does not compile on purpose + VERIFY_EVALUATION_COUNT( call_ref_5(A.row(2), A.row(2).transpose()), 1); +} + +void test_sparse_ref() +{ + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1( check_const_correctness(SparseMatrix()) ); + CALL_SUBTEST_1( check_const_correctness(SparseMatrix()) ); + CALL_SUBTEST_2( call_ref() ); + + CALL_SUBTEST_3( check_const_correctness(SparseVector()) ); + CALL_SUBTEST_3( check_const_correctness(SparseVector()) ); + } +} diff --git a/ground/gcs/src/libs/eigen/test/sparse_solver.h b/ground/gcs/src/libs/eigen/test/sparse_solver.h index e1619d62a3..5145bc3eb5 100644 --- a/ground/gcs/src/libs/eigen/test/sparse_solver.h +++ b/ground/gcs/src/libs/eigen/test/sparse_solver.h @@ -9,68 +9,123 @@ #include "sparse.h" #include +#include + +template +void solve_with_guess(IterativeSolverBase& solver, const MatrixBase& b, const Guess& g, Result &x) { + if(internal::random()) + { + // With a temporary through evaluator + x = solver.derived().solveWithGuess(b,g) + Result::Zero(x.rows(), x.cols()); + } + else + { + // direct evaluation within x through Assignment + x = solver.derived().solveWithGuess(b.derived(),g); + } +} + +template +void solve_with_guess(SparseSolverBase& solver, const MatrixBase& b, const Guess& , Result& x) { + if(internal::random()) + x = solver.derived().solve(b) + Result::Zero(x.rows(), x.cols()); + else + x = solver.derived().solve(b); +} + +template +void solve_with_guess(SparseSolverBase& solver, const SparseMatrixBase& b, const Guess& , Result& x) { + x = solver.derived().solve(b); +} template void check_sparse_solving(Solver& solver, const typename Solver::MatrixType& A, const Rhs& b, const DenseMat& dA, const DenseRhs& db) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; + typedef typename Mat::StorageIndex StorageIndex; - DenseRhs refX = dA.lu().solve(db); + DenseRhs refX = dA.householderQr().solve(db); { - Rhs x(b.rows(), b.cols()); + Rhs x(A.cols(), b.cols()); Rhs oldb = b; solver.compute(A); if (solver.info() != Success) { - std::cerr << "sparse solver testing: factorization failed (check_sparse_solving)\n"; - exit(0); - return; + std::cerr << "ERROR | sparse solver testing, factorization failed (" << typeid(Solver).name() << ")\n"; + VERIFY(solver.info() == Success); } x = solver.solve(b); if (solver.info() != Success) { - std::cerr << "sparse solver testing: solving failed\n"; + std::cerr << "WARNING | sparse solver testing: solving failed (" << typeid(Solver).name() << ")\n"; return; } VERIFY(oldb.isApprox(b) && "sparse solver testing: the rhs should not be modified!"); + VERIFY(x.isApprox(refX,test_precision())); + x.setZero(); + solve_with_guess(solver, b, x, x); + VERIFY(solver.info() == Success && "solving failed when using analyzePattern/factorize API"); + VERIFY(oldb.isApprox(b) && "sparse solver testing: the rhs should not be modified!"); VERIFY(x.isApprox(refX,test_precision())); + x.setZero(); // test the analyze/factorize API solver.analyzePattern(A); solver.factorize(A); - if (solver.info() != Success) - { - std::cerr << "sparse solver testing: factorization failed (check_sparse_solving)\n"; - exit(0); - return; - } + VERIFY(solver.info() == Success && "factorization failed when using analyzePattern/factorize API"); x = solver.solve(b); - if (solver.info() != Success) - { - std::cerr << "sparse solver testing: solving failed\n"; - return; - } + VERIFY(solver.info() == Success && "solving failed when using analyzePattern/factorize API"); VERIFY(oldb.isApprox(b) && "sparse solver testing: the rhs should not be modified!"); - VERIFY(x.isApprox(refX,test_precision())); - } - - // test dense Block as the result and rhs: - { - DenseRhs x(db.rows(), db.cols()); - DenseRhs oldb(db); + x.setZero(); - x.block(0,0,x.rows(),x.cols()) = solver.solve(db.block(0,0,db.rows(),db.cols())); - VERIFY(oldb.isApprox(db) && "sparse solver testing: the rhs should not be modified!"); - VERIFY(x.isApprox(refX,test_precision())); + // test with Map + MappedSparseMatrix Am(A.rows(), A.cols(), A.nonZeros(), const_cast(A.outerIndexPtr()), const_cast(A.innerIndexPtr()), const_cast(A.valuePtr())); + solver.compute(Am); + VERIFY(solver.info() == Success && "factorization failed when using Map"); + DenseRhs dx(refX); + dx.setZero(); + Map xm(dx.data(), dx.rows(), dx.cols()); + Map bm(db.data(), db.rows(), db.cols()); + xm = solver.solve(bm); + VERIFY(solver.info() == Success && "solving failed when using Map"); + VERIFY(oldb.isApprox(bm) && "sparse solver testing: the rhs should not be modified!"); + VERIFY(xm.isApprox(refX,test_precision())); } - + // if not too large, do some extra check: if(A.rows()<2000) { + // test initialization ctor + { + Rhs x(b.rows(), b.cols()); + Solver solver2(A); + VERIFY(solver2.info() == Success); + x = solver2.solve(b); + VERIFY(x.isApprox(refX,test_precision())); + } + + // test dense Block as the result and rhs: + { + DenseRhs x(refX.rows(), refX.cols()); + DenseRhs oldb(db); + x.setZero(); + x.block(0,0,x.rows(),x.cols()) = solver.solve(db.block(0,0,db.rows(),db.cols())); + VERIFY(oldb.isApprox(db) && "sparse solver testing: the rhs should not be modified!"); + VERIFY(x.isApprox(refX,test_precision())); + } + + // test uncompressed inputs + { + Mat A2 = A; + A2.reserve((ArrayXf::Random(A.outerSize())+2).template cast().eval()); + solver.compute(A2); + Rhs x = solver.solve(b); + VERIFY(x.isApprox(refX,test_precision())); + } // test expression as input { @@ -86,41 +141,35 @@ void check_sparse_solving(Solver& solver, const typename Solver::MatrixType& A, } template -void check_sparse_solving_real_cases(Solver& solver, const typename Solver::MatrixType& A, const Rhs& b, const Rhs& refX) +void check_sparse_solving_real_cases(Solver& solver, const typename Solver::MatrixType& A, const Rhs& b, const typename Solver::MatrixType& fullA, const Rhs& refX) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; typedef typename Mat::RealScalar RealScalar; - Rhs x(b.rows(), b.cols()); - + Rhs x(A.cols(), b.cols()); + solver.compute(A); if (solver.info() != Success) { - std::cerr << "sparse solver testing: factorization failed (check_sparse_solving_real_cases)\n"; - exit(0); - return; + std::cerr << "ERROR | sparse solver testing, factorization failed (" << typeid(Solver).name() << ")\n"; + VERIFY(solver.info() == Success); } x = solver.solve(b); + if (solver.info() != Success) { - std::cerr << "sparse solver testing: solving failed\n"; + std::cerr << "WARNING | sparse solver testing, solving failed (" << typeid(Solver).name() << ")\n"; return; } - RealScalar res_error; - // Compute the norm of the relative error - if(refX.size() != 0) - res_error = (refX - x).norm()/refX.norm(); - else - { - // Compute the relative residual norm - res_error = (b - A * x).norm()/b.norm(); - } - if (res_error > test_precision() ){ - std::cerr << "Test " << g_test_stack.back() << " failed in "EI_PP_MAKE_STRING(__FILE__) - << " (" << EI_PP_MAKE_STRING(__LINE__) << ")" << std::endl << std::endl; - abort(); + RealScalar res_error = (fullA*x-b).norm()/b.norm(); + VERIFY( (res_error <= test_precision() ) && "sparse solver failed without noticing it"); + + + if(refX.size() != 0 && (refX - x).norm()/refX.norm() > test_precision()) + { + std::cerr << "WARNING | found solution is different from the provided reference one\n"; } } @@ -133,7 +182,7 @@ void check_sparse_determinant(Solver& solver, const typename Solver::MatrixType& solver.compute(A); if (solver.info() != Success) { - std::cerr << "sparse solver testing: factorization failed (check_sparse_determinant)\n"; + std::cerr << "WARNING | sparse solver testing: factorization failed (check_sparse_determinant)\n"; return; } @@ -150,7 +199,7 @@ void check_sparse_abs_determinant(Solver& solver, const typename Solver::MatrixT solver.compute(A); if (solver.info() != Success) { - std::cerr << "sparse solver testing: factorization failed (check_sparse_abs_determinant)\n"; + std::cerr << "WARNING | sparse solver testing: factorization failed (check_sparse_abs_determinant)\n"; return; } @@ -197,13 +246,33 @@ inline std::string get_matrixfolder() mat_folder = mat_folder + static_cast("/real/"); return mat_folder; } +std::string sym_to_string(int sym) +{ + if(sym==Symmetric) return "Symmetric "; + if(sym==SPD) return "SPD "; + return ""; +} +template +std::string solver_stats(const IterativeSolverBase &solver) +{ + std::stringstream ss; + ss << solver.iterations() << " iters, error: " << solver.error(); + return ss.str(); +} +template +std::string solver_stats(const SparseSolverBase &/*solver*/) +{ + return ""; +} #endif -template void check_sparse_spd_solving(Solver& solver) +template void check_sparse_spd_solving(Solver& solver, int maxSize = 300, int maxRealWorldSize = 100000) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; - typedef SparseMatrix SpMat; + typedef typename Mat::StorageIndex StorageIndex; + typedef SparseMatrix SpMat; + typedef SparseVector SpVec; typedef Matrix DenseMatrix; typedef Matrix DenseVector; @@ -211,7 +280,7 @@ template void check_sparse_spd_solving(Solver& solver) Mat A, halfA; DenseMatrix dA; for (int i = 0; i < g_repeat; i++) { - int size = generate_sparse_spd_problem(solver, A, halfA, dA); + int size = generate_sparse_spd_problem(solver, A, halfA, dA, maxSize); // generate the right hand sides int rhsCols = internal::random(1,16); @@ -220,13 +289,17 @@ template void check_sparse_spd_solving(Solver& solver) DenseVector b = DenseVector::Random(size); DenseMatrix dB(size,rhsCols); initSparse(density, dB, B, ForceNonZeroDiag); + SpVec c = B.col(0); + DenseVector dc = dB.col(0); - check_sparse_solving(solver, A, b, dA, b); - check_sparse_solving(solver, halfA, b, dA, b); - check_sparse_solving(solver, A, dB, dA, dB); - check_sparse_solving(solver, halfA, dB, dA, dB); - check_sparse_solving(solver, A, B, dA, dB); - check_sparse_solving(solver, halfA, B, dA, dB); + CALL_SUBTEST( check_sparse_solving(solver, A, b, dA, b) ); + CALL_SUBTEST( check_sparse_solving(solver, halfA, b, dA, b) ); + CALL_SUBTEST( check_sparse_solving(solver, A, dB, dA, dB) ); + CALL_SUBTEST( check_sparse_solving(solver, halfA, dB, dA, dB) ); + CALL_SUBTEST( check_sparse_solving(solver, A, B, dA, dB) ); + CALL_SUBTEST( check_sparse_solving(solver, halfA, B, dA, dB) ); + CALL_SUBTEST( check_sparse_solving(solver, A, c, dA, dc) ); + CALL_SUBTEST( check_sparse_solving(solver, halfA, c, dA, dc) ); // check only once if(i==0) @@ -237,25 +310,44 @@ template void check_sparse_spd_solving(Solver& solver) } // First, get the folder -#ifdef TEST_REAL_CASES - if (internal::is_same::value - || internal::is_same >::value) - return ; - - std::string mat_folder = get_matrixfolder(); - MatrixMarketIterator it(mat_folder); - for (; it; ++it) +#ifdef TEST_REAL_CASES + // Test real problems with double precision only + if (internal::is_same::Real, double>::value) { - if (it.sym() == SPD){ - Mat halfA; - PermutationMatrix pnull; - halfA.template selfadjointView() = it.matrix().template triangularView().twistedBy(pnull); - - std::cout<< " ==== SOLVING WITH MATRIX " << it.matname() << " ==== \n"; - check_sparse_solving_real_cases(solver, it.matrix(), it.rhs(), it.refX()); - check_sparse_solving_real_cases(solver, halfA, it.rhs(), it.refX()); + std::string mat_folder = get_matrixfolder(); + MatrixMarketIterator it(mat_folder); + for (; it; ++it) + { + if (it.sym() == SPD){ + A = it.matrix(); + if(A.diagonal().size() <= maxRealWorldSize) + { + DenseVector b = it.rhs(); + DenseVector refX = it.refX(); + PermutationMatrix pnull; + halfA.resize(A.rows(), A.cols()); + if(Solver::UpLo == (Lower|Upper)) + halfA = A; + else + halfA.template selfadjointView() = A.template triangularView().twistedBy(pnull); + + std::cout << "INFO | Testing " << sym_to_string(it.sym()) << "sparse problem " << it.matname() + << " (" << A.rows() << "x" << A.cols() << ") using " << typeid(Solver).name() << "..." << std::endl; + CALL_SUBTEST( check_sparse_solving_real_cases(solver, A, b, A, refX) ); + std::string stats = solver_stats(solver); + if(stats.size()>0) + std::cout << "INFO | " << stats << std::endl; + CALL_SUBTEST( check_sparse_solving_real_cases(solver, halfA, b, A, refX) ); + } + else + { + std::cout << "INFO | Skip sparse problem \"" << it.matname() << "\" (too large)" << std::endl; + } + } } } +#else + EIGEN_UNUSED_VARIABLE(maxRealWorldSize); #endif } @@ -277,37 +369,39 @@ template void check_sparse_spd_determinant(Solver& solver) } template -int generate_sparse_square_problem(Solver&, typename Solver::MatrixType& A, DenseMat& dA, int maxSize = 300) +Index generate_sparse_square_problem(Solver&, typename Solver::MatrixType& A, DenseMat& dA, int maxSize = 300, int options = ForceNonZeroDiag) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; - int size = internal::random(1,maxSize); + Index size = internal::random(1,maxSize); double density = (std::max)(8./(size*size), 0.01); A.resize(size,size); dA.resize(size,size); - initSparse(density, dA, A, ForceNonZeroDiag); + initSparse(density, dA, A, options); return size; } struct prune_column { - int m_col; - prune_column(int col) : m_col(col) {} + Index m_col; + prune_column(Index col) : m_col(col) {} template - bool operator()(int, int col, const Scalar&) const { + bool operator()(Index, Index col, const Scalar&) const { return col != m_col; } }; -template void check_sparse_square_solving(Solver& solver, bool checkDeficient = false) + +template void check_sparse_square_solving(Solver& solver, int maxSize = 300, int maxRealWorldSize = 100000, bool checkDeficient = false) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; - typedef SparseMatrix SpMat; + typedef SparseMatrix SpMat; + typedef SparseVector SpVec; typedef Matrix DenseMatrix; typedef Matrix DenseVector; @@ -316,7 +410,7 @@ template void check_sparse_square_solving(Solver& solver, bool Mat A; DenseMatrix dA; for (int i = 0; i < g_repeat; i++) { - int size = generate_sparse_square_problem(solver, A, dA); + Index size = generate_sparse_square_problem(solver, A, dA, maxSize); A.makeCompressed(); DenseVector b = DenseVector::Random(size); @@ -325,9 +419,12 @@ template void check_sparse_square_solving(Solver& solver, bool double density = (std::max)(8./(size*rhsCols), 0.1); initSparse(density, dB, B, ForceNonZeroDiag); B.makeCompressed(); - check_sparse_solving(solver, A, b, dA, b); - check_sparse_solving(solver, A, dB, dA, dB); - check_sparse_solving(solver, A, B, dA, dB); + SpVec c = B.col(0); + DenseVector dc = dB.col(0); + CALL_SUBTEST(check_sparse_solving(solver, A, b, dA, b)); + CALL_SUBTEST(check_sparse_solving(solver, A, dB, dA, dB)); + CALL_SUBTEST(check_sparse_solving(solver, A, B, dA, dB)); + CALL_SUBTEST(check_sparse_solving(solver, A, c, dA, dc)); // check only once if(i==0) @@ -337,7 +434,7 @@ template void check_sparse_square_solving(Solver& solver, bool } // regression test for Bug 792 (structurally rank deficient matrices): if(checkDeficient && size>1) { - int col = internal::random(0,size-1); + Index col = internal::random(0,int(size-1)); A.prune(prune_column(col)); solver.compute(A); VERIFY_IS_EQUAL(solver.info(), NumericalIssue); @@ -346,17 +443,33 @@ template void check_sparse_square_solving(Solver& solver, bool // First, get the folder #ifdef TEST_REAL_CASES - if (internal::is_same::value - || internal::is_same >::value) - return ; - - std::string mat_folder = get_matrixfolder(); - MatrixMarketIterator it(mat_folder); - for (; it; ++it) + // Test real problems with double precision only + if (internal::is_same::Real, double>::value) { - std::cout<< " ==== SOLVING WITH MATRIX " << it.matname() << " ==== \n"; - check_sparse_solving_real_cases(solver, it.matrix(), it.rhs(), it.refX()); + std::string mat_folder = get_matrixfolder(); + MatrixMarketIterator it(mat_folder); + for (; it; ++it) + { + A = it.matrix(); + if(A.diagonal().size() <= maxRealWorldSize) + { + DenseVector b = it.rhs(); + DenseVector refX = it.refX(); + std::cout << "INFO | Testing " << sym_to_string(it.sym()) << "sparse problem " << it.matname() + << " (" << A.rows() << "x" << A.cols() << ") using " << typeid(Solver).name() << "..." << std::endl; + CALL_SUBTEST(check_sparse_solving_real_cases(solver, A, b, A, refX)); + std::string stats = solver_stats(solver); + if(stats.size()>0) + std::cout << "INFO | " << stats << std::endl; + } + else + { + std::cout << "INFO | SKIP sparse problem \"" << it.matname() << "\" (too large)" << std::endl; + } + } } +#else + EIGEN_UNUSED_VARIABLE(maxRealWorldSize); #endif } @@ -366,13 +479,20 @@ template void check_sparse_square_determinant(Solver& solver) typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; typedef Matrix DenseMatrix; - - // generate the problem - Mat A; - DenseMatrix dA; - generate_sparse_square_problem(solver, A, dA, 30); - A.makeCompressed(); + for (int i = 0; i < g_repeat; i++) { + // generate the problem + Mat A; + DenseMatrix dA; + + int size = internal::random(1,30); + dA.setRandom(size,size); + + dA = (dA.array().abs()<0.3).select(0,dA); + dA.diagonal() = (dA.diagonal().array()==0).select(1,dA.diagonal()); + A = dA.sparseView(); + A.makeCompressed(); + check_sparse_determinant(solver, A, dA); } } @@ -383,13 +503,63 @@ template void check_sparse_square_abs_determinant(Solver& solve typedef typename Mat::Scalar Scalar; typedef Matrix DenseMatrix; - // generate the problem - Mat A; - DenseMatrix dA; - generate_sparse_square_problem(solver, A, dA, 30); - A.makeCompressed(); for (int i = 0; i < g_repeat; i++) { + // generate the problem + Mat A; + DenseMatrix dA; + generate_sparse_square_problem(solver, A, dA, 30); + A.makeCompressed(); check_sparse_abs_determinant(solver, A, dA); } } +template +void generate_sparse_leastsquare_problem(Solver&, typename Solver::MatrixType& A, DenseMat& dA, int maxSize = 300, int options = ForceNonZeroDiag) +{ + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + + int rows = internal::random(1,maxSize); + int cols = internal::random(1,rows); + double density = (std::max)(8./(rows*cols), 0.01); + + A.resize(rows,cols); + dA.resize(rows,cols); + + initSparse(density, dA, A, options); +} + +template void check_sparse_leastsquare_solving(Solver& solver) +{ + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + typedef SparseMatrix SpMat; + typedef Matrix DenseMatrix; + typedef Matrix DenseVector; + + int rhsCols = internal::random(1,16); + + Mat A; + DenseMatrix dA; + for (int i = 0; i < g_repeat; i++) { + generate_sparse_leastsquare_problem(solver, A, dA); + + A.makeCompressed(); + DenseVector b = DenseVector::Random(A.rows()); + DenseMatrix dB(A.rows(),rhsCols); + SpMat B(A.rows(),rhsCols); + double density = (std::max)(8./(A.rows()*rhsCols), 0.1); + initSparse(density, dB, B, ForceNonZeroDiag); + B.makeCompressed(); + check_sparse_solving(solver, A, b, dA, b); + check_sparse_solving(solver, A, dB, dA, dB); + check_sparse_solving(solver, A, B, dA, dB); + + // check only once + if(i==0) + { + b = DenseVector::Zero(A.rows()); + check_sparse_solving(solver, A, b, dA, b); + } + } +} diff --git a/ground/gcs/src/libs/eigen/test/sparse_vector.cpp b/ground/gcs/src/libs/eigen/test/sparse_vector.cpp index 0c94768034..b3e1dda259 100644 --- a/ground/gcs/src/libs/eigen/test/sparse_vector.cpp +++ b/ground/gcs/src/libs/eigen/test/sparse_vector.cpp @@ -9,22 +9,22 @@ #include "sparse.h" -template void sparse_vector(int rows, int cols) +template void sparse_vector(int rows, int cols) { double densityMat = (std::max)(8./(rows*cols), 0.01); - double densityVec = (std::max)(8./float(rows), 0.1); + double densityVec = (std::max)(8./(rows), 0.1); typedef Matrix DenseMatrix; typedef Matrix DenseVector; - typedef SparseVector SparseVectorType; - typedef SparseMatrix SparseMatrixType; + typedef SparseVector SparseVectorType; + typedef SparseMatrix SparseMatrixType; Scalar eps = 1e-6; SparseMatrixType m1(rows,rows); SparseVectorType v1(rows), v2(rows), v3(rows); DenseMatrix refM1 = DenseMatrix::Zero(rows, rows); DenseVector refV1 = DenseVector::Random(rows), - refV2 = DenseVector::Random(rows), - refV3 = DenseVector::Random(rows); + refV2 = DenseVector::Random(rows), + refV3 = DenseVector::Random(rows); std::vector zerocoords, nonzerocoords; initSparse(densityVec, refV1, v1, &zerocoords, &nonzerocoords); @@ -52,6 +52,20 @@ template void sparse_vector(int rows, int cols) } } VERIFY_IS_APPROX(v1, refV1); + + // test coeffRef with reallocation + { + SparseVectorType v4(rows); + DenseVector v5 = DenseVector::Zero(rows); + for(int k=0; k(0,rows-1); + Scalar v = internal::random(); + v4.coeffRef(i) += v; + v5.coeffRef(i) += v; + } + VERIFY_IS_APPROX(v4,v5); + } v1.coeffRef(nonzerocoords[0]) = Scalar(5); refV1.coeffRef(nonzerocoords[0]) = Scalar(5); @@ -71,9 +85,12 @@ template void sparse_vector(int rows, int cols) VERIFY_IS_APPROX(v1.dot(v2), refV1.dot(refV2)); VERIFY_IS_APPROX(v1.dot(refV2), refV1.dot(refV2)); + VERIFY_IS_APPROX(m1*v2, refM1*refV2); VERIFY_IS_APPROX(v1.dot(m1*v2), refV1.dot(refM1*refV2)); - int i = internal::random(0,rows-1); - VERIFY_IS_APPROX(v1.dot(m1.col(i)), refV1.dot(refM1.col(i))); + { + int i = internal::random(0,rows-1); + VERIFY_IS_APPROX(v1.dot(m1.col(i)), refV1.dot(refM1.col(i))); + } VERIFY_IS_APPROX(v1.squaredNorm(), refV1.squaredNorm()); @@ -96,15 +113,51 @@ template void sparse_vector(int rows, int cols) VERIFY_IS_APPROX(refV3 = v1.transpose(),v1.toDense()); VERIFY_IS_APPROX(DenseVector(v1),v1.toDense()); + // test conservative resize + { + std::vector inc; + if(rows > 3) + inc.push_back(-3); + inc.push_back(0); + inc.push_back(3); + inc.push_back(1); + inc.push_back(10); + + for(std::size_t i = 0; i< inc.size(); i++) { + StorageIndex incRows = inc[i]; + SparseVectorType vec1(rows); + DenseVector refVec1 = DenseVector::Zero(rows); + initSparse(densityVec, refVec1, vec1); + + vec1.conservativeResize(rows+incRows); + refVec1.conservativeResize(rows+incRows); + if (incRows > 0) refVec1.tail(incRows).setZero(); + + VERIFY_IS_APPROX(vec1, refVec1); + + // Insert new values + if (incRows > 0) + vec1.insert(vec1.rows()-1) = refVec1(refVec1.rows()-1) = 1; + + VERIFY_IS_APPROX(vec1, refVec1); + } + } + } void test_sparse_vector() { for(int i = 0; i < g_repeat; i++) { + int r = Eigen::internal::random(1,500), c = Eigen::internal::random(1,500); + if(Eigen::internal::random(0,4) == 0) { + r = c; // check square matrices in 25% of tries + } + EIGEN_UNUSED_VARIABLE(r+c); + CALL_SUBTEST_1(( sparse_vector(8, 8) )); - CALL_SUBTEST_2(( sparse_vector, int>(16, 16) )); - CALL_SUBTEST_1(( sparse_vector(299, 535) )); - CALL_SUBTEST_1(( sparse_vector(299, 535) )); + CALL_SUBTEST_2(( sparse_vector, int>(r, c) )); + CALL_SUBTEST_1(( sparse_vector(r, c) )); + CALL_SUBTEST_1(( sparse_vector(r, c) )); } } diff --git a/ground/gcs/src/libs/eigen/test/sparselu.cpp b/ground/gcs/src/libs/eigen/test/sparselu.cpp index b3d67aea88..bd000baf10 100644 --- a/ground/gcs/src/libs/eigen/test/sparselu.cpp +++ b/ground/gcs/src/libs/eigen/test/sparselu.cpp @@ -3,25 +3,9 @@ // // Copyright (C) 2012 Désiré Nuentsa-Wakam // -// Eigen is free software; you can redistribute it and/or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 3 of the License, or (at your option) any later version. -// -// Alternatively, 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 2 of -// the License, or (at your option) any later version. -// -// Eigen 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 Lesser General Public License or the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public -// License and a copy of the GNU General Public License along with -// Eigen. If not, see . - +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. // SparseLU solve does not accept column major matrices for the destination. // However, as expected, the generic check_sparse_square_solving routines produces row-major @@ -41,9 +25,9 @@ template void test_sparselu_T() SparseLU, AMDOrdering > sparselu_amd; SparseLU, NaturalOrdering > sparselu_natural; - check_sparse_square_solving(sparselu_colamd, true); - check_sparse_square_solving(sparselu_amd, true); - check_sparse_square_solving(sparselu_natural,true); + check_sparse_square_solving(sparselu_colamd, 300, 100000, true); + check_sparse_square_solving(sparselu_amd, 300, 10000, true); + check_sparse_square_solving(sparselu_natural, 300, 2000, true); check_sparse_square_abs_determinant(sparselu_colamd); check_sparse_square_abs_determinant(sparselu_amd); diff --git a/ground/gcs/src/libs/eigen/test/sparseqr.cpp b/ground/gcs/src/libs/eigen/test/sparseqr.cpp index 451c0e7f82..e8605fd210 100644 --- a/ground/gcs/src/libs/eigen/test/sparseqr.cpp +++ b/ground/gcs/src/libs/eigen/test/sparseqr.cpp @@ -10,11 +10,12 @@ #include template -int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300) +int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150) { + eigen_assert(maxRows >= maxCols); typedef typename MatrixType::Scalar Scalar; int rows = internal::random(1,maxRows); - int cols = internal::random(1,rows); + int cols = internal::random(1,maxCols); double density = (std::max)(8./(rows*cols), 0.01); A.resize(rows,cols); @@ -53,7 +54,7 @@ template void test_sparseqr_scalar() b = dA * DenseVector::Random(A.cols()); solver.compute(A); - if(internal::random(0,1)>0.5) + if(internal::random(0,1)>0.5f) solver.factorize(A); // this checks that calling analyzePattern is not needed if the pattern do not change. if (solver.info() != Success) { @@ -88,6 +89,11 @@ template void test_sparseqr_scalar() QtQ = Q * Q.adjoint(); idM.resize(Q.rows(), Q.rows()); idM.setIdentity(); VERIFY(idM.isApprox(QtQ)); + + // Q to dense + DenseMat dQ; + dQ = solver.matrixQ(); + VERIFY_IS_APPROX(Q, dQ); } void test_sparseqr() { diff --git a/ground/gcs/src/libs/eigen/test/spqr_support.cpp b/ground/gcs/src/libs/eigen/test/spqr_support.cpp index b8980e0816..81e63b6a57 100644 --- a/ground/gcs/src/libs/eigen/test/spqr_support.cpp +++ b/ground/gcs/src/libs/eigen/test/spqr_support.cpp @@ -5,6 +5,8 @@ // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed + +#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS #include "sparse.h" #include @@ -18,8 +20,8 @@ int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows int cols = internal::random(1,rows); double density = (std::max)(8./(rows*cols), 0.01); - A.resize(rows,rows); - dA.resize(rows,rows); + A.resize(rows,cols); + dA.resize(rows,cols); initSparse(density, dA, A,ForceNonZeroDiag); A.makeCompressed(); return rows; @@ -35,7 +37,7 @@ template void test_spqr_scalar() SPQR solver; generate_sparse_rectangular_problem(A,dA); - int m = A.rows(); + Index m = A.rows(); b = DenseVector::Random(m); solver.compute(A); if (solver.info() != Success) diff --git a/ground/gcs/src/libs/eigen/test/stable_norm.cpp b/ground/gcs/src/libs/eigen/test/stable_norm.cpp index 231dd9189d..c3eb5ff313 100644 --- a/ground/gcs/src/libs/eigen/test/stable_norm.cpp +++ b/ground/gcs/src/libs/eigen/test/stable_norm.cpp @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2009 Gael Guennebaud +// Copyright (C) 2009-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -9,14 +9,6 @@ #include "main.h" -// workaround aggressive optimization in ICC -template EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; } - -template bool isFinite(const T& x) -{ - return isNotNaN(sub(x,x)); -} - template EIGEN_DONT_INLINE T copy(const T& x) { return x; @@ -32,6 +24,8 @@ template void stable_norm(const MatrixType& m) typedef typename MatrixType::Index Index; typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; + + bool complex_real_product_ok = true; // Check the basic machine-dependent constants. { @@ -44,6 +38,16 @@ template void stable_norm(const MatrixType& m) VERIFY( (!(iemin > 1 - 2*it || 1+it>iemax || (it==2 && ibeta<5) || (it<=4 && ibeta <= 3 ) || it<2)) && "the stable norm algorithm cannot be guaranteed on this computer"); + + Scalar inf = std::numeric_limits::infinity(); + if(NumTraits::IsComplex && (numext::isnan)(inf*RealScalar(1)) ) + { + complex_real_product_ok = false; + static bool first = true; + if(first) + std::cerr << "WARNING: compiler mess up complex*real product, " << inf << " * " << 1.0 << " = " << inf*RealScalar(1) << std::endl; + first = false; + } } @@ -76,19 +80,19 @@ template void stable_norm(const MatrixType& m) RealScalar size = static_cast(m.size()); - // test isFinite - VERIFY(!isFinite( std::numeric_limits::infinity())); - VERIFY(!isFinite(sqrt(-abs(big)))); + // test numext::isfinite + VERIFY(!(numext::isfinite)( std::numeric_limits::infinity())); + VERIFY(!(numext::isfinite)(sqrt(-abs(big)))); // test overflow - VERIFY(isFinite(sqrt(size)*abs(big))); + VERIFY((numext::isfinite)(sqrt(size)*abs(big))); VERIFY_IS_NOT_APPROX(sqrt(copy(vbig.squaredNorm())), abs(sqrt(size)*big)); // here the default norm must fail VERIFY_IS_APPROX(vbig.stableNorm(), sqrt(size)*abs(big)); VERIFY_IS_APPROX(vbig.blueNorm(), sqrt(size)*abs(big)); VERIFY_IS_APPROX(vbig.hypotNorm(), sqrt(size)*abs(big)); // test underflow - VERIFY(isFinite(sqrt(size)*abs(small))); + VERIFY((numext::isfinite)(sqrt(size)*abs(small))); VERIFY_IS_NOT_APPROX(sqrt(copy(vsmall.squaredNorm())), abs(sqrt(size)*small)); // here the default norm must fail VERIFY_IS_APPROX(vsmall.stableNorm(), sqrt(size)*abs(small)); VERIFY_IS_APPROX(vsmall.blueNorm(), sqrt(size)*abs(small)); @@ -101,6 +105,79 @@ template void stable_norm(const MatrixType& m) VERIFY_IS_APPROX(vrand.rowwise().stableNorm(), vrand.rowwise().norm()); VERIFY_IS_APPROX(vrand.rowwise().blueNorm(), vrand.rowwise().norm()); VERIFY_IS_APPROX(vrand.rowwise().hypotNorm(), vrand.rowwise().norm()); + + // test NaN, +inf, -inf + MatrixType v; + Index i = internal::random(0,rows-1); + Index j = internal::random(0,cols-1); + + // NaN + { + v = vrand; + v(i,j) = std::numeric_limits::quiet_NaN(); + VERIFY(!(numext::isfinite)(v.squaredNorm())); VERIFY((numext::isnan)(v.squaredNorm())); + VERIFY(!(numext::isfinite)(v.norm())); VERIFY((numext::isnan)(v.norm())); + VERIFY(!(numext::isfinite)(v.stableNorm())); VERIFY((numext::isnan)(v.stableNorm())); + VERIFY(!(numext::isfinite)(v.blueNorm())); VERIFY((numext::isnan)(v.blueNorm())); + VERIFY(!(numext::isfinite)(v.hypotNorm())); VERIFY((numext::isnan)(v.hypotNorm())); + } + + // +inf + { + v = vrand; + v(i,j) = std::numeric_limits::infinity(); + VERIFY(!(numext::isfinite)(v.squaredNorm())); VERIFY(isPlusInf(v.squaredNorm())); + VERIFY(!(numext::isfinite)(v.norm())); VERIFY(isPlusInf(v.norm())); + VERIFY(!(numext::isfinite)(v.stableNorm())); + if(complex_real_product_ok){ + VERIFY(isPlusInf(v.stableNorm())); + } + VERIFY(!(numext::isfinite)(v.blueNorm())); VERIFY(isPlusInf(v.blueNorm())); + VERIFY(!(numext::isfinite)(v.hypotNorm())); VERIFY(isPlusInf(v.hypotNorm())); + } + + // -inf + { + v = vrand; + v(i,j) = -std::numeric_limits::infinity(); + VERIFY(!(numext::isfinite)(v.squaredNorm())); VERIFY(isPlusInf(v.squaredNorm())); + VERIFY(!(numext::isfinite)(v.norm())); VERIFY(isPlusInf(v.norm())); + VERIFY(!(numext::isfinite)(v.stableNorm())); + if(complex_real_product_ok) { + VERIFY(isPlusInf(v.stableNorm())); + } + VERIFY(!(numext::isfinite)(v.blueNorm())); VERIFY(isPlusInf(v.blueNorm())); + VERIFY(!(numext::isfinite)(v.hypotNorm())); VERIFY(isPlusInf(v.hypotNorm())); + } + + // mix + { + Index i2 = internal::random(0,rows-1); + Index j2 = internal::random(0,cols-1); + v = vrand; + v(i,j) = -std::numeric_limits::infinity(); + v(i2,j2) = std::numeric_limits::quiet_NaN(); + VERIFY(!(numext::isfinite)(v.squaredNorm())); VERIFY((numext::isnan)(v.squaredNorm())); + VERIFY(!(numext::isfinite)(v.norm())); VERIFY((numext::isnan)(v.norm())); + VERIFY(!(numext::isfinite)(v.stableNorm())); VERIFY((numext::isnan)(v.stableNorm())); + VERIFY(!(numext::isfinite)(v.blueNorm())); VERIFY((numext::isnan)(v.blueNorm())); + VERIFY(!(numext::isfinite)(v.hypotNorm())); VERIFY((numext::isnan)(v.hypotNorm())); + } + + // stableNormalize[d] + { + VERIFY_IS_APPROX(vrand.stableNormalized(), vrand.normalized()); + MatrixType vcopy(vrand); + vcopy.stableNormalize(); + VERIFY_IS_APPROX(vcopy, vrand.normalized()); + VERIFY_IS_APPROX((vrand.stableNormalized()).norm(), RealScalar(1)); + VERIFY_IS_APPROX(vcopy.norm(), RealScalar(1)); + VERIFY_IS_APPROX((vbig.stableNormalized()).norm(), RealScalar(1)); + VERIFY_IS_APPROX((vsmall.stableNormalized()).norm(), RealScalar(1)); + RealScalar big_scaling = ((std::numeric_limits::max)() * RealScalar(1e-4)); + VERIFY_IS_APPROX(vbig/big_scaling, (vbig.stableNorm() * vbig.stableNormalized()).eval()/big_scaling); + VERIFY_IS_APPROX(vsmall, vsmall.stableNorm() * vsmall.stableNormalized()); + } } void test_stable_norm() diff --git a/ground/gcs/src/libs/eigen/test/stdvector.cpp b/ground/gcs/src/libs/eigen/test/stdvector.cpp index 6e173c678b..50cb3341da 100644 --- a/ground/gcs/src/libs/eigen/test/stdvector.cpp +++ b/ground/gcs/src/libs/eigen/test/stdvector.cpp @@ -34,7 +34,7 @@ void check_stdvector_matrix(const MatrixType& m) VERIFY_IS_APPROX(v[21], y); v.push_back(x); VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(MatrixType)); + VERIFY((internal::UIntPtr)&(v[22]) == (internal::UIntPtr)&(v[21]) + sizeof(MatrixType)); // do a lot of push_back such that the vector gets internally resized // (with memory reallocation) @@ -69,7 +69,7 @@ void check_stdvector_transform(const TransformType&) VERIFY_IS_APPROX(v[21], y); v.push_back(x); VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(TransformType)); + VERIFY((internal::UIntPtr)&(v[22]) == (internal::UIntPtr)&(v[21]) + sizeof(TransformType)); // do a lot of push_back such that the vector gets internally resized // (with memory reallocation) @@ -104,7 +104,7 @@ void check_stdvector_quaternion(const QuaternionType&) VERIFY_IS_APPROX(v[21], y); v.push_back(x); VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(QuaternionType)); + VERIFY((internal::UIntPtr)&(v[22]) == (internal::UIntPtr)&(v[21]) + sizeof(QuaternionType)); // do a lot of push_back such that the vector gets internally resized // (with memory reallocation) diff --git a/ground/gcs/src/libs/eigen/test/stdvector_overload.cpp b/ground/gcs/src/libs/eigen/test/stdvector_overload.cpp index 736ff0ee79..959665954e 100644 --- a/ground/gcs/src/libs/eigen/test/stdvector_overload.cpp +++ b/ground/gcs/src/libs/eigen/test/stdvector_overload.cpp @@ -48,7 +48,7 @@ void check_stdvector_matrix(const MatrixType& m) VERIFY_IS_APPROX(v[21], y); v.push_back(x); VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(MatrixType)); + VERIFY((internal::UIntPtr)&(v[22]) == (internal::UIntPtr)&(v[21]) + sizeof(MatrixType)); // do a lot of push_back such that the vector gets internally resized // (with memory reallocation) @@ -83,7 +83,7 @@ void check_stdvector_transform(const TransformType&) VERIFY_IS_APPROX(v[21], y); v.push_back(x); VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(TransformType)); + VERIFY((internal::UIntPtr)&(v[22]) == (internal::UIntPtr)&(v[21]) + sizeof(TransformType)); // do a lot of push_back such that the vector gets internally resized // (with memory reallocation) @@ -118,7 +118,7 @@ void check_stdvector_quaternion(const QuaternionType&) VERIFY_IS_APPROX(v[21], y); v.push_back(x); VERIFY_IS_APPROX(v[22], x); - VERIFY((size_t)&(v[22]) == (size_t)&(v[21]) + sizeof(QuaternionType)); + VERIFY((internal::UIntPtr)&(v[22]) == (internal::UIntPtr)&(v[21]) + sizeof(QuaternionType)); // do a lot of push_back such that the vector gets internally resized // (with memory reallocation) diff --git a/ground/gcs/src/libs/eigen/test/superlu_support.cpp b/ground/gcs/src/libs/eigen/test/superlu_support.cpp index 3b16135bc3..98a7bc5c82 100644 --- a/ground/gcs/src/libs/eigen/test/superlu_support.cpp +++ b/ground/gcs/src/libs/eigen/test/superlu_support.cpp @@ -7,6 +7,7 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS #include "sparse_solver.h" #include diff --git a/ground/gcs/src/libs/eigen/test/svd_common.h b/ground/gcs/src/libs/eigen/test/svd_common.h new file mode 100644 index 0000000000..605d5dfeff --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/svd_common.h @@ -0,0 +1,483 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2014 Gael Guennebaud +// Copyright (C) 2009 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef SVD_DEFAULT +#error a macro SVD_DEFAULT(MatrixType) must be defined prior to including svd_common.h +#endif + +#ifndef SVD_FOR_MIN_NORM +#error a macro SVD_FOR_MIN_NORM(MatrixType) must be defined prior to including svd_common.h +#endif + +#include "svd_fill.h" + +// Check that the matrix m is properly reconstructed and that the U and V factors are unitary +// The SVD must have already been computed. +template +void svd_check_full(const MatrixType& m, const SvdType& svd) +{ + typedef typename MatrixType::Index Index; + Index rows = m.rows(); + Index cols = m.cols(); + + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime + }; + + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef Matrix MatrixUType; + typedef Matrix MatrixVType; + + MatrixType sigma = MatrixType::Zero(rows,cols); + sigma.diagonal() = svd.singularValues().template cast(); + MatrixUType u = svd.matrixU(); + MatrixVType v = svd.matrixV(); + RealScalar scaling = m.cwiseAbs().maxCoeff(); + if(scaling<(std::numeric_limits::min)()) + { + VERIFY(sigma.cwiseAbs().maxCoeff() <= (std::numeric_limits::min)()); + } + else + { + VERIFY_IS_APPROX(m/scaling, u * (sigma/scaling) * v.adjoint()); + } + VERIFY_IS_UNITARY(u); + VERIFY_IS_UNITARY(v); +} + +// Compare partial SVD defined by computationOptions to a full SVD referenceSvd +template +void svd_compare_to_full(const MatrixType& m, + unsigned int computationOptions, + const SvdType& referenceSvd) +{ + typedef typename MatrixType::RealScalar RealScalar; + Index rows = m.rows(); + Index cols = m.cols(); + Index diagSize = (std::min)(rows, cols); + RealScalar prec = test_precision(); + + SvdType svd(m, computationOptions); + + VERIFY_IS_APPROX(svd.singularValues(), referenceSvd.singularValues()); + + if(computationOptions & (ComputeFullV|ComputeThinV)) + { + VERIFY( (svd.matrixV().adjoint()*svd.matrixV()).isIdentity(prec) ); + VERIFY_IS_APPROX( svd.matrixV().leftCols(diagSize) * svd.singularValues().asDiagonal() * svd.matrixV().leftCols(diagSize).adjoint(), + referenceSvd.matrixV().leftCols(diagSize) * referenceSvd.singularValues().asDiagonal() * referenceSvd.matrixV().leftCols(diagSize).adjoint()); + } + + if(computationOptions & (ComputeFullU|ComputeThinU)) + { + VERIFY( (svd.matrixU().adjoint()*svd.matrixU()).isIdentity(prec) ); + VERIFY_IS_APPROX( svd.matrixU().leftCols(diagSize) * svd.singularValues().cwiseAbs2().asDiagonal() * svd.matrixU().leftCols(diagSize).adjoint(), + referenceSvd.matrixU().leftCols(diagSize) * referenceSvd.singularValues().cwiseAbs2().asDiagonal() * referenceSvd.matrixU().leftCols(diagSize).adjoint()); + } + + // The following checks are not critical. + // For instance, with Dived&Conquer SVD, if only the factor 'V' is computedt then different matrix-matrix product implementation will be used + // and the resulting 'V' factor might be significantly different when the SVD decomposition is not unique, especially with single precision float. + ++g_test_level; + if(computationOptions & ComputeFullU) VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU()); + if(computationOptions & ComputeThinU) VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU().leftCols(diagSize)); + if(computationOptions & ComputeFullV) VERIFY_IS_APPROX(svd.matrixV().cwiseAbs(), referenceSvd.matrixV().cwiseAbs()); + if(computationOptions & ComputeThinV) VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV().leftCols(diagSize)); + --g_test_level; +} + +// +template +void svd_least_square(const MatrixType& m, unsigned int computationOptions) +{ + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::Index Index; + Index rows = m.rows(); + Index cols = m.cols(); + + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime + }; + + typedef Matrix RhsType; + typedef Matrix SolutionType; + + RhsType rhs = RhsType::Random(rows, internal::random(1, cols)); + SvdType svd(m, computationOptions); + + if(internal::is_same::value) svd.setThreshold(1e-8); + else if(internal::is_same::value) svd.setThreshold(2e-4); + + SolutionType x = svd.solve(rhs); + + RealScalar residual = (m*x-rhs).norm(); + RealScalar rhs_norm = rhs.norm(); + if(!test_isMuchSmallerThan(residual,rhs.norm())) + { + // ^^^ If the residual is very small, then we have an exact solution, so we are already good. + + // evaluate normal equation which works also for least-squares solutions + if(internal::is_same::value || svd.rank()==m.diagonal().size()) + { + using std::sqrt; + // This test is not stable with single precision. + // This is probably because squaring m signicantly affects the precision. + if(internal::is_same::value) ++g_test_level; + + VERIFY_IS_APPROX(m.adjoint()*(m*x),m.adjoint()*rhs); + + if(internal::is_same::value) --g_test_level; + } + + // Check that there is no significantly better solution in the neighborhood of x + for(Index k=0;k::epsilon())*x.row(k); + RealScalar residual_y = (m*y-rhs).norm(); + VERIFY( test_isMuchSmallerThan(abs(residual_y-residual), rhs_norm) || residual < residual_y ); + if(internal::is_same::value) ++g_test_level; + VERIFY( test_isApprox(residual_y,residual) || residual < residual_y ); + if(internal::is_same::value) --g_test_level; + + y.row(k) = (RealScalar(1)-2*NumTraits::epsilon())*x.row(k); + residual_y = (m*y-rhs).norm(); + VERIFY( test_isMuchSmallerThan(abs(residual_y-residual), rhs_norm) || residual < residual_y ); + if(internal::is_same::value) ++g_test_level; + VERIFY( test_isApprox(residual_y,residual) || residual < residual_y ); + if(internal::is_same::value) --g_test_level; + } + } +} + +// check minimal norm solutions, the inoput matrix m is only used to recover problem size +template +void svd_min_norm(const MatrixType& m, unsigned int computationOptions) +{ + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + Index cols = m.cols(); + + enum { + ColsAtCompileTime = MatrixType::ColsAtCompileTime + }; + + typedef Matrix SolutionType; + + // generate a full-rank m x n problem with m MatrixType2; + typedef Matrix RhsType2; + typedef Matrix MatrixType2T; + Index rank = RankAtCompileTime2==Dynamic ? internal::random(1,cols) : Index(RankAtCompileTime2); + MatrixType2 m2(rank,cols); + int guard = 0; + do { + m2.setRandom(); + } while(SVD_FOR_MIN_NORM(MatrixType2)(m2).setThreshold(test_precision()).rank()!=rank && (++guard)<10); + VERIFY(guard<10); + + RhsType2 rhs2 = RhsType2::Random(rank); + // use QR to find a reference minimal norm solution + HouseholderQR qr(m2.adjoint()); + Matrix tmp = qr.matrixQR().topLeftCorner(rank,rank).template triangularView().adjoint().solve(rhs2); + tmp.conservativeResize(cols); + tmp.tail(cols-rank).setZero(); + SolutionType x21 = qr.householderQ() * tmp; + // now check with SVD + SVD_FOR_MIN_NORM(MatrixType2) svd2(m2, computationOptions); + SolutionType x22 = svd2.solve(rhs2); + VERIFY_IS_APPROX(m2*x21, rhs2); + VERIFY_IS_APPROX(m2*x22, rhs2); + VERIFY_IS_APPROX(x21, x22); + + // Now check with a rank deficient matrix + typedef Matrix MatrixType3; + typedef Matrix RhsType3; + Index rows3 = RowsAtCompileTime3==Dynamic ? internal::random(rank+1,2*cols) : Index(RowsAtCompileTime3); + Matrix C = Matrix::Random(rows3,rank); + MatrixType3 m3 = C * m2; + RhsType3 rhs3 = C * rhs2; + SVD_FOR_MIN_NORM(MatrixType3) svd3(m3, computationOptions); + SolutionType x3 = svd3.solve(rhs3); + VERIFY_IS_APPROX(m3*x3, rhs3); + VERIFY_IS_APPROX(m3*x21, rhs3); + VERIFY_IS_APPROX(m2*x3, rhs2); + VERIFY_IS_APPROX(x21, x3); +} + +// Check full, compare_to_full, least_square, and min_norm for all possible compute-options +template +void svd_test_all_computation_options(const MatrixType& m, bool full_only) +{ +// if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols()) +// return; + SvdType fullSvd(m, ComputeFullU|ComputeFullV); + CALL_SUBTEST(( svd_check_full(m, fullSvd) )); + CALL_SUBTEST(( svd_least_square(m, ComputeFullU | ComputeFullV) )); + CALL_SUBTEST(( svd_min_norm(m, ComputeFullU | ComputeFullV) )); + + #if defined __INTEL_COMPILER + // remark #111: statement is unreachable + #pragma warning disable 111 + #endif + if(full_only) + return; + + CALL_SUBTEST(( svd_compare_to_full(m, ComputeFullU, fullSvd) )); + CALL_SUBTEST(( svd_compare_to_full(m, ComputeFullV, fullSvd) )); + CALL_SUBTEST(( svd_compare_to_full(m, 0, fullSvd) )); + + if (MatrixType::ColsAtCompileTime == Dynamic) { + // thin U/V are only available with dynamic number of columns + CALL_SUBTEST(( svd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd) )); + CALL_SUBTEST(( svd_compare_to_full(m, ComputeThinV, fullSvd) )); + CALL_SUBTEST(( svd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd) )); + CALL_SUBTEST(( svd_compare_to_full(m, ComputeThinU , fullSvd) )); + CALL_SUBTEST(( svd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd) )); + + CALL_SUBTEST(( svd_least_square(m, ComputeFullU | ComputeThinV) )); + CALL_SUBTEST(( svd_least_square(m, ComputeThinU | ComputeFullV) )); + CALL_SUBTEST(( svd_least_square(m, ComputeThinU | ComputeThinV) )); + + CALL_SUBTEST(( svd_min_norm(m, ComputeFullU | ComputeThinV) )); + CALL_SUBTEST(( svd_min_norm(m, ComputeThinU | ComputeFullV) )); + CALL_SUBTEST(( svd_min_norm(m, ComputeThinU | ComputeThinV) )); + + // test reconstruction + typedef typename MatrixType::Index Index; + Index diagSize = (std::min)(m.rows(), m.cols()); + SvdType svd(m, ComputeThinU | ComputeThinV); + VERIFY_IS_APPROX(m, svd.matrixU().leftCols(diagSize) * svd.singularValues().asDiagonal() * svd.matrixV().leftCols(diagSize).adjoint()); + } +} + + +// work around stupid msvc error when constructing at compile time an expression that involves +// a division by zero, even if the numeric type has floating point +template +EIGEN_DONT_INLINE Scalar zero() { return Scalar(0); } + +// workaround aggressive optimization in ICC +template EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; } + +// all this function does is verify we don't iterate infinitely on nan/inf values +template +void svd_inf_nan() +{ + SvdType svd; + typedef typename MatrixType::Scalar Scalar; + Scalar some_inf = Scalar(1) / zero(); + VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf)); + svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV); + + Scalar nan = std::numeric_limits::quiet_NaN(); + VERIFY(nan != nan); + svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV); + + MatrixType m = MatrixType::Zero(10,10); + m(internal::random(0,9), internal::random(0,9)) = some_inf; + svd.compute(m, ComputeFullU | ComputeFullV); + + m = MatrixType::Zero(10,10); + m(internal::random(0,9), internal::random(0,9)) = nan; + svd.compute(m, ComputeFullU | ComputeFullV); + + // regression test for bug 791 + m.resize(3,3); + m << 0, 2*NumTraits::epsilon(), 0.5, + 0, -0.5, 0, + nan, 0, 0; + svd.compute(m, ComputeFullU | ComputeFullV); + + m.resize(4,4); + m << 1, 0, 0, 0, + 0, 3, 1, 2e-308, + 1, 0, 1, nan, + 0, nan, nan, 0; + svd.compute(m, ComputeFullU | ComputeFullV); +} + +// Regression test for bug 286: JacobiSVD loops indefinitely with some +// matrices containing denormal numbers. +template +void svd_underoverflow() +{ +#if defined __INTEL_COMPILER +// shut up warning #239: floating point underflow +#pragma warning push +#pragma warning disable 239 +#endif + Matrix2d M; + M << -7.90884e-313, -4.94e-324, + 0, 5.60844e-313; + SVD_DEFAULT(Matrix2d) svd; + svd.compute(M,ComputeFullU|ComputeFullV); + CALL_SUBTEST( svd_check_full(M,svd) ); + + // Check all 2x2 matrices made with the following coefficients: + VectorXd value_set(9); + value_set << 0, 1, -1, 5.60844e-313, -5.60844e-313, 4.94e-324, -4.94e-324, -4.94e-223, 4.94e-223; + Array4i id(0,0,0,0); + int k = 0; + do + { + M << value_set(id(0)), value_set(id(1)), value_set(id(2)), value_set(id(3)); + svd.compute(M,ComputeFullU|ComputeFullV); + CALL_SUBTEST( svd_check_full(M,svd) ); + + id(k)++; + if(id(k)>=value_set.size()) + { + while(k<3 && id(k)>=value_set.size()) id(++k)++; + id.head(k).setZero(); + k=0; + } + + } while((id +void svd_all_trivial_2x2( void (*cb)(const MatrixType&,bool) ) +{ + MatrixType M; + VectorXd value_set(3); + value_set << 0, 1, -1; + Array4i id(0,0,0,0); + int k = 0; + do + { + M << value_set(id(0)), value_set(id(1)), value_set(id(2)), value_set(id(3)); + + cb(M,false); + + id(k)++; + if(id(k)>=value_set.size()) + { + while(k<3 && id(k)>=value_set.size()) id(++k)++; + id.head(k).setZero(); + k=0; + } + + } while((id +void svd_preallocate() +{ + Vector3f v(3.f, 2.f, 1.f); + MatrixXf m = v.asDiagonal(); + + internal::set_is_malloc_allowed(false); + VERIFY_RAISES_ASSERT(VectorXf tmp(10);) + SVD_DEFAULT(MatrixXf) svd; + internal::set_is_malloc_allowed(true); + svd.compute(m); + VERIFY_IS_APPROX(svd.singularValues(), v); + + SVD_DEFAULT(MatrixXf) svd2(3,3); + internal::set_is_malloc_allowed(false); + svd2.compute(m); + internal::set_is_malloc_allowed(true); + VERIFY_IS_APPROX(svd2.singularValues(), v); + VERIFY_RAISES_ASSERT(svd2.matrixU()); + VERIFY_RAISES_ASSERT(svd2.matrixV()); + svd2.compute(m, ComputeFullU | ComputeFullV); + VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity()); + VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity()); + internal::set_is_malloc_allowed(false); + svd2.compute(m); + internal::set_is_malloc_allowed(true); + + SVD_DEFAULT(MatrixXf) svd3(3,3,ComputeFullU|ComputeFullV); + internal::set_is_malloc_allowed(false); + svd2.compute(m); + internal::set_is_malloc_allowed(true); + VERIFY_IS_APPROX(svd2.singularValues(), v); + VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity()); + VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity()); + internal::set_is_malloc_allowed(false); + svd2.compute(m, ComputeFullU|ComputeFullV); + internal::set_is_malloc_allowed(true); +} + +template +void svd_verify_assert(const MatrixType& m) +{ + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::Index Index; + Index rows = m.rows(); + Index cols = m.cols(); + + enum { + RowsAtCompileTime = MatrixType::RowsAtCompileTime, + ColsAtCompileTime = MatrixType::ColsAtCompileTime + }; + + typedef Matrix RhsType; + RhsType rhs(rows); + SvdType svd; + VERIFY_RAISES_ASSERT(svd.matrixU()) + VERIFY_RAISES_ASSERT(svd.singularValues()) + VERIFY_RAISES_ASSERT(svd.matrixV()) + VERIFY_RAISES_ASSERT(svd.solve(rhs)) + MatrixType a = MatrixType::Zero(rows, cols); + a.setZero(); + svd.compute(a, 0); + VERIFY_RAISES_ASSERT(svd.matrixU()) + VERIFY_RAISES_ASSERT(svd.matrixV()) + svd.singularValues(); + VERIFY_RAISES_ASSERT(svd.solve(rhs)) + + if (ColsAtCompileTime == Dynamic) + { + svd.compute(a, ComputeThinU); + svd.matrixU(); + VERIFY_RAISES_ASSERT(svd.matrixV()) + VERIFY_RAISES_ASSERT(svd.solve(rhs)) + svd.compute(a, ComputeThinV); + svd.matrixV(); + VERIFY_RAISES_ASSERT(svd.matrixU()) + VERIFY_RAISES_ASSERT(svd.solve(rhs)) + } + else + { + VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinU)) + VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinV)) + } +} + +#undef SVD_DEFAULT +#undef SVD_FOR_MIN_NORM diff --git a/ground/gcs/src/libs/eigen/test/svd_fill.h b/ground/gcs/src/libs/eigen/test/svd_fill.h new file mode 100644 index 0000000000..3877c0c7ef --- /dev/null +++ b/ground/gcs/src/libs/eigen/test/svd_fill.h @@ -0,0 +1,119 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014-2015 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +template +Array four_denorms(); + +template<> +Array4f four_denorms() { return Array4f(5.60844e-39f, -5.60844e-39f, 4.94e-44f, -4.94e-44f); } +template<> +Array4d four_denorms() { return Array4d(5.60844e-313, -5.60844e-313, 4.94e-324, -4.94e-324); } +template +Array four_denorms() { return four_denorms().cast(); } + +template +void svd_fill_random(MatrixType &m, int Option = 0) +{ + using std::pow; + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::Index Index; + Index diagSize = (std::min)(m.rows(), m.cols()); + RealScalar s = std::numeric_limits::max_exponent10/4; + s = internal::random(1,s); + Matrix d = Matrix::Random(diagSize); + for(Index k=0; k(-s,s)); + + bool dup = internal::random(0,10) < 3; + bool unit_uv = internal::random(0,10) < (dup?7:3); // if we duplicate some diagonal entries, then increase the chance to preserve them using unitary U and V factors + + // duplicate some singular values + if(dup) + { + Index n = internal::random(0,d.size()-1); + for(Index i=0; i(0,d.size()-1)) = d(internal::random(0,d.size()-1)); + } + + Matrix U(m.rows(),diagSize); + Matrix VT(diagSize,m.cols()); + if(unit_uv) + { + // in very rare cases let's try with a pure diagonal matrix + if(internal::random(0,10) < 1) + { + U.setIdentity(); + VT.setIdentity(); + } + else + { + createRandomPIMatrixOfRank(diagSize,U.rows(), U.cols(), U); + createRandomPIMatrixOfRank(diagSize,VT.rows(), VT.cols(), VT); + } + } + else + { + U.setRandom(); + VT.setRandom(); + } + + Matrix samples(9); + samples << 0, four_denorms(), + -RealScalar(1)/NumTraits::highest(), RealScalar(1)/NumTraits::highest(), (std::numeric_limits::min)(), pow((std::numeric_limits::min)(),0.8); + + if(Option==Symmetric) + { + m = U * d.asDiagonal() * U.transpose(); + + // randomly nullify some rows/columns + { + Index count = internal::random(-diagSize,diagSize); + for(Index k=0; k(0,diagSize-1); + m.row(i).setZero(); + m.col(i).setZero(); + } + if(count<0) + // (partly) cancel some coeffs + if(!(dup && unit_uv)) + { + + Index n = internal::random(0,m.size()-1); + for(Index k=0; k(0,m.rows()-1); + Index j = internal::random(0,m.cols()-1); + m(j,i) = m(i,j) = samples(internal::random(0,samples.size()-1)); + if(NumTraits::IsComplex) + *(&numext::real_ref(m(j,i))+1) = *(&numext::real_ref(m(i,j))+1) = samples.real()(internal::random(0,samples.size()-1)); + } + } + } + } + else + { + m = U * d.asDiagonal() * VT; + // (partly) cancel some coeffs + if(!(dup && unit_uv)) + { + Index n = internal::random(0,m.size()-1); + for(Index k=0; k(0,m.rows()-1); + Index j = internal::random(0,m.cols()-1); + m(i,j) = samples(internal::random(0,samples.size()-1)); + if(NumTraits::IsComplex) + *(&numext::real_ref(m(i,j))+1) = samples.real()(internal::random(0,samples.size()-1)); + } + } + } +} + diff --git a/ground/gcs/src/libs/eigen/test/swap.cpp b/ground/gcs/src/libs/eigen/test/swap.cpp index 36b3531486..f76e3624dd 100644 --- a/ground/gcs/src/libs/eigen/test/swap.cpp +++ b/ground/gcs/src/libs/eigen/test/swap.cpp @@ -41,9 +41,15 @@ template void swap(const MatrixType& m) OtherMatrixType m3_copy = m3; // test swapping 2 matrices of same type + Scalar *d1=m1.data(), *d2=m2.data(); m1.swap(m2); VERIFY_IS_APPROX(m1,m2_copy); VERIFY_IS_APPROX(m2,m1_copy); + if(MatrixType::SizeAtCompileTime==Dynamic) + { + VERIFY(m1.data()==d2); + VERIFY(m2.data()==d1); + } m1 = m1_copy; m2 = m2_copy; @@ -68,16 +74,21 @@ template void swap(const MatrixType& m) m1 = m1_copy; m3 = m3_copy; - // test assertion on mismatching size -- matrix case - VERIFY_RAISES_ASSERT(m1.swap(m1.row(0))); - // test assertion on mismatching size -- xpr case - VERIFY_RAISES_ASSERT(m1.row(0).swap(m1)); + if(m1.rows()>1) + { + // test assertion on mismatching size -- matrix case + VERIFY_RAISES_ASSERT(m1.swap(m1.row(0))); + // test assertion on mismatching size -- xpr case + VERIFY_RAISES_ASSERT(m1.row(0).swap(m1)); + } } void test_swap() { + int s = internal::random(1,EIGEN_TEST_MAX_SIZE); CALL_SUBTEST_1( swap(Matrix3f()) ); // fixed size, no vectorization CALL_SUBTEST_2( swap(Matrix4d()) ); // fixed size, possible vectorization - CALL_SUBTEST_3( swap(MatrixXd(3,3)) ); // dyn size, no vectorization - CALL_SUBTEST_4( swap(MatrixXf(30,30)) ); // dyn size, possible vectorization + CALL_SUBTEST_3( swap(MatrixXd(s,s)) ); // dyn size, no vectorization + CALL_SUBTEST_4( swap(MatrixXf(s,s)) ); // dyn size, possible vectorization + TEST_SET_BUT_UNUSED_VARIABLE(s) } diff --git a/ground/gcs/src/libs/eigen/test/testsuite.cmake b/ground/gcs/src/libs/eigen/test/testsuite.cmake deleted file mode 100644 index 3bec56b3fa..0000000000 --- a/ground/gcs/src/libs/eigen/test/testsuite.cmake +++ /dev/null @@ -1,229 +0,0 @@ - -#################################################################### -# -# Usage: -# - create a new folder, let's call it cdash -# - in that folder, do: -# ctest -S path/to/eigen/test/testsuite.cmake[,option1=value1[,option2=value2]] -# -# Options: -# - EIGEN_CXX: compiler, eg.: g++-4.2 -# default: default c++ compiler -# - EIGEN_SITE: eg, INRIA-Bdx_pc-gael, or the name of the contributor, etc. -# default: hostname -# - EIGEN_BUILD_STRING: a string which identify the system/compiler. It should be formed like that: -# --- -# with: -# = opensuse, debian, osx, windows, cygwin, freebsd, solaris, etc. -# = 11.1, XP, vista, leopard, etc. -# = i386, x86_64, ia64, powerpc, etc. -# = gcc-4.3.2, icc-11.0, MSVC-2008, etc. -# - EIGEN_EXPLICIT_VECTORIZATION: novec, SSE2, Altivec -# default: SSE2 for x86_64 systems, novec otherwise -# Its value is automatically appended to EIGEN_BUILD_STRING -# - EIGEN_CMAKE_DIR: path to cmake executable -# - EIGEN_MODE: dashboard model, can be Experimental, Nightly, or Continuous -# default: Nightly -# - EIGEN_WORK_DIR: directory used to download the source files and make the builds -# default: folder which contains this script -# - EIGEN_CMAKE_ARGS: additional arguments passed to cmake -# - EIGEN_GENERATOR_TYPE: allows to overwrite the generator type -# default: nmake (windows -# See http://www.cmake.org/cmake/help/cmake2.6docs.html#section_Generators for a complete -# list of supported generators. -# - EIGEN_NO_UPDATE: allows to submit dash boards from local repositories -# This might be interesting in case you want to submit dashboards -# including local changes. -# - CTEST_SOURCE_DIRECTORY: path to eigen's src (use a new and empty folder, not the one you are working on) -# default: /src -# - CTEST_BINARY_DIRECTORY: build directory -# default: /nightly- -# -# Here is an example running several compilers on a linux system: -# #!/bin/bash -# ARCH=`uname -m` -# SITE=`hostname` -# VERSION=opensuse-11.1 -# WORK_DIR=/home/gael/Coding/eigen/cdash -# # get the last version of the script -# wget http://bitbucket.org/eigen/eigen/raw/tip/test/testsuite.cmake -o $WORK_DIR/testsuite.cmake -# COMMON="ctest -S $WORK_DIR/testsuite.cmake,EIGEN_WORK_DIR=$WORK_DIR,EIGEN_SITE=$SITE,EIGEN_MODE=$1,EIGEN_BUILD_STRING=$OS_VERSION-$ARCH" -# $COMMON-gcc-3.4.6,EIGEN_CXX=g++-3.4 -# $COMMON-gcc-4.0.1,EIGEN_CXX=g++-4.0.1 -# $COMMON-gcc-4.3.2,EIGEN_CXX=g++-4.3,EIGEN_EXPLICIT_VECTORIZATION=novec -# $COMMON-gcc-4.3.2,EIGEN_CXX=g++-4.3,EIGEN_EXPLICIT_VECTORIZATION=SSE2 -# $COMMON-icc-11.0,EIGEN_CXX=icpc -# -#################################################################### - -# process the arguments - -set(ARGLIST ${CTEST_SCRIPT_ARG}) -while(${ARGLIST} MATCHES ".+.*") - - # pick first - string(REGEX MATCH "([^,]*)(,.*)?" DUMMY ${ARGLIST}) - SET(TOP ${CMAKE_MATCH_1}) - - # remove first - string(REGEX MATCHALL "[^,]*,(.*)" DUMMY ${ARGLIST}) - SET(ARGLIST ${CMAKE_MATCH_1}) - - # decompose as a pair key=value - string(REGEX MATCH "([^=]*)(=.*)?" DUMMY ${TOP}) - SET(KEY ${CMAKE_MATCH_1}) - - string(REGEX MATCH "[^=]*=(.*)" DUMMY ${TOP}) - SET(VALUE ${CMAKE_MATCH_1}) - - # set the variable to the specified value - if(VALUE) - SET(${KEY} ${VALUE}) - else(VALUE) - SET(${KEY} ON) - endif(VALUE) - -endwhile(${ARGLIST} MATCHES ".+.*") - -#################################################################### -# Automatically set some user variables if they have not been defined manually -#################################################################### -cmake_minimum_required(VERSION 2.6 FATAL_ERROR) - -if(NOT EIGEN_SITE) - site_name(EIGEN_SITE) -endif(NOT EIGEN_SITE) - -if(NOT EIGEN_CMAKE_DIR) - SET(EIGEN_CMAKE_DIR "") -endif(NOT EIGEN_CMAKE_DIR) - -if(NOT EIGEN_BUILD_STRING) - - # let's try to find all information we need to make the build string ourself - - # OS - build_name(EIGEN_OS_VERSION) - - # arch - set(EIGEN_ARCH ${CMAKE_SYSTEM_PROCESSOR}) - if(WIN32) - set(EIGEN_ARCH $ENV{PROCESSOR_ARCHITECTURE}) - else(WIN32) - execute_process(COMMAND uname -m OUTPUT_VARIABLE EIGEN_ARCH OUTPUT_STRIP_TRAILING_WHITESPACE) - endif(WIN32) - - set(EIGEN_BUILD_STRING ${EIGEN_OS_VERSION}${EIGEN_ARCH}-${EIGEN_CXX}) - -endif(NOT EIGEN_BUILD_STRING) - -if(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - set(EIGEN_BUILD_STRING ${EIGEN_BUILD_STRING}-${EIGEN_EXPLICIT_VECTORIZATION}) -endif(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - -if(NOT EIGEN_WORK_DIR) - set(EIGEN_WORK_DIR ${CTEST_SCRIPT_DIRECTORY}) -endif(NOT EIGEN_WORK_DIR) - -if(NOT CTEST_SOURCE_DIRECTORY) - SET (CTEST_SOURCE_DIRECTORY "${EIGEN_WORK_DIR}/src") -endif(NOT CTEST_SOURCE_DIRECTORY) - -if(NOT CTEST_BINARY_DIRECTORY) - SET (CTEST_BINARY_DIRECTORY "${EIGEN_WORK_DIR}/nightly_${EIGEN_CXX}") -endif(NOT CTEST_BINARY_DIRECTORY) - -if(NOT EIGEN_MODE) - set(EIGEN_MODE Nightly) -endif(NOT EIGEN_MODE) - -## mandatory variables (the default should be ok in most cases): - -if(NOT EIGEN_NO_UPDATE) - SET (CTEST_CVS_COMMAND "hg") - SET (CTEST_CVS_CHECKOUT "${CTEST_CVS_COMMAND} clone http://bitbucket.org/eigen/eigen \"${CTEST_SOURCE_DIRECTORY}\"") - SET(CTEST_BACKUP_AND_RESTORE TRUE) # the backup is CVS related ... -endif(NOT EIGEN_NO_UPDATE) - -# which ctest command to use for running the dashboard -SET (CTEST_COMMAND "${EIGEN_CMAKE_DIR}ctest -D ${EIGEN_MODE} --no-compress-output") -if($ENV{EIGEN_CTEST_ARGS}) -SET (CTEST_COMMAND "${CTEST_COMMAND} $ENV{EIGEN_CTEST_ARGS}") -endif($ENV{EIGEN_CTEST_ARGS}) -# what cmake command to use for configuring this dashboard -SET (CTEST_CMAKE_COMMAND "${EIGEN_CMAKE_DIR}cmake -DEIGEN_LEAVE_TEST_IN_ALL_TARGET=ON") - -#################################################################### -# The values in this section are optional you can either -# have them or leave them commented out -#################################################################### - -# this make sure we get consistent outputs -SET($ENV{LC_MESSAGES} "en_EN") - -# should ctest wipe the binary tree before running -SET(CTEST_START_WITH_EMPTY_BINARY_DIRECTORY TRUE) - -# raise the warning/error limit -set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "33331") -set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "33331") - -# this is the initial cache to use for the binary tree, be careful to escape -# any quotes inside of this string if you use it -if(WIN32 AND NOT UNIX) - #message(SEND_ERROR "win32") - if(EIGEN_GENERATOR_TYPE) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -G \"${EIGEN_GENERATOR_TYPE}\"") - SET (CTEST_INITIAL_CACHE " - CMAKE_BUILD_TYPE:STRING=Release - BUILDNAME:STRING=${EIGEN_BUILD_STRING} - SITE:STRING=${EIGEN_SITE} - ") - else(EIGEN_GENERATOR_TYPE) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -G \"NMake Makefiles\" -DCMAKE_MAKE_PROGRAM=nmake") - SET (CTEST_INITIAL_CACHE " - MAKECOMMAND:STRING=nmake /i - CMAKE_MAKE_PROGRAM:FILEPATH=nmake - CMAKE_GENERATOR:INTERNAL=NMake Makefiles - CMAKE_BUILD_TYPE:STRING=Release - BUILDNAME:STRING=${EIGEN_BUILD_STRING} - SITE:STRING=${EIGEN_SITE} - ") - endif(EIGEN_GENERATOR_TYPE) -else(WIN32 AND NOT UNIX) - SET (CTEST_INITIAL_CACHE " - BUILDNAME:STRING=${EIGEN_BUILD_STRING} - SITE:STRING=${EIGEN_SITE} - ") -endif(WIN32 AND NOT UNIX) - -# set any extra environment variables to use during the execution of the script here: -# setting this variable on windows machines causes trouble ... - -if(EIGEN_CXX AND NOT WIN32) - set(CTEST_ENVIRONMENT "CXX=${EIGEN_CXX}") -endif(EIGEN_CXX AND NOT WIN32) - -if(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - if(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE2) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE3) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON -DEIGEN_TEST_SSE3=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSSE3) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON -DEIGEN_TEST_SSE3=ON -DEIGEN_TEST_SSSE3=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE4_1) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON -DEIGEN_TEST_SSE3=ON -DEIGEN_TEST_SSSE3=ON -DEIGEN_TEST_SSE4_1=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE4_2) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_SSE2=ON -DEIGEN_TEST_SSE3=ON -DEIGEN_TEST_SSSE3=ON -DEIGEN_TEST_SSE4_1=ON -DEIGEN_TEST_SSE4_2=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES Altivec) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_ALTIVEC=ON") - elseif(EIGEN_EXPLICIT_VECTORIZATION MATCHES novec) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} -DEIGEN_TEST_NO_EXPLICIT_VECTORIZATION=ON") - else(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE2) - message(FATAL_ERROR "Invalid value for EIGEN_EXPLICIT_VECTORIZATION (${EIGEN_EXPLICIT_VECTORIZATION}), must be: novec, SSE2, SSE3, Altivec") - endif(EIGEN_EXPLICIT_VECTORIZATION MATCHES SSE2) -endif(DEFINED EIGEN_EXPLICIT_VECTORIZATION) - -if(DEFINED EIGEN_CMAKE_ARGS) - set(CTEST_CMAKE_COMMAND "${CTEST_CMAKE_COMMAND} ${EIGEN_CMAKE_ARGS}") -endif(DEFINED EIGEN_CMAKE_ARGS) diff --git a/ground/gcs/src/libs/eigen/test/triangular.cpp b/ground/gcs/src/libs/eigen/test/triangular.cpp index 54320390ba..b968564864 100644 --- a/ground/gcs/src/libs/eigen/test/triangular.cpp +++ b/ground/gcs/src/libs/eigen/test/triangular.cpp @@ -65,7 +65,7 @@ template void triangular_square(const MatrixType& m) m1 = MatrixType::Random(rows, cols); for (int i=0; i(); + while (numext::abs2(m1(i,i))(); Transpose trm4(m4); // test back and forward subsitution with a vector as the rhs @@ -78,7 +78,7 @@ template void triangular_square(const MatrixType& m) m3 = m1.template triangularView(); VERIFY(v2.isApprox(m3.conjugate() * (m1.conjugate().template triangularView().solve(v2)), largerEps)); - // test back and forward subsitution with a matrix as the rhs + // test back and forward substitution with a matrix as the rhs m3 = m1.template triangularView(); VERIFY(m2.isApprox(m3.adjoint() * (m1.adjoint().template triangularView().solve(m2)), largerEps)); m3 = m1.template triangularView(); @@ -113,6 +113,21 @@ template void triangular_square(const MatrixType& m) m3.setZero(); m3.template triangularView().setOnes(); VERIFY_IS_APPROX(m2,m3); + + m1.setRandom(); + m3 = m1.template triangularView(); + Matrix m5(cols, internal::random(1,20)); m5.setRandom(); + Matrix m6(internal::random(1,20), rows); m6.setRandom(); + VERIFY_IS_APPROX(m1.template triangularView() * m5, m3*m5); + VERIFY_IS_APPROX(m6*m1.template triangularView(), m6*m3); + + m1up = m1.template triangularView(); + VERIFY_IS_APPROX(m1.template selfadjointView().template triangularView().toDenseMatrix(), m1up); + VERIFY_IS_APPROX(m1up.template selfadjointView().template triangularView().toDenseMatrix(), m1up); + VERIFY_IS_APPROX(m1.template selfadjointView().template triangularView().toDenseMatrix(), m1up.adjoint()); + VERIFY_IS_APPROX(m1up.template selfadjointView().template triangularView().toDenseMatrix(), m1up.adjoint()); + + VERIFY_IS_APPROX(m1.template selfadjointView().diagonal(), m1.diagonal()); } diff --git a/ground/gcs/src/libs/eigen/test/umfpack_support.cpp b/ground/gcs/src/libs/eigen/test/umfpack_support.cpp index 9eb84c14bc..37ab11f0b9 100644 --- a/ground/gcs/src/libs/eigen/test/umfpack_support.cpp +++ b/ground/gcs/src/libs/eigen/test/umfpack_support.cpp @@ -7,6 +7,7 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS #include "sparse_solver.h" #include diff --git a/ground/gcs/src/libs/eigen/test/unalignedassert.cpp b/ground/gcs/src/libs/eigen/test/unalignedassert.cpp index 601dbf214a..731a08977c 100644 --- a/ground/gcs/src/libs/eigen/test/unalignedassert.cpp +++ b/ground/gcs/src/libs/eigen/test/unalignedassert.cpp @@ -2,13 +2,39 @@ // for linear algebra. // // Copyright (C) 2008 Benoit Jacob +// Copyright (C) 2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#if defined(EIGEN_TEST_PART_1) + // default +#elif defined(EIGEN_TEST_PART_2) + #define EIGEN_MAX_STATIC_ALIGN_BYTES 16 + #define EIGEN_MAX_ALIGN_BYTES 16 +#elif defined(EIGEN_TEST_PART_3) + #define EIGEN_MAX_STATIC_ALIGN_BYTES 32 + #define EIGEN_MAX_ALIGN_BYTES 32 +#elif defined(EIGEN_TEST_PART_4) + #define EIGEN_MAX_STATIC_ALIGN_BYTES 64 + #define EIGEN_MAX_ALIGN_BYTES 64 +#endif + #include "main.h" +typedef Matrix Vector6f; +typedef Matrix Vector8f; +typedef Matrix Vector12f; + +typedef Matrix Vector5d; +typedef Matrix Vector6d; +typedef Matrix Vector7d; +typedef Matrix Vector8d; +typedef Matrix Vector9d; +typedef Matrix Vector10d; +typedef Matrix Vector12d; + struct TestNew1 { MatrixXd m; // good: m will allocate its own array, taking care of alignment. @@ -36,7 +62,7 @@ struct TestNew4 struct TestNew5 { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - float f; // try the f at first -- the EIGEN_ALIGN16 attribute of m should make that still work + float f; // try the f at first -- the EIGEN_ALIGN_MAX attribute of m should make that still work Matrix4f m; }; @@ -63,13 +89,13 @@ void check_unalignedassert_good() delete[] y; } -#if EIGEN_ALIGN_STATICALLY +#if EIGEN_MAX_STATIC_ALIGN_BYTES>0 template void construct_at_boundary(int boundary) { char buf[sizeof(T)+256]; - size_t _buf = reinterpret_cast(buf); - _buf += (16 - (_buf % 16)); // make 16-byte aligned + size_t _buf = reinterpret_cast(buf); + _buf += (EIGEN_MAX_ALIGN_BYTES - (_buf % EIGEN_MAX_ALIGN_BYTES)); // make 16/32/...-byte aligned _buf += boundary; // make exact boundary-aligned T *x = ::new(reinterpret_cast(_buf)) T; x[0].setZero(); // just in order to silence warnings @@ -79,26 +105,36 @@ void construct_at_boundary(int boundary) void unalignedassert() { - #if EIGEN_ALIGN_STATICALLY +#if EIGEN_MAX_STATIC_ALIGN_BYTES>0 construct_at_boundary(4); construct_at_boundary(4); construct_at_boundary(16); + construct_at_boundary(4); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); + construct_at_boundary(16); construct_at_boundary(16); construct_at_boundary(4); - construct_at_boundary(16); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); construct_at_boundary(16); construct_at_boundary(4); - construct_at_boundary(16); - construct_at_boundary(16); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); + construct_at_boundary(4); + construct_at_boundary(16); + construct_at_boundary(4); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); + construct_at_boundary(4); + construct_at_boundary(16); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); construct_at_boundary(4); - construct_at_boundary(16); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); construct_at_boundary(16); construct_at_boundary(4); - construct_at_boundary(16); + construct_at_boundary(EIGEN_MAX_ALIGN_BYTES); construct_at_boundary(16); - #endif +#endif check_unalignedassert_good(); check_unalignedassert_good(); @@ -109,15 +145,32 @@ void unalignedassert() check_unalignedassert_good(); check_unalignedassert_good >(); -#if EIGEN_ALIGN_STATICALLY - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); - VERIFY_RAISES_ASSERT(construct_at_boundary(8)); +#if EIGEN_MAX_STATIC_ALIGN_BYTES>0 + if(EIGEN_MAX_ALIGN_BYTES>=16) + { + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + // Complexes are disabled because the compiler might aggressively vectorize + // the initialization of complex coeffs to 0 before we can check for alignedness + //VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + VERIFY_RAISES_ASSERT(construct_at_boundary(8)); + } + for(int b=8; b(b)); + if(b<64) VERIFY_RAISES_ASSERT(construct_at_boundary(b)); + if(b<32) VERIFY_RAISES_ASSERT(construct_at_boundary(b)); + if(b<32) VERIFY_RAISES_ASSERT(construct_at_boundary(b)); + if(b<128) VERIFY_RAISES_ASSERT(construct_at_boundary(b)); + //if(b<32) VERIFY_RAISES_ASSERT(construct_at_boundary(b)); + } #endif } diff --git a/ground/gcs/src/libs/eigen/test/unalignedcount.cpp b/ground/gcs/src/libs/eigen/test/unalignedcount.cpp index ca7e159f3b..d6ffeafdf9 100644 --- a/ground/gcs/src/libs/eigen/test/unalignedcount.cpp +++ b/ground/gcs/src/libs/eigen/test/unalignedcount.cpp @@ -30,7 +30,14 @@ static int nb_storeu; void test_unalignedcount() { - #ifdef EIGEN_VECTORIZE_SSE + #if defined(EIGEN_VECTORIZE_AVX) + VectorXf a(40), b(40); + VERIFY_ALIGNED_UNALIGNED_COUNT(a += b, 10, 0, 5, 0); + VERIFY_ALIGNED_UNALIGNED_COUNT(a.segment(0,40) += b.segment(0,40), 5, 5, 5, 0); + VERIFY_ALIGNED_UNALIGNED_COUNT(a.segment(0,40) -= b.segment(0,40), 5, 5, 5, 0); + VERIFY_ALIGNED_UNALIGNED_COUNT(a.segment(0,40) *= 3.5, 5, 0, 5, 0); + VERIFY_ALIGNED_UNALIGNED_COUNT(a.segment(0,40) /= 3.5, 5, 0, 5, 0); + #elif defined(EIGEN_VECTORIZE_SSE) VectorXf a(40), b(40); VERIFY_ALIGNED_UNALIGNED_COUNT(a += b, 20, 0, 10, 0); VERIFY_ALIGNED_UNALIGNED_COUNT(a.segment(0,40) += b.segment(0,40), 10, 10, 10, 0); diff --git a/ground/gcs/src/libs/eigen/test/upperbidiagonalization.cpp b/ground/gcs/src/libs/eigen/test/upperbidiagonalization.cpp index d15bf588b2..847b34b550 100644 --- a/ground/gcs/src/libs/eigen/test/upperbidiagonalization.cpp +++ b/ground/gcs/src/libs/eigen/test/upperbidiagonalization.cpp @@ -35,7 +35,7 @@ void test_upperbidiagonalization() CALL_SUBTEST_1( upperbidiag(MatrixXf(3,3)) ); CALL_SUBTEST_2( upperbidiag(MatrixXd(17,12)) ); CALL_SUBTEST_3( upperbidiag(MatrixXcf(20,20)) ); - CALL_SUBTEST_4( upperbidiag(MatrixXcd(16,15)) ); + CALL_SUBTEST_4( upperbidiag(Matrix,Dynamic,Dynamic,RowMajor>(16,15)) ); CALL_SUBTEST_5( upperbidiag(Matrix()) ); CALL_SUBTEST_6( upperbidiag(Matrix()) ); CALL_SUBTEST_7( upperbidiag(Matrix()) ); diff --git a/ground/gcs/src/libs/eigen/test/vectorization_logic.cpp b/ground/gcs/src/libs/eigen/test/vectorization_logic.cpp index aee68a87f5..83c1439ada 100644 --- a/ground/gcs/src/libs/eigen/test/vectorization_logic.cpp +++ b/ground/gcs/src/libs/eigen/test/vectorization_logic.cpp @@ -1,45 +1,51 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#ifdef EIGEN_TEST_PART_1 +#define EIGEN_UNALIGNED_VECTORIZE 1 +#endif + +#ifdef EIGEN_TEST_PART_2 +#define EIGEN_UNALIGNED_VECTORIZE 0 +#endif + +#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR +#undef EIGEN_DEFAULT_TO_ROW_MAJOR +#endif #define EIGEN_DEBUG_ASSIGN #include "main.h" #include -std::string demangle_traversal(int t) -{ - if(t==DefaultTraversal) return "DefaultTraversal"; - if(t==LinearTraversal) return "LinearTraversal"; - if(t==InnerVectorizedTraversal) return "InnerVectorizedTraversal"; - if(t==LinearVectorizedTraversal) return "LinearVectorizedTraversal"; - if(t==SliceVectorizedTraversal) return "SliceVectorizedTraversal"; - return "?"; -} -std::string demangle_unrolling(int t) -{ - if(t==NoUnrolling) return "NoUnrolling"; - if(t==InnerUnrolling) return "InnerUnrolling"; - if(t==CompleteUnrolling) return "CompleteUnrolling"; - return "?"; -} +using internal::demangle_flags; +using internal::demangle_traversal; +using internal::demangle_unrolling; template bool test_assign(const Dst&, const Src&, int traversal, int unrolling) { - internal::assign_traits::debug(); - bool res = internal::assign_traits::Traversal==traversal - && internal::assign_traits::Unrolling==unrolling; + typedef internal::copy_using_evaluator_traits,internal::evaluator, internal::assign_op > traits; + bool res = traits::Traversal==traversal; + if(unrolling==InnerUnrolling+CompleteUnrolling) + res = res && (int(traits::Unrolling)==InnerUnrolling || int(traits::Unrolling)==CompleteUnrolling); + else + res = res && int(traits::Unrolling)==unrolling; if(!res) { + std::cerr << "Src: " << demangle_flags(Src::Flags) << std::endl; + std::cerr << " " << demangle_flags(internal::evaluator::Flags) << std::endl; + std::cerr << "Dst: " << demangle_flags(Dst::Flags) << std::endl; + std::cerr << " " << demangle_flags(internal::evaluator::Flags) << std::endl; + traits::debug(); std::cerr << " Expected Traversal == " << demangle_traversal(traversal) - << " got " << demangle_traversal(internal::assign_traits::Traversal) << "\n"; + << " got " << demangle_traversal(traits::Traversal) << "\n"; std::cerr << " Expected Unrolling == " << demangle_unrolling(unrolling) - << " got " << demangle_unrolling(internal::assign_traits::Unrolling) << "\n"; + << " got " << demangle_unrolling(traits::Unrolling) << "\n"; } return res; } @@ -47,15 +53,19 @@ bool test_assign(const Dst&, const Src&, int traversal, int unrolling) template bool test_assign(int traversal, int unrolling) { - internal::assign_traits::debug(); - bool res = internal::assign_traits::Traversal==traversal - && internal::assign_traits::Unrolling==unrolling; + typedef internal::copy_using_evaluator_traits,internal::evaluator, internal::assign_op > traits; + bool res = traits::Traversal==traversal && traits::Unrolling==unrolling; if(!res) { + std::cerr << "Src: " << demangle_flags(Src::Flags) << std::endl; + std::cerr << " " << demangle_flags(internal::evaluator::Flags) << std::endl; + std::cerr << "Dst: " << demangle_flags(Dst::Flags) << std::endl; + std::cerr << " " << demangle_flags(internal::evaluator::Flags) << std::endl; + traits::debug(); std::cerr << " Expected Traversal == " << demangle_traversal(traversal) - << " got " << demangle_traversal(internal::assign_traits::Traversal) << "\n"; + << " got " << demangle_traversal(traits::Traversal) << "\n"; std::cerr << " Expected Unrolling == " << demangle_unrolling(unrolling) - << " got " << demangle_unrolling(internal::assign_traits::Unrolling) << "\n"; + << " got " << demangle_unrolling(traits::Unrolling) << "\n"; } return res; } @@ -63,10 +73,16 @@ bool test_assign(int traversal, int unrolling) template bool test_redux(const Xpr&, int traversal, int unrolling) { - typedef internal::redux_traits,Xpr> traits; + typedef typename Xpr::Scalar Scalar; + typedef internal::redux_traits,internal::redux_evaluator > traits; + bool res = traits::Traversal==traversal && traits::Unrolling==unrolling; if(!res) { + std::cerr << demangle_flags(Xpr::Flags) << std::endl; + std::cerr << demangle_flags(internal::evaluator::Flags) << std::endl; + traits::debug(); + std::cerr << " Expected Traversal == " << demangle_traversal(traversal) << " got " << demangle_traversal(traits::Traversal) << "\n"; std::cerr << " Expected Unrolling == " << demangle_unrolling(unrolling) @@ -75,10 +91,16 @@ bool test_redux(const Xpr&, int traversal, int unrolling) return res; } -template::Vectorizable> struct vectorization_logic +template::Vectorizable> +struct vectorization_logic { + typedef internal::packet_traits PacketTraits; + + typedef typename internal::packet_traits::type PacketType; + typedef typename internal::unpacket_traits::half HalfPacketType; enum { - PacketSize = internal::packet_traits::size + PacketSize = internal::unpacket_traits::size, + HalfPacketSize = internal::unpacket_traits::size }; static void run() { @@ -90,8 +112,8 @@ template::Vectori typedef Matrix Matrix22; typedef Matrix Matrix44; typedef Matrix Matrix44u; - typedef Matrix Matrix44c; - typedef Matrix Matrix44r; + typedef Matrix Matrix44c; + typedef Matrix Matrix44r; typedef Matrix::Vectori InnerVectorizedTraversal,InnerUnrolling)); VERIFY(test_assign(Matrix44u(),Matrix44()+Matrix44(), - LinearTraversal,NoUnrolling)); + EIGEN_UNALIGNED_VECTORIZE ? InnerVectorizedTraversal : LinearTraversal, + EIGEN_UNALIGNED_VECTORIZE ? InnerUnrolling : NoUnrolling)); + + VERIFY(test_assign(Matrix1(),Matrix1()+Matrix1(), + (Matrix1::InnerSizeAtCompileTime % PacketSize)==0 ? InnerVectorizedTraversal : LinearVectorizedTraversal, + CompleteUnrolling)); VERIFY(test_assign(Matrix1u(),Matrix1()+Matrix1(), - LinearTraversal,CompleteUnrolling)); + EIGEN_UNALIGNED_VECTORIZE ? ((Matrix1::InnerSizeAtCompileTime % PacketSize)==0 ? InnerVectorizedTraversal : LinearVectorizedTraversal) + : LinearTraversal, CompleteUnrolling)); VERIFY(test_assign(Matrix44c().col(1),Matrix44c().col(2)+Matrix44c().col(3), InnerVectorizedTraversal,CompleteUnrolling)); - + VERIFY(test_assign(Matrix44r().row(2),Matrix44r().row(1)+Matrix44r().row(1), InnerVectorizedTraversal,CompleteUnrolling)); - + if(PacketSize>1) { typedef Matrix Matrix33c; + typedef Matrix Vector3; VERIFY(test_assign(Matrix33c().row(2),Matrix33c().row(1)+Matrix33c().row(1), LinearTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector3(),Vector3()+Vector3(), + EIGEN_UNALIGNED_VECTORIZE ? (HalfPacketSize==1 ? InnerVectorizedTraversal : LinearVectorizedTraversal) : (HalfPacketSize==1 ? InnerVectorizedTraversal : LinearTraversal), CompleteUnrolling)); VERIFY(test_assign(Matrix33c().col(0),Matrix33c().col(1)+Matrix33c().col(1), - LinearTraversal,CompleteUnrolling)); - - VERIFY(test_assign(Matrix3(),Matrix3().cwiseQuotient(Matrix3()), + EIGEN_UNALIGNED_VECTORIZE ? (HalfPacketSize==1 ? InnerVectorizedTraversal : LinearVectorizedTraversal) : (HalfPacketSize==1 ? SliceVectorizedTraversal : LinearTraversal), + ((!EIGEN_UNALIGNED_VECTORIZE) && HalfPacketSize==1) ? NoUnrolling : CompleteUnrolling)); + + VERIFY(test_assign(Matrix3(),Matrix3().cwiseProduct(Matrix3()), LinearVectorizedTraversal,CompleteUnrolling)); VERIFY(test_assign(Matrix(),Matrix()+Matrix(), - LinearTraversal,NoUnrolling)); + HalfPacketSize==1 ? InnerVectorizedTraversal : + EIGEN_UNALIGNED_VECTORIZE ? LinearVectorizedTraversal : + LinearTraversal, + NoUnrolling)); - VERIFY(test_assign(Matrix11(),Matrix().template block(2,3)+Matrix().template block(10,4), - DefaultTraversal,CompleteUnrolling)); + VERIFY(test_assign(Matrix11(), Matrix11()+Matrix11(),InnerVectorizedTraversal,CompleteUnrolling)); + + + VERIFY(test_assign(Matrix11(),Matrix().template block(2,3)+Matrix().template block(8,4), + (EIGEN_UNALIGNED_VECTORIZE) ? InnerVectorizedTraversal : DefaultTraversal, CompleteUnrolling|InnerUnrolling)); + + VERIFY(test_assign(Vector1(),Matrix11()*Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_assign(Matrix11(),Matrix11().lazyProduct(Matrix11()), + InnerVectorizedTraversal,InnerUnrolling+CompleteUnrolling)); } - + + VERIFY(test_redux(Vector1(), + LinearVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_redux(Matrix(), + LinearVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_redux(Matrix3(), LinearVectorizedTraversal,CompleteUnrolling)); @@ -174,18 +224,19 @@ template::Vectori VERIFY(test_redux(Matrix44r().template block<1,2*PacketSize>(2,1), LinearVectorizedTraversal,CompleteUnrolling)); - + VERIFY((test_assign< - Map >, + Map >, Matrix22 >(InnerVectorizedTraversal,CompleteUnrolling))); VERIFY((test_assign< - Map >, - Matrix22 - >(DefaultTraversal,CompleteUnrolling))); + Map, AlignedMax, InnerStride<3*PacketSize> >, + Matrix + >(DefaultTraversal,PacketSize>=8?InnerUnrolling:CompleteUnrolling))); - VERIFY((test_assign(Matrix11(), Matrix11()*Matrix11(), InnerVectorizedTraversal, CompleteUnrolling))); + VERIFY((test_assign(Matrix11(), Matrix()*Matrix(), + InnerVectorizedTraversal, CompleteUnrolling))); #endif VERIFY(test_assign(MatrixXX(10,10),MatrixXX(20,20).block(10,10,2,3), @@ -193,12 +244,138 @@ template::Vectori VERIFY(test_redux(VectorX(10), LinearVectorizedTraversal,NoUnrolling)); + } +}; + +template struct vectorization_logic +{ + static void run() {} +}; + +template::type>::half, + typename internal::packet_traits::type>::value > +struct vectorization_logic_half +{ + typedef internal::packet_traits PacketTraits; + typedef typename internal::unpacket_traits::type>::half PacketType; + enum { + PacketSize = internal::unpacket_traits::size + }; + static void run() + { + + typedef Matrix Vector1; + typedef Matrix Matrix11; + typedef Matrix Matrix57; + typedef Matrix Matrix35; + typedef Matrix Matrix57u; +// typedef Matrix Matrix44; +// typedef Matrix Matrix44u; +// typedef Matrix Matrix44c; +// typedef Matrix Matrix44r; + + typedef Matrix Matrix1; + + typedef Matrix Matrix1u; + // this type is made such that it can only be vectorized when viewed as a linear 1D vector + typedef Matrix Matrix3; + #if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT + VERIFY(test_assign(Vector1(),Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Vector1()+Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Vector1().template segment(0).derived(), + EIGEN_UNALIGNED_VECTORIZE ? InnerVectorizedTraversal : LinearVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Scalar(2.1)*Vector1()-Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),(Scalar(2.1)*Vector1().template segment(0)-Vector1().template segment(0)).derived(), + EIGEN_UNALIGNED_VECTORIZE ? InnerVectorizedTraversal : LinearVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Vector1().cwiseProduct(Vector1()), + InnerVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Vector1().template cast(), + InnerVectorizedTraversal,CompleteUnrolling)); + + + VERIFY(test_assign(Vector1(),Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Vector1()+Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + VERIFY(test_assign(Vector1(),Vector1().cwiseProduct(Vector1()), + InnerVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_assign(Matrix57(),Matrix57()+Matrix57(), + InnerVectorizedTraversal,InnerUnrolling)); + + VERIFY(test_assign(Matrix57u(),Matrix57()+Matrix57(), + EIGEN_UNALIGNED_VECTORIZE ? InnerVectorizedTraversal : LinearTraversal, + EIGEN_UNALIGNED_VECTORIZE ? InnerUnrolling : NoUnrolling)); + + VERIFY(test_assign(Matrix1u(),Matrix1()+Matrix1(), + EIGEN_UNALIGNED_VECTORIZE ? ((Matrix1::InnerSizeAtCompileTime % PacketSize)==0 ? InnerVectorizedTraversal : LinearVectorizedTraversal) : LinearTraversal,CompleteUnrolling)); + + if(PacketSize>1) + { + typedef Matrix Matrix33c; + VERIFY(test_assign(Matrix33c().row(2),Matrix33c().row(1)+Matrix33c().row(1), + LinearTraversal,CompleteUnrolling)); + VERIFY(test_assign(Matrix33c().col(0),Matrix33c().col(1)+Matrix33c().col(1), + EIGEN_UNALIGNED_VECTORIZE ? (PacketSize==1 ? InnerVectorizedTraversal : LinearVectorizedTraversal) : LinearTraversal,CompleteUnrolling)); + + VERIFY(test_assign(Matrix3(),Matrix3().cwiseQuotient(Matrix3()), + PacketTraits::HasDiv ? LinearVectorizedTraversal : LinearTraversal,CompleteUnrolling)); + + VERIFY(test_assign(Matrix(),Matrix()+Matrix(), + EIGEN_UNALIGNED_VECTORIZE ? (PacketSize==1 ? InnerVectorizedTraversal : LinearVectorizedTraversal) : LinearTraversal, + NoUnrolling)); + + VERIFY(test_assign(Matrix11(),Matrix().template block(2,3)+Matrix().template block(8,4), + EIGEN_UNALIGNED_VECTORIZE ? InnerVectorizedTraversal : DefaultTraversal,PacketSize>4?InnerUnrolling:CompleteUnrolling)); + + VERIFY(test_assign(Vector1(),Matrix11()*Vector1(), + InnerVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_assign(Matrix11(),Matrix11().lazyProduct(Matrix11()), + InnerVectorizedTraversal,InnerUnrolling+CompleteUnrolling)); + } + + VERIFY(test_redux(Vector1(), + LinearVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_redux(Matrix(), + LinearVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_redux(Matrix3(), + LinearVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_redux(Matrix35(), + LinearVectorizedTraversal,CompleteUnrolling)); + + VERIFY(test_redux(Matrix57().template block(1,0), + DefaultTraversal,CompleteUnrolling)); + + VERIFY((test_assign< + Map, AlignedMax, InnerStride<3*PacketSize> >, + Matrix + >(DefaultTraversal,CompleteUnrolling))); + + VERIFY((test_assign(Matrix57(), Matrix()*Matrix(), + InnerVectorizedTraversal, InnerUnrolling|CompleteUnrolling))); + #endif } }; -template struct vectorization_logic +template struct vectorization_logic_half { static void run() {} }; @@ -208,27 +385,34 @@ void test_vectorization_logic() #ifdef EIGEN_VECTORIZE + CALL_SUBTEST( vectorization_logic::run() ); CALL_SUBTEST( vectorization_logic::run() ); CALL_SUBTEST( vectorization_logic::run() ); CALL_SUBTEST( vectorization_logic >::run() ); CALL_SUBTEST( vectorization_logic >::run() ); + CALL_SUBTEST( vectorization_logic_half::run() ); + CALL_SUBTEST( vectorization_logic_half::run() ); + CALL_SUBTEST( vectorization_logic_half::run() ); + CALL_SUBTEST( vectorization_logic_half >::run() ); + CALL_SUBTEST( vectorization_logic_half >::run() ); + if(internal::packet_traits::Vectorizable) { VERIFY(test_assign(Matrix(),Matrix()+Matrix(), - LinearTraversal,CompleteUnrolling)); + EIGEN_UNALIGNED_VECTORIZE ? LinearVectorizedTraversal : LinearTraversal,CompleteUnrolling)); VERIFY(test_redux(Matrix(), - DefaultTraversal,CompleteUnrolling)); + EIGEN_UNALIGNED_VECTORIZE ? LinearVectorizedTraversal : DefaultTraversal,CompleteUnrolling)); } if(internal::packet_traits::Vectorizable) { VERIFY(test_assign(Matrix(),Matrix()+Matrix(), - LinearTraversal,CompleteUnrolling)); + EIGEN_UNALIGNED_VECTORIZE ? LinearVectorizedTraversal : LinearTraversal,CompleteUnrolling)); VERIFY(test_redux(Matrix(), - DefaultTraversal,CompleteUnrolling)); + EIGEN_UNALIGNED_VECTORIZE ? LinearVectorizedTraversal : DefaultTraversal,CompleteUnrolling)); } #endif // EIGEN_VECTORIZE diff --git a/ground/gcs/src/libs/eigen/test/vectorwiseop.cpp b/ground/gcs/src/libs/eigen/test/vectorwiseop.cpp index d32fd10ccc..f3ab561ee7 100644 --- a/ground/gcs/src/libs/eigen/test/vectorwiseop.cpp +++ b/ground/gcs/src/libs/eigen/test/vectorwiseop.cpp @@ -2,11 +2,13 @@ // for linear algebra. // // Copyright (C) 2011 Benoit Jacob +// Copyright (C) 2015 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#define TEST_ENABLE_TEMPORARY_TRACKING #define EIGEN_NO_STATIC_ASSERT #include "main.h" @@ -101,11 +103,11 @@ template void vectorwiseop_array(const ArrayType& m) VERIFY_RAISES_ASSERT(m2.rowwise() /= rowvec.transpose()); VERIFY_RAISES_ASSERT(m1.rowwise() / rowvec.transpose()); - + m2 = m1; // yes, there might be an aliasing issue there but ".rowwise() /=" - // is suppposed to evaluate " m2.colwise().sum()" into to temporary to avoid - // evaluating the reducions multiple times + // is supposed to evaluate " m2.colwise().sum()" into a temporary to avoid + // evaluating the reduction multiple times if(ArrayType::RowsAtCompileTime>2 || ArrayType::RowsAtCompileTime==Dynamic) { m2.rowwise() /= m2.colwise().sum(); @@ -156,16 +158,22 @@ template void vectorwiseop_matrix(const MatrixType& m) VERIFY_IS_APPROX(m2, m1.colwise() + colvec); VERIFY_IS_APPROX(m2.col(c), m1.col(c) + colvec); - VERIFY_RAISES_ASSERT(m2.colwise() += colvec.transpose()); - VERIFY_RAISES_ASSERT(m1.colwise() + colvec.transpose()); + if(rows>1) + { + VERIFY_RAISES_ASSERT(m2.colwise() += colvec.transpose()); + VERIFY_RAISES_ASSERT(m1.colwise() + colvec.transpose()); + } m2 = m1; m2.rowwise() += rowvec; VERIFY_IS_APPROX(m2, m1.rowwise() + rowvec); VERIFY_IS_APPROX(m2.row(r), m1.row(r) + rowvec); - VERIFY_RAISES_ASSERT(m2.rowwise() += rowvec.transpose()); - VERIFY_RAISES_ASSERT(m1.rowwise() + rowvec.transpose()); + if(cols>1) + { + VERIFY_RAISES_ASSERT(m2.rowwise() += rowvec.transpose()); + VERIFY_RAISES_ASSERT(m1.rowwise() + rowvec.transpose()); + } // test substraction @@ -174,29 +182,43 @@ template void vectorwiseop_matrix(const MatrixType& m) VERIFY_IS_APPROX(m2, m1.colwise() - colvec); VERIFY_IS_APPROX(m2.col(c), m1.col(c) - colvec); - VERIFY_RAISES_ASSERT(m2.colwise() -= colvec.transpose()); - VERIFY_RAISES_ASSERT(m1.colwise() - colvec.transpose()); + if(rows>1) + { + VERIFY_RAISES_ASSERT(m2.colwise() -= colvec.transpose()); + VERIFY_RAISES_ASSERT(m1.colwise() - colvec.transpose()); + } m2 = m1; m2.rowwise() -= rowvec; VERIFY_IS_APPROX(m2, m1.rowwise() - rowvec); VERIFY_IS_APPROX(m2.row(r), m1.row(r) - rowvec); - VERIFY_RAISES_ASSERT(m2.rowwise() -= rowvec.transpose()); - VERIFY_RAISES_ASSERT(m1.rowwise() - rowvec.transpose()); - + if(cols>1) + { + VERIFY_RAISES_ASSERT(m2.rowwise() -= rowvec.transpose()); + VERIFY_RAISES_ASSERT(m1.rowwise() - rowvec.transpose()); + } + // test norm rrres = m1.colwise().norm(); VERIFY_IS_APPROX(rrres(c), m1.col(c).norm()); rcres = m1.rowwise().norm(); VERIFY_IS_APPROX(rcres(r), m1.row(r).norm()); - + + VERIFY_IS_APPROX(m1.cwiseAbs().colwise().sum(), m1.colwise().template lpNorm<1>()); + VERIFY_IS_APPROX(m1.cwiseAbs().rowwise().sum(), m1.rowwise().template lpNorm<1>()); + VERIFY_IS_APPROX(m1.cwiseAbs().colwise().maxCoeff(), m1.colwise().template lpNorm()); + VERIFY_IS_APPROX(m1.cwiseAbs().rowwise().maxCoeff(), m1.rowwise().template lpNorm()); + + // regression for bug 1158 + VERIFY_IS_APPROX(m1.cwiseAbs().colwise().sum().x(), m1.col(0).cwiseAbs().sum()); + // test normalized m2 = m1.colwise().normalized(); VERIFY_IS_APPROX(m2.col(c), m1.col(c).normalized()); m2 = m1.rowwise().normalized(); VERIFY_IS_APPROX(m2.row(r), m1.row(r).normalized()); - + // test normalize m2 = m1; m2.colwise().normalize(); @@ -204,14 +226,27 @@ template void vectorwiseop_matrix(const MatrixType& m) m2 = m1; m2.rowwise().normalize(); VERIFY_IS_APPROX(m2.row(r), m1.row(r).normalized()); + + // test with partial reduction of products + Matrix m1m1 = m1 * m1.transpose(); + VERIFY_IS_APPROX( (m1 * m1.transpose()).colwise().sum(), m1m1.colwise().sum()); + Matrix tmp(rows); + VERIFY_EVALUATION_COUNT( tmp = (m1 * m1.transpose()).colwise().sum(), 1); + + m2 = m1.rowwise() - (m1.colwise().sum()/RealScalar(m1.rows())).eval(); + m1 = m1.rowwise() - (m1.colwise().sum()/RealScalar(m1.rows())); + VERIFY_IS_APPROX( m1, m2 ); + VERIFY_EVALUATION_COUNT( m2 = (m1.rowwise() - m1.colwise().sum()/RealScalar(m1.rows())), (MatrixType::RowsAtCompileTime!=1 ? 1 : 0) ); } void test_vectorwiseop() { - CALL_SUBTEST_1(vectorwiseop_array(Array22cd())); - CALL_SUBTEST_2(vectorwiseop_array(Array())); - CALL_SUBTEST_3(vectorwiseop_array(ArrayXXf(3, 4))); - CALL_SUBTEST_4(vectorwiseop_matrix(Matrix4cf())); - CALL_SUBTEST_5(vectorwiseop_matrix(Matrix())); - CALL_SUBTEST_6(vectorwiseop_matrix(MatrixXd(7,2))); + CALL_SUBTEST_1( vectorwiseop_array(Array22cd()) ); + CALL_SUBTEST_2( vectorwiseop_array(Array()) ); + CALL_SUBTEST_3( vectorwiseop_array(ArrayXXf(3, 4)) ); + CALL_SUBTEST_4( vectorwiseop_matrix(Matrix4cf()) ); + CALL_SUBTEST_5( vectorwiseop_matrix(Matrix()) ); + CALL_SUBTEST_6( vectorwiseop_matrix(MatrixXd(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_7( vectorwiseop_matrix(VectorXd(internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_7( vectorwiseop_matrix(RowVectorXd(internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } diff --git a/ground/gcs/src/libs/eigen/test/zerosized.cpp b/ground/gcs/src/libs/eigen/test/zerosized.cpp index da7dd04814..477ff0070c 100644 --- a/ground/gcs/src/libs/eigen/test/zerosized.cpp +++ b/ground/gcs/src/libs/eigen/test/zerosized.cpp @@ -25,6 +25,7 @@ template void zeroReduction(const MatrixType& m) { template void zeroSizedMatrix() { MatrixType t1; + typedef typename MatrixType::Scalar Scalar; if (MatrixType::SizeAtCompileTime == Dynamic || MatrixType::SizeAtCompileTime == 0) { @@ -37,7 +38,7 @@ template void zeroSizedMatrix() if (MatrixType::RowsAtCompileTime == Dynamic && MatrixType::ColsAtCompileTime == Dynamic) { - MatrixType t2(0, 0); + MatrixType t2(0, 0), t3(t1); VERIFY(t2.rows() == 0); VERIFY(t2.cols() == 0); @@ -45,6 +46,23 @@ template void zeroSizedMatrix() VERIFY(t1==t2); } } + + if(MatrixType::MaxColsAtCompileTime!=0 && MatrixType::MaxRowsAtCompileTime!=0) + { + Index rows = MatrixType::RowsAtCompileTime==Dynamic ? internal::random(1,10) : Index(MatrixType::RowsAtCompileTime); + Index cols = MatrixType::ColsAtCompileTime==Dynamic ? internal::random(1,10) : Index(MatrixType::ColsAtCompileTime); + MatrixType m(rows,cols); + zeroReduction(m.template block<0,MatrixType::ColsAtCompileTime>(0,0,0,cols)); + zeroReduction(m.template block(0,0,rows,0)); + zeroReduction(m.template block<0,1>(0,0)); + zeroReduction(m.template block<1,0>(0,0)); + Matrix prod = m.template block(0,0,rows,0) * m.template block<0,MatrixType::ColsAtCompileTime>(0,0,0,cols); + VERIFY(prod.rows()==rows && prod.cols()==cols); + VERIFY(prod.isZero()); + prod = m.template block<1,0>(0,0) * m.template block<0,1>(0,0); + VERIFY(prod.size()==1); + VERIFY(prod.isZero()); + } } template void zeroSizedVector() diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/AdolcForward b/ground/gcs/src/libs/eigen/unsupported/Eigen/AdolcForward index 2627decd0f..15f5f0731a 100644 --- a/ground/gcs/src/libs/eigen/unsupported/Eigen/AdolcForward +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/AdolcForward @@ -25,7 +25,7 @@ #ifndef NUMBER_DIRECTIONS # define NUMBER_DIRECTIONS 2 #endif -#include +#include // adolc defines some very stupid macros: #if defined(malloc) diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/AlignedVector3 b/ground/gcs/src/libs/eigen/unsupported/Eigen/AlignedVector3 index 29d5c90fb5..47a86d4c00 100644 --- a/ground/gcs/src/libs/eigen/unsupported/Eigen/AlignedVector3 +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/AlignedVector3 @@ -57,6 +57,11 @@ template class AlignedVector3 inline Index rows() const { return 3; } inline Index cols() const { return 1; } + + Scalar* data() { return m_coeffs.data(); } + const Scalar* data() const { return m_coeffs.data(); } + Index innerStride() const { return 1; } + Index outerStride() const { return 3; } inline const Scalar& coeff(Index row, Index col) const { return m_coeffs.coeff(row, col); } @@ -100,7 +105,7 @@ template class AlignedVector3 }; template - inline explicit AlignedVector3(const MatrixBase& other) + inline AlignedVector3(const MatrixBase& other) { generic_assign_selector::run(*this,other.derived()); } @@ -108,6 +113,12 @@ template class AlignedVector3 inline AlignedVector3& operator=(const AlignedVector3& other) { m_coeffs = other.m_coeffs; return *this; } + template + inline AlignedVector3& operator=(const MatrixBase& other) + { + generic_assign_selector::run(*this,other.derived()); + return *this; + } inline AlignedVector3 operator+(const AlignedVector3& other) const { return AlignedVector3(m_coeffs + other.m_coeffs); } @@ -148,7 +159,7 @@ template class AlignedVector3 m_coeffs /= norm(); } - inline AlignedVector3 normalized() + inline AlignedVector3 normalized() const { return AlignedVector3(m_coeffs / norm()); } @@ -181,8 +192,31 @@ template class AlignedVector3 { return m_coeffs.template head<3>().isApprox(other,eps); } + + CoeffType& coeffs() { return m_coeffs; } + const CoeffType& coeffs() const { return m_coeffs; } }; +namespace internal { + +template +struct eval, Dense> +{ + typedef const AlignedVector3<_Scalar>& type; +}; + +template +struct evaluator > + : evaluator > +{ + typedef AlignedVector3 XprType; + typedef evaluator > Base; + + evaluator(const XprType &m) : Base(m.coeffs()) {} +}; + +} + //@} } diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CMakeLists.txt b/ground/gcs/src/libs/eigen/unsupported/Eigen/CMakeLists.txt index e1fbf97e2b..631a060145 100644 --- a/ground/gcs/src/libs/eigen/unsupported/Eigen/CMakeLists.txt +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CMakeLists.txt @@ -1,11 +1,32 @@ -set(Eigen_HEADERS AdolcForward AlignedVector3 ArpackSupport AutoDiff BVH FFT IterativeSolvers KroneckerProduct LevenbergMarquardt - MatrixFunctions MoreVectorization MPRealSupport NonLinearOptimization NumericalDiff OpenGLSupport Polynomials - Skyline SparseExtra Splines - ) +set(Eigen_HEADERS + AdolcForward + AlignedVector3 + ArpackSupport + AutoDiff + BVH + EulerAngles + FFT + IterativeSolvers + KroneckerProduct + LevenbergMarquardt + MatrixFunctions + MoreVectorization + MPRealSupport + NonLinearOptimization + NumericalDiff + OpenGLSupport + Polynomials + Skyline + SparseExtra + SpecialFunctions + Splines + ) install(FILES ${Eigen_HEADERS} DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen COMPONENT Devel ) -add_subdirectory(src) +install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") + +add_subdirectory(CXX11) diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/CMakeLists.txt b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/CMakeLists.txt new file mode 100644 index 0000000000..385ed240c2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/CMakeLists.txt @@ -0,0 +1,8 @@ +set(Eigen_CXX11_HEADERS Tensor TensorSymmetry ThreadPool) + +install(FILES + ${Eigen_CXX11_HEADERS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/CXX11 COMPONENT Devel + ) + +install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/CXX11 COMPONENT Devel FILES_MATCHING PATTERN "*.h") diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/Tensor b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/Tensor new file mode 100644 index 0000000000..7ecb4c74d8 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/Tensor @@ -0,0 +1,152 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// Copyright (C) 2013 Christian Seiler +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +//#ifndef EIGEN_CXX11_TENSOR_MODULE +//#define EIGEN_CXX11_TENSOR_MODULE + +#include "../../../Eigen/Core" + +#ifdef EIGEN_USE_SYCL +#undef min +#undef max +#undef isnan +#undef isinf +#undef isfinite +#include +#include +#include +#include +#endif + +#include + +#include "../SpecialFunctions" +#include "src/util/CXX11Meta.h" +#include "src/util/MaxSizeVector.h" + +/** \defgroup CXX11_Tensor_Module Tensor Module + * + * This module provides a Tensor class for storing arbitrarily indexed + * objects. + * + * \code + * #include + * \endcode + */ + +#include +#include +#include + +#ifdef _WIN32 +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; +#else +#include +#endif + +#if __cplusplus > 199711 || EIGEN_COMP_MSVC >= 1900 +#include +#endif + +#ifdef _WIN32 +#include +#elif defined(__APPLE__) +#include +#else +#include +#endif + +#ifdef EIGEN_USE_THREADS +#include "ThreadPool" +#endif + +#ifdef EIGEN_USE_GPU +#include +#include +#if __cplusplus >= 201103L +#include +#include +#endif +#endif + +#include "src/Tensor/TensorMacros.h" +#include "src/Tensor/TensorForwardDeclarations.h" +#include "src/Tensor/TensorMeta.h" +#include "src/Tensor/TensorFunctors.h" +#include "src/Tensor/TensorCostModel.h" +#include "src/Tensor/TensorDeviceDefault.h" +#include "src/Tensor/TensorDeviceThreadPool.h" +#include "src/Tensor/TensorDeviceCuda.h" +#include "src/Tensor/TensorDeviceSycl.h" +#include "src/Tensor/TensorIndexList.h" +#include "src/Tensor/TensorDimensionList.h" +#include "src/Tensor/TensorDimensions.h" +#include "src/Tensor/TensorInitializer.h" +#include "src/Tensor/TensorTraits.h" +#include "src/Tensor/TensorRandom.h" +#include "src/Tensor/TensorUInt128.h" +#include "src/Tensor/TensorIntDiv.h" +#include "src/Tensor/TensorGlobalFunctions.h" + +#include "src/Tensor/TensorBase.h" + +#include "src/Tensor/TensorEvaluator.h" +#include "src/Tensor/TensorExpr.h" +#include "src/Tensor/TensorReduction.h" +#include "src/Tensor/TensorReductionCuda.h" +#include "src/Tensor/TensorArgMax.h" +#include "src/Tensor/TensorConcatenation.h" +#include "src/Tensor/TensorContractionMapper.h" +#include "src/Tensor/TensorContractionBlocking.h" +#include "src/Tensor/TensorContraction.h" +#include "src/Tensor/TensorContractionThreadPool.h" +#include "src/Tensor/TensorContractionCuda.h" +#include "src/Tensor/TensorConversion.h" +#include "src/Tensor/TensorConvolution.h" +#include "src/Tensor/TensorFFT.h" +#include "src/Tensor/TensorPatch.h" +#include "src/Tensor/TensorImagePatch.h" +#include "src/Tensor/TensorVolumePatch.h" +#include "src/Tensor/TensorBroadcasting.h" +#include "src/Tensor/TensorChipping.h" +#include "src/Tensor/TensorInflation.h" +#include "src/Tensor/TensorLayoutSwap.h" +#include "src/Tensor/TensorMorphing.h" +#include "src/Tensor/TensorPadding.h" +#include "src/Tensor/TensorReverse.h" +#include "src/Tensor/TensorShuffling.h" +#include "src/Tensor/TensorStriding.h" +#include "src/Tensor/TensorCustomOp.h" +#include "src/Tensor/TensorEvalTo.h" +#include "src/Tensor/TensorForcedEval.h" +#include "src/Tensor/TensorGenerator.h" +#include "src/Tensor/TensorAssign.h" +#include "src/Tensor/TensorScan.h" + +#include "src/Tensor/TensorSycl.h" +#include "src/Tensor/TensorExecutor.h" +#include "src/Tensor/TensorDevice.h" + +#include "src/Tensor/TensorStorage.h" +#include "src/Tensor/Tensor.h" +#include "src/Tensor/TensorFixedSize.h" +#include "src/Tensor/TensorMap.h" +#include "src/Tensor/TensorRef.h" + +#include "src/Tensor/TensorIO.h" + +#include + +//#endif // EIGEN_CXX11_TENSOR_MODULE diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/TensorSymmetry b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/TensorSymmetry new file mode 100644 index 0000000000..fb1b0c0fbc --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/TensorSymmetry @@ -0,0 +1,42 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2013 Christian Seiler +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSORSYMMETRY_MODULE +#define EIGEN_CXX11_TENSORSYMMETRY_MODULE + +#include + +#include + +#include "src/util/CXX11Meta.h" + +/** \defgroup CXX11_TensorSymmetry_Module Tensor Symmetry Module + * + * This module provides a classes that allow for the definition of + * symmetries w.r.t. tensor indices. + * + * Including this module will implicitly include the Tensor module. + * + * \code + * #include + * \endcode + */ + +#include "src/TensorSymmetry/util/TemplateGroupTheory.h" +#include "src/TensorSymmetry/Symmetry.h" +#include "src/TensorSymmetry/StaticSymmetry.h" +#include "src/TensorSymmetry/DynamicSymmetry.h" + +#include + +#endif // EIGEN_CXX11_TENSORSYMMETRY_MODULE + +/* + * kate: space-indent on; indent-width 2; mixedindent off; indent-mode cstyle; + */ diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/ThreadPool b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/ThreadPool new file mode 100644 index 0000000000..09d637e9a0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/ThreadPool @@ -0,0 +1,65 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_THREADPOOL_MODULE +#define EIGEN_CXX11_THREADPOOL_MODULE + +#include "../../../Eigen/Core" + +#include + +/** \defgroup CXX11_ThreadPool_Module C++11 ThreadPool Module + * + * This module provides 2 threadpool implementations + * - a simple reference implementation + * - a faster non blocking implementation + * + * This module requires C++11. + * + * \code + * #include + * \endcode + */ + + +// The code depends on CXX11, so only include the module if the +// compiler supports it. +#if __cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900 +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "src/util/CXX11Meta.h" +#include "src/util/MaxSizeVector.h" + +#include "src/ThreadPool/ThreadLocal.h" +#include "src/ThreadPool/ThreadYield.h" +#include "src/ThreadPool/EventCount.h" +#include "src/ThreadPool/RunQueue.h" +#include "src/ThreadPool/ThreadPoolInterface.h" +#include "src/ThreadPool/ThreadEnvironment.h" +#include "src/ThreadPool/SimpleThreadPool.h" +#include "src/ThreadPool/NonBlockingThreadPool.h" + +#endif + +#include + +#endif // EIGEN_CXX11_THREADPOOL_MODULE + diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/README.md b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/README.md new file mode 100644 index 0000000000..02146527b5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/README.md @@ -0,0 +1,1757 @@ +# Eigen Tensors + +Tensors are multidimensional arrays of elements. Elements are typically scalars, +but more complex types such as strings are also supported. + +[TOC] + +## Tensor Classes + +You can manipulate a tensor with one of the following classes. They all are in +the namespace ```::Eigen.``` + + +### Class Tensor + +This is the class to use to create a tensor and allocate memory for it. The +class is templatized with the tensor datatype, such as float or int, and the +tensor rank. The rank is the number of dimensions, for example rank 2 is a +matrix. + +Tensors of this class are resizable. For example, if you assign a tensor of a +different size to a Tensor, that tensor is resized to match its new value. + +#### Constructor Tensor(size0, size1, ...) + +Constructor for a Tensor. The constructor must be passed ```rank``` integers +indicating the sizes of the instance along each of the the ```rank``` +dimensions. + + // Create a tensor of rank 3 of sizes 2, 3, 4. This tensor owns + // memory to hold 24 floating point values (24 = 2 x 3 x 4). + Tensor t_3d(2, 3, 4); + + // Resize t_3d by assigning a tensor of different sizes, but same rank. + t_3d = Tensor(3, 4, 3); + +#### Constructor Tensor(size_array) + +Constructor where the sizes for the constructor are specified as an array of +values instead of an explicitly list of parameters. The array type to use is +```Eigen::array```. The array can be constructed automatically +from an initializer list. + + // Create a tensor of strings of rank 2 with sizes 5, 7. + Tensor t_2d({5, 7}); + + +### Class TensorFixedSize> + +Class to use for tensors of fixed size, where the size is known at compile +time. Fixed sized tensors can provide very fast computations because all their +dimensions are known by the compiler. FixedSize tensors are not resizable. + +If the total number of elements in a fixed size tensor is small enough the +tensor data is held onto the stack and does not cause heap allocation and free. + + // Create a 4 x 3 tensor of floats. + TensorFixedSize> t_4x3; + +### Class TensorMap> + +This is the class to use to create a tensor on top of memory allocated and +owned by another part of your code. It allows to view any piece of allocated +memory as a Tensor. Instances of this class do not own the memory where the +data are stored. + +A TensorMap is not resizable because it does not own the memory where its data +are stored. + +#### Constructor TensorMap>(data, size0, size1, ...) + +Constructor for a Tensor. The constructor must be passed a pointer to the +storage for the data, and "rank" size attributes. The storage has to be +large enough to hold all the data. + + // Map a tensor of ints on top of stack-allocated storage. + int storage[128]; // 2 x 4 x 2 x 8 = 128 + TensorMap t_4d(storage, 2, 4, 2, 8); + + // The same storage can be viewed as a different tensor. + // You can also pass the sizes as an array. + TensorMap t_2d(storage, 16, 8); + + // You can also map fixed-size tensors. Here we get a 1d view of + // the 2d fixed-size tensor. + Tensor> t_4x3; + TensorMap t_12(t_4x3, 12); + + +#### Class TensorRef + +See Assigning to a TensorRef below. + +## Accessing Tensor Elements + +#### tensor(index0, index1...) + +Return the element at position ```(index0, index1...)``` in tensor +```tensor```. You must pass as many parameters as the rank of ```tensor```. +The expression can be used as an l-value to set the value of the element at the +specified position. The value returned is of the datatype of the tensor. + + // Set the value of the element at position (0, 1, 0); + Tensor t_3d(2, 3, 4); + t_3d(0, 1, 0) = 12.0f; + + // Initialize all elements to random values. + for (int i = 0; i < 2; ++i) { + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 4; ++k) { + t_3d(i, j, k) = ...some random value...; + } + } + } + + // Print elements of a tensor. + for (int i = 0; i < 2; ++i) { + LOG(INFO) << t_3d(i, 0, 0); + } + + +## TensorLayout + +The tensor library supports 2 layouts: ```ColMajor``` (the default) and +```RowMajor```. Only the default column major layout is currently fully +supported, and it is therefore not recommended to attempt to use the row major +layout at the moment. + +The layout of a tensor is optionally specified as part of its type. If not +specified explicitly column major is assumed. + + Tensor col_major; // equivalent to Tensor + TensorMap > row_major(data, ...); + +All the arguments to an expression must use the same layout. Attempting to mix +different layouts will result in a compilation error. + +It is possible to change the layout of a tensor or an expression using the +```swap_layout()``` method. Note that this will also reverse the order of the +dimensions. + + Tensor col_major(2, 4); + Tensor row_major(2, 4); + + Tensor col_major_result = col_major; // ok, layouts match + Tensor col_major_result = row_major; // will not compile + + // Simple layout swap + col_major_result = row_major.swap_layout(); + eigen_assert(col_major_result.dimension(0) == 4); + eigen_assert(col_major_result.dimension(1) == 2); + + // Swap the layout and preserve the order of the dimensions + array shuffle(1, 0); + col_major_result = row_major.swap_layout().shuffle(shuffle); + eigen_assert(col_major_result.dimension(0) == 2); + eigen_assert(col_major_result.dimension(1) == 4); + + +## Tensor Operations + +The Eigen Tensor library provides a vast library of operations on Tensors: +numerical operations such as addition and multiplication, geometry operations +such as slicing and shuffling, etc. These operations are available as methods +of the Tensor classes, and in some cases as operator overloads. For example +the following code computes the elementwise addition of two tensors: + + Tensor t1(2, 3, 4); + ...set some values in t1... + Tensor t2(2, 3, 4); + ...set some values in t2... + // Set t3 to the element wise sum of t1 and t2 + Tensor t3 = t1 + t2; + +While the code above looks easy enough, it is important to understand that the +expression ```t1 + t2``` is not actually adding the values of the tensors. The +expression instead constructs a "tensor operator" object of the class +TensorCwiseBinaryOp, which has references to the tensors +```t1``` and ```t2```. This is a small C++ object that knows how to add +```t1``` and ```t2```. It is only when the value of the expression is assigned +to the tensor ```t3``` that the addition is actually performed. Technically, +this happens through the overloading of ```operator=()``` in the Tensor class. + +This mechanism for computing tensor expressions allows for lazy evaluation and +optimizations which are what make the tensor library very fast. + +Of course, the tensor operators do nest, and the expression ```t1 + t2 * +0.3f``` is actually represented with the (approximate) tree of operators: + + TensorCwiseBinaryOp(t1, TensorCwiseUnaryOp(t2, 0.3f)) + + +### Tensor Operations and C++ "auto" + +Because Tensor operations create tensor operators, the C++ ```auto``` keyword +does not have its intuitive meaning. Consider these 2 lines of code: + + Tensor t3 = t1 + t2; + auto t4 = t1 + t2; + +In the first line we allocate the tensor ```t3``` and it will contain the +result of the addition of ```t1``` and ```t2```. In the second line, ```t4``` +is actually the tree of tensor operators that will compute the addition of +```t1``` and ```t2```. In fact, ```t4``` is *not* a tensor and you cannot get +the values of its elements: + + Tensor t3 = t1 + t2; + cout << t3(0, 0, 0); // OK prints the value of t1(0, 0, 0) + t2(0, 0, 0) + + auto t4 = t1 + t2; + cout << t4(0, 0, 0); // Compilation error! + +When you use ```auto``` you do not get a Tensor as a result but instead a +non-evaluated expression. So only use ```auto``` to delay evaluation. + +Unfortunately, there is no single underlying concrete type for holding +non-evaluated expressions, hence you have to use auto in the case when you do +want to hold non-evaluated expressions. + +When you need the results of set of tensor computations you have to assign the +result to a Tensor that will be capable of holding onto them. This can be +either a normal Tensor, a fixed size Tensor, or a TensorMap on an existing +piece of memory. All the following will work: + + auto t4 = t1 + t2; + + Tensor result = t4; // Could also be: result(t4); + cout << result(0, 0, 0); + + TensorMap result(, , ...) = t4; + cout << result(0, 0, 0); + + TensorFixedSize> result = t4; + cout << result(0, 0, 0); + +Until you need the results, you can keep the operation around, and even reuse +it for additional operations. As long as you keep the expression as an +operation, no computation is performed. + + // One way to compute exp((t1 + t2) * 0.2f); + auto t3 = t1 + t2; + auto t4 = t3 * 0.2f; + auto t5 = t4.exp(); + Tensor result = t5; + + // Another way, exactly as efficient as the previous one: + Tensor result = ((t1 + t2) * 0.2f).exp(); + +### Controlling When Expression are Evaluated + +There are several ways to control when expressions are evaluated: + +* Assignment to a Tensor, TensorFixedSize, or TensorMap. +* Use of the eval() method. +* Assignment to a TensorRef. + +#### Assigning to a Tensor, TensorFixedSize, or TensorMap. + +The most common way to evaluate an expression is to assign it to a Tensor. In +the example below, the ```auto``` declarations make the intermediate values +"Operations", not Tensors, and do not cause the expressions to be evaluated. +The assignment to the Tensor ```result``` causes the evaluation of all the +operations. + + auto t3 = t1 + t2; // t3 is an Operation. + auto t4 = t3 * 0.2f; // t4 is an Operation. + auto t5 = t4.exp(); // t5 is an Operation. + Tensor result = t5; // The operations are evaluated. + +If you know the ranks and sizes of the Operation value you can assign the +Operation to a TensorFixedSize instead of a Tensor, which is a bit more +efficient. + + // We know that the result is a 4x4x2 tensor! + TensorFixedSize result = t5; + +Simiarly, assigning an expression to a TensorMap causes its evaluation. Like +tensors of type TensorFixedSize, TensorMaps cannot be resized so they have to +have the rank and sizes of the expression that are assigned to them. + +#### Calling eval(). + +When you compute large composite expressions, you sometimes want to tell Eigen +that an intermediate value in the expression tree is worth evaluating ahead of +time. This is done by inserting a call to the ```eval()``` method of the +expression Operation. + + // The previous example could have been written: + Tensor result = ((t1 + t2) * 0.2f).exp(); + + // If you want to compute (t1 + t2) once ahead of time you can write: + Tensor result = ((t1 + t2).eval() * 0.2f).exp(); + +Semantically, calling ```eval()``` is equivalent to materializing the value of +the expression in a temporary Tensor of the right size. The code above in +effect does: + + // .eval() knows the size! + TensorFixedSize tmp = t1 + t2; + Tensor result = (tmp * 0.2f).exp(); + +Note that the return value of ```eval()``` is itself an Operation, so the +following code does not do what you may think: + + // Here t3 is an evaluation Operation. t3 has not been evaluated yet. + auto t3 = (t1 + t2).eval(); + + // You can use t3 in another expression. Still no evaluation. + auto t4 = (t3 * 0.2f).exp(); + + // The value is evaluated when you assign the Operation to a Tensor, using + // an intermediate tensor to represent t3.x + Tensor result = t4; + +While in the examples above calling ```eval()``` does not make a difference in +performance, in other cases it can make a huge difference. In the expression +below the ```broadcast()``` expression causes the ```X.maximum()``` expression +to be evaluated many times: + + Tensor<...> X ...; + Tensor<...> Y = ((X - X.maximum(depth_dim).reshape(dims2d).broadcast(bcast)) + * beta).exp(); + +Inserting a call to ```eval()``` between the ```maximum()``` and +```reshape()``` calls guarantees that maximum() is only computed once and +greatly speeds-up execution: + + Tensor<...> Y = + ((X - X.maximum(depth_dim).eval().reshape(dims2d).broadcast(bcast)) + * beta).exp(); + +In the other example below, the tensor ```Y``` is both used in the expression +and its assignment. This is an aliasing problem and if the evaluation is not +done in the right order Y will be updated incrementally during the evaluation +resulting in bogus results: + + Tensor<...> Y ...; + Y = Y / (Y.sum(depth_dim).reshape(dims2d).broadcast(bcast)); + +Inserting a call to ```eval()``` between the ```sum()``` and ```reshape()``` +expressions ensures that the sum is computed before any updates to ```Y``` are +done. + + Y = Y / (Y.sum(depth_dim).eval().reshape(dims2d).broadcast(bcast)); + +Note that an eval around the full right hand side expression is not needed +because the generated has to compute the i-th value of the right hand side +before assigning it to the left hand side. + +However, if you were assigning the expression value to a shuffle of ```Y``` +then you would need to force an eval for correctness by adding an ```eval()``` +call for the right hand side: + + Y.shuffle(...) = + (Y / (Y.sum(depth_dim).eval().reshape(dims2d).broadcast(bcast))).eval(); + + +#### Assigning to a TensorRef. + +If you need to access only a few elements from the value of an expression you +can avoid materializing the value in a full tensor by using a TensorRef. + +A TensorRef is a small wrapper class for any Eigen Operation. It provides +overloads for the ```()``` operator that let you access individual values in +the expression. TensorRef is convenient, because the Operation themselves do +not provide a way to access individual elements. + + // Create a TensorRef for the expression. The expression is not + // evaluated yet. + TensorRef > ref = ((t1 + t2) * 0.2f).exp(); + + // Use "ref" to access individual elements. The expression is evaluated + // on the fly. + float at_0 = ref(0, 0, 0); + cout << ref(0, 1, 0); + +Only use TensorRef when you need a subset of the values of the expression. +TensorRef only computes the values you access. However note that if you are +going to access all the values it will be much faster to materialize the +results in a Tensor first. + +In some cases, if the full Tensor result would be very large, you may save +memory by accessing it as a TensorRef. But not always. So don't count on it. + + +### Controlling How Expressions Are Evaluated + +The tensor library provides several implementations of the various operations +such as contractions and convolutions. The implementations are optimized for +different environments: single threaded on CPU, multi threaded on CPU, or on a +GPU using cuda. Additional implementations may be added later. + +You can choose which implementation to use with the ```device()``` call. If +you do not choose an implementation explicitly the default implementation that +uses a single thread on the CPU is used. + +The default implementation has been optimized for recent Intel CPUs, taking +advantage of SSE, AVX, and FMA instructions. Work is ongoing to tune the +library on ARM CPUs. Note that you need to pass compiler-dependent flags +to enable the use of SSE, AVX, and other instructions. + +For example, the following code adds two tensors using the default +single-threaded CPU implementation: + + Tensor a(30, 40); + Tensor b(30, 40); + Tensor c = a + b; + +To choose a different implementation you have to insert a ```device()``` call +before the assignment of the result. For technical C++ reasons this requires +that the Tensor for the result be declared on its own. This means that you +have to know the size of the result. + + Eigen::Tensor c(30, 40); + c.device(...) = a + b; + +The call to ```device()``` must be the last call on the left of the operator=. + +You must pass to the ```device()``` call an Eigen device object. There are +presently three devices you can use: DefaultDevice, ThreadPoolDevice and +GpuDevice. + + +#### Evaluating With the DefaultDevice + +This is exactly the same as not inserting a ```device()``` call. + + DefaultDevice my_device; + c.device(my_device) = a + b; + +#### Evaluating with a Thread Pool + + // Create the Eigen ThreadPoolDevice. + Eigen::ThreadPoolDevice my_device(4 /* number of threads to use */); + + // Now just use the device when evaluating expressions. + Eigen::Tensor c(30, 50); + c.device(my_device) = a.contract(b, dot_product_dims); + + +#### Evaluating On GPU + +This is presently a bit more complicated than just using a thread pool device. +You need to create a GPU device but you also need to explicitly allocate the +memory for tensors with cuda. + + +## API Reference + +### Datatypes + +In the documentation of the tensor methods and Operation we mention datatypes +that are tensor-type specific: + +#### ::Dimensions + +Acts like an array of ints. Has an ```int size``` attribute, and can be +indexed like an array to access individual values. Used to represent the +dimensions of a tensor. See ```dimensions()```. + +#### ::Index + +Acts like an ```int```. Used for indexing tensors along their dimensions. See +```operator()```, ```dimension()```, and ```size()```. + +#### ::Scalar + +Represents the datatype of individual tensor elements. For example, for a +```Tensor```, ```Scalar``` is the type ```float```. See +```setConstant()```. + +#### + +We use this pseudo type to indicate that a tensor Operation is returned by a +method. We indicate in the text the type and dimensions of the tensor that the +Operation returns after evaluation. + +The Operation will have to be evaluated, for example by assigning it to a +tensor, before you can access the values of the resulting tensor. You can also +access the values through a TensorRef. + + +## Built-in Tensor Methods + +These are usual C++ methods that act on tensors immediately. They are not +Operations which provide delayed evaluation of their results. Unless specified +otherwise, all the methods listed below are available on all tensor classes: +Tensor, TensorFixedSize, and TensorMap. + +## Metadata + +### int NumDimensions + +Constant value indicating the number of dimensions of a Tensor. This is also +known as the tensor "rank". + + Eigen::Tensor a(3, 4); + cout << "Dims " << a.NumDimensions; + => Dims 2 + +### Dimensions dimensions() + +Returns an array-like object representing the dimensions of the tensor. +The actual type of the dimensions() result is ::Dimensions. + + Eigen::Tensor a(3, 4); + const Eigen::Tensor::Dimensions& d = a.dimensions(); + cout << "Dim size: " << d.size << ", dim 0: " << d[0] + << ", dim 1: " << d[1]; + => Dim size: 2, dim 0: 3, dim 1: 4 + +If you use a C++11 compiler, you can use ```auto``` to simplify the code: + + const auto& d = a.dimensions(); + cout << "Dim size: " << d.size << ", dim 0: " << d[0] + << ", dim 1: " << d[1]; + => Dim size: 2, dim 0: 3, dim 1: 4 + +### Index dimension(Index n) + +Returns the n-th dimension of the tensor. The actual type of the +```dimension()``` result is ```::Index```, but you can +always use it like an int. + + Eigen::Tensor a(3, 4); + int dim1 = a.dimension(1); + cout << "Dim 1: " << dim1; + => Dim 1: 4 + +### Index size() + +Returns the total number of elements in the tensor. This is the product of all +the tensor dimensions. The actual type of the ```size()``` result is +```::Index```, but you can always use it like an int. + + Eigen::Tensor a(3, 4); + cout << "Size: " << a.size(); + => Size: 12 + + +### Getting Dimensions From An Operation + +A few operations provide ```dimensions()``` directly, +e.g. ```TensorReslicingOp```. Most operations defer calculating dimensions +until the operation is being evaluated. If you need access to the dimensions +of a deferred operation, you can wrap it in a TensorRef (see Assigning to a +TensorRef above), which provides ```dimensions()``` and ```dimension()``` as +above. + +TensorRef can also wrap the plain Tensor types, so this is a useful idiom in +templated contexts where the underlying object could be either a raw Tensor +or some deferred operation (e.g. a slice of a Tensor). In this case, the +template code can wrap the object in a TensorRef and reason about its +dimensionality while remaining agnostic to the underlying type. + + +## Constructors + +### Tensor + +Creates a tensor of the specified size. The number of arguments must be equal +to the rank of the tensor. The content of the tensor is not initialized. + + Eigen::Tensor a(3, 4); + cout << "NumRows: " << a.dimension(0) << " NumCols: " << a.dimension(1) << endl; + => NumRows: 3 NumCols: 4 + +### TensorFixedSize + +Creates a tensor of the specified size. The number of arguments in the Size<> +template parameter determines the rank of the tensor. The content of the tensor +is not initialized. + + Eigen::TensorFixedSize> a; + cout << "Rank: " << a.rank() << endl; + => Rank: 2 + cout << "NumRows: " << a.dimension(0) << " NumCols: " << a.dimension(1) << endl; + => NumRows: 3 NumCols: 4 + +### TensorMap + +Creates a tensor mapping an existing array of data. The data must not be freed +until the TensorMap is discarded, and the size of the data must be large enough +to accomodate of the coefficients of the tensor. + + float data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; + Eigen::TensorMap a(data, 3, 4); + cout << "NumRows: " << a.dimension(0) << " NumCols: " << a.dimension(1) << endl; + => NumRows: 3 NumCols: 4 + cout << "a(1, 2): " << a(1, 2) << endl; + => a(1, 2): 9 + + +## Contents Initialization + +When a new Tensor or a new TensorFixedSize are created, memory is allocated to +hold all the tensor elements, but the memory is not initialized. Similarly, +when a new TensorMap is created on top of non-initialized memory the memory its +contents are not initialized. + +You can use one of the methods below to initialize the tensor memory. These +have an immediate effect on the tensor and return the tensor itself as a +result. These are not tensor Operations which delay evaluation. + +### setConstant(const Scalar& val) + +Sets all elements of the tensor to the constant value ```val```. ```Scalar``` +is the type of data stored in the tensor. You can pass any value that is +convertible to that type. + +Returns the tensor itself in case you want to chain another call. + + a.setConstant(12.3f); + cout << "Constant: " << endl << a << endl << endl; + => + Constant: + 12.3 12.3 12.3 12.3 + 12.3 12.3 12.3 12.3 + 12.3 12.3 12.3 12.3 + +Note that ```setConstant()``` can be used on any tensor where the element type +has a copy constructor and an ```operator=()```: + + Eigen::Tensor a(2, 3); + a.setConstant("yolo"); + cout << "String tensor: " << endl << a << endl << endl; + => + String tensor: + yolo yolo yolo + yolo yolo yolo + + +### setZero() + +Fills the tensor with zeros. Equivalent to ```setConstant(Scalar(0))```. +Returns the tensor itself in case you want to chain another call. + + a.setZero(); + cout << "Zeros: " << endl << a << endl << endl; + => + Zeros: + 0 0 0 0 + 0 0 0 0 + 0 0 0 0 + + +### setValues({..initializer_list}) + +Fills the tensor with explicit values specified in a std::initializer_list. +The type of the initializer list depends on the type and rank of the tensor. + +If the tensor has rank N, the initializer list must be nested N times. The +most deeply nested lists must contains P scalars of the Tensor type where P is +the size of the last dimension of the Tensor. + +For example, for a ```TensorFixedSize``` the initializer list must +contains 2 lists of 3 floats each. + +```setValues()``` returns the tensor itself in case you want to chain another +call. + + Eigen::Tensor a(2, 3); + a.setValues({{0.0f, 1.0f, 2.0f}, {3.0f, 4.0f, 5.0f}}); + cout << "a" << endl << a << endl << endl; + => + a + 0 1 2 + 3 4 5 + +If a list is too short, the corresponding elements of the tensor will not be +changed. This is valid at each level of nesting. For example the following +code only sets the values of the first row of the tensor. + + Eigen::Tensor a(2, 3); + a.setConstant(1000); + a.setValues({{10, 20, 30}}); + cout << "a" << endl << a << endl << endl; + => + a + 10 20 30 + 1000 1000 1000 + +### setRandom() + +Fills the tensor with random values. Returns the tensor itself in case you +want to chain another call. + + a.setRandom(); + cout << "Random: " << endl << a << endl << endl; + => + Random: + 0.680375 0.59688 -0.329554 0.10794 + -0.211234 0.823295 0.536459 -0.0452059 + 0.566198 -0.604897 -0.444451 0.257742 + +You can customize ```setRandom()``` by providing your own random number +generator as a template argument: + + a.setRandom(); + +Here, ```MyRandomGenerator``` must be a struct with the following member +functions, where Scalar and Index are the same as ```::Scalar``` +and ```::Index```. + +See ```struct UniformRandomGenerator``` in TensorFunctors.h for an example. + + // Custom number generator for use with setRandom(). + struct MyRandomGenerator { + // Default and copy constructors. Both are needed + MyRandomGenerator() { } + MyRandomGenerator(const MyRandomGenerator& ) { } + + // Return a random value to be used. "element_location" is the + // location of the entry to set in the tensor, it can typically + // be ignored. + Scalar operator()(Eigen::DenseIndex element_location, + Eigen::DenseIndex /*unused*/ = 0) const { + return ; + } + + // Same as above but generates several numbers at a time. + typename internal::packet_traits::type packetOp( + Eigen::DenseIndex packet_location, Eigen::DenseIndex /*unused*/ = 0) const { + return ; + } + }; + +You can also use one of the 2 random number generators that are part of the +tensor library: +* UniformRandomGenerator +* NormalRandomGenerator + + +## Data Access + +The Tensor, TensorFixedSize, and TensorRef classes provide the following +accessors to access the tensor coefficients: + + const Scalar& operator()(const array& indices) + const Scalar& operator()(Index firstIndex, IndexTypes... otherIndices) + Scalar& operator()(const array& indices) + Scalar& operator()(Index firstIndex, IndexTypes... otherIndices) + +The number of indices must be equal to the rank of the tensor. Moreover, these +accessors are not available on tensor expressions. In order to access the +values of a tensor expression, the expression must either be evaluated or +wrapped in a TensorRef. + + +### Scalar* data() and const Scalar* data() const + +Returns a pointer to the storage for the tensor. The pointer is const if the +tensor was const. This allows direct access to the data. The layout of the +data depends on the tensor layout: RowMajor or ColMajor. + +This access is usually only needed for special cases, for example when mixing +Eigen Tensor code with other libraries. + +Scalar is the type of data stored in the tensor. + + Eigen::Tensor a(3, 4); + float* a_data = a.data(); + a_data[0] = 123.45f; + cout << "a(0, 0): " << a(0, 0); + => a(0, 0): 123.45 + + +## Tensor Operations + +All the methods documented below return non evaluated tensor ```Operations```. +These can be chained: you can apply another Tensor Operation to the value +returned by the method. + +The chain of Operation is evaluated lazily, typically when it is assigned to a +tensor. See "Controlling when Expression are Evaluated" for more details about +their evaluation. + +### constant(const Scalar& val) + +Returns a tensor of the same type and dimensions as the original tensor but +where all elements have the value ```val```. + +This is useful, for example, when you want to add or subtract a constant from a +tensor, or multiply every element of a tensor by a scalar. + + Eigen::Tensor a(2, 3); + a.setConstant(1.0f); + Eigen::Tensor b = a + a.constant(2.0f); + Eigen::Tensor c = b * b.constant(0.2f); + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + cout << "c" << endl << c << endl << endl; + => + a + 1 1 1 + 1 1 1 + + b + 3 3 3 + 3 3 3 + + c + 0.6 0.6 0.6 + 0.6 0.6 0.6 + +### random() + +Returns a tensor of the same type and dimensions as the current tensor +but where all elements have random values. + +This is for example useful to add random values to an existing tensor. +The generation of random values can be customized in the same manner +as for ```setRandom()```. + + Eigen::Tensor a(2, 3); + a.setConstant(1.0f); + Eigen::Tensor b = a + a.random(); + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + => + a + 1 1 1 + 1 1 1 + + b + 1.68038 1.5662 1.82329 + 0.788766 1.59688 0.395103 + + +## Unary Element Wise Operations + +All these operations take a single input tensor as argument and return a tensor +of the same type and dimensions as the tensor to which they are applied. The +requested operations are applied to each element independently. + +### operator-() + +Returns a tensor of the same type and dimensions as the original tensor +containing the opposite values of the original tensor. + + Eigen::Tensor a(2, 3); + a.setConstant(1.0f); + Eigen::Tensor b = -a; + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + => + a + 1 1 1 + 1 1 1 + + b + -1 -1 -1 + -1 -1 -1 + +### sqrt() + +Returns a tensor of the same type and dimensions as the original tensor +containing the square roots of the original tensor. + +### rsqrt() + +Returns a tensor of the same type and dimensions as the original tensor +containing the inverse square roots of the original tensor. + +### square() + +Returns a tensor of the same type and dimensions as the original tensor +containing the squares of the original tensor values. + +### inverse() + +Returns a tensor of the same type and dimensions as the original tensor +containing the inverse of the original tensor values. + +### exp() + +Returns a tensor of the same type and dimensions as the original tensor +containing the exponential of the original tensor. + +### log() + +Returns a tensor of the same type and dimensions as the original tensor +containing the natural logarithms of the original tensor. + +### abs() + +Returns a tensor of the same type and dimensions as the original tensor +containing the absolute values of the original tensor. + +### pow(Scalar exponent) + +Returns a tensor of the same type and dimensions as the original tensor +containing the coefficients of the original tensor to the power of the +exponent. + +The type of the exponent, Scalar, is always the same as the type of the +tensor coefficients. For example, only integer exponents can be used in +conjuntion with tensors of integer values. + +You can use cast() to lift this restriction. For example this computes +cubic roots of an int Tensor: + + Eigen::Tensor a(2, 3); + a.setValues({{0, 1, 8}, {27, 64, 125}}); + Eigen::Tensor b = a.cast().pow(1.0 / 3.0); + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + => + a + 0 1 8 + 27 64 125 + + b + 0 1 2 + 3 4 5 + +### operator * (Scalar scale) + +Multiplies all the coefficients of the input tensor by the provided scale. + +### cwiseMax(Scalar threshold) +TODO + +### cwiseMin(Scalar threshold) +TODO + +### unaryExpr(const CustomUnaryOp& func) +TODO + + +## Binary Element Wise Operations + +These operations take two input tensors as arguments. The 2 input tensors should +be of the same type and dimensions. The result is a tensor of the same +dimensions as the tensors to which they are applied, and unless otherwise +specified it is also of the same type. The requested operations are applied to +each pair of elements independently. + +### operator+(const OtherDerived& other) + +Returns a tensor of the same type and dimensions as the input tensors +containing the coefficient wise sums of the inputs. + +### operator-(const OtherDerived& other) + +Returns a tensor of the same type and dimensions as the input tensors +containing the coefficient wise differences of the inputs. + +### operator*(const OtherDerived& other) + +Returns a tensor of the same type and dimensions as the input tensors +containing the coefficient wise products of the inputs. + +### operator/(const OtherDerived& other) + +Returns a tensor of the same type and dimensions as the input tensors +containing the coefficient wise quotients of the inputs. + +This operator is not supported for integer types. + +### cwiseMax(const OtherDerived& other) + +Returns a tensor of the same type and dimensions as the input tensors +containing the coefficient wise maximums of the inputs. + +### cwiseMin(const OtherDerived& other) + +Returns a tensor of the same type and dimensions as the input tensors +containing the coefficient wise mimimums of the inputs. + +### Logical operators + +The following logical operators are supported as well: + +* operator&&(const OtherDerived& other) +* operator||(const OtherDerived& other) +* operator<(const OtherDerived& other) +* operator<=(const OtherDerived& other) +* operator>(const OtherDerived& other) +* operator>=(const OtherDerived& other) +* operator==(const OtherDerived& other) +* operator!=(const OtherDerived& other) + +They all return a tensor of boolean values. + + +## Selection (select(const ThenDerived& thenTensor, const ElseDerived& elseTensor) + +Selection is a coefficient-wise ternary operator that is the tensor equivalent +to the if-then-else operation. + + Tensor if = ...; + Tensor then = ...; + Tensor else = ...; + Tensor result = if.select(then, else); + +The 3 arguments must be of the same dimensions, which will also be the dimension +of the result. The 'if' tensor must be of type boolean, the 'then' and the +'else' tensor must be of the same type, which will also be the type of the +result. + +Each coefficient in the result is equal to the corresponding coefficient in the +'then' tensor if the corresponding value in the 'if' tensor is true. If not, the +resulting coefficient will come from the 'else' tensor. + + +## Contraction + +Tensor *contractions* are a generalization of the matrix product to the +multidimensional case. + + // Create 2 matrices using tensors of rank 2 + Eigen::Tensor a(2, 3); + a.setValues({{1, 2, 3}, {6, 5, 4}}); + Eigen::Tensor b(3, 2); + a.setValues({{1, 2}, {4, 5}, {5, 6}}); + + // Compute the traditional matrix product + array, 1> product_dims = { IndexPair(1, 0) }; + Eigen::Tensor AB = a.contract(b, product_dims); + + // Compute the product of the transpose of the matrices + array, 1> transpose_product_dims = { IndexPair(0, 1) }; + Eigen::Tensor AtBt = a.contract(b, transposed_product_dims); + + +## Reduction Operations + +A *Reduction* operation returns a tensor with fewer dimensions than the +original tensor. The values in the returned tensor are computed by applying a +*reduction operator* to slices of values from the original tensor. You specify +the dimensions along which the slices are made. + +The Eigen Tensor library provides a set of predefined reduction operators such +as ```maximum()``` and ```sum()``` and lets you define additional operators by +implementing a few methods from a reductor template. + +### Reduction Dimensions + +All reduction operations take a single parameter of type +```::Dimensions``` which can always be specified as an array of +ints. These are called the "reduction dimensions." The values are the indices +of the dimensions of the input tensor over which the reduction is done. The +parameter can have at most as many element as the rank of the input tensor; +each element must be less than the tensor rank, as it indicates one of the +dimensions to reduce. + +Each dimension of the input tensor should occur at most once in the reduction +dimensions as the implementation does not remove duplicates. + +The order of the values in the reduction dimensions does not affect the +results, but the code may execute faster if you list the dimensions in +increasing order. + +Example: Reduction along one dimension. + + // Create a tensor of 2 dimensions + Eigen::Tensor a(2, 3); + a.setValues({{1, 2, 3}, {6, 5, 4}}); + // Reduce it along the second dimension (1)... + Eigen::array dims({1 /* dimension to reduce */}); + // ...using the "maximum" operator. + // The result is a tensor with one dimension. The size of + // that dimension is the same as the first (non-reduced) dimension of a. + Eigen::Tensor b = a.maximum(dims); + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + => + a + 1 2 3 + 6 5 4 + + b + 3 + 6 + +Example: Reduction along two dimensions. + + Eigen::Tensor a(2, 3, 4); + a.setValues({{{0.0f, 1.0f, 2.0f, 3.0f}, + {7.0f, 6.0f, 5.0f, 4.0f}, + {8.0f, 9.0f, 10.0f, 11.0f}}, + {{12.0f, 13.0f, 14.0f, 15.0f}, + {19.0f, 18.0f, 17.0f, 16.0f}, + {20.0f, 21.0f, 22.0f, 23.0f}}}); + // The tensor a has 3 dimensions. We reduce along the + // first 2, resulting in a tensor with a single dimension + // of size 4 (the last dimension of a.) + // Note that we pass the array of reduction dimensions + // directly to the maximum() call. + Eigen::Tensor b = + a.maximum(Eigen::array({0, 1})); + cout << "b" << endl << b << endl << endl; + => + b + 20 + 21 + 22 + 23 + +#### Reduction along all dimensions + +As a special case, if you pass no parameter to a reduction operation the +original tensor is reduced along *all* its dimensions. The result is a +scalar, represented as a zero-dimension tensor. + + Eigen::Tensor a(2, 3, 4); + a.setValues({{{0.0f, 1.0f, 2.0f, 3.0f}, + {7.0f, 6.0f, 5.0f, 4.0f}, + {8.0f, 9.0f, 10.0f, 11.0f}}, + {{12.0f, 13.0f, 14.0f, 15.0f}, + {19.0f, 18.0f, 17.0f, 16.0f}, + {20.0f, 21.0f, 22.0f, 23.0f}}}); + // Reduce along all dimensions using the sum() operator. + Eigen::Tensor b = a.sum(); + cout << "b" << endl << b << endl << endl; + => + b + 276 + + +### sum(const Dimensions& new_dims) +### sum() + +Reduce a tensor using the sum() operator. The resulting values +are the sum of the reduced values. + +### mean(const Dimensions& new_dims) +### mean() + +Reduce a tensor using the mean() operator. The resulting values +are the mean of the reduced values. + +### maximum(const Dimensions& new_dims) +### maximum() + +Reduce a tensor using the maximum() operator. The resulting values are the +largest of the reduced values. + +### minimum(const Dimensions& new_dims) +### minimum() + +Reduce a tensor using the minimum() operator. The resulting values +are the smallest of the reduced values. + +### prod(const Dimensions& new_dims) +### prod() + +Reduce a tensor using the prod() operator. The resulting values +are the product of the reduced values. + +### all(const Dimensions& new_dims) +### all() +Reduce a tensor using the all() operator. Casts tensor to bool and then checks +whether all elements are true. Runs through all elements rather than +short-circuiting, so may be significantly inefficient. + +### any(const Dimensions& new_dims) +### any() +Reduce a tensor using the any() operator. Casts tensor to bool and then checks +whether any element is true. Runs through all elements rather than +short-circuiting, so may be significantly inefficient. + + +### reduce(const Dimensions& new_dims, const Reducer& reducer) + +Reduce a tensor using a user-defined reduction operator. See ```SumReducer``` +in TensorFunctors.h for information on how to implement a reduction operator. + + +## Scan Operations + +A *Scan* operation returns a tensor with the same dimensions as the original +tensor. The operation performs an inclusive scan along the specified +axis, which means it computes a running total along the axis for a given +reduction operation. +If the reduction operation corresponds to summation, then this computes the +prefix sum of the tensor along the given axis. + +Example: +dd a comment to this line + + // Create a tensor of 2 dimensions + Eigen::Tensor a(2, 3); + a.setValues({{1, 2, 3}, {4, 5, 6}}); + // Scan it along the second dimension (1) using summation + Eigen::Tensor b = a.cumsum(1); + // The result is a tensor with the same size as the input + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + => + a + 1 2 3 + 6 5 4 + + b + 1 3 6 + 4 9 15 + +### cumsum(const Index& axis) + +Perform a scan by summing consecutive entries. + +### cumprod(const Index& axis) + +Perform a scan by multiplying consecutive entries. + + +## Convolutions + +### convolve(const Kernel& kernel, const Dimensions& dims) + +Returns a tensor that is the output of the convolution of the input tensor with the kernel, +along the specified dimensions of the input tensor. The dimension size for dimensions of the output tensor +which were part of the convolution will be reduced by the formula: +output_dim_size = input_dim_size - kernel_dim_size + 1 (requires: input_dim_size >= kernel_dim_size). +The dimension sizes for dimensions that were not part of the convolution will remain the same. +Performance of the convolution can depend on the length of the stride(s) of the input tensor dimension(s) along which the +convolution is computed (the first dimension has the shortest stride for ColMajor, whereas RowMajor's shortest stride is +for the last dimension). + + // Compute convolution along the second and third dimension. + Tensor input(3, 3, 7, 11); + Tensor kernel(2, 2); + Tensor output(3, 2, 6, 11); + input.setRandom(); + kernel.setRandom(); + + Eigen::array dims({1, 2}); // Specify second and third dimension for convolution. + output = input.convolve(kernel, dims); + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 2; ++j) { + for (int k = 0; k < 6; ++k) { + for (int l = 0; l < 11; ++l) { + const float result = output(i,j,k,l); + const float expected = input(i,j+0,k+0,l) * kernel(0,0) + + input(i,j+1,k+0,l) * kernel(1,0) + + input(i,j+0,k+1,l) * kernel(0,1) + + input(i,j+1,k+1,l) * kernel(1,1); + VERIFY_IS_APPROX(result, expected); + } + } + } + } + + +## Geometrical Operations + +These operations return a Tensor with different dimensions than the original +Tensor. They can be used to access slices of tensors, see them with different +dimensions, or pad tensors with additional data. + +### reshape(const Dimensions& new_dims) + +Returns a view of the input tensor that has been reshaped to the specified +new dimensions. The argument new_dims is an array of Index values. The +rank of the resulting tensor is equal to the number of elements in new_dims. + +The product of all the sizes in the new dimension array must be equal to +the number of elements in the input tensor. + + // Increase the rank of the input tensor by introducing a new dimension + // of size 1. + Tensor input(7, 11); + array three_dims{{7, 11, 1}}; + Tensor result = input.reshape(three_dims); + + // Decrease the rank of the input tensor by merging 2 dimensions; + array one_dim{{7 * 11}}; + Tensor result = input.reshape(one_dim); + +This operation does not move any data in the input tensor, so the resulting +contents of a reshaped Tensor depend on the data layout of the original Tensor. + +For example this is what happens when you ```reshape()``` a 2D ColMajor tensor +to one dimension: + + Eigen::Tensor a(2, 3); + a.setValues({{0.0f, 100.0f, 200.0f}, {300.0f, 400.0f, 500.0f}}); + Eigen::array one_dim({3 * 2}); + Eigen::Tensor b = a.reshape(one_dim); + cout << "b" << endl << b << endl; + => + b + 0 + 300 + 100 + 400 + 200 + 500 + +This is what happens when the 2D Tensor is RowMajor: + + Eigen::Tensor a(2, 3); + a.setValues({{0.0f, 100.0f, 200.0f}, {300.0f, 400.0f, 500.0f}}); + Eigen::array one_dim({3 * 2}); + Eigen::Tensor b = a.reshape(one_dim); + cout << "b" << endl << b << endl; + => + b + 0 + 100 + 200 + 300 + 400 + 500 + +The reshape operation is a lvalue. In other words, it can be used on the left +side of the assignment operator. + +The previous example can be rewritten as follow: + + Eigen::Tensor a(2, 3); + a.setValues({{0.0f, 100.0f, 200.0f}, {300.0f, 400.0f, 500.0f}}); + Eigen::array two_dim({2, 3}); + Eigen::Tensor b; + b.reshape(two_dim) = a; + cout << "b" << endl << b << endl; + => + b + 0 + 300 + 100 + 400 + 200 + 500 + +Note that "b" itself was not reshaped but that instead the assignment is done to +the reshape view of b. + + +### shuffle(const Shuffle& shuffle) + +Returns a copy of the input tensor whose dimensions have been +reordered according to the specified permutation. The argument shuffle +is an array of Index values. Its size is the rank of the input +tensor. It must contain a permutation of 0, 1, ..., rank - 1. The i-th +dimension of the output tensor equals to the size of the shuffle[i]-th +dimension of the input tensor. For example: + + // Shuffle all dimensions to the left by 1. + Tensor input(20, 30, 50); + // ... set some values in input. + Tensor output = input.shuffle({1, 2, 0}) + + eigen_assert(output.dimension(0) == 30); + eigen_assert(output.dimension(1) == 50); + eigen_assert(output.dimension(2) == 20); + +Indices into the output tensor are shuffled accordingly to formulate +indices into the input tensor. For example, one can assert in the above +code snippet that: + + eigen_assert(output(3, 7, 11) == input(11, 3, 7)); + +In general, one can assert that + + eigen_assert(output(..., indices[shuffle[i]], ...) == + input(..., indices[i], ...)) + +The shuffle operation results in a lvalue, which means that it can be assigned +to. In other words, it can be used on the left side of the assignment operator. + +Let's rewrite the previous example to take advantage of this feature: + + // Shuffle all dimensions to the left by 1. + Tensor input(20, 30, 50); + // ... set some values in input. + Tensor output(30, 50, 20); + output.shuffle({2, 0, 1}) = input; + + +### stride(const Strides& strides) + +Returns a view of the input tensor that strides (skips stride-1 +elements) along each of the dimensions. The argument strides is an +array of Index values. The dimensions of the resulting tensor are +ceil(input_dimensions[i] / strides[i]). + +For example this is what happens when you ```stride()``` a 2D tensor: + + Eigen::Tensor a(4, 3); + a.setValues({{0, 100, 200}, {300, 400, 500}, {600, 700, 800}, {900, 1000, 1100}}); + Eigen::array strides({3, 2}); + Eigen::Tensor b = a.stride(strides); + cout << "b" << endl << b << endl; + => + b + 0 200 + 900 1100 + +It is possible to assign a tensor to a stride: + Tensor input(20, 30, 50); + // ... set some values in input. + Tensor output(40, 90, 200); + output.stride({2, 3, 4}) = input; + + +### slice(const StartIndices& offsets, const Sizes& extents) + +Returns a sub-tensor of the given tensor. For each dimension i, the slice is +made of the coefficients stored between offset[i] and offset[i] + extents[i] in +the input tensor. + + Eigen::Tensor a(4, 3); + a.setValues({{0, 100, 200}, {300, 400, 500}, + {600, 700, 800}, {900, 1000, 1100}}); + Eigen::array offsets = {1, 0}; + Eigen::array extents = {2, 2}; + Eigen::Tensor slice = a.slice(offsets, extents); + cout << "a" << endl << a << endl; + => + a + 0 100 200 + 300 400 500 + 600 700 800 + 900 1000 1100 + cout << "slice" << endl << slice << endl; + => + slice + 300 400 + 600 700 + + +### chip(const Index offset, const Index dim) + +A chip is a special kind of slice. It is the subtensor at the given offset in +the dimension dim. The returned tensor has one fewer dimension than the input +tensor: the dimension dim is removed. + +For example, a matrix chip would be either a row or a column of the input +matrix. + + Eigen::Tensor a(4, 3); + a.setValues({{0, 100, 200}, {300, 400, 500}, + {600, 700, 800}, {900, 1000, 1100}}); + Eigen::Tensor row_3 = a.chip(2, 0); + Eigen::Tensor col_2 = a.chip(1, 1); + cout << "a" << endl << a << endl; + => + a + 0 100 200 + 300 400 500 + 600 700 800 + 900 1000 1100 + cout << "row_3" << endl << row_3 << endl; + => + row_3 + 600 700 800 + cout << "col_2" << endl << col_2 << endl; + => + col_2 + 100 400 700 1000 + +It is possible to assign values to a tensor chip since the chip operation is a +lvalue. For example: + + Eigen::Tensor a(3); + a.setValues({{100, 200, 300}}); + Eigen::Tensor b(2, 3); + b.setZero(); + b.chip(0, 0) = a; + cout << "a" << endl << a << endl; + => + a + 100 + 200 + 300 + cout << "b" << endl << b << endl; + => + b + 100 200 300 + 0 0 0 + + +### reverse(const ReverseDimensions& reverse) + +Returns a view of the input tensor that reverses the order of the coefficients +along a subset of the dimensions. The argument reverse is an array of boolean +values that indicates whether or not the order of the coefficients should be +reversed along each of the dimensions. This operation preserves the dimensions +of the input tensor. + +For example this is what happens when you ```reverse()``` the first dimension +of a 2D tensor: + + Eigen::Tensor a(4, 3); + a.setValues({{0, 100, 200}, {300, 400, 500}, + {600, 700, 800}, {900, 1000, 1100}}); + Eigen::array reverse({true, false}); + Eigen::Tensor b = a.reverse(reverse); + cout << "a" << endl << a << endl << "b" << endl << b << endl; + => + a + 0 100 200 + 300 400 500 + 600 700 800 + 900 1000 1100 + b + 900 1000 1100 + 600 700 800 + 300 400 500 + 0 100 200 + + +### broadcast(const Broadcast& broadcast) + +Returns a view of the input tensor in which the input is replicated one to many +times. +The broadcast argument specifies how many copies of the input tensor need to be +made in each of the dimensions. + + Eigen::Tensor a(2, 3); + a.setValues({{0, 100, 200}, {300, 400, 500}}); + Eigen::array bcast({3, 2}); + Eigen::Tensor b = a.broadcast(bcast); + cout << "a" << endl << a << endl << "b" << endl << b << endl; + => + a + 0 100 200 + 300 400 500 + b + 0 100 200 0 100 200 + 300 400 500 300 400 500 + 0 100 200 0 100 200 + 300 400 500 300 400 500 + 0 100 200 0 100 200 + 300 400 500 300 400 500 + +### concatenate(const OtherDerived& other, Axis axis) + +TODO + +### pad(const PaddingDimensions& padding) + +Returns a view of the input tensor in which the input is padded with zeros. + + Eigen::Tensor a(2, 3); + a.setValues({{0, 100, 200}, {300, 400, 500}}); + Eigen::array, 2> paddings; + paddings[0] = make_pair(0, 1); + paddings[1] = make_pair(2, 3); + Eigen::Tensor b = a.pad(paddings); + cout << "a" << endl << a << endl << "b" << endl << b << endl; + => + a + 0 100 200 + 300 400 500 + b + 0 0 0 0 + 0 0 0 0 + 0 100 200 0 + 300 400 500 0 + 0 0 0 0 + 0 0 0 0 + 0 0 0 0 + + +### extract_patches(const PatchDims& patch_dims) + +Returns a tensor of coefficient patches extracted from the input tensor, where +each patch is of dimension specified by 'patch_dims'. The returned tensor has +one greater dimension than the input tensor, which is used to index each patch. +The patch index in the output tensor depends on the data layout of the input +tensor: the patch index is the last dimension ColMajor layout, and the first +dimension in RowMajor layout. + +For example, given the following input tensor: + + Eigen::Tensor tensor(3,4); + tensor.setValues({{0.0f, 1.0f, 2.0f, 3.0f}, + {4.0f, 5.0f, 6.0f, 7.0f}, + {8.0f, 9.0f, 10.0f, 11.0f}}); + + cout << "tensor: " << endl << tensor << endl; +=> +tensor: + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + +Six 2x2 patches can be extracted and indexed using the following code: + + Eigen::Tensor patch; + Eigen::array patch_dims; + patch_dims[0] = 2; + patch_dims[1] = 2; + patch = tensor.extract_patches(patch_dims); + for (int k = 0; k < 6; ++k) { + cout << "patch index: " << k << endl; + for (int i = 0; i < 2; ++i) { + for (int j = 0; j < 2; ++j) { + if (DataLayout == ColMajor) { + cout << patch(i, j, k) << " "; + } else { + cout << patch(k, i, j) << " "; + } + } + cout << endl; + } + } + +This code results in the following output when the data layout is ColMajor: + +patch index: 0 +0 1 +4 5 +patch index: 1 +4 5 +8 9 +patch index: 2 +1 2 +5 6 +patch index: 3 +5 6 +9 10 +patch index: 4 +2 3 +6 7 +patch index: 5 +6 7 +10 11 + +This code results in the following output when the data layout is RowMajor: +(NOTE: the set of patches is the same as in ColMajor, but are indexed differently). + +patch index: 0 +0 1 +4 5 +patch index: 1 +1 2 +5 6 +patch index: 2 +2 3 +6 7 +patch index: 3 +4 5 +8 9 +patch index: 4 +5 6 +9 10 +patch index: 5 +6 7 +10 11 + +### extract_image_patches(const Index patch_rows, const Index patch_cols, + const Index row_stride, const Index col_stride, + const PaddingType padding_type) + +Returns a tensor of coefficient image patches extracted from the input tensor, +which is expected to have dimensions ordered as follows (depending on the data +layout of the input tensor, and the number of additional dimensions 'N'): + +*) ColMajor +1st dimension: channels (of size d) +2nd dimension: rows (of size r) +3rd dimension: columns (of size c) +4th-Nth dimension: time (for video) or batch (for bulk processing). + +*) RowMajor (reverse order of ColMajor) +1st-Nth dimension: time (for video) or batch (for bulk processing). +N+1'th dimension: columns (of size c) +N+2'th dimension: rows (of size r) +N+3'th dimension: channels (of size d) + +The returned tensor has one greater dimension than the input tensor, which is +used to index each patch. The patch index in the output tensor depends on the +data layout of the input tensor: the patch index is the 4'th dimension in +ColMajor layout, and the 4'th from the last dimension in RowMajor layout. + +For example, given the following input tensor with the following dimension +sizes: + *) depth: 2 + *) rows: 3 + *) columns: 5 + *) batch: 7 + + Tensor tensor(2,3,5,7); + Tensor tensor_row_major = tensor.swap_layout(); + +2x2 image patches can be extracted and indexed using the following code: + +*) 2D patch: ColMajor (patch indexed by second-to-last dimension) + Tensor twod_patch; + twod_patch = tensor.extract_image_patches<2, 2>(); + // twod_patch.dimension(0) == 2 + // twod_patch.dimension(1) == 2 + // twod_patch.dimension(2) == 2 + // twod_patch.dimension(3) == 3*5 + // twod_patch.dimension(4) == 7 + +*) 2D patch: RowMajor (patch indexed by the second dimension) + Tensor twod_patch_row_major; + twod_patch_row_major = tensor_row_major.extract_image_patches<2, 2>(); + // twod_patch_row_major.dimension(0) == 7 + // twod_patch_row_major.dimension(1) == 3*5 + // twod_patch_row_major.dimension(2) == 2 + // twod_patch_row_major.dimension(3) == 2 + // twod_patch_row_major.dimension(4) == 2 + +## Special Operations + +### cast() + +Returns a tensor of type T with the same dimensions as the original tensor. +The returned tensor contains the values of the original tensor converted to +type T. + + Eigen::Tensor a(2, 3); + Eigen::Tensor b = a.cast(); + +This can be useful for example if you need to do element-wise division of +Tensors of integers. This is not currently supported by the Tensor library +but you can easily cast the tensors to floats to do the division: + + Eigen::Tensor a(2, 3); + a.setValues({{0, 1, 2}, {3, 4, 5}}); + Eigen::Tensor b = + (a.cast() / a.constant(2).cast()).cast(); + cout << "a" << endl << a << endl << endl; + cout << "b" << endl << b << endl << endl; + => + a + 0 1 2 + 3 4 5 + + b + 0 0 1 + 1 2 2 + + +### eval() + +TODO + + +## Representation of scalar values + +Scalar values are often represented by tensors of size 1 and rank 1. It would be +more logical and user friendly to use tensors of rank 0 instead. For example +Tensor::maximum() currently returns a Tensor. Similarly, the inner +product of 2 1d tensors (through contractions) returns a 1d tensor. In the +future these operations might be updated to return 0d tensors instead. + +## Limitations + +* The number of tensor dimensions is currently limited to 250 when using a + compiler that supports cxx11. It is limited to only 5 for older compilers. +* The IndexList class requires a cxx11 compliant compiler. You can use an + array of indices instead if you don't have access to a modern compiler. +* On GPUs only floating point values are properly tested and optimized for. +* Complex and integer values are known to be broken on GPUs. If you try to use + them you'll most likely end up triggering a static assertion failure such as + EIGEN_STATIC_ASSERT(packetSize > 1, YOU_MADE_A_PROGRAMMING_MISTAKE) + + diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h new file mode 100644 index 0000000000..1940a9692f --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h @@ -0,0 +1,527 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// Copyright (C) 2013 Christian Seiler +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_H +#define EIGEN_CXX11_TENSOR_TENSOR_H + +namespace Eigen { + +/** \class Tensor + * \ingroup CXX11_Tensor_Module + * + * \brief The tensor class. + * + * The %Tensor class is the work-horse for all \em dense tensors within Eigen. + * + * The %Tensor class encompasses only dynamic-size objects so far. + * + * The first two template parameters are required: + * \tparam Scalar_ \anchor tensor_tparam_scalar Numeric type, e.g. float, double, int or std::complex. + * User defined scalar types are supported as well (see \ref user_defined_scalars "here"). + * \tparam NumIndices_ Number of indices (i.e. rank of the tensor) + * + * The remaining template parameters are optional -- in most cases you don't have to worry about them. + * \tparam Options_ \anchor tensor_tparam_options A combination of either \b #RowMajor or \b #ColMajor, and of either + * \b #AutoAlign or \b #DontAlign. + * The former controls \ref TopicStorageOrders "storage order", and defaults to column-major. The latter controls alignment, which is required + * for vectorization. It defaults to aligning tensors. Note that tensors currently do not support any operations that profit from vectorization. + * Support for such operations (i.e. adding two tensors etc.) is planned. + * + * You can access elements of tensors using normal subscripting: + * + * \code + * Eigen::Tensor t(10, 10, 10, 10); + * t(0, 1, 2, 3) = 42.0; + * \endcode + * + * This class can be extended with the help of the plugin mechanism described on the page + * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_TENSOR_PLUGIN. + * + * Some notes: + * + *

+ *
Relation to other parts of Eigen:
+ *
The midterm developement goal for this class is to have a similar hierarchy as Eigen uses for matrices, so that + * taking blocks or using tensors in expressions is easily possible, including an interface with the vector/matrix code + * by providing .asMatrix() and .asVector() (or similar) methods for rank 2 and 1 tensors. However, currently, the %Tensor + * class does not provide any of these features and is only available as a stand-alone class that just allows for + * coefficient access. Also, when fixed-size tensors are implemented, the number of template arguments is likely to + * change dramatically.
+ *
+ * + * \ref TopicStorageOrders + */ + +template +class Tensor : public TensorBase > +{ + public: + typedef Tensor Self; + typedef TensorBase > Base; + typedef typename Eigen::internal::nested::type Nested; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + typedef Scalar_ Scalar; + typedef typename NumTraits::Real RealScalar; + typedef typename Base::CoeffReturnType CoeffReturnType; + + enum { + IsAligned = bool(EIGEN_MAX_ALIGN_BYTES>0) & !(Options_&DontAlign), + Layout = Options_ & RowMajor ? RowMajor : ColMajor, + CoordAccess = true, + RawAccess = true + }; + + static const int Options = Options_; + static const int NumIndices = NumIndices_; + typedef DSizes Dimensions; + + protected: + TensorStorage m_storage; + +#ifdef EIGEN_HAS_SFINAE + template + struct isOfNormalIndex{ + static const bool is_array = internal::is_base_of, CustomIndices>::value; + static const bool is_int = NumTraits::IsInteger; + static const bool value = is_array | is_int; + }; +#endif + + public: + // Metadata + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rank() const { return NumIndices; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index dimension(std::size_t n) const { return m_storage.dimensions()[n]; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_storage.dimensions(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const { return m_storage.size(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar *data() { return m_storage.data(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar *data() const { return m_storage.data(); } + + // This makes EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + // work, because that uses base().coeffRef() - and we don't yet + // implement a similar class hierarchy + inline Self& base() { return *this; } + inline const Self& base() const { return *this; } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + EIGEN_DEVICE_FUNC inline const Scalar& coeff(Index firstIndex, Index secondIndex, IndexTypes... otherIndices) const + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return coeff(array{{firstIndex, secondIndex, otherIndices...}}); + } +#endif + + // normal indices + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(const array& indices) const + { + eigen_internal_assert(checkIndexRange(indices)); + return m_storage.data()[linearizedIndex(indices)]; + } + + // custom indices +#ifdef EIGEN_HAS_SFINAE + template::value) ) + > + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(CustomIndices& indices) const + { + return coeff(internal::customIndices2Array(indices)); + } +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff() const + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return m_storage.data()[0]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const + { + eigen_internal_assert(index >= 0 && index < size()); + return m_storage.data()[index]; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + inline Scalar& coeffRef(Index firstIndex, Index secondIndex, IndexTypes... otherIndices) + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return coeffRef(array{{firstIndex, secondIndex, otherIndices...}}); + } +#endif + + // normal indices + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(const array& indices) + { + eigen_internal_assert(checkIndexRange(indices)); + return m_storage.data()[linearizedIndex(indices)]; + } + + // custom indices +#ifdef EIGEN_HAS_SFINAE + template::value) ) + > + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(CustomIndices& indices) + { + return coeffRef(internal::customIndices2Array(indices)); + } +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef() + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return m_storage.data()[0]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) + { + eigen_internal_assert(index >= 0 && index < size()); + return m_storage.data()[index]; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + inline const Scalar& operator()(Index firstIndex, Index secondIndex, IndexTypes... otherIndices) const + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return this->operator()(array{{firstIndex, secondIndex, otherIndices...}}); + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1) const + { + return coeff(array(i0, i1)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2) const + { + return coeff(array(i0, i1, i2)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3) const + { + return coeff(array(i0, i1, i2, i3)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) const + { + return coeff(array(i0, i1, i2, i3, i4)); + } +#endif + + // custom indices +#ifdef EIGEN_HAS_SFINAE + template::value) ) + > + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& operator()(CustomIndices& indices) const + { + return coeff(internal::customIndices2Array(indices)); + } +#endif + + // normal indices + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& operator()(const array& indices) const + { + return coeff(indices); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& operator()(Index index) const + { + eigen_internal_assert(index >= 0 && index < size()); + return coeff(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& operator()() const + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return coeff(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& operator[](Index index) const + { + // The bracket operator is only for vectors, use the parenthesis operator instead. + EIGEN_STATIC_ASSERT(NumIndices == 1, YOU_MADE_A_PROGRAMMING_MISTAKE); + return coeff(index); + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + inline Scalar& operator()(Index firstIndex, Index secondIndex, IndexTypes... otherIndices) + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return operator()(array{{firstIndex, secondIndex, otherIndices...}}); + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1) + { + return coeffRef(array(i0, i1)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2) + { + return coeffRef(array(i0, i1, i2)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2, Index i3) + { + return coeffRef(array(i0, i1, i2, i3)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) + { + return coeffRef(array(i0, i1, i2, i3, i4)); + } +#endif + + // normal indices + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(const array& indices) + { + return coeffRef(indices); + } + + // custom indices +#ifdef EIGEN_HAS_SFINAE + template::value) ) + > + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(CustomIndices& indices) + { + return coeffRef(internal::customIndices2Array(indices)); + } +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(Index index) + { + eigen_assert(index >= 0 && index < size()); + return coeffRef(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()() + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return coeffRef(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator[](Index index) + { + // The bracket operator is only for vectors, use the parenthesis operator instead + EIGEN_STATIC_ASSERT(NumIndices == 1, YOU_MADE_A_PROGRAMMING_MISTAKE) + return coeffRef(index); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Tensor() + : m_storage() + { + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Tensor(const Self& other) + : m_storage(other.m_storage) + { + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index firstDimension, IndexTypes... otherDimensions) + : m_storage(firstDimension, otherDimensions...) + { + // The number of dimensions used to construct a tensor must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherDimensions) + 1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + } +#else + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Tensor(Index dim1) + : m_storage(dim1, array(dim1)) + { + EIGEN_STATIC_ASSERT(1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2) + : m_storage(dim1*dim2, array(dim1, dim2)) + { + EIGEN_STATIC_ASSERT(2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3) + : m_storage(dim1*dim2*dim3, array(dim1, dim2, dim3)) + { + EIGEN_STATIC_ASSERT(3 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3, Index dim4) + : m_storage(dim1*dim2*dim3*dim4, array(dim1, dim2, dim3, dim4)) + { + EIGEN_STATIC_ASSERT(4 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3, Index dim4, Index dim5) + : m_storage(dim1*dim2*dim3*dim4*dim5, array(dim1, dim2, dim3, dim4, dim5)) + { + EIGEN_STATIC_ASSERT(5 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + } +#endif + + /** Normal Dimension */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Tensor(const array& dimensions) + : m_storage(internal::array_prod(dimensions), dimensions) + { + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Tensor(const TensorBase& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other.derived()); + resize(TensorEvaluator(assign, DefaultDevice()).dimensions()); + internal::TensorExecutor::run(assign, DefaultDevice()); + } + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Tensor(const TensorBase& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other.derived()); + resize(TensorEvaluator(assign, DefaultDevice()).dimensions()); + internal::TensorExecutor::run(assign, DefaultDevice()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Tensor& operator=(const Tensor& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + resize(TensorEvaluator(assign, DefaultDevice()).dimensions()); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Tensor& operator=(const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + resize(TensorEvaluator(assign, DefaultDevice()).dimensions()); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template EIGEN_DEVICE_FUNC + void resize(Index firstDimension, IndexTypes... otherDimensions) + { + // The number of dimensions used to resize a tensor must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherDimensions) + 1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + resize(array{{firstDimension, otherDimensions...}}); + } +#endif + + /** Normal Dimension */ + EIGEN_DEVICE_FUNC void resize(const array& dimensions) + { + int i; + Index size = Index(1); + for (i = 0; i < NumIndices; i++) { + internal::check_rows_cols_for_overflow::run(size, dimensions[i]); + size *= dimensions[i]; + } + #ifdef EIGEN_INITIALIZE_COEFFS + bool size_changed = size != this->size(); + m_storage.resize(size, dimensions); + if(size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + #else + m_storage.resize(size, dimensions); + #endif + } + + // Why this overload, DSizes is derived from array ??? // + EIGEN_DEVICE_FUNC void resize(const DSizes& dimensions) { + array dims; + for (int i = 0; i < NumIndices; ++i) { + dims[i] = dimensions[i]; + } + resize(dims); + } + + EIGEN_DEVICE_FUNC + void resize() + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + // Nothing to do: rank 0 tensors have fixed size + } + + /** Custom Dimension */ +#ifdef EIGEN_HAS_SFINAE + template::value) ) + > + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(CustomDimension& dimensions) + { + resize(internal::customIndices2Array(dimensions)); + } +#endif + +#ifndef EIGEN_EMULATE_CXX11_META_H + template + EIGEN_DEVICE_FUNC + void resize(const Sizes& dimensions) { + array dims; + for (int i = 0; i < NumIndices; ++i) { + dims[i] = static_cast(dimensions[i]); + } + resize(dims); + } +#else + template + EIGEN_DEVICE_FUNC + void resize(const Sizes& dimensions) { + array dims; + for (int i = 0; i < NumIndices; ++i) { + dims[i] = static_cast(dimensions[i]); + } + resize(dims); + } +#endif + + protected: + + bool checkIndexRange(const array& indices) const + { + using internal::array_apply_and_reduce; + using internal::array_zip_and_reduce; + using internal::greater_equal_zero_op; + using internal::logical_and_op; + using internal::lesser_op; + + return + // check whether the indices are all >= 0 + array_apply_and_reduce(indices) && + // check whether the indices fit in the dimensions + array_zip_and_reduce(indices, m_storage.dimensions()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index linearizedIndex(const array& indices) const + { + if (Options&RowMajor) { + return m_storage.dimensions().IndexOfRowMajor(indices); + } else { + return m_storage.dimensions().IndexOfColMajor(indices); + } + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorArgMax.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorArgMax.h new file mode 100644 index 0000000000..d06f40cd86 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorArgMax.h @@ -0,0 +1,299 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Eugene Brevdo +// Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_ARG_MAX_H +#define EIGEN_CXX11_TENSOR_TENSOR_ARG_MAX_H + +namespace Eigen { +namespace internal { + +/** \class TensorIndexTuple + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor + Index Tuple class. + * + * + */ +template +struct traits > : public traits +{ + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef Tuple Scalar; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorIndexTupleOp& type; +}; + +template +struct nested, 1, + typename eval >::type> +{ + typedef TensorIndexTupleOp type; +}; + +} // end namespace internal + +template +class TensorIndexTupleOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + typedef Tuple CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorIndexTupleOp(const XprType& expr) + : m_xpr(expr) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; +}; + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorIndexTupleOp XprType; + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + + typedef typename TensorEvaluator::Dimensions Dimensions; + static const int NumDims = internal::array_size::value; + + enum { + IsAligned = /*TensorEvaluator::IsAligned*/ false, + PacketAccess = /*TensorEvaluator::PacketAccess*/ false, + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device) { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { + return m_impl.dimensions(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return CoeffReturnType(index, m_impl.coeff(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + return m_impl.costPerCoeff(vectorized) + TensorOpCost(0, 0, 1); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + protected: + TensorEvaluator m_impl; +}; + +namespace internal { + +/** \class TensorTupleIndex + * \ingroup CXX11_Tensor_Module + * + * \brief Converts to Tensor > and reduces to Tensor. + * + */ +template +struct traits > : public traits +{ + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef Index Scalar; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions - array_size::value; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorTupleReducerOp& type; +}; + +template +struct nested, 1, + typename eval >::type> +{ + typedef TensorTupleReducerOp type; +}; + +} // end namespace internal + +template +class TensorTupleReducerOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + typedef Index CoeffReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorTupleReducerOp(const XprType& expr, + const ReduceOp& reduce_op, + const int return_dim, + const Dims& reduce_dims) + : m_xpr(expr), m_reduce_op(reduce_op), m_return_dim(return_dim), m_reduce_dims(reduce_dims) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + const ReduceOp& reduce_op() const { return m_reduce_op; } + + EIGEN_DEVICE_FUNC + const Dims& reduce_dims() const { return m_reduce_dims; } + + EIGEN_DEVICE_FUNC + int return_dim() const { return m_return_dim; } + + protected: + typename XprType::Nested m_xpr; + const ReduceOp m_reduce_op; + const int m_return_dim; + const Dims m_reduce_dims; +}; + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorTupleReducerOp XprType; + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename TensorIndexTupleOp::CoeffReturnType TupleType; + typedef typename TensorEvaluator >, Device>::Dimensions Dimensions; + typedef typename TensorEvaluator , Device>::Dimensions InputDimensions; + static const int NumDims = internal::array_size::value; + typedef array StrideDims; + + enum { + IsAligned = /*TensorEvaluator::IsAligned*/ false, + PacketAccess = /*TensorEvaluator::PacketAccess*/ false, + BlockAccess = false, + Layout = TensorEvaluator >, Device>::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_orig_impl(op.expression(), device), + m_impl(op.expression().index_tuples().reduce(op.reduce_dims(), op.reduce_op()), device), + m_return_dim(op.return_dim()) { + + gen_strides(m_orig_impl.dimensions(), m_strides); + if (Layout == static_cast(ColMajor)) { + const Index total_size = internal::array_prod(m_orig_impl.dimensions()); + m_stride_mod = (m_return_dim < NumDims - 1) ? m_strides[m_return_dim + 1] : total_size; + } else { + const Index total_size = internal::array_prod(m_orig_impl.dimensions()); + m_stride_mod = (m_return_dim > 0) ? m_strides[m_return_dim - 1] : total_size; + } + m_stride_div = m_strides[m_return_dim]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { + return m_impl.dimensions(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + const TupleType v = m_impl.coeff(index); + return (m_return_dim < 0) ? v.first : (v.first % m_stride_mod) / m_stride_div; + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + const double compute_cost = 1.0 + + (m_return_dim < 0 ? 0.0 : (TensorOpCost::ModCost() + TensorOpCost::DivCost())); + return m_orig_impl.costPerCoeff(vectorized) + + m_impl.costPerCoeff(vectorized) + TensorOpCost(0, 0, compute_cost); + } + + private: + EIGEN_DEVICE_FUNC void gen_strides(const InputDimensions& dims, StrideDims& strides) { + if (m_return_dim < 0) { + return; // Won't be using the strides. + } + eigen_assert(m_return_dim < NumDims && + "Asking to convert index to a dimension outside of the rank"); + + // Calculate m_stride_div and m_stride_mod, which are used to + // calculate the value of an index w.r.t. the m_return_dim. + if (Layout == static_cast(ColMajor)) { + strides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + strides[i] = strides[i-1] * dims[i-1]; + } + } else { + strides[NumDims-1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + strides[i] = strides[i+1] * dims[i+1]; + } + } + } + + protected: + TensorEvaluator, Device> m_orig_impl; + TensorEvaluator >, Device> m_impl; + const int m_return_dim; + StrideDims m_strides; + Index m_stride_mod; + Index m_stride_div; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_ARG_MAX_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorAssign.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorAssign.h new file mode 100644 index 0000000000..166be200c5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorAssign.h @@ -0,0 +1,181 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_ASSIGN_H +#define EIGEN_CXX11_TENSOR_TENSOR_ASSIGN_H + +namespace Eigen { + +/** \class TensorAssign + * \ingroup CXX11_Tensor_Module + * + * \brief The tensor assignment class. + * + * This class is represents the assignment of the values resulting from the evaluation of + * the rhs expression to the memory locations denoted by the lhs expression. + */ +namespace internal { +template +struct traits > +{ + typedef typename LhsXprType::Scalar Scalar; + typedef typename traits::StorageKind StorageKind; + typedef typename promote_index_type::Index, + typename traits::Index>::type Index; + typedef typename LhsXprType::Nested LhsNested; + typedef typename RhsXprType::Nested RhsNested; + typedef typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _RhsNested; + static const std::size_t NumDimensions = internal::traits::NumDimensions; + static const int Layout = internal::traits::Layout; + + enum { + Flags = 0 + }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorAssignOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorAssignOp type; +}; + +} // end namespace internal + + + +template +class TensorAssignOp : public TensorBase > +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename LhsXprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorAssignOp(LhsXprType& lhs, const RhsXprType& rhs) + : m_lhs_xpr(lhs), m_rhs_xpr(rhs) {} + + /** \returns the nested expressions */ + EIGEN_DEVICE_FUNC + typename internal::remove_all::type& + lhsExpression() const { return *((typename internal::remove_all::type*)&m_lhs_xpr); } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + rhsExpression() const { return m_rhs_xpr; } + + protected: + typename internal::remove_all::type& m_lhs_xpr; + const typename internal::remove_all::type& m_rhs_xpr; +}; + + +template +struct TensorEvaluator, Device> +{ + typedef TensorAssignOp XprType; + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef typename TensorEvaluator::Dimensions Dimensions; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = TensorEvaluator::IsAligned & TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + RawAccess = TensorEvaluator::RawAccess + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) : + m_leftImpl(op.lhsExpression(), device), + m_rightImpl(op.rhsExpression(), device) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout)), YOU_MADE_A_PROGRAMMING_MISTAKE); + } + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const + { + // The dimensions of the lhs and the rhs tensors should be equal to prevent + // overflows and ensure the result is fully initialized. + // TODO: use left impl instead if right impl dimensions are known at compile time. + return m_rightImpl.dimensions(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar*) { + eigen_assert(dimensions_match(m_leftImpl.dimensions(), m_rightImpl.dimensions())); + m_leftImpl.evalSubExprsIfNeeded(NULL); + // If the lhs provides raw access to its storage area (i.e. if m_leftImpl.data() returns a non + // null value), attempt to evaluate the rhs expression in place. Returns true iff in place + // evaluation isn't supported and the caller still needs to manually assign the values generated + // by the rhs to the lhs. + return m_rightImpl.evalSubExprsIfNeeded(m_leftImpl.data()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_leftImpl.cleanup(); + m_rightImpl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalScalar(Index i) { + m_leftImpl.coeffRef(i) = m_rightImpl.coeff(i); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalPacket(Index i) { + const int LhsStoreMode = TensorEvaluator::IsAligned ? Aligned : Unaligned; + const int RhsLoadMode = TensorEvaluator::IsAligned ? Aligned : Unaligned; + m_leftImpl.template writePacket(i, m_rightImpl.template packet(i)); + } + EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index index) const + { + return m_leftImpl.coeff(index); + } + template + EIGEN_DEVICE_FUNC PacketReturnType packet(Index index) const + { + return m_leftImpl.template packet(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + // We assume that evalPacket or evalScalar is called to perform the + // assignment and account for the cost of the write here, but reduce left + // cost by one load because we are using m_leftImpl.coeffRef. + TensorOpCost left = m_leftImpl.costPerCoeff(vectorized); + return m_rightImpl.costPerCoeff(vectorized) + + TensorOpCost( + numext::maxi(0.0, left.bytes_loaded() - sizeof(CoeffReturnType)), + left.bytes_stored(), left.compute_cycles()) + + TensorOpCost(0, sizeof(CoeffReturnType), 0, vectorized, PacketSize); + } + + /// required by sycl in order to extract the accessor + const TensorEvaluator& left_impl() const { return m_leftImpl; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& right_impl() const { return m_rightImpl; } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return m_leftImpl.data(); } + + private: + TensorEvaluator m_leftImpl; + TensorEvaluator m_rightImpl; +}; + +} + + +#endif // EIGEN_CXX11_TENSOR_TENSOR_ASSIGN_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h new file mode 100644 index 0000000000..7a45a5cf48 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h @@ -0,0 +1,1010 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_BASE_H +#define EIGEN_CXX11_TENSOR_TENSOR_BASE_H + +// clang-format off + +namespace Eigen { + +/** \class TensorBase + * \ingroup CXX11_Tensor_Module + * + * \brief The tensor base class. + * + * This class is the common parent of the Tensor and TensorMap class, thus + * making it possible to use either class interchangably in expressions. + */ + +template +class TensorBase +{ + public: + typedef internal::traits DerivedTraits; + typedef typename DerivedTraits::Scalar Scalar; + typedef typename DerivedTraits::Index Index; + typedef typename internal::remove_const::type CoeffReturnType; + static const int NumDimensions = DerivedTraits::NumDimensions; + + // Generic nullary operation support. + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseNullaryOp + nullaryExpr(const CustomNullaryOp& func) const { + return TensorCwiseNullaryOp(derived(), func); + } + + // Coefficient-wise nullary operators + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseNullaryOp, const Derived> + constant(const Scalar& value) const { + return nullaryExpr(internal::scalar_constant_op(value)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseNullaryOp, const Derived> + random() const { + return nullaryExpr(internal::UniformRandomGenerator()); + } + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseNullaryOp + random(const RandomGenerator& gen = RandomGenerator()) const { + return nullaryExpr(gen); + } + + // Tensor generation + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorGeneratorOp + generate(const Generator& generator) const { + return TensorGeneratorOp(derived(), generator); + } + + // Generic unary operation support. + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp + unaryExpr(const CustomUnaryOp& func) const { + return TensorCwiseUnaryOp(derived(), func); + } + + // Coefficient-wise unary operators + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + operator-() const { + return unaryExpr(internal::scalar_opposite_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + sqrt() const { + return unaryExpr(internal::scalar_sqrt_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + sign() const { + return unaryExpr(internal::scalar_sign_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + rsqrt() const { + return unaryExpr(internal::scalar_rsqrt_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + square() const { + return unaryExpr(internal::scalar_square_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + cube() const { + return unaryExpr(internal::scalar_cube_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + inverse() const { + return unaryExpr(internal::scalar_inverse_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + tanh() const { + return unaryExpr(internal::scalar_tanh_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + lgamma() const { + return unaryExpr(internal::scalar_lgamma_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + digamma() const { + return unaryExpr(internal::scalar_digamma_op()); + } + + // igamma(a = this, x = other) + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + igamma(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_igamma_op()); + } + + // igammac(a = this, x = other) + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + igammac(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_igammac_op()); + } + + // zeta(x = this, q = other) + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + zeta(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_zeta_op()); + } + + // polygamma(n = this, x = other) + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + polygamma(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_polygamma_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + erf() const { + return unaryExpr(internal::scalar_erf_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + erfc() const { + return unaryExpr(internal::scalar_erfc_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + sigmoid() const { + return unaryExpr(internal::scalar_sigmoid_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + exp() const { + return unaryExpr(internal::scalar_exp_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + log() const { + return unaryExpr(internal::scalar_log_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + log1p() const { + return unaryExpr(internal::scalar_log1p_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + abs() const { + return unaryExpr(internal::scalar_abs_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + conjugate() const { + return unaryExpr(internal::scalar_conjugate_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp >, const Derived> + pow(Scalar exponent) const { + return unaryExpr(internal::bind2nd_op >(exponent)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + real() const { + return unaryExpr(internal::scalar_real_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + imag() const { + return unaryExpr(internal::scalar_imag_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp >, const Derived> + operator+ (Scalar rhs) const { + return unaryExpr(internal::bind2nd_op >(rhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE friend + const TensorCwiseUnaryOp >, const Derived> + operator+ (Scalar lhs, const Derived& rhs) { + return rhs.unaryExpr(internal::bind1st_op >(lhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp >, const Derived> + operator- (Scalar rhs) const { + EIGEN_STATIC_ASSERT((NumTraits::IsSigned || internal::is_same >::value), YOU_MADE_A_PROGRAMMING_MISTAKE); + return unaryExpr(internal::bind2nd_op >(rhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE friend + const TensorCwiseUnaryOp >, const Derived> + operator- (Scalar lhs, const Derived& rhs) { + return rhs.unaryExpr(internal::bind1st_op >(lhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp >, const Derived> + operator* (Scalar rhs) const { + return unaryExpr(internal::bind2nd_op >(rhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE friend + const TensorCwiseUnaryOp >, const Derived> + operator* (Scalar lhs, const Derived& rhs) { + return rhs.unaryExpr(internal::bind1st_op >(lhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp >, const Derived> + operator/ (Scalar rhs) const { + return unaryExpr(internal::bind2nd_op >(rhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE friend + const TensorCwiseUnaryOp >, const Derived> + operator/ (Scalar lhs, const Derived& rhs) { + return rhs.unaryExpr(internal::bind1st_op >(lhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + operator% (Scalar rhs) const { + EIGEN_STATIC_ASSERT(NumTraits::IsInteger, YOU_MADE_A_PROGRAMMING_MISTAKE_TRY_MOD); + return unaryExpr(internal::scalar_mod_op(rhs)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + cwiseMax(Scalar threshold) const { + return cwiseMax(constant(threshold)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + cwiseMin(Scalar threshold) const { + return cwiseMin(constant(threshold)); + } + + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorConversionOp + cast() const { + return TensorConversionOp(derived()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + round() const { + return unaryExpr(internal::scalar_round_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + ceil() const { + return unaryExpr(internal::scalar_ceil_op()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + floor() const { + return unaryExpr(internal::scalar_floor_op()); + } + + // Generic binary operation support. + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp + binaryExpr(const OtherDerived& other, const CustomBinaryOp& func) const { + return TensorCwiseBinaryOp(derived(), other, func); + } + + // Coefficient-wise binary operators. + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator+(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_sum_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator-(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_difference_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator*(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_product_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator/(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_quotient_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + cwiseMax(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_max_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + cwiseMin(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_min_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp + operator&&(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_boolean_and_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp + operator||(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_boolean_or_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp + operator^(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_boolean_xor_op()); + } + + // Comparisons and tests. + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator<(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_cmp_op()); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator<=(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_cmp_op()); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator>(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_cmp_op()); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator>=(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_cmp_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator==(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_cmp_op()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCwiseBinaryOp, const Derived, const OtherDerived> + operator!=(const OtherDerived& other) const { + return binaryExpr(other.derived(), internal::scalar_cmp_op()); + } + + // comparisons and tests for Scalars + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + operator<(Scalar threshold) const { + return operator<(constant(threshold)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + operator<=(Scalar threshold) const { + return operator<=(constant(threshold)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + operator>(Scalar threshold) const { + return operator>(constant(threshold)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + operator>=(Scalar threshold) const { + return operator>=(constant(threshold)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + operator==(Scalar threshold) const { + return operator==(constant(threshold)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseBinaryOp, const Derived, const TensorCwiseNullaryOp, const Derived> > + operator!=(Scalar threshold) const { + return operator!=(constant(threshold)); + } + + // Checks + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + (isnan)() const { + return unaryExpr(internal::scalar_isnan_op()); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + (isinf)() const { + return unaryExpr(internal::scalar_isinf_op()); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const TensorCwiseUnaryOp, const Derived> + (isfinite)() const { + return unaryExpr(internal::scalar_isfinite_op()); + } + + // Coefficient-wise ternary operators. + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorSelectOp + select(const ThenDerived& thenTensor, const ElseDerived& elseTensor) const { + return TensorSelectOp(derived(), thenTensor.derived(), elseTensor.derived()); + } + + // Contractions. + typedef Eigen::IndexPair DimensionPair; + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorContractionOp + contract(const OtherDerived& other, const Dimensions& dims) const { + return TensorContractionOp(derived(), other.derived(), dims); + } + + // Convolutions. + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorConvolutionOp + convolve(const KernelDerived& kernel, const Dimensions& dims) const { + return TensorConvolutionOp(derived(), kernel.derived(), dims); + } + + // Fourier transforms + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorFFTOp + fft(const FFT& fft) const { + return TensorFFTOp(derived(), fft); + } + + // Scan. + typedef TensorScanOp, const Derived> TensorScanSumOp; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorScanSumOp + cumsum(const Index& axis, bool exclusive = false) const { + return TensorScanSumOp(derived(), axis, exclusive); + } + + typedef TensorScanOp, const Derived> TensorScanProdOp; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorScanProdOp + cumprod(const Index& axis, bool exclusive = false) const { + return TensorScanProdOp(derived(), axis, exclusive); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorScanOp + scan(const Index& axis, const Reducer& reducer, bool exclusive = false) const { + return TensorScanOp(derived(), axis, exclusive, reducer); + } + + // Reductions. + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const Dims, const Derived> + sum(const Dims& dims) const { + return TensorReductionOp, const Dims, const Derived>(derived(), dims, internal::SumReducer()); + } + + const TensorReductionOp, const DimensionList, const Derived> + sum() const { + DimensionList in_dims; + return TensorReductionOp, const DimensionList, const Derived>(derived(), in_dims, internal::SumReducer()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const Dims, const Derived> + mean(const Dims& dims) const { + return TensorReductionOp, const Dims, const Derived>(derived(), dims, internal::MeanReducer()); + } + + const TensorReductionOp, const DimensionList, const Derived> + mean() const { + DimensionList in_dims; + return TensorReductionOp, const DimensionList, const Derived>(derived(), in_dims, internal::MeanReducer()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const Dims, const Derived> + prod(const Dims& dims) const { + return TensorReductionOp, const Dims, const Derived>(derived(), dims, internal::ProdReducer()); + } + + const TensorReductionOp, const DimensionList, const Derived> + prod() const { + DimensionList in_dims; + return TensorReductionOp, const DimensionList, const Derived>(derived(), in_dims, internal::ProdReducer()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const Dims, const Derived> + maximum(const Dims& dims) const { + return TensorReductionOp, const Dims, const Derived>(derived(), dims, internal::MaxReducer()); + } + + const TensorReductionOp, const DimensionList, const Derived> + maximum() const { + DimensionList in_dims; + return TensorReductionOp, const DimensionList, const Derived>(derived(), in_dims, internal::MaxReducer()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const Dims, const Derived> + minimum(const Dims& dims) const { + return TensorReductionOp, const Dims, const Derived>(derived(), dims, internal::MinReducer()); + } + + const TensorReductionOp, const DimensionList, const Derived> + minimum() const { + DimensionList in_dims; + return TensorReductionOp, const DimensionList, const Derived>(derived(), in_dims, internal::MinReducer()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp > + all(const Dims& dims) const { + return cast().reduce(dims, internal::AndReducer()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const TensorConversionOp > + all() const { + DimensionList in_dims; + return cast().reduce(in_dims, internal::AndReducer()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp > + any(const Dims& dims) const { + return cast().reduce(dims, internal::OrReducer()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp, const TensorConversionOp > + any() const { + DimensionList in_dims; + return cast().reduce(in_dims, internal::OrReducer()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorTupleReducerOp< + internal::ArgMaxTupleReducer >, + const array, const Derived> + argmax() const { + array in_dims; + for (int d = 0; d < NumDimensions; ++d) in_dims[d] = d; + return TensorTupleReducerOp< + internal::ArgMaxTupleReducer >, + const array, + const Derived>(derived(), internal::ArgMaxTupleReducer >(), -1, in_dims); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorTupleReducerOp< + internal::ArgMinTupleReducer >, + const array, const Derived> + argmin() const { + array in_dims; + for (int d = 0; d < NumDimensions; ++d) in_dims[d] = d; + return TensorTupleReducerOp< + internal::ArgMinTupleReducer >, + const array, + const Derived>(derived(), internal::ArgMinTupleReducer >(), -1, in_dims); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorTupleReducerOp< + internal::ArgMaxTupleReducer >, + const array, const Derived> + argmax(const int return_dim) const { + array in_dims; + in_dims[0] = return_dim; + return TensorTupleReducerOp< + internal::ArgMaxTupleReducer >, + const array, + const Derived>(derived(), internal::ArgMaxTupleReducer >(), return_dim, in_dims); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorTupleReducerOp< + internal::ArgMinTupleReducer >, + const array, const Derived> + argmin(const int return_dim) const { + array in_dims; + in_dims[0] = return_dim; + return TensorTupleReducerOp< + internal::ArgMinTupleReducer >, + const array, + const Derived>(derived(), internal::ArgMinTupleReducer >(), return_dim, in_dims); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReductionOp + reduce(const Dims& dims, const Reducer& reducer) const { + return TensorReductionOp(derived(), dims, reducer); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorBroadcastingOp + broadcast(const Broadcast& broadcast) const { + return TensorBroadcastingOp(derived(), broadcast); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorConcatenationOp + concatenate(const OtherDerived& other, Axis axis) const { + return TensorConcatenationOp(derived(), other.derived(), axis); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorPatchOp + extract_patches(const PatchDims& patch_dims) const { + return TensorPatchOp(derived(), patch_dims); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorImagePatchOp + extract_image_patches(const Index patch_rows = 1, const Index patch_cols = 1, + const Index row_stride = 1, const Index col_stride = 1, + const Index in_row_stride = 1, const Index in_col_stride = 1, + const PaddingType padding_type = PADDING_SAME, const Scalar padding_value = Scalar(0)) const { + return TensorImagePatchOp(derived(), patch_rows, patch_cols, row_stride, col_stride, + in_row_stride, in_col_stride, 1, 1, padding_type, padding_value); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorImagePatchOp + extract_image_patches(const Index patch_rows, const Index patch_cols, + const Index row_stride, const Index col_stride, + const Index in_row_stride, const Index in_col_stride, + const Index row_inflate_stride, const Index col_inflate_stride, + const Index padding_top, const Index padding_bottom, + const Index padding_left,const Index padding_right, + const Scalar padding_value) const { + return TensorImagePatchOp(derived(), patch_rows, patch_cols, row_stride, col_stride, + in_row_stride, in_col_stride, row_inflate_stride, col_inflate_stride, + padding_top, padding_bottom, padding_left, padding_right, padding_value); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorVolumePatchOp + extract_volume_patches(const Index patch_planes, const Index patch_rows, const Index patch_cols, + const Index plane_stride = 1, const Index row_stride = 1, const Index col_stride = 1, + const PaddingType padding_type = PADDING_SAME, const Scalar padding_value = Scalar(0)) const { + return TensorVolumePatchOp(derived(), patch_planes, patch_rows, patch_cols, plane_stride, row_stride, col_stride, 1, 1, 1, 1, 1, 1, padding_type, padding_value); + } + + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorVolumePatchOp + extract_volume_patches(const Index patch_planes, const Index patch_rows, const Index patch_cols, + const Index plane_stride, const Index row_stride, const Index col_stride, + const Index plane_inflate_stride, const Index row_inflate_stride, const Index col_inflate_stride, + const Index padding_top_z, const Index padding_bottom_z, + const Index padding_top, const Index padding_bottom, + const Index padding_left, const Index padding_right, const Scalar padding_value = Scalar(0)) const { + return TensorVolumePatchOp(derived(), patch_planes, patch_rows, patch_cols, plane_stride, row_stride, col_stride, 1, 1, 1, plane_inflate_stride, row_inflate_stride, col_inflate_stride, padding_top_z, padding_bottom_z, padding_top, padding_bottom, padding_left, padding_right, padding_value); + } + + // Morphing operators. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorLayoutSwapOp + swap_layout() const { + return TensorLayoutSwapOp(derived()); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReshapingOp + reshape(const NewDimensions& newDimensions) const { + return TensorReshapingOp(derived(), newDimensions); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorSlicingOp + slice(const StartIndices& startIndices, const Sizes& sizes) const { + return TensorSlicingOp(derived(), startIndices, sizes); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorStridingSlicingOp + stridedSlice(const StartIndices& startIndices, const StopIndices& stopIndices, const Strides& strides) const { + return TensorStridingSlicingOp(derived(), startIndices, stopIndices, strides); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorChippingOp + chip(const Index offset) const { + return TensorChippingOp(derived(), offset, DimId); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorChippingOp + chip(const Index offset, const Index dim) const { + return TensorChippingOp(derived(), offset, dim); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReverseOp + reverse(const ReverseDimensions& rev) const { + return TensorReverseOp(derived(), rev); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorPaddingOp + pad(const PaddingDimensions& padding) const { + return TensorPaddingOp(derived(), padding, internal::scalar_cast_op()(0)); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorPaddingOp + pad(const PaddingDimensions& padding, const Scalar padding_value) const { + return TensorPaddingOp(derived(), padding, padding_value); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorShufflingOp + shuffle(const Shuffle& shuffle) const { + return TensorShufflingOp(derived(), shuffle); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorStridingOp + stride(const Strides& strides) const { + return TensorStridingOp(derived(), strides); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorInflationOp + inflate(const Strides& strides) const { + return TensorInflationOp(derived(), strides); + } + + // Returns a tensor containing index/value tuples + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorIndexTupleOp + index_tuples() const { + return TensorIndexTupleOp(derived()); + } + + // Support for custom unary and binary operations + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCustomUnaryOp customOp(const CustomUnaryFunc& op) const { + return TensorCustomUnaryOp(derived(), op); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorCustomBinaryOp customOp(const OtherDerived& other, const CustomBinaryFunc& op) const { + return TensorCustomBinaryOp(derived(), other, op); + } + + // Force the evaluation of the expression. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorForcedEvalOp eval() const { + return TensorForcedEvalOp(derived()); + } + + protected: + template friend class Tensor; + template friend class TensorFixedSize; + template friend class TensorBase; + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Derived& derived() const { return *static_cast(this); } +}; + +template::value> +class TensorBase : public TensorBase { + public: + typedef internal::traits DerivedTraits; + typedef typename DerivedTraits::Scalar Scalar; + typedef typename DerivedTraits::Index Index; + typedef Scalar CoeffReturnType; + static const int NumDimensions = DerivedTraits::NumDimensions; + + template friend class Tensor; + template friend class TensorFixedSize; + template friend class TensorBase; + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Derived& setZero() { + return setConstant(Scalar(0)); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Derived& setConstant(const Scalar& val) { + return derived() = this->constant(val); + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Derived& setRandom() { + return derived() = this->random(); + } + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Derived& setRandom() { + return derived() = this->template random(); + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Derived& setValues( + const typename internal::Initializer::InitList& vals) { + TensorEvaluator eval(derived(), DefaultDevice()); + internal::initialize_tensor(eval, vals); + return derived(); + } +#endif // EIGEN_HAS_VARIADIC_TEMPLATES + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator+=(const OtherDerived& other) { + return derived() = derived() + other.derived(); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator-=(const OtherDerived& other) { + return derived() = derived() - other.derived(); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator*=(const OtherDerived& other) { + return derived() = derived() * other.derived(); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Derived& operator/=(const OtherDerived& other) { + return derived() = derived() / other.derived(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorLayoutSwapOp + swap_layout() const { + return TensorLayoutSwapOp(derived()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorLayoutSwapOp + swap_layout() { + return TensorLayoutSwapOp(derived()); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorConcatenationOp + concatenate(const OtherDerived& other, const Axis& axis) const { + return TensorConcatenationOp(derived(), other, axis); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorConcatenationOp + concatenate(const OtherDerived& other, const Axis& axis) { + return TensorConcatenationOp(derived(), other, axis); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReshapingOp + reshape(const NewDimensions& newDimensions) const { + return TensorReshapingOp(derived(), newDimensions); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorReshapingOp + reshape(const NewDimensions& newDimensions) { + return TensorReshapingOp(derived(), newDimensions); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorSlicingOp + slice(const StartIndices& startIndices, const Sizes& sizes) const { + return TensorSlicingOp(derived(), startIndices, sizes); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorSlicingOp + slice(const StartIndices& startIndices, const Sizes& sizes) { + return TensorSlicingOp(derived(), startIndices, sizes); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorStridingSlicingOp + stridedSlice(const StartIndices& startIndices, const StopIndices& stopIndices, const Strides& strides) const { + return TensorStridingSlicingOp(derived(), startIndices, stopIndices, strides); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorStridingSlicingOp + stridedSlice(const StartIndices& startIndices, const StopIndices& stopIndices, const Strides& strides) { + return TensorStridingSlicingOp(derived(), startIndices, stopIndices, strides); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorChippingOp + chip(const Index offset) const { + return TensorChippingOp(derived(), offset, DimId); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorChippingOp + chip(const Index offset) { + return TensorChippingOp(derived(), offset, DimId); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorChippingOp + chip(const Index offset, const Index dim) const { + return TensorChippingOp(derived(), offset, dim); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorChippingOp + chip(const Index offset, const Index dim) { + return TensorChippingOp(derived(), offset, dim); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorReverseOp + reverse(const ReverseDimensions& rev) const { + return TensorReverseOp(derived(), rev); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorReverseOp + reverse(const ReverseDimensions& rev) { + return TensorReverseOp(derived(), rev); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorShufflingOp + shuffle(const Shuffle& shuffle) const { + return TensorShufflingOp(derived(), shuffle); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorShufflingOp + shuffle(const Shuffle& shuffle) { + return TensorShufflingOp(derived(), shuffle); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const TensorStridingOp + stride(const Strides& strides) const { + return TensorStridingOp(derived(), strides); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorStridingOp + stride(const Strides& strides) { + return TensorStridingOp(derived(), strides); + } + + // Select the device on which to evaluate the expression. + template + TensorDevice device(const DeviceType& device) { + return TensorDevice(device, derived()); + } + + protected: + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Derived& derived() { return *static_cast(this); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Derived& derived() const { return *static_cast(this); } +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_BASE_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBroadcasting.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBroadcasting.h new file mode 100644 index 0000000000..4cfe300eb4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBroadcasting.h @@ -0,0 +1,392 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_BROADCASTING_H +#define EIGEN_CXX11_TENSOR_TENSOR_BROADCASTING_H + +namespace Eigen { + +/** \class TensorBroadcasting + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor broadcasting class. + * + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorBroadcastingOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorBroadcastingOp type; +}; + +template +struct is_input_scalar { + static const bool value = false; +}; +template <> +struct is_input_scalar > { + static const bool value = true; +}; +#ifndef EIGEN_EMULATE_CXX11_META_H +template +struct is_input_scalar > { + static const bool value = (Sizes::total_size == 1); +}; +#endif + +} // end namespace internal + + + +template +class TensorBroadcastingOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorBroadcastingOp(const XprType& expr, const Broadcast& broadcast) + : m_xpr(expr), m_broadcast(broadcast) {} + + EIGEN_DEVICE_FUNC + const Broadcast& broadcast() const { return m_broadcast; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const Broadcast m_broadcast; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorBroadcastingOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::Dimensions>::value; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename TensorEvaluator::Dimensions InputDimensions; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = true, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_broadcast(op.broadcast()),m_impl(op.expression(), device) + { + // The broadcasting op doesn't change the rank of the tensor. One can't broadcast a scalar + // and store the result in a scalar. Instead one should reshape the scalar into a a N-D + // tensor with N >= 1 of 1 element first and then broadcast. + EIGEN_STATIC_ASSERT((NumDims > 0), YOU_MADE_A_PROGRAMMING_MISTAKE); + const InputDimensions& input_dims = m_impl.dimensions(); + const Broadcast& broadcast = op.broadcast(); + for (int i = 0; i < NumDims; ++i) { + eigen_assert(input_dims[i] > 0); + m_dimensions[i] = input_dims[i] * broadcast[i]; + } + + if (static_cast(Layout) == static_cast(ColMajor)) { + m_inputStrides[0] = 1; + m_outputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_inputStrides[i] = m_inputStrides[i-1] * input_dims[i-1]; + m_outputStrides[i] = m_outputStrides[i-1] * m_dimensions[i-1]; + } + } else { + m_inputStrides[NumDims-1] = 1; + m_outputStrides[NumDims-1] = 1; + for (int i = NumDims-2; i >= 0; --i) { + m_inputStrides[i] = m_inputStrides[i+1] * input_dims[i+1]; + m_outputStrides[i] = m_outputStrides[i+1] * m_dimensions[i+1]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE CoeffReturnType coeff(Index index) const + { + if (internal::is_input_scalar::type>::value) { + return m_impl.coeff(0); + } + + if (static_cast(Layout) == static_cast(ColMajor)) { + return coeffColMajor(index); + } else { + return coeffRowMajor(index); + } + } + + // TODO: attempt to speed this up. The integer divisions and modulo are slow + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffColMajor(Index index) const + { + Index inputIndex = 0; + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_outputStrides[i]; + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx < m_impl.dimensions()[i]); + inputIndex += idx * m_inputStrides[i]; + } else { + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx % m_impl.dimensions()[i] == 0); + } else { + inputIndex += (idx % m_impl.dimensions()[i]) * m_inputStrides[i]; + } + } + index -= idx * m_outputStrides[i]; + } + if (internal::index_statically_eq(0, 1)) { + eigen_assert(index < m_impl.dimensions()[0]); + inputIndex += index; + } else { + if (internal::index_statically_eq(0, 1)) { + eigen_assert(index % m_impl.dimensions()[0] == 0); + } else { + inputIndex += (index % m_impl.dimensions()[0]); + } + } + return m_impl.coeff(inputIndex); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeffRowMajor(Index index) const + { + Index inputIndex = 0; + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_outputStrides[i]; + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx < m_impl.dimensions()[i]); + inputIndex += idx * m_inputStrides[i]; + } else { + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx % m_impl.dimensions()[i] == 0); + } else { + inputIndex += (idx % m_impl.dimensions()[i]) * m_inputStrides[i]; + } + } + index -= idx * m_outputStrides[i]; + } + if (internal::index_statically_eq(NumDims-1, 1)) { + eigen_assert(index < m_impl.dimensions()[NumDims-1]); + inputIndex += index; + } else { + if (internal::index_statically_eq(NumDims-1, 1)) { + eigen_assert(index % m_impl.dimensions()[NumDims-1] == 0); + } else { + inputIndex += (index % m_impl.dimensions()[NumDims-1]); + } + } + return m_impl.coeff(inputIndex); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketReturnType packet(Index index) const + { + if (internal::is_input_scalar::type>::value) { + return internal::pset1(m_impl.coeff(0)); + } + + if (static_cast(Layout) == static_cast(ColMajor)) { + return packetColMajor(index); + } else { + return packetRowMajor(index); + } + } + + // Ignore the LoadMode and always use unaligned loads since we can't guarantee + // the alignment at compile time. + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packetColMajor(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + const Index originalIndex = index; + + Index inputIndex = 0; + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_outputStrides[i]; + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx < m_impl.dimensions()[i]); + inputIndex += idx * m_inputStrides[i]; + } else { + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx % m_impl.dimensions()[i] == 0); + } else { + inputIndex += (idx % m_impl.dimensions()[i]) * m_inputStrides[i]; + } + } + index -= idx * m_outputStrides[i]; + } + Index innermostLoc; + if (internal::index_statically_eq(0, 1)) { + eigen_assert(index < m_impl.dimensions()[0]); + innermostLoc = index; + } else { + if (internal::index_statically_eq(0, 1)) { + eigen_assert(index % m_impl.dimensions()[0] == 0); + innermostLoc = 0; + } else { + innermostLoc = index % m_impl.dimensions()[0]; + } + } + inputIndex += innermostLoc; + + // Todo: this could be extended to the second dimension if we're not + // broadcasting alongside the first dimension, and so on. + if (innermostLoc + PacketSize <= m_impl.dimensions()[0]) { + return m_impl.template packet(inputIndex); + } else { + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + values[0] = m_impl.coeff(inputIndex); + for (int i = 1; i < PacketSize; ++i) { + values[i] = coeffColMajor(originalIndex+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packetRowMajor(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + const Index originalIndex = index; + + Index inputIndex = 0; + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_outputStrides[i]; + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx < m_impl.dimensions()[i]); + inputIndex += idx * m_inputStrides[i]; + } else { + if (internal::index_statically_eq(i, 1)) { + eigen_assert(idx % m_impl.dimensions()[i] == 0); + } else { + inputIndex += (idx % m_impl.dimensions()[i]) * m_inputStrides[i]; + } + } + index -= idx * m_outputStrides[i]; + } + Index innermostLoc; + if (internal::index_statically_eq(NumDims-1, 1)) { + eigen_assert(index < m_impl.dimensions()[NumDims-1]); + innermostLoc = index; + } else { + if (internal::index_statically_eq(NumDims-1, 1)) { + eigen_assert(index % m_impl.dimensions()[NumDims-1] == 0); + innermostLoc = 0; + } else { + innermostLoc = index % m_impl.dimensions()[NumDims-1]; + } + } + inputIndex += innermostLoc; + + // Todo: this could be extended to the second dimension if we're not + // broadcasting alongside the first dimension, and so on. + if (innermostLoc + PacketSize <= m_impl.dimensions()[NumDims-1]) { + return m_impl.template packet(inputIndex); + } else { + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + values[0] = m_impl.coeff(inputIndex); + for (int i = 1; i < PacketSize; ++i) { + values[i] = coeffRowMajor(originalIndex+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + double compute_cost = TensorOpCost::AddCost(); + if (NumDims > 0) { + for (int i = NumDims - 1; i > 0; --i) { + compute_cost += TensorOpCost::DivCost(); + if (internal::index_statically_eq(i, 1)) { + compute_cost += + TensorOpCost::MulCost() + TensorOpCost::AddCost(); + } else { + if (!internal::index_statically_eq(i, 1)) { + compute_cost += TensorOpCost::MulCost() + + TensorOpCost::ModCost() + + TensorOpCost::AddCost(); + } + } + compute_cost += + TensorOpCost::MulCost() + TensorOpCost::AddCost(); + } + } + return m_impl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, compute_cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + const TensorEvaluator& impl() const { return m_impl; } + + Broadcast functor() const { return m_broadcast; } + + protected: + const Broadcast m_broadcast; + Dimensions m_dimensions; + array m_outputStrides; + array m_inputStrides; + TensorEvaluator m_impl; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_BROADCASTING_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorChipping.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorChipping.h new file mode 100644 index 0000000000..1ba7ef170c --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorChipping.h @@ -0,0 +1,384 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CHIPPING_H +#define EIGEN_CXX11_TENSOR_TENSOR_CHIPPING_H + +namespace Eigen { + +/** \class TensorKChippingReshaping + * \ingroup CXX11_Tensor_Module + * + * \brief A chip is a thin slice, corresponding to a column or a row in a 2-d tensor. + * + * + */ + +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions - 1; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorChippingOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorChippingOp type; +}; + +template +struct DimensionId +{ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DimensionId(DenseIndex dim) { + eigen_assert(dim == DimId); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex actualDim() const { + return DimId; + } +}; +template <> +struct DimensionId +{ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DimensionId(DenseIndex dim) : actual_dim(dim) { + eigen_assert(dim >= 0); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex actualDim() const { + return actual_dim; + } + private: + const DenseIndex actual_dim; +}; + + +} // end namespace internal + + + +template +class TensorChippingOp : public TensorBase > +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorChippingOp(const XprType& expr, const Index offset, const Index dim) + : m_xpr(expr), m_offset(offset), m_dim(dim) { + } + + EIGEN_DEVICE_FUNC + const Index offset() const { return m_offset; } + EIGEN_DEVICE_FUNC + const Index dim() const { return m_dim.actualDim(); } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorChippingOp& operator = (const TensorChippingOp& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorChippingOp& operator = (const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + protected: + typename XprType::Nested m_xpr; + const Index m_offset; + const internal::DimensionId m_dim; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorChippingOp XprType; + static const int NumInputDims = internal::array_size::Dimensions>::value; + static const int NumDims = NumInputDims-1; + typedef typename XprType::Index Index; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + + enum { + // Alignment can't be guaranteed at compile time since it depends on the + // slice offsets. + IsAligned = false, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_dim(op.dim()), m_device(device) + { + EIGEN_STATIC_ASSERT((NumInputDims >= 1), YOU_MADE_A_PROGRAMMING_MISTAKE); + eigen_assert(NumInputDims > m_dim.actualDim()); + + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + eigen_assert(op.offset() < input_dims[m_dim.actualDim()]); + + int j = 0; + for (int i = 0; i < NumInputDims; ++i) { + if (i != m_dim.actualDim()) { + m_dimensions[j] = input_dims[i]; + ++j; + } + } + + m_stride = 1; + m_inputStride = 1; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < m_dim.actualDim(); ++i) { + m_stride *= input_dims[i]; + m_inputStride *= input_dims[i]; + } + } else { + for (int i = NumInputDims-1; i > m_dim.actualDim(); --i) { + m_stride *= input_dims[i]; + m_inputStride *= input_dims[i]; + } + } + m_inputStride *= input_dims[m_dim.actualDim()]; + m_inputOffset = m_stride * op.offset(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_impl.coeff(srcCoeff(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + if ((static_cast(Layout) == static_cast(ColMajor) && m_dim.actualDim() == 0) || + (static_cast(Layout) == static_cast(RowMajor) && m_dim.actualDim() == NumInputDims-1)) { + // m_stride is equal to 1, so let's avoid the integer division. + eigen_assert(m_stride == 1); + Index inputIndex = index * m_inputStride + m_inputOffset; + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + for (int i = 0; i < PacketSize; ++i) { + values[i] = m_impl.coeff(inputIndex); + inputIndex += m_inputStride; + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } else if ((static_cast(Layout) == static_cast(ColMajor) && m_dim.actualDim() == NumInputDims - 1) || + (static_cast(Layout) == static_cast(RowMajor) && m_dim.actualDim() == 0)) { + // m_stride is aways greater than index, so let's avoid the integer division. + eigen_assert(m_stride > index); + return m_impl.template packet(index + m_inputOffset); + } else { + const Index idx = index / m_stride; + const Index rem = index - idx * m_stride; + if (rem + PacketSize <= m_stride) { + Index inputIndex = idx * m_inputStride + m_inputOffset + rem; + return m_impl.template packet(inputIndex); + } else { + // Cross the stride boundary. Fallback to slow path. + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + for (int i = 0; i < PacketSize; ++i) { + values[i] = coeff(index); + ++index; + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + double cost = 0; + if ((static_cast(Layout) == static_cast(ColMajor) && + m_dim.actualDim() == 0) || + (static_cast(Layout) == static_cast(RowMajor) && + m_dim.actualDim() == NumInputDims - 1)) { + cost += TensorOpCost::MulCost() + TensorOpCost::AddCost(); + } else if ((static_cast(Layout) == static_cast(ColMajor) && + m_dim.actualDim() == NumInputDims - 1) || + (static_cast(Layout) == static_cast(RowMajor) && + m_dim.actualDim() == 0)) { + cost += TensorOpCost::AddCost(); + } else { + cost += 3 * TensorOpCost::MulCost() + TensorOpCost::DivCost() + + 3 * TensorOpCost::AddCost(); + } + + return m_impl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType* data() const { + CoeffReturnType* result = const_cast(m_impl.data()); + if (((static_cast(Layout) == static_cast(ColMajor) && m_dim.actualDim() == NumDims) || + (static_cast(Layout) == static_cast(RowMajor) && m_dim.actualDim() == 0)) && + result) { + return result + m_inputOffset; + } else { + return NULL; + } + } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index srcCoeff(Index index) const + { + Index inputIndex; + if ((static_cast(Layout) == static_cast(ColMajor) && m_dim.actualDim() == 0) || + (static_cast(Layout) == static_cast(RowMajor) && m_dim.actualDim() == NumInputDims-1)) { + // m_stride is equal to 1, so let's avoid the integer division. + eigen_assert(m_stride == 1); + inputIndex = index * m_inputStride + m_inputOffset; + } else if ((static_cast(Layout) == static_cast(ColMajor) && m_dim.actualDim() == NumInputDims-1) || + (static_cast(Layout) == static_cast(RowMajor) && m_dim.actualDim() == 0)) { + // m_stride is aways greater than index, so let's avoid the integer division. + eigen_assert(m_stride > index); + inputIndex = index + m_inputOffset; + } else { + const Index idx = index / m_stride; + inputIndex = idx * m_inputStride + m_inputOffset; + index -= idx * m_stride; + inputIndex += index; + } + return inputIndex; + } + + Dimensions m_dimensions; + Index m_stride; + Index m_inputOffset; + Index m_inputStride; + TensorEvaluator m_impl; + const internal::DimensionId m_dim; + const Device& m_device; +}; + + +// Eval as lvalue +template +struct TensorEvaluator, Device> + : public TensorEvaluator, Device> +{ + typedef TensorEvaluator, Device> Base; + typedef TensorChippingOp XprType; + static const int NumInputDims = internal::array_size::Dimensions>::value; + static const int NumDims = NumInputDims-1; + typedef typename XprType::Index Index; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = TensorEvaluator::PacketAccess, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : Base(op, device) + { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType& coeffRef(Index index) + { + return this->m_impl.coeffRef(this->srcCoeff(index)); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketReturnType& x) + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + + if ((static_cast(this->Layout) == static_cast(ColMajor) && this->m_dim.actualDim() == 0) || + (static_cast(this->Layout) == static_cast(RowMajor) && this->m_dim.actualDim() == NumInputDims-1)) { + // m_stride is equal to 1, so let's avoid the integer division. + eigen_assert(this->m_stride == 1); + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + internal::pstore(values, x); + Index inputIndex = index * this->m_inputStride + this->m_inputOffset; + for (int i = 0; i < PacketSize; ++i) { + this->m_impl.coeffRef(inputIndex) = values[i]; + inputIndex += this->m_inputStride; + } + } else if ((static_cast(this->Layout) == static_cast(ColMajor) && this->m_dim.actualDim() == NumInputDims-1) || + (static_cast(this->Layout) == static_cast(RowMajor) && this->m_dim.actualDim() == 0)) { + // m_stride is aways greater than index, so let's avoid the integer division. + eigen_assert(this->m_stride > index); + this->m_impl.template writePacket(index + this->m_inputOffset, x); + } else { + const Index idx = index / this->m_stride; + const Index rem = index - idx * this->m_stride; + if (rem + PacketSize <= this->m_stride) { + const Index inputIndex = idx * this->m_inputStride + this->m_inputOffset + rem; + this->m_impl.template writePacket(inputIndex, x); + } else { + // Cross stride boundary. Fallback to slow path. + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + internal::pstore(values, x); + for (int i = 0; i < PacketSize; ++i) { + this->coeffRef(index) = values[i]; + ++index; + } + } + } + } +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CHIPPING_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConcatenation.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConcatenation.h new file mode 100644 index 0000000000..59bf90d939 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConcatenation.h @@ -0,0 +1,361 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONCATENATION_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONCATENATION_H + +namespace Eigen { + +/** \class TensorConcatenationOp + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor concatenation class. + * + * + */ +namespace internal { +template +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs are different. + typedef typename promote_storage_type::ret Scalar; + typedef typename promote_storage_type::StorageKind, + typename traits::StorageKind>::ret StorageKind; + typedef typename promote_index_type::Index, + typename traits::Index>::type Index; + typedef typename LhsXprType::Nested LhsNested; + typedef typename RhsXprType::Nested RhsNested; + typedef typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _RhsNested; + static const int NumDimensions = traits::NumDimensions; + static const int Layout = traits::Layout; + enum { Flags = 0 }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorConcatenationOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorConcatenationOp type; +}; + +} // end namespace internal + + +template +class TensorConcatenationOp : public TensorBase, WriteAccessors> +{ + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + typedef typename internal::nested::type Nested; + typedef typename internal::promote_storage_type::ret CoeffReturnType; + typedef typename NumTraits::Real RealScalar; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorConcatenationOp(const LhsXprType& lhs, const RhsXprType& rhs, Axis axis) + : m_lhs_xpr(lhs), m_rhs_xpr(rhs), m_axis(axis) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + lhsExpression() const { return m_lhs_xpr; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + rhsExpression() const { return m_rhs_xpr; } + + EIGEN_DEVICE_FUNC const Axis& axis() const { return m_axis; } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorConcatenationOp& operator = (const TensorConcatenationOp& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorConcatenationOp& operator = (const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + protected: + typename LhsXprType::Nested m_lhs_xpr; + typename RhsXprType::Nested m_rhs_xpr; + const Axis m_axis; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorConcatenationOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::Dimensions>::value; + static const int RightNumDims = internal::array_size::Dimensions>::value; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + enum { + IsAligned = false, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_leftImpl(op.lhsExpression(), device), m_rightImpl(op.rhsExpression(), device), m_axis(op.axis()) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout) || NumDims == 1), YOU_MADE_A_PROGRAMMING_MISTAKE); + EIGEN_STATIC_ASSERT((NumDims == RightNumDims), YOU_MADE_A_PROGRAMMING_MISTAKE); + EIGEN_STATIC_ASSERT((NumDims > 0), YOU_MADE_A_PROGRAMMING_MISTAKE); + + eigen_assert(0 <= m_axis && m_axis < NumDims); + const Dimensions& lhs_dims = m_leftImpl.dimensions(); + const Dimensions& rhs_dims = m_rightImpl.dimensions(); + { + int i = 0; + for (; i < m_axis; ++i) { + eigen_assert(lhs_dims[i] > 0); + eigen_assert(lhs_dims[i] == rhs_dims[i]); + m_dimensions[i] = lhs_dims[i]; + } + eigen_assert(lhs_dims[i] > 0); // Now i == m_axis. + eigen_assert(rhs_dims[i] > 0); + m_dimensions[i] = lhs_dims[i] + rhs_dims[i]; + for (++i; i < NumDims; ++i) { + eigen_assert(lhs_dims[i] > 0); + eigen_assert(lhs_dims[i] == rhs_dims[i]); + m_dimensions[i] = lhs_dims[i]; + } + } + + if (static_cast(Layout) == static_cast(ColMajor)) { + m_leftStrides[0] = 1; + m_rightStrides[0] = 1; + m_outputStrides[0] = 1; + + for (int j = 1; j < NumDims; ++j) { + m_leftStrides[j] = m_leftStrides[j-1] * lhs_dims[j-1]; + m_rightStrides[j] = m_rightStrides[j-1] * rhs_dims[j-1]; + m_outputStrides[j] = m_outputStrides[j-1] * m_dimensions[j-1]; + } + } else { + m_leftStrides[NumDims - 1] = 1; + m_rightStrides[NumDims - 1] = 1; + m_outputStrides[NumDims - 1] = 1; + + for (int j = NumDims - 2; j >= 0; --j) { + m_leftStrides[j] = m_leftStrides[j+1] * lhs_dims[j+1]; + m_rightStrides[j] = m_rightStrides[j+1] * rhs_dims[j+1]; + m_outputStrides[j] = m_outputStrides[j+1] * m_dimensions[j+1]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + // TODO(phli): Add short-circuit memcpy evaluation if underlying data are linear? + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) + { + m_leftImpl.evalSubExprsIfNeeded(NULL); + m_rightImpl.evalSubExprsIfNeeded(NULL); + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() + { + m_leftImpl.cleanup(); + m_rightImpl.cleanup(); + } + + // TODO(phli): attempt to speed this up. The integer divisions and modulo are slow. + // See CL/76180724 comments for more ideas. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + // Collect dimension-wise indices (subs). + array subs; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + subs[i] = index / m_outputStrides[i]; + index -= subs[i] * m_outputStrides[i]; + } + subs[0] = index; + } else { + for (int i = 0; i < NumDims - 1; ++i) { + subs[i] = index / m_outputStrides[i]; + index -= subs[i] * m_outputStrides[i]; + } + subs[NumDims - 1] = index; + } + + const Dimensions& left_dims = m_leftImpl.dimensions(); + if (subs[m_axis] < left_dims[m_axis]) { + Index left_index; + if (static_cast(Layout) == static_cast(ColMajor)) { + left_index = subs[0]; + for (int i = 1; i < NumDims; ++i) { + left_index += (subs[i] % left_dims[i]) * m_leftStrides[i]; + } + } else { + left_index = subs[NumDims - 1]; + for (int i = NumDims - 2; i >= 0; --i) { + left_index += (subs[i] % left_dims[i]) * m_leftStrides[i]; + } + } + return m_leftImpl.coeff(left_index); + } else { + subs[m_axis] -= left_dims[m_axis]; + const Dimensions& right_dims = m_rightImpl.dimensions(); + Index right_index; + if (static_cast(Layout) == static_cast(ColMajor)) { + right_index = subs[0]; + for (int i = 1; i < NumDims; ++i) { + right_index += (subs[i] % right_dims[i]) * m_rightStrides[i]; + } + } else { + right_index = subs[NumDims - 1]; + for (int i = NumDims - 2; i >= 0; --i) { + right_index += (subs[i] % right_dims[i]) * m_rightStrides[i]; + } + } + return m_rightImpl.coeff(right_index); + } + } + + // TODO(phli): Add a real vectorization. + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + const int packetSize = internal::unpacket_traits::size; + EIGEN_STATIC_ASSERT((packetSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index + packetSize - 1 < dimensions().TotalSize()); + + EIGEN_ALIGN_MAX CoeffReturnType values[packetSize]; + for (int i = 0; i < packetSize; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + const double compute_cost = NumDims * (2 * TensorOpCost::AddCost() + + 2 * TensorOpCost::MulCost() + + TensorOpCost::DivCost() + + TensorOpCost::ModCost()); + const double lhs_size = m_leftImpl.dimensions().TotalSize(); + const double rhs_size = m_rightImpl.dimensions().TotalSize(); + return (lhs_size / (lhs_size + rhs_size)) * + m_leftImpl.costPerCoeff(vectorized) + + (rhs_size / (lhs_size + rhs_size)) * + m_rightImpl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, compute_cost); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + protected: + Dimensions m_dimensions; + array m_outputStrides; + array m_leftStrides; + array m_rightStrides; + TensorEvaluator m_leftImpl; + TensorEvaluator m_rightImpl; + const Axis m_axis; +}; + +// Eval as lvalue +template + struct TensorEvaluator, Device> + : public TensorEvaluator, Device> +{ + typedef TensorEvaluator, Device> Base; + typedef TensorConcatenationOp XprType; + typedef typename Base::Dimensions Dimensions; + enum { + IsAligned = false, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(XprType& op, const Device& device) + : Base(op, device) + { + EIGEN_STATIC_ASSERT((static_cast(Layout) == static_cast(ColMajor)), YOU_MADE_A_PROGRAMMING_MISTAKE); + } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType& coeffRef(Index index) + { + // Collect dimension-wise indices (subs). + array subs; + for (int i = Base::NumDims - 1; i > 0; --i) { + subs[i] = index / this->m_outputStrides[i]; + index -= subs[i] * this->m_outputStrides[i]; + } + subs[0] = index; + + const Dimensions& left_dims = this->m_leftImpl.dimensions(); + if (subs[this->m_axis] < left_dims[this->m_axis]) { + Index left_index = subs[0]; + for (int i = 1; i < Base::NumDims; ++i) { + left_index += (subs[i] % left_dims[i]) * this->m_leftStrides[i]; + } + return this->m_leftImpl.coeffRef(left_index); + } else { + subs[this->m_axis] -= left_dims[this->m_axis]; + const Dimensions& right_dims = this->m_rightImpl.dimensions(); + Index right_index = subs[0]; + for (int i = 1; i < Base::NumDims; ++i) { + right_index += (subs[i] % right_dims[i]) * this->m_rightStrides[i]; + } + return this->m_rightImpl.coeffRef(right_index); + } + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketReturnType& x) + { + const int packetSize = internal::unpacket_traits::size; + EIGEN_STATIC_ASSERT((packetSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index + packetSize - 1 < this->dimensions().TotalSize()); + + EIGEN_ALIGN_MAX CoeffReturnType values[packetSize]; + internal::pstore(values, x); + for (int i = 0; i < packetSize; ++i) { + coeffRef(index+i) = values[i]; + } + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONCATENATION_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContraction.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContraction.h new file mode 100644 index 0000000000..20b29e5fde --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContraction.h @@ -0,0 +1,628 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_H + +namespace Eigen { + +/** \class TensorContraction + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor contraction class. + * + * + */ +namespace internal { + +template +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs are different. + typedef typename gebp_traits::type, + typename remove_const::type>::ResScalar Scalar; + + typedef typename promote_storage_type::StorageKind, + typename traits::StorageKind>::ret StorageKind; + typedef typename promote_index_type::Index, + typename traits::Index>::type Index; + typedef typename LhsXprType::Nested LhsNested; + typedef typename RhsXprType::Nested RhsNested; + typedef typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _RhsNested; + + // From NumDims below. + static const int NumDimensions = traits::NumDimensions + traits::NumDimensions - 2 * array_size::value; + static const int Layout = traits::Layout; + + enum { + Flags = 0 + }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorContractionOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorContractionOp type; +}; + +template +struct traits, Device_> > { + typedef Indices_ Indices; + typedef LeftArgType_ LeftArgType; + typedef RightArgType_ RightArgType; + typedef Device_ Device; + + // From NumDims below. + static const int NumDimensions = traits::NumDimensions + traits::NumDimensions - 2 * array_size::value; +}; + +} // end namespace internal + +template +class TensorContractionOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename internal::gebp_traits::ResScalar CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorContractionOp( + const LhsXprType& lhs, const RhsXprType& rhs, const Indices& dims) + : m_lhs_xpr(lhs), m_rhs_xpr(rhs), m_indices(dims) {} + + EIGEN_DEVICE_FUNC + const Indices& indices() const { return m_indices; } + + /** \returns the nested expressions */ + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + lhsExpression() const { return m_lhs_xpr; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + rhsExpression() const { return m_rhs_xpr; } + + protected: + typename LhsXprType::Nested m_lhs_xpr; + typename RhsXprType::Nested m_rhs_xpr; + const Indices m_indices; +}; + + +template +struct TensorContractionEvaluatorBase +{ + typedef typename internal::traits::Indices Indices; + typedef typename internal::traits::LeftArgType LeftArgType; + typedef typename internal::traits::RightArgType RightArgType; + typedef typename internal::traits::Device Device; + + typedef TensorContractionOp XprType; + typedef typename internal::remove_const::type Scalar; + typedef typename XprType::Index Index; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + enum { + IsAligned = true, + PacketAccess = (internal::unpacket_traits::size > 1), + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = true + }; + + // Most of the code is assuming that both input tensors are ColMajor. If the + // inputs are RowMajor, we will "cheat" by swapping the LHS and RHS: + // If we want to compute A * B = C, where A is LHS and B is RHS, the code + // will pretend B is LHS and A is RHS. + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), LeftArgType, RightArgType>::type EvalLeftArgType; + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), RightArgType, LeftArgType>::type EvalRightArgType; + + static const int LDims = + internal::array_size::Dimensions>::value; + static const int RDims = + internal::array_size::Dimensions>::value; + static const int ContractDims = internal::array_size::value; + static const int NumDims = LDims + RDims - 2 * ContractDims; + + typedef array contract_t; + typedef array left_nocontract_t; + typedef array right_nocontract_t; + + typedef DSizes Dimensions; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorContractionEvaluatorBase(const XprType& op, const Device& device) + : m_leftImpl(choose(Cond(Layout) == static_cast(ColMajor)>(), + op.lhsExpression(), op.rhsExpression()), device), + m_rightImpl(choose(Cond(Layout) == static_cast(ColMajor)>(), + op.rhsExpression(), op.lhsExpression()), device), + m_device(device), + m_result(NULL) { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == + static_cast(TensorEvaluator::Layout)), + YOU_MADE_A_PROGRAMMING_MISTAKE); + + + DSizes eval_left_dims; + DSizes eval_right_dims; + array, ContractDims> eval_op_indices; + if (static_cast(Layout) == static_cast(ColMajor)) { + // For ColMajor, we keep using the existing dimensions + for (int i = 0; i < LDims; i++) { + eval_left_dims[i] = m_leftImpl.dimensions()[i]; + } + for (int i = 0; i < RDims; i++) { + eval_right_dims[i] = m_rightImpl.dimensions()[i]; + } + // We keep the pairs of contracting indices. + for (int i = 0; i < ContractDims; i++) { + eval_op_indices[i].first = op.indices()[i].first; + eval_op_indices[i].second = op.indices()[i].second; + } + } else { + // For RowMajor, we need to reverse the existing dimensions + for (int i = 0; i < LDims; i++) { + eval_left_dims[i] = m_leftImpl.dimensions()[LDims - i - 1]; + } + for (int i = 0; i < RDims; i++) { + eval_right_dims[i] = m_rightImpl.dimensions()[RDims - i - 1]; + } + // We need to flip all the pairs of contracting indices as well as + // reversing the dimensions. + for (int i = 0; i < ContractDims; i++) { + eval_op_indices[i].first = LDims - 1 - op.indices()[ContractDims - 1 - i].second; + eval_op_indices[i].second = RDims - 1 - op.indices()[ContractDims - 1 - i].first; + } + } + + // Check for duplicate axes and make sure the first index in eval_op_indices + // is increasing. Using O(n^2) sorting is OK since ContractDims is small + for (int i = 0; i < ContractDims; i++) { + for (int j = i + 1; j < ContractDims; j++) { + eigen_assert(eval_op_indices[j].first != eval_op_indices[i].first && + eval_op_indices[j].second != eval_op_indices[i].second && + "contraction axes should be unique"); + if (eval_op_indices[j].first < eval_op_indices[i].first) { + numext::swap(eval_op_indices[j], eval_op_indices[i]); + } + } + } + + array lhs_strides; + lhs_strides[0] = 1; + for (int i = 0; i < LDims-1; ++i) { + lhs_strides[i+1] = lhs_strides[i] * eval_left_dims[i]; + } + + array rhs_strides; + rhs_strides[0] = 1; + for (int i = 0; i < RDims-1; ++i) { + rhs_strides[i+1] = rhs_strides[i] * eval_right_dims[i]; + } + + if (m_i_strides.size() > 0) m_i_strides[0] = 1; + if (m_j_strides.size() > 0) m_j_strides[0] = 1; + if (m_k_strides.size() > 0) m_k_strides[0] = 1; + + m_i_size = 1; + m_j_size = 1; + m_k_size = 1; + + // To compute the dimension, we simply concatenate the non-contracting + // dimensions of the left and then the right tensor. Additionally, we also + // compute the strides corresponding to the left non-contracting + // dimensions and right non-contracting dimensions. + m_lhs_inner_dim_contiguous = true; + int dim_idx = 0; + unsigned int nocontract_idx = 0; + + for (int i = 0; i < LDims; i++) { + // find if we are contracting on index i of left tensor + bool contracting = false; + for (int j = 0; j < ContractDims; j++) { + if (eval_op_indices[j].first == i) { + contracting = true; + break; + } + } + if (!contracting) { + // add dimension size to output dimensions + m_dimensions[dim_idx] = eval_left_dims[i]; + m_left_nocontract_strides[nocontract_idx] = lhs_strides[i]; + if (dim_idx != i) { + m_lhs_inner_dim_contiguous = false; + } + if (nocontract_idx+1 < internal::array_size::value) { + m_i_strides[nocontract_idx+1] = + m_i_strides[nocontract_idx] * eval_left_dims[i]; + } else { + m_i_size = m_i_strides[nocontract_idx] * eval_left_dims[i]; + } + dim_idx++; + nocontract_idx++; + } + } + + nocontract_idx = 0; + for (int i = 0; i < RDims; i++) { + bool contracting = false; + // find if we are contracting on index i of right tensor + for (int j = 0; j < ContractDims; j++) { + if (eval_op_indices[j].second == i) { + contracting = true; + break; + } + } + if (!contracting) { + m_dimensions[dim_idx] = eval_right_dims[i]; + if (nocontract_idx+1 < internal::array_size::value) { + m_j_strides[nocontract_idx+1] = + m_j_strides[nocontract_idx] * eval_right_dims[i]; + } else { + m_j_size = m_j_strides[nocontract_idx] * eval_right_dims[i]; + } + m_right_nocontract_strides[nocontract_idx] = rhs_strides[i]; + dim_idx++; + nocontract_idx++; + } + } + + // Now compute the strides corresponding to the contracting dimensions. We + // assumed above that non-contracting axes are represented in the same order + // in the matrix as they are in the tensor. This is not the case for + // contracting axes. As the contracting axes must be of the same size in + // each tensor, we'll only look at the first tensor here. + m_rhs_inner_dim_contiguous = true; + m_rhs_inner_dim_reordered = false; + for (int i = 0; i < ContractDims; i++) { + Index left = eval_op_indices[i].first; + Index right = eval_op_indices[i].second; + + Index size = eval_left_dims[left]; + eigen_assert(size == eval_right_dims[right] && + "Contraction axes must be same size"); + + if (i+1 < static_cast(internal::array_size::value)) { + m_k_strides[i+1] = m_k_strides[i] * size; + } else { + m_k_size = m_k_strides[i] * size; + } + m_left_contracting_strides[i] = lhs_strides[left]; + m_right_contracting_strides[i] = rhs_strides[right]; + + if (i > 0 && right < eval_op_indices[i-1].second) { + m_rhs_inner_dim_reordered = true; + } + if (right != i) { + m_rhs_inner_dim_contiguous = false; + } + } + + // If the layout is RowMajor, we need to reverse the m_dimensions + if (static_cast(Layout) == static_cast(RowMajor)) { + for (int i = 0, j = NumDims - 1; i < j; i++, j--) { + numext::swap(m_dimensions[i], m_dimensions[j]); + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* data) { + m_leftImpl.evalSubExprsIfNeeded(NULL); + m_rightImpl.evalSubExprsIfNeeded(NULL); + if (data) { + evalTo(data); + return false; + } else { + m_result = static_cast(m_device.allocate(dimensions().TotalSize() * sizeof(Scalar))); + evalTo(m_result); + return true; + } + } + + EIGEN_DEVICE_FUNC void evalTo(Scalar* buffer) const { + if (this->m_lhs_inner_dim_contiguous) { + if (this->m_rhs_inner_dim_contiguous) { + if (this->m_rhs_inner_dim_reordered) { + static_cast(this)->template evalProduct(buffer); + } + else { + static_cast(this)->template evalProduct(buffer); + } + } + else { + if (this->m_rhs_inner_dim_reordered) { + static_cast(this)->template evalProduct(buffer); + } + else { + static_cast(this)->template evalProduct(buffer); + } + } + } + else { + if (this->m_rhs_inner_dim_contiguous) { + if (this->m_rhs_inner_dim_reordered) { + static_cast(this)->template evalProduct(buffer); + } + else { + static_cast(this)->template evalProduct(buffer); + } + } + else { + if (this->m_rhs_inner_dim_reordered) { + static_cast(this)->template evalProduct(buffer); + } + else { + static_cast(this)->template evalProduct(buffer); + } + } + } + } + + template + EIGEN_DEVICE_FUNC void evalGemv(Scalar* buffer) const { + const Index rows = m_i_size; + const Index cols = m_k_size; + + typedef typename internal::remove_const::type LhsScalar; + typedef typename internal::remove_const::type RhsScalar; + typedef TensorEvaluator LeftEvaluator; + typedef TensorEvaluator RightEvaluator; + const Index lhs_packet_size = internal::unpacket_traits::size; + const Index rhs_packet_size = internal::unpacket_traits::size; + const int lhs_alignment = LeftEvaluator::IsAligned ? Aligned : Unaligned; + const int rhs_alignment = RightEvaluator::IsAligned ? Aligned : Unaligned; + typedef internal::TensorContractionInputMapper LhsMapper; + + typedef internal::TensorContractionInputMapper RhsMapper; + + LhsMapper lhs(m_leftImpl, m_left_nocontract_strides, m_i_strides, + m_left_contracting_strides, m_k_strides); + RhsMapper rhs(m_rightImpl, m_right_nocontract_strides, m_j_strides, + m_right_contracting_strides, m_k_strides); + + const Scalar alpha(1); + const Index resIncr(1); + + // zero out the result buffer (which must be of size at least rows * sizeof(Scalar) + m_device.memset(buffer, 0, rows * sizeof(Scalar)); + + internal::general_matrix_vector_product::run( + rows, cols, lhs, rhs, + buffer, resIncr, alpha); + } + + template + EIGEN_DEVICE_FUNC void evalGemm(Scalar* buffer) const { + // columns in left side, rows in right side + const Index k = this->m_k_size; + + // rows in left side + const Index m = this->m_i_size; + + // columns in right side + const Index n = this->m_j_size; + + // zero out the result buffer (which must be of size at least m * n * sizeof(Scalar) + this->m_device.memset(buffer, 0, m * n * sizeof(Scalar)); + + // define mr, nr, and all of my data mapper types + typedef typename internal::remove_const::type LhsScalar; + typedef typename internal::remove_const::type RhsScalar; + typedef typename internal::gebp_traits Traits; + + const Index nr = Traits::nr; + const Index mr = Traits::mr; + + typedef TensorEvaluator LeftEvaluator; + typedef TensorEvaluator RightEvaluator; + + const Index lhs_packet_size = internal::unpacket_traits::size; + const Index rhs_packet_size = internal::unpacket_traits::size; + + typedef internal::TensorContractionInputMapper LhsMapper; + + typedef internal::TensorContractionInputMapper RhsMapper; + + typedef internal::blas_data_mapper OutputMapper; + + // Declare GEBP packing and kernel structs + internal::gemm_pack_lhs pack_lhs; + internal::gemm_pack_rhs pack_rhs; + + internal::gebp_kernel gebp; + + // initialize data mappers + LhsMapper lhs(this->m_leftImpl, this->m_left_nocontract_strides, this->m_i_strides, + this->m_left_contracting_strides, this->m_k_strides); + + RhsMapper rhs(this->m_rightImpl, this->m_right_nocontract_strides, this->m_j_strides, + this->m_right_contracting_strides, this->m_k_strides); + + OutputMapper output(buffer, m); + + // Sizes of the blocks to load in cache. See the Goto paper for details. + internal::TensorContractionBlocking blocking(k, m, n, 1); + const Index kc = blocking.kc(); + const Index mc = numext::mini(m, blocking.mc()); + const Index nc = numext::mini(n, blocking.nc()); + const Index sizeA = mc * kc; + const Index sizeB = kc * nc; + + LhsScalar* blockA = static_cast(this->m_device.allocate(sizeA * sizeof(LhsScalar))); + RhsScalar* blockB = static_cast(this->m_device.allocate(sizeB * sizeof(RhsScalar))); + + for(Index i2=0; i2m_device.deallocate(blockA); + this->m_device.deallocate(blockB); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_leftImpl.cleanup(); + m_rightImpl.cleanup(); + + if (m_result != NULL) { + m_device.deallocate(m_result); + m_result = NULL; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_result[index]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool) const { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const { + return internal::ploadt(m_result + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar* data() const { return m_result; } + + protected: + // Prevent assignment + TensorContractionEvaluatorBase& operator = (const TensorContractionEvaluatorBase&); + Dimensions m_dimensions; + + contract_t m_k_strides; + contract_t m_left_contracting_strides; + contract_t m_right_contracting_strides; + + bool m_lhs_inner_dim_contiguous; + bool m_rhs_inner_dim_contiguous; + bool m_rhs_inner_dim_reordered; + + left_nocontract_t m_i_strides; + right_nocontract_t m_j_strides; + left_nocontract_t m_left_nocontract_strides; + right_nocontract_t m_right_nocontract_strides; + + Index m_i_size; + Index m_j_size; + Index m_k_size; + + TensorEvaluator m_leftImpl; + TensorEvaluator m_rightImpl; + const Device& m_device; + Scalar* m_result; +}; + + +// evaluator for default device +template +struct TensorEvaluator, Device> : + public TensorContractionEvaluatorBase< + TensorEvaluator, Device> > { + typedef TensorEvaluator, Device> Self; + typedef TensorContractionEvaluatorBase Base; + + typedef TensorContractionOp XprType; + typedef typename internal::remove_const::type Scalar; + typedef typename XprType::Index Index; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + enum { + Layout = TensorEvaluator::Layout + }; + + // Most of the code is assuming that both input tensors are ColMajor. If the + // inputs are RowMajor, we will "cheat" by swapping the LHS and RHS: + // If we want to compute A * B = C, where A is LHS and B is RHS, the code + // will pretend B is LHS and A is RHS. + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), LeftArgType, RightArgType>::type EvalLeftArgType; + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), RightArgType, LeftArgType>::type EvalRightArgType; + + static const int LDims = + internal::array_size::Dimensions>::value; + static const int RDims = + internal::array_size::Dimensions>::value; + static const int ContractDims = internal::array_size::value; + + typedef array contract_t; + typedef array left_nocontract_t; + typedef array right_nocontract_t; + + static const int NumDims = LDims + RDims - 2 * ContractDims; + + // Could we use NumDimensions here? + typedef DSizes Dimensions; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) : + Base(op, device) { } + + template + EIGEN_DEVICE_FUNC void evalProduct(Scalar* buffer) const { + if (this->m_j_size == 1) { + this->template evalGemv(buffer); + return; + } + + this->template evalGemm(buffer); + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionBlocking.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionBlocking.h new file mode 100644 index 0000000000..5cf7b4f718 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionBlocking.h @@ -0,0 +1,56 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H + + +namespace Eigen { +namespace internal { + +enum { + ShardByRow = 0, + ShardByCol = 1 +}; + + +// Default Blocking Strategy +template +class TensorContractionBlocking { + public: + + typedef typename LhsMapper::Scalar LhsScalar; + typedef typename RhsMapper::Scalar RhsScalar; + + EIGEN_DEVICE_FUNC TensorContractionBlocking(Index k, Index m, Index n, Index num_threads = 1) : + kc_(k), mc_(m), nc_(n) + { + if (ShardingType == ShardByCol) { + computeProductBlockingSizes(kc_, mc_, nc_, num_threads); + } + else { + computeProductBlockingSizes(kc_, nc_, mc_, num_threads); + } + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index kc() const { return kc_; } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index mc() const { return mc_; } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index nc() const { return nc_; } + + private: + Index kc_; + Index mc_; + Index nc_; +}; + + +} // end namespace internal +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionCuda.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionCuda.h new file mode 100644 index 0000000000..d65dbb40f1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionCuda.h @@ -0,0 +1,1391 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014-2015 Benoit Steiner +// Copyright (C) 2015 Navdeep Jaitly +// Copyright (C) 2014 Eric Martin +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_CUDA_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_CUDA_H + +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) + +namespace Eigen { + +template +__device__ EIGEN_STRONG_INLINE void +EigenContractionKernelInternal(const LhsMapper lhs, const RhsMapper rhs, + const OutputMapper output, Scalar* lhs_shmem, Scalar* rhs_shmem, + const Index m_size, const Index n_size, const Index k_size) { + + const Index m_block_idx = blockIdx.x; + const Index n_block_idx = blockIdx.y; + + const Index base_m = 64 * m_block_idx; + const Index base_n = 64 * n_block_idx; + + // declare and initialize 64 registers for output 8x8 block + + // prefetch registers + Scalar lhs_pf0; + Scalar lhs_pf1; + Scalar lhs_pf2; + Scalar lhs_pf3; + Scalar lhs_pf4; + Scalar lhs_pf5; + Scalar lhs_pf6; + Scalar lhs_pf7; + + Scalar rhs_pf0; + Scalar rhs_pf1; + Scalar rhs_pf2; + Scalar rhs_pf3; + Scalar rhs_pf4; + Scalar rhs_pf5; + Scalar rhs_pf6; + Scalar rhs_pf7; + + // shared memory is formatted + // (contract idx in block, nocontract idx in block, block idx) + // where block idx is column major. This transposition limits the number of + // bank conflicts when reading the LHS. The core idea is that since the contracting + // index is shared by both sides, then the contracting index should be in threadIdx.x. + + // On the LHS, we pad each row inside of each block with an extra element. This makes + // each block 8 rows of 9 elements, which is 72 elements. This gives no bank conflicts + // on writes and very few 2-way conflicts on reads. There is an 8x8 grid of these blocks. + + // On the RHS we just add 8 padding elements to the end of each block. This gives no bank + // conflicts on writes and also none on reads. + + // storage indices + const Index lhs_store_idx_base = threadIdx.y * 72 + threadIdx.x * 9 + threadIdx.z; + const Index rhs_store_idx_base = threadIdx.y * 72 + threadIdx.z * 8 + threadIdx.x; + + const Index lhs_store_idx_0 = lhs_store_idx_base + 576 * 0; + const Index lhs_store_idx_1 = lhs_store_idx_base + 576 * 1; + const Index lhs_store_idx_2 = lhs_store_idx_base + 576 * 2; + const Index lhs_store_idx_3 = lhs_store_idx_base + 576 * 3; + const Index lhs_store_idx_4 = lhs_store_idx_base + 576 * 4; + const Index lhs_store_idx_5 = lhs_store_idx_base + 576 * 5; + const Index lhs_store_idx_6 = lhs_store_idx_base + 576 * 6; + const Index lhs_store_idx_7 = lhs_store_idx_base + 576 * 7; + + const Index rhs_store_idx_0 = rhs_store_idx_base + 576 * 0; + const Index rhs_store_idx_1 = rhs_store_idx_base + 576 * 1; + const Index rhs_store_idx_2 = rhs_store_idx_base + 576 * 2; + const Index rhs_store_idx_3 = rhs_store_idx_base + 576 * 3; + const Index rhs_store_idx_4 = rhs_store_idx_base + 576 * 4; + const Index rhs_store_idx_5 = rhs_store_idx_base + 576 * 5; + const Index rhs_store_idx_6 = rhs_store_idx_base + 576 * 6; + const Index rhs_store_idx_7 = rhs_store_idx_base + 576 * 7; + + // in the loading code, the following variables are important: + // threadIdx.x: the vertical position in an 8x8 block + // threadIdx.y: the vertical index of the 8x8 block in the grid + // threadIdx.z: the horizontal position in an 8x8 block + // k: the horizontal index of the 8x8 block in the grid + // + // The k parameter is implicit (it was the loop counter for a loop that went + // from 0 to <8, but now that loop is unrolled in the below code. + + const Index load_idx_vert = threadIdx.x + 8 * threadIdx.y; + const Index lhs_vert = base_m + load_idx_vert; + +#define prefetchIntoRegisters(base_k) \ + { \ + lhs_pf0 = conv(0); \ + lhs_pf1 = conv(0); \ + lhs_pf2 = conv(0); \ + lhs_pf3 = conv(0); \ + lhs_pf4 = conv(0); \ + lhs_pf5 = conv(0); \ + lhs_pf6 = conv(0); \ + lhs_pf7 = conv(0); \ + \ + rhs_pf0 = conv(0); \ + rhs_pf1 = conv(0); \ + rhs_pf2 = conv(0); \ + rhs_pf3 = conv(0); \ + rhs_pf4 = conv(0); \ + rhs_pf5 = conv(0); \ + rhs_pf6 = conv(0); \ + rhs_pf7 = conv(0); \ + \ + if (!needs_edge_check || lhs_vert < m_size) { \ + const Index lhs_horiz_0 = base_k + threadIdx.z + 0 * 8; \ + const Index lhs_horiz_1 = base_k + threadIdx.z + 1 * 8; \ + const Index lhs_horiz_2 = base_k + threadIdx.z + 2 * 8; \ + const Index lhs_horiz_3 = base_k + threadIdx.z + 3 * 8; \ + const Index lhs_horiz_4 = base_k + threadIdx.z + 4 * 8; \ + const Index lhs_horiz_5 = base_k + threadIdx.z + 5 * 8; \ + const Index lhs_horiz_6 = base_k + threadIdx.z + 6 * 8; \ + const Index lhs_horiz_7 = base_k + threadIdx.z + 7 * 8; \ + \ + if (!needs_edge_check || lhs_horiz_7 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + lhs_pf2 = lhs(lhs_vert, lhs_horiz_2); \ + lhs_pf3 = lhs(lhs_vert, lhs_horiz_3); \ + lhs_pf4 = lhs(lhs_vert, lhs_horiz_4); \ + lhs_pf5 = lhs(lhs_vert, lhs_horiz_5); \ + lhs_pf6 = lhs(lhs_vert, lhs_horiz_6); \ + lhs_pf7 = lhs(lhs_vert, lhs_horiz_7); \ + } else if (lhs_horiz_6 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + lhs_pf2 = lhs(lhs_vert, lhs_horiz_2); \ + lhs_pf3 = lhs(lhs_vert, lhs_horiz_3); \ + lhs_pf4 = lhs(lhs_vert, lhs_horiz_4); \ + lhs_pf5 = lhs(lhs_vert, lhs_horiz_5); \ + lhs_pf6 = lhs(lhs_vert, lhs_horiz_6); \ + } else if (lhs_horiz_5 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + lhs_pf2 = lhs(lhs_vert, lhs_horiz_2); \ + lhs_pf3 = lhs(lhs_vert, lhs_horiz_3); \ + lhs_pf4 = lhs(lhs_vert, lhs_horiz_4); \ + lhs_pf5 = lhs(lhs_vert, lhs_horiz_5); \ + } else if (lhs_horiz_4 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + lhs_pf2 = lhs(lhs_vert, lhs_horiz_2); \ + lhs_pf3 = lhs(lhs_vert, lhs_horiz_3); \ + lhs_pf4 = lhs(lhs_vert, lhs_horiz_4); \ + } else if (lhs_horiz_3 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + lhs_pf2 = lhs(lhs_vert, lhs_horiz_2); \ + lhs_pf3 = lhs(lhs_vert, lhs_horiz_3); \ + } else if (lhs_horiz_2 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + lhs_pf2 = lhs(lhs_vert, lhs_horiz_2); \ + } else if (lhs_horiz_1 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + lhs_pf1 = lhs(lhs_vert, lhs_horiz_1); \ + } else if (lhs_horiz_0 < k_size) { \ + lhs_pf0 = lhs(lhs_vert, lhs_horiz_0); \ + } \ + } \ + \ + const Index rhs_vert = base_k + load_idx_vert; \ + if (!needs_edge_check || rhs_vert < k_size) { \ + const Index rhs_horiz_0 = base_n + threadIdx.z + 0 * 8; \ + const Index rhs_horiz_1 = base_n + threadIdx.z + 1 * 8; \ + const Index rhs_horiz_2 = base_n + threadIdx.z + 2 * 8; \ + const Index rhs_horiz_3 = base_n + threadIdx.z + 3 * 8; \ + const Index rhs_horiz_4 = base_n + threadIdx.z + 4 * 8; \ + const Index rhs_horiz_5 = base_n + threadIdx.z + 5 * 8; \ + const Index rhs_horiz_6 = base_n + threadIdx.z + 6 * 8; \ + const Index rhs_horiz_7 = base_n + threadIdx.z + 7 * 8; \ + \ + if (rhs_horiz_7 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + rhs_pf2 = rhs(rhs_vert, rhs_horiz_2); \ + rhs_pf3 = rhs(rhs_vert, rhs_horiz_3); \ + rhs_pf4 = rhs(rhs_vert, rhs_horiz_4); \ + rhs_pf5 = rhs(rhs_vert, rhs_horiz_5); \ + rhs_pf6 = rhs(rhs_vert, rhs_horiz_6); \ + rhs_pf7 = rhs(rhs_vert, rhs_horiz_7); \ + } else if (rhs_horiz_6 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + rhs_pf2 = rhs(rhs_vert, rhs_horiz_2); \ + rhs_pf3 = rhs(rhs_vert, rhs_horiz_3); \ + rhs_pf4 = rhs(rhs_vert, rhs_horiz_4); \ + rhs_pf5 = rhs(rhs_vert, rhs_horiz_5); \ + rhs_pf6 = rhs(rhs_vert, rhs_horiz_6); \ + } else if (rhs_horiz_5 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + rhs_pf2 = rhs(rhs_vert, rhs_horiz_2); \ + rhs_pf3 = rhs(rhs_vert, rhs_horiz_3); \ + rhs_pf4 = rhs(rhs_vert, rhs_horiz_4); \ + rhs_pf5 = rhs(rhs_vert, rhs_horiz_5); \ + } else if (rhs_horiz_4 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + rhs_pf2 = rhs(rhs_vert, rhs_horiz_2); \ + rhs_pf3 = rhs(rhs_vert, rhs_horiz_3); \ + rhs_pf4 = rhs(rhs_vert, rhs_horiz_4); \ + } else if (rhs_horiz_3 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + rhs_pf2 = rhs(rhs_vert, rhs_horiz_2); \ + rhs_pf3 = rhs(rhs_vert, rhs_horiz_3); \ + } else if (rhs_horiz_2 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + rhs_pf2 = rhs(rhs_vert, rhs_horiz_2); \ + } else if (rhs_horiz_1 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + rhs_pf1 = rhs(rhs_vert, rhs_horiz_1); \ + } else if (rhs_horiz_0 < n_size) { \ + rhs_pf0 = rhs(rhs_vert, rhs_horiz_0); \ + } \ + } \ + } \ + +#define writeRegToShmem(_) \ + lhs_shmem[lhs_store_idx_0] = lhs_pf0; \ + rhs_shmem[rhs_store_idx_0] = rhs_pf0; \ + \ + lhs_shmem[lhs_store_idx_1] = lhs_pf1; \ + rhs_shmem[rhs_store_idx_1] = rhs_pf1; \ + \ + lhs_shmem[lhs_store_idx_2] = lhs_pf2; \ + rhs_shmem[rhs_store_idx_2] = rhs_pf2; \ + \ + lhs_shmem[lhs_store_idx_3] = lhs_pf3; \ + rhs_shmem[rhs_store_idx_3] = rhs_pf3; \ + \ + lhs_shmem[lhs_store_idx_4] = lhs_pf4; \ + rhs_shmem[rhs_store_idx_4] = rhs_pf4; \ + \ + lhs_shmem[lhs_store_idx_5] = lhs_pf5; \ + rhs_shmem[rhs_store_idx_5] = rhs_pf5; \ + \ + lhs_shmem[lhs_store_idx_6] = lhs_pf6; \ + rhs_shmem[rhs_store_idx_6] = rhs_pf6; \ + \ + lhs_shmem[lhs_store_idx_7] = lhs_pf7; \ + rhs_shmem[rhs_store_idx_7] = rhs_pf7; \ + + // declare and initialize result array +#define res(i, j) _res_##i##j +#define initResultRow(i) \ + Scalar res(i, 0) = conv(0); \ + Scalar res(i, 1) = conv(0); \ + Scalar res(i, 2) = conv(0); \ + Scalar res(i, 3) = conv(0); \ + Scalar res(i, 4) = conv(0); \ + Scalar res(i, 5) = conv(0); \ + Scalar res(i, 6) = conv(0); \ + Scalar res(i, 7) = conv(0); \ + + internal::scalar_cast_op conv; + initResultRow(0); + initResultRow(1); + initResultRow(2); + initResultRow(3); + initResultRow(4); + initResultRow(5); + initResultRow(6); + initResultRow(7); +#undef initResultRow + + for (Index base_k = 0; base_k < k_size; base_k += 64) { + // wait for previous iteration to finish with shmem. Despite common sense, + // the code is a bit faster with this here then at bottom of loop + __syncthreads(); + + prefetchIntoRegisters(base_k); + writeRegToShmem(); + + #undef prefetchIntoRegisters + #undef writeRegToShmem + + // wait for shared mem packing to be done before starting computation + __syncthreads(); + + // compute 8x8 matrix product by outer product. This involves packing one column + // of LHS and one row of RHS into registers (takes 16 registers). + +#define lcol(i) _lcol##i + Scalar lcol(0); + Scalar lcol(1); + Scalar lcol(2); + Scalar lcol(3); + Scalar lcol(4); + Scalar lcol(5); + Scalar lcol(6); + Scalar lcol(7); + +#define rrow(j) _rrow##j + Scalar rrow(0); + Scalar rrow(1); + Scalar rrow(2); + Scalar rrow(3); + Scalar rrow(4); + Scalar rrow(5); + Scalar rrow(6); + Scalar rrow(7); + + // Now x corresponds to k, y to m, and z to n + const Scalar* lhs_block = &lhs_shmem[threadIdx.x + 9 * threadIdx.y]; + const Scalar* rhs_block = &rhs_shmem[threadIdx.x + 8 * threadIdx.z]; + +#define lhs_element(i, j) lhs_block[72 * ((i) + 8 * (j))] +#define rhs_element(i, j) rhs_block[72 * ((i) + 8 * (j))] + +#define loadData(i, j) \ + lcol(0) = lhs_element(0, j); \ + rrow(0) = rhs_element(i, 0); \ + lcol(1) = lhs_element(1, j); \ + rrow(1) = rhs_element(i, 1); \ + lcol(2) = lhs_element(2, j); \ + rrow(2) = rhs_element(i, 2); \ + lcol(3) = lhs_element(3, j); \ + rrow(3) = rhs_element(i, 3); \ + lcol(4) = lhs_element(4, j); \ + rrow(4) = rhs_element(i, 4); \ + lcol(5) = lhs_element(5, j); \ + rrow(5) = rhs_element(i, 5); \ + lcol(6) = lhs_element(6, j); \ + rrow(6) = rhs_element(i, 6); \ + lcol(7) = lhs_element(7, j); \ + rrow(7) = rhs_element(i, 7); \ + +#define computeCol(j) \ + res(0, j) += lcol(0) * rrow(j); \ + res(1, j) += lcol(1) * rrow(j); \ + res(2, j) += lcol(2) * rrow(j); \ + res(3, j) += lcol(3) * rrow(j); \ + res(4, j) += lcol(4) * rrow(j); \ + res(5, j) += lcol(5) * rrow(j); \ + res(6, j) += lcol(6) * rrow(j); \ + res(7, j) += lcol(7) * rrow(j); \ + +#define computePass(i) \ + loadData(i, i); \ + \ + computeCol(0); \ + computeCol(1); \ + computeCol(2); \ + computeCol(3); \ + computeCol(4); \ + computeCol(5); \ + computeCol(6); \ + computeCol(7); \ + + computePass(0); + computePass(1); + computePass(2); + computePass(3); + computePass(4); + computePass(5); + computePass(6); + computePass(7); + +#undef lcol +#undef rrow +#undef lhs_element +#undef rhs_element +#undef loadData +#undef computeCol +#undef computePass + } // end loop over k + + // we've now iterated over all of the large (ie width 64) k blocks and + // accumulated results in registers. At this point thread (x, y, z) contains + // the sum across all big k blocks of the product of little k block of index (x, y) + // with block of index (y, z). To compute the final output, we need to reduce + // the 8 threads over y by summation. +#define shuffleInc(i, j, mask) res(i, j) += __shfl_xor(res(i, j), mask) + +#define reduceRow(i, mask) \ + shuffleInc(i, 0, mask); \ + shuffleInc(i, 1, mask); \ + shuffleInc(i, 2, mask); \ + shuffleInc(i, 3, mask); \ + shuffleInc(i, 4, mask); \ + shuffleInc(i, 5, mask); \ + shuffleInc(i, 6, mask); \ + shuffleInc(i, 7, mask); \ + +#define reduceMatrix(mask) \ + reduceRow(0, mask); \ + reduceRow(1, mask); \ + reduceRow(2, mask); \ + reduceRow(3, mask); \ + reduceRow(4, mask); \ + reduceRow(5, mask); \ + reduceRow(6, mask); \ + reduceRow(7, mask); \ + + // actually perform the reduction, now each thread of index (_, y, z) + // contains the correct values in its registers that belong in the output + // block + reduceMatrix(1); + reduceMatrix(2); + reduceMatrix(4); + +#undef shuffleInc +#undef reduceRow +#undef reduceMatrix + + // now we need to copy the 64 values into main memory. We can't split work + // among threads because all variables are in registers. There's 2 ways + // to do this: + // (1) have 1 thread do 64 writes from registers into global memory + // (2) have 1 thread do 64 writes into shared memory, and then 8 threads + // each do 8 writes into global memory. We can just overwrite the shared + // memory from the problem we just solved. + // (2) is slightly faster than (1) due to less branching and more ILP + + // TODO: won't yield much gain, but could just use currently unused shared mem + // and then we won't have to sync + // wait for shared mem to be out of use + __syncthreads(); + +#define writeResultShmem(i, j) \ + lhs_shmem[i + 8 * threadIdx.y + 64 * threadIdx.z + 512 * j] = res(i, j); \ + +#define writeRow(i) \ + writeResultShmem(i, 0); \ + writeResultShmem(i, 1); \ + writeResultShmem(i, 2); \ + writeResultShmem(i, 3); \ + writeResultShmem(i, 4); \ + writeResultShmem(i, 5); \ + writeResultShmem(i, 6); \ + writeResultShmem(i, 7); \ + + if (threadIdx.x == 0) { + writeRow(0); + writeRow(1); + writeRow(2); + writeRow(3); + writeRow(4); + writeRow(5); + writeRow(6); + writeRow(7); + } +#undef writeResultShmem +#undef writeRow + + const int max_i_write = numext::mini((int)((m_size - base_m - threadIdx.y + 7) / 8), 8); + const int max_j_write = numext::mini((int)((n_size - base_n - threadIdx.z + 7) / 8), 8); + + if (threadIdx.x < max_i_write) { + if (max_j_write == 8) { + // TODO: can i trade bank conflicts for coalesced writes? + Scalar val0 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 0]; + Scalar val1 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 1]; + Scalar val2 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 2]; + Scalar val3 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 3]; + Scalar val4 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 4]; + Scalar val5 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 5]; + Scalar val6 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 6]; + Scalar val7 = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * 7]; + + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 0) = val0; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 1) = val1; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 2) = val2; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 3) = val3; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 4) = val4; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 5) = val5; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 6) = val6; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * 7) = val7; + } else { +#pragma unroll 7 + for (int j = 0; j < max_j_write; j++) { + Scalar val = lhs_shmem[threadIdx.x + 8 * threadIdx.y + 64 * threadIdx.z + 512 * j]; + output(base_m + threadIdx.y + 8 * threadIdx.x, base_n + threadIdx.z + 8 * j) = val; + } + } + } +#undef res +} + + +template +__global__ void +__launch_bounds__(512) +EigenContractionKernel(const LhsMapper lhs, const RhsMapper rhs, + const OutputMapper output, + const Index m_size, const Index n_size, const Index k_size) { + __shared__ Scalar lhs_shmem[72 * 64]; + __shared__ Scalar rhs_shmem[72 * 64]; + + const Index m_block_idx = blockIdx.x; + const Index n_block_idx = blockIdx.y; + + const Index base_m = 64 * m_block_idx; + const Index base_n = 64 * n_block_idx; + + if (base_m + 63 < m_size && base_n + 63 < n_size) { + EigenContractionKernelInternal(lhs, rhs, output, lhs_shmem, rhs_shmem, m_size, n_size, k_size); + } else { + EigenContractionKernelInternal(lhs, rhs, output, lhs_shmem, rhs_shmem, m_size, n_size, k_size); + } +} + + +template +__device__ EIGEN_STRONG_INLINE void +EigenFloatContractionKernelInternal16x16(const LhsMapper lhs, const RhsMapper rhs, + const OutputMapper output, float2 lhs_shmem2[][16], + float2 rhs_shmem2[][8], const Index m_size, + const Index n_size, const Index k_size, + const Index base_m, const Index base_n) { + typedef float Scalar; + + // prefetch registers + float4 lhs_pf0, rhs_pf0; + + float4 results[4]; + for (int i=0; i < 4; i++) { + results[i].x = results[i].y = results[i].z = results[i].w = 0; + } + + +#define prefetch_lhs(reg, row, col) \ + if (!CHECK_LHS_BOUNDARY) { \ + if (col < k_size) { \ + reg =lhs.loadPacket(row, col); \ + } \ + } else { \ + if (col < k_size) { \ + if (row + 3 < m_size) { \ + reg =lhs.loadPacket(row, col); \ + } else if (row + 2 < m_size) { \ + reg.x =lhs(row + 0, col); \ + reg.y =lhs(row + 1, col); \ + reg.z =lhs(row + 2, col); \ + } else if (row + 1 < m_size) { \ + reg.x =lhs(row + 0, col); \ + reg.y =lhs(row + 1, col); \ + } else if (row < m_size) { \ + reg.x =lhs(row + 0, col); \ + } \ + } \ + } \ + + + Index lhs_vert = base_m+threadIdx.x*4; + + for (Index k = 0; k < k_size; k += 16) { + lhs_pf0 = internal::pset1(0); + rhs_pf0 = internal::pset1(0); + + Index lhs_horiz = threadIdx.y+k; + prefetch_lhs(lhs_pf0, lhs_vert, lhs_horiz) + + Index rhs_vert = k+(threadIdx.x%4)*4; + Index rhs_horiz0 = (threadIdx.x>>2)+threadIdx.y*4+base_n; + + if (!CHECK_RHS_BOUNDARY) { + if ((rhs_vert + 3) < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0 = rhs.loadPacket(rhs_vert, rhs_horiz0); + } else if (rhs_vert + 2 < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf0.z = rhs(rhs_vert + 2, rhs_horiz0); + } else if (rhs_vert + 1 < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + } else if (rhs_vert < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + } + } else { + if (rhs_horiz0 < n_size) { + if ((rhs_vert + 3) < k_size) { + rhs_pf0 = rhs.loadPacket(rhs_vert, rhs_horiz0); + } else if ((rhs_vert + 2) < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf0.z = rhs(rhs_vert + 2, rhs_horiz0); + } else if ((rhs_vert + 1) < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + } else if (rhs_vert < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + } + } + } + float x1, x2 ; + // the following can be a bitwise operation..... some day. + if((threadIdx.x%8) < 4) { + x1 = rhs_pf0.y; + x2 = rhs_pf0.w; + } else { + x1 = rhs_pf0.x; + x2 = rhs_pf0.z; + } + x1 = __shfl_xor(x1, 4); + x2 = __shfl_xor(x2, 4); + if((threadIdx.x%8) < 4) { + rhs_pf0.y = x1; + rhs_pf0.w = x2; + } else { + rhs_pf0.x = x1; + rhs_pf0.z = x2; + } + + // We have 64 features. + // Row 0 -> times (0, 4, 8, 12, 1, 5, 9, 13) for features 0, 1. + // Row 1 -> times (0, 4, 8, 12, 1, 5, 9, 13) for features 2, 3. + // ... + // Row 31 -> times (0, 4, 8, 12, 1, 5, 9, 13) for features 62, 63 + // Row 32 -> times (2, 6, 10, 14, 3, 7, 11, 15) for features 0, 1 + // ... + rhs_shmem2[(threadIdx.x>>3)+ threadIdx.y*2][threadIdx.x%8] = make_float2(rhs_pf0.x, rhs_pf0.y); + rhs_shmem2[(threadIdx.x>>3)+ threadIdx.y*2+32][threadIdx.x%8] = make_float2(rhs_pf0.z, rhs_pf0.w); + + // Row 0 (time 0) -> features (0, 1), (4, 5), .. (28, 29), (32, 33), .. (60, 61) + // Row 1 (time 1) -> features (0, 1), (4, 5), .. (28, 29), (32, 33), .. (60, 61) + // ... + // Row 15 (time 15) -> features (0, 1), (4, 5), .. (28, 29), (32, 33), .. (60, 61) + // Row 16 (time 0) -> features (2, 3), (6, 7), .. (30, 31), (34, 35), .. (62, 63) + // ... + + lhs_shmem2[threadIdx.y][threadIdx.x] = make_float2(lhs_pf0.x, lhs_pf0.y); + lhs_shmem2[threadIdx.y+16][threadIdx.x] = make_float2(lhs_pf0.z, lhs_pf0.w); + + +#define add_vals(fl1, fl2, fr1, fr2)\ + results[0].x += fl1.x * fr1.x;\ + results[0].y += fl1.y * fr1.x;\ + results[0].z += fl2.x * fr1.x;\ + results[0].w += fl2.y * fr1.x;\ +\ + results[1].x += fl1.x * fr1.y;\ + results[1].y += fl1.y * fr1.y;\ + results[1].z += fl2.x * fr1.y;\ + results[1].w += fl2.y * fr1.y;\ +\ + results[2].x += fl1.x * fr2.x;\ + results[2].y += fl1.y * fr2.x;\ + results[2].z += fl2.x * fr2.x;\ + results[2].w += fl2.y * fr2.x;\ +\ + results[3].x += fl1.x * fr2.y;\ + results[3].y += fl1.y * fr2.y;\ + results[3].z += fl2.x * fr2.y;\ + results[3].w += fl2.y * fr2.y;\ + + __syncthreads(); + + // Do the multiplies. + #pragma unroll + for (int koff = 0; koff < 16; koff ++) { + // 32 x threads. + float2 fl1 = lhs_shmem2[koff][threadIdx.x]; + float2 fl2 = lhs_shmem2[koff + 16][threadIdx.x]; + + int start_feature = threadIdx.y * 4; + float2 fr1 = rhs_shmem2[(start_feature>>1) + 32*((koff%4)/2)][koff/4 + (koff%2)*4]; + float2 fr2 = rhs_shmem2[(start_feature>>1) + 1 + 32*((koff%4)/2)][koff/4 + (koff%2)*4]; + + add_vals(fl1, fl2, fr1, fr2) + } + __syncthreads(); + } + +#undef prefetch_lhs +#undef add_vals + + Index horiz_base = threadIdx.y*4+base_n; + if (!CHECK_LHS_BOUNDARY && !CHECK_RHS_BOUNDARY) { + for (int i = 0; i < 4; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } else if (!CHECK_RHS_BOUNDARY) { + // CHECK LHS + if (lhs_vert + 3 < m_size) { + for (int i = 0; i < 4; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } else if (lhs_vert + 2 < m_size) { + for (int i = 0; i < 4; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + } + } else if (lhs_vert + 1 < m_size) { + for (int i = 0; i < 4; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + } + } else if (lhs_vert < m_size) { + for (int i = 0; i < 4; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + } + } + } else if (!CHECK_LHS_BOUNDARY) { + // CHECK RHS + /* + int ncols_rem = fminf(n_size- horiz_base, 4); + for (int i = 0; i < ncols_rem; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + }*/ + for (int i = 0; i < 4; i++) { + if (horiz_base+i < n_size) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } + } else { + // CHECK both boundaries. + for (int i = 0; i < 4; i++) { + if (horiz_base+i < n_size) { + if (lhs_vert < m_size) + output(lhs_vert, horiz_base + i) = results[i].x; + if (lhs_vert + 1 < m_size) + output(lhs_vert + 1, horiz_base + i) = results[i].y; + if (lhs_vert + 2 < m_size) + output(lhs_vert + 2, horiz_base + i) = results[i].z; + if (lhs_vert + 3 < m_size) + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } + } +} + + +template +__device__ EIGEN_STRONG_INLINE void +EigenFloatContractionKernelInternal(const LhsMapper lhs, const RhsMapper rhs, + const OutputMapper output, float2 lhs_shmem2[][32], + float2 rhs_shmem2[][8], const Index m_size, + const Index n_size, const Index k_size, + const Index base_m, const Index base_n) { + typedef float Scalar; + + // prefetch registers + float4 lhs_pf0, lhs_pf1, lhs_pf2, lhs_pf3; + float4 rhs_pf0, rhs_pf1; + + float4 results[8]; + for (int i=0; i < 8; i++) { + results[i].x = results[i].y = results[i].z = results[i].w = 0; + } + + + Index lhs_vert = base_m+threadIdx.x*4+(threadIdx.y%4)*32; + for (Index k = 0; k < k_size; k += 32) { + lhs_pf0 = internal::pset1(0); + lhs_pf1 = internal::pset1(0); + lhs_pf2 = internal::pset1(0); + lhs_pf3 = internal::pset1(0); + + rhs_pf0 = internal::pset1(0); + rhs_pf1 = internal::pset1(0); + + if (!CHECK_LHS_BOUNDARY) { + if ((threadIdx.y/4+k+24) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + lhs_pf1 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+8)); + lhs_pf2 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+16)); + lhs_pf3 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+24)); + } else if ((threadIdx.y/4+k+16) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + lhs_pf1 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+8)); + lhs_pf2 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+16)); + } else if ((threadIdx.y/4+k+8) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + lhs_pf1 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+8)); + } else if ((threadIdx.y/4+k) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + } + } else { + // just CHECK_LHS_BOUNDARY + if (lhs_vert + 3 < m_size) { + if ((threadIdx.y/4+k+24) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + lhs_pf1 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+8)); + lhs_pf2 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+16)); + lhs_pf3 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+24)); + } else if ((threadIdx.y/4+k+16) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + lhs_pf1 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+8)); + lhs_pf2 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+16)); + } else if ((threadIdx.y/4+k+8) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + lhs_pf1 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k+8)); + } else if ((threadIdx.y/4+k) < k_size) { + lhs_pf0 =lhs.loadPacket(lhs_vert, (threadIdx.y/4+k)); + } + } else if (lhs_vert + 2 < m_size) { + if ((threadIdx.y/4+k+24) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf0.z =lhs(lhs_vert + 2, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf1.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+8)); + lhs_pf1.z =lhs(lhs_vert + 2, (threadIdx.y/4+k+8)); + lhs_pf2.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+16)); + lhs_pf2.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+16)); + lhs_pf2.z =lhs(lhs_vert + 2, (threadIdx.y/4+k+16)); + lhs_pf3.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+24)); + lhs_pf3.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+24)); + lhs_pf3.z =lhs(lhs_vert + 2, (threadIdx.y/4+k+24)); + } else if ((threadIdx.y/4+k+16) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf0.z =lhs(lhs_vert + 2, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf1.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+8)); + lhs_pf1.z =lhs(lhs_vert + 2, (threadIdx.y/4+k+8)); + lhs_pf2.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+16)); + lhs_pf2.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+16)); + lhs_pf2.z =lhs(lhs_vert + 2, (threadIdx.y/4+k+16)); + } else if ((threadIdx.y/4+k+8) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf0.z =lhs(lhs_vert + 2, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf1.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+8)); + lhs_pf1.z =lhs(lhs_vert + 2, (threadIdx.y/4+k+8)); + } else if ((threadIdx.y/4+k) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf0.z =lhs(lhs_vert + 2, (threadIdx.y/4+k)); + } + } else if (lhs_vert + 1 < m_size) { + if ((threadIdx.y/4+k+24) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf1.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+8)); + lhs_pf2.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+16)); + lhs_pf2.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+16)); + lhs_pf3.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+24)); + lhs_pf3.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+24)); + } else if ((threadIdx.y/4+k+16) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf1.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+8)); + lhs_pf2.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+16)); + lhs_pf2.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+16)); + } else if ((threadIdx.y/4+k+8) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf1.y =lhs(lhs_vert + 1, (threadIdx.y/4+k+8)); + } else if ((threadIdx.y/4+k) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf0.y =lhs(lhs_vert + 1, (threadIdx.y/4+k)); + } + } else if (lhs_vert < m_size) { + if ((threadIdx.y/4+k+24) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf2.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+16)); + lhs_pf3.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+24)); + } else if ((threadIdx.y/4+k+16) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + lhs_pf2.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+16)); + } else if ((threadIdx.y/4+k+8) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + lhs_pf1.x =lhs(lhs_vert + 0, (threadIdx.y/4+k+8)); + } else if ((threadIdx.y/4+k) < k_size) { + lhs_pf0.x =lhs(lhs_vert + 0, (threadIdx.y/4+k)); + } + } + } + __syncthreads(); + Index rhs_vert = k+threadIdx.x*4; + Index rhs_horiz0 = threadIdx.y*2+base_n; + Index rhs_horiz1 = threadIdx.y*2+1+base_n; + if (!CHECK_RHS_BOUNDARY) { + if ((rhs_vert + 3) < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0 = rhs.loadPacket(rhs_vert, rhs_horiz0); + rhs_pf1 = rhs.loadPacket(rhs_vert, rhs_horiz1); + } else if (rhs_vert + 2 < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf0.z = rhs(rhs_vert + 2, rhs_horiz0); + rhs_pf1.x = rhs(rhs_vert, rhs_horiz1); + rhs_pf1.y = rhs(rhs_vert + 1, rhs_horiz1); + rhs_pf1.z = rhs(rhs_vert + 2, rhs_horiz1); + } else if (rhs_vert + 1 < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf1.x = rhs(rhs_vert, rhs_horiz1); + rhs_pf1.y = rhs(rhs_vert + 1, rhs_horiz1); + } else if (rhs_vert < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf1.x = rhs(rhs_vert, rhs_horiz1); + } + } else { + if (rhs_horiz1 < n_size) { + if ((rhs_vert + 3) < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0 = rhs.loadPacket(rhs_vert, rhs_horiz0); + rhs_pf1 = rhs.loadPacket(rhs_vert, rhs_horiz1); + } else if (rhs_vert + 2 < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf0.z = rhs(rhs_vert + 2, rhs_horiz0); + rhs_pf1.x = rhs(rhs_vert, rhs_horiz1); + rhs_pf1.y = rhs(rhs_vert + 1, rhs_horiz1); + rhs_pf1.z = rhs(rhs_vert + 2, rhs_horiz1); + } else if (k+threadIdx.x*4 + 1 < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf1.x = rhs(rhs_vert, rhs_horiz1); + rhs_pf1.y = rhs(rhs_vert + 1, rhs_horiz1); + } else if (k+threadIdx.x*4 < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf1.x = rhs(rhs_vert, rhs_horiz1); + } + } else if (rhs_horiz0 < n_size) { + if ((rhs_vert + 3) < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0 = rhs.loadPacket(rhs_vert, rhs_horiz0); + } else if ((rhs_vert + 2) < k_size) { + // just CHECK_RHS_BOUNDARY + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + rhs_pf0.z = rhs(rhs_vert + 2, rhs_horiz0); + } else if ((rhs_vert + 1) < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + rhs_pf0.y = rhs(rhs_vert + 1, rhs_horiz0); + } else if (rhs_vert < k_size) { + rhs_pf0.x = rhs(rhs_vert, rhs_horiz0); + } + } + } + __syncthreads(); + // Loaded. Do computation + // Row 0 -> times (0, 4, 8, .. 28) for features 0, 1. + // Row 1 -> times (0, 4, 8, .. 28) for features 2, 3. + // .. + // Row 31 -> times (0, 4, 8, .. 28) for features 62, 63 + rhs_shmem2[threadIdx.y][threadIdx.x] = make_float2(rhs_pf0.x, rhs_pf1.x); + // Row 32 -> times (1, 5, 9, .. 29) for features 0, 1. + // Row 33 -> times (1, 5, 9, .. 29) for features 2, 3. + // .. + rhs_shmem2[threadIdx.y+32][threadIdx.x] = make_float2(rhs_pf0.y, rhs_pf1.y); + // Row 64 -> times (2, 6, 10, .. 30) for features 0, 1. + // Row 65 -> times (2, 6, 10, .. 30) for features 2, 3. + rhs_shmem2[threadIdx.y+64][threadIdx.x] = make_float2(rhs_pf0.z, rhs_pf1.z); + // Row 96 -> times (3, 7, 11, .. 31) for features 0, 1. + // Row 97 -> times (3, 7, 11, .. 31) for features 2, 3. + rhs_shmem2[threadIdx.y+96][threadIdx.x] = make_float2(rhs_pf0.w, rhs_pf1.w); + + // LHS. + // Row 0 (time 0) -> features (0, 1), (4, 5), .. (28, 29), (32, 33), .. (60, 61) .. (124, 125) + // Row 1 (time 1) -> features (0, 1), (4, 5), .. (28, 29), (32, 33), .. (60, 61) .. (124, 125) + // ... + // Row 8 (time 0) -> features (2, 3), (6, 7), .. (30, 31), (34, 35), .. (62, 63) .. (126, 127) + // Row 15 (time 7) -> features (2, 3), (6, 7), .. (30, 31), (34, 35), .. (62, 63) .. (126, 127) + + +#define add_vals(a_feat1, a_feat2, f1, f2, f3, f4)\ + results[0].x += a_feat1.x * f1.x;\ + results[1].x += a_feat1.x * f1.y;\ + results[2].x += a_feat1.x * f2.x;\ + results[3].x += a_feat1.x * f2.y;\ + results[4].x += a_feat1.x * f3.x;\ + results[5].x += a_feat1.x * f3.y;\ + results[6].x += a_feat1.x * f4.x;\ + results[7].x += a_feat1.x * f4.y;\ +\ + results[0].y += a_feat1.y * f1.x;\ + results[1].y += a_feat1.y * f1.y;\ + results[2].y += a_feat1.y * f2.x;\ + results[3].y += a_feat1.y * f2.y;\ + results[4].y += a_feat1.y * f3.x;\ + results[5].y += a_feat1.y * f3.y;\ + results[6].y += a_feat1.y * f4.x;\ + results[7].y += a_feat1.y * f4.y;\ +\ + results[0].z += a_feat2.x * f1.x;\ + results[1].z += a_feat2.x * f1.y;\ + results[2].z += a_feat2.x * f2.x;\ + results[3].z += a_feat2.x * f2.y;\ + results[4].z += a_feat2.x * f3.x;\ + results[5].z += a_feat2.x * f3.y;\ + results[6].z += a_feat2.x * f4.x;\ + results[7].z += a_feat2.x * f4.y;\ +\ + results[0].w += a_feat2.y * f1.x;\ + results[1].w += a_feat2.y * f1.y;\ + results[2].w += a_feat2.y * f2.x;\ + results[3].w += a_feat2.y * f2.y;\ + results[4].w += a_feat2.y * f3.x;\ + results[5].w += a_feat2.y * f3.y;\ + results[6].w += a_feat2.y * f4.x;\ + results[7].w += a_feat2.y * f4.y;\ + + lhs_shmem2[threadIdx.y/4][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf0.x, lhs_pf0.y); + lhs_shmem2[threadIdx.y/4+8][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf1.x, lhs_pf1.y); + lhs_shmem2[threadIdx.y/4+16][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf2.x, lhs_pf2.y); + lhs_shmem2[threadIdx.y/4+24][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf3.x, lhs_pf3.y); + + lhs_shmem2[threadIdx.y/4 + 32][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf0.z, lhs_pf0.w); + lhs_shmem2[threadIdx.y/4 + 40][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf1.z, lhs_pf1.w); + lhs_shmem2[threadIdx.y/4 + 48][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf2.z, lhs_pf2.w); + lhs_shmem2[threadIdx.y/4 + 56][threadIdx.x+(threadIdx.y%4)*8] = make_float2(lhs_pf3.z, lhs_pf3.w); + + __syncthreads(); + + // Do the multiplies. + #pragma unroll + for (int koff = 0; koff < 32; koff ++) { + float2 a3 = lhs_shmem2[koff][threadIdx.x + (threadIdx.y % 4) * 8]; + float2 a4 = lhs_shmem2[koff + 32][threadIdx.x + (threadIdx.y % 4) * 8]; + + // first feature is at (threadIdx.y/4) * 8 last is at start + 8. + int start_feature = (threadIdx.y / 4) * 8; + + float2 br1 = rhs_shmem2[start_feature/2 + (koff % 4) * 32][koff/4]; + float2 br2 = rhs_shmem2[start_feature/2 + 1 + (koff % 4) * 32][koff/4]; + float2 br3 = rhs_shmem2[start_feature/2 + 2 + (koff % 4) * 32][koff/4]; + float2 br4 = rhs_shmem2[start_feature/2 + 3 + (koff % 4) * 32][koff/4]; + + add_vals(a3, a4, br1, br2, br3, br4) + } + __syncthreads(); + } // end loop over k + + + __syncthreads(); + Index horiz_base = (threadIdx.y/4)*8+base_n; + if (!CHECK_LHS_BOUNDARY && !CHECK_RHS_BOUNDARY) { + for (int i = 0; i < 8; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } else if (!CHECK_RHS_BOUNDARY) { + if (lhs_vert + 3 < m_size) { + for (int i = 0; i < 8; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } else if (lhs_vert + 2 < m_size) { + for (int i = 0; i < 8; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + } + } else if (lhs_vert + 1 < m_size) { + for (int i = 0; i < 8; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + } + } else if (lhs_vert < m_size) { + for (int i = 0; i < 8; i++) { + output(lhs_vert, horiz_base + i) = results[i].x; + } + } + } else if (!CHECK_LHS_BOUNDARY) { + // CHECK BOUNDARY_B + for (int i = 0; i < 8; i++) { + if (horiz_base + i < n_size) { + output(lhs_vert, horiz_base + i) = results[i].x; + output(lhs_vert + 1, horiz_base + i) = results[i].y; + output(lhs_vert + 2, horiz_base + i) = results[i].z; + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } + } else { + // CHECK both boundaries. + for (int i = 0; i < 8; i++) { + if (horiz_base + i < n_size) { + if (lhs_vert < m_size) + output(lhs_vert, horiz_base + i) = results[i].x; + if (lhs_vert + 1 < m_size) + output(lhs_vert + 1, horiz_base + i) = results[i].y; + if (lhs_vert + 2 < m_size) + output(lhs_vert + 2, horiz_base + i) = results[i].z; + if (lhs_vert + 3 < m_size) + output(lhs_vert + 3, horiz_base + i) = results[i].w; + } + } + } +} + + +template +__global__ void +__launch_bounds__(256) +EigenFloatContractionKernel(const LhsMapper lhs, const RhsMapper rhs, + const OutputMapper output, + const Index m_size, const Index n_size, const Index k_size) { + __shared__ float2 lhs_shmem[64*32]; + __shared__ float2 rhs_shmem[128*8]; + + typedef float2 LHS_MEM[64][32]; + typedef float2 RHS_MEM[128][8]; + + typedef float2 LHS_MEM16x16[32][16]; + typedef float2 RHS_MEM16x16[64][8]; + + const Index m_block_idx = blockIdx.x; + const Index n_block_idx = blockIdx.y; + + const Index base_m = 128 * m_block_idx; + const Index base_n = 64 * n_block_idx; + + bool check_rhs = (base_n + 63) >= n_size; + bool check_lhs128 = (base_m + 127) >= m_size; + + if (!check_rhs) { + if (!check_lhs128) { + // >= 128 rows left + EigenFloatContractionKernelInternal( + lhs, rhs, output, *((LHS_MEM *) lhs_shmem), *((RHS_MEM *) rhs_shmem), m_size, n_size, k_size, base_m, base_n); + } else { + EigenFloatContractionKernelInternal( + lhs, rhs, output, *((LHS_MEM *) lhs_shmem), *((RHS_MEM *) rhs_shmem), m_size, n_size, k_size, base_m, base_n); + } + } else { + if (!check_lhs128) { + // >= 128 rows left + EigenFloatContractionKernelInternal( + lhs, rhs, output, *((LHS_MEM *) lhs_shmem), *((RHS_MEM *) rhs_shmem), m_size, n_size, k_size, base_m, base_n); + } else { + EigenFloatContractionKernelInternal( + lhs, rhs, output, *((LHS_MEM *) lhs_shmem), *((RHS_MEM *) rhs_shmem), m_size, n_size, k_size, base_m, base_n); + } + } +} + +template +__global__ void +__launch_bounds__(256) +EigenFloatContractionKernel16x16(const LhsMapper lhs, const RhsMapper rhs, + const OutputMapper output, + const Index m_size, const Index n_size, const Index k_size) { + __shared__ float2 lhs_shmem[32][16]; + __shared__ float2 rhs_shmem[64][8]; + + const Index m_block_idx = blockIdx.x; + const Index n_block_idx = blockIdx.y; + + const Index base_m = 64 * m_block_idx; + const Index base_n = 64 * n_block_idx; + + if (base_m + 63 < m_size) { + if (base_n + 63 < n_size) { + EigenFloatContractionKernelInternal16x16(lhs, rhs, output, lhs_shmem, rhs_shmem, m_size, n_size, k_size, base_m, base_n); + } else { + EigenFloatContractionKernelInternal16x16(lhs, rhs, output, lhs_shmem, rhs_shmem, m_size, n_size, k_size, base_m, base_n); + } + } else { + if (base_n + 63 < n_size) { + EigenFloatContractionKernelInternal16x16(lhs, rhs, output, lhs_shmem, rhs_shmem, m_size, n_size, k_size, base_m, base_n); + } else { + EigenFloatContractionKernelInternal16x16(lhs, rhs, output, lhs_shmem, rhs_shmem, m_size, n_size, k_size, base_m, base_n); + } + } +} + + +template +struct TensorEvaluator, GpuDevice> : + public TensorContractionEvaluatorBase, GpuDevice> > { + + typedef GpuDevice Device; + + typedef TensorEvaluator, Device> Self; + typedef TensorContractionEvaluatorBase Base; + + typedef TensorContractionOp XprType; + typedef typename internal::remove_const::type Scalar; + typedef typename XprType::Index Index; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + enum { + Layout = TensorEvaluator::Layout, + }; + + // Most of the code is assuming that both input tensors are ColMajor. If the + // inputs are RowMajor, we will "cheat" by swapping the LHS and RHS: + // If we want to compute A * B = C, where A is LHS and B is RHS, the code + // will pretend B is LHS and A is RHS. + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), LeftArgType, RightArgType>::type EvalLeftArgType; + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), RightArgType, LeftArgType>::type EvalRightArgType; + + static const int LDims = + internal::array_size::Dimensions>::value; + static const int RDims = + internal::array_size::Dimensions>::value; + static const int ContractDims = internal::array_size::value; + + typedef array left_dim_mapper_t; + typedef array right_dim_mapper_t; + + typedef array contract_t; + typedef array left_nocontract_t; + typedef array right_nocontract_t; + + static const int NumDims = LDims + RDims - 2 * ContractDims; + + typedef DSizes Dimensions; + + // typedefs needed in evalTo + typedef typename internal::remove_const::type LhsScalar; + typedef typename internal::remove_const::type RhsScalar; + + typedef TensorEvaluator LeftEvaluator; + typedef TensorEvaluator RightEvaluator; + + typedef typename LeftEvaluator::Dimensions LeftDimensions; + typedef typename RightEvaluator::Dimensions RightDimensions; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) : + Base(op, device) {} + + // We need to redefine this method to make nvcc happy + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* data) { + this->m_leftImpl.evalSubExprsIfNeeded(NULL); + this->m_rightImpl.evalSubExprsIfNeeded(NULL); + if (data) { + evalTo(data); + return false; + } else { + this->m_result = static_cast(this->m_device.allocate(this->dimensions().TotalSize() * sizeof(Scalar))); + evalTo(this->m_result); + return true; + } + } + + void evalTo(Scalar* buffer) const { + if (this->m_lhs_inner_dim_contiguous) { + if (this->m_rhs_inner_dim_contiguous) { + if (this->m_rhs_inner_dim_reordered) { + evalTyped(buffer); + } + else { + evalTyped(buffer); + } + } + else { + if (this->m_rhs_inner_dim_reordered) { + evalTyped(buffer); + } + else { + evalTyped(buffer); + } + } + } + else { + if (this->m_rhs_inner_dim_contiguous) { + if (this->m_rhs_inner_dim_reordered) { + evalTyped(buffer); + } + else { + evalTyped(buffer); + } + } + else { + if (this->m_rhs_inner_dim_reordered) { + evalTyped(buffer); + } + else { + evalTyped(buffer); + } + } + } + } + + template struct LaunchKernels { + static void Run(const LhsMapper& lhs, const RhsMapper& rhs, const OutputMapper& output, Index m, Index n, Index k, const GpuDevice& device) { + const Index m_blocks = (m + 63) / 64; + const Index n_blocks = (n + 63) / 64; + const dim3 num_blocks(m_blocks, n_blocks, 1); + const dim3 block_size(8, 8, 8); + LAUNCH_CUDA_KERNEL((EigenContractionKernel), num_blocks, block_size, 0, device, lhs, rhs, output, m, n, k); + } + }; + + template struct LaunchKernels { + static void Run(const LhsMapper& lhs, const RhsMapper& rhs, const OutputMapper& output, Index m, Index n, Index k, const GpuDevice& device) { + if (m < 768 || n < 768) { + const Index m_blocks = (m + 63) / 64; + const Index n_blocks = (n + 63) / 64; + const dim3 num_blocks(m_blocks, n_blocks, 1); + const dim3 block_size(16, 16, 1); + LAUNCH_CUDA_KERNEL((EigenFloatContractionKernel16x16), num_blocks, block_size, 0, device, lhs, rhs, output, m, n, k); + } else { + const Index m_blocks = (m + 127) / 128; + const Index n_blocks = (n + 63) / 64; + const dim3 num_blocks(m_blocks, n_blocks, 1); + const dim3 block_size(8, 32, 1); + LAUNCH_CUDA_KERNEL((EigenFloatContractionKernel), num_blocks, block_size, 0, device, lhs, rhs, output, m, n, k); + } + } + }; + + template + void evalTyped(Scalar* buffer) const { + // columns in left side, rows in right side + const Index k = this->m_k_size; + EIGEN_UNUSED_VARIABLE(k) + + // rows in left side + const Index m = this->m_i_size; + + // columns in right side + const Index n = this->m_j_size; + + // zero out the result buffer (which must be of size at least m * n * sizeof(Scalar) + this->m_device.memset(buffer, 0, m * n * sizeof(Scalar)); + + typedef internal::TensorContractionInputMapper LhsMapper; + + typedef internal::TensorContractionInputMapper RhsMapper; + + typedef internal::blas_data_mapper OutputMapper; + + + // initialize data mappers + LhsMapper lhs(this->m_leftImpl, this->m_left_nocontract_strides, this->m_i_strides, + this->m_left_contracting_strides, this->m_k_strides); + + RhsMapper rhs(this->m_rightImpl, this->m_right_nocontract_strides, this->m_j_strides, + this->m_right_contracting_strides, this->m_k_strides); + + OutputMapper output(buffer, m); + + setCudaSharedMemConfig(cudaSharedMemBankSizeEightByte); + LaunchKernels::Run(lhs, rhs, output, m, n, k, this->m_device); + } +}; + +} // end namespace Eigen + +#endif // EIGEN_USE_GPU and __CUDACC__ +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_CUDA_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h new file mode 100644 index 0000000000..9b2cb3ff6b --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h @@ -0,0 +1,467 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_MAPPER_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_MAPPER_H + +namespace Eigen { + +namespace internal { + +enum { + Rhs = 0, + Lhs = 1 +}; + +/* + * Implementation of the Eigen blas_data_mapper class for tensors. + */ + +template struct CoeffLoader { + enum { + DirectOffsets = false + }; + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE CoeffLoader(const Tensor& tensor) : m_tensor(tensor) { } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void offsetBuffer(typename Tensor::Index) { + eigen_assert(false && "unsupported"); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE typename Tensor::Scalar coeff(typename Tensor::Index index) const { return m_tensor.coeff(index); } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename Tensor::PacketReturnType packet(typename Tensor::Index index) const + { + return m_tensor.template packet(index); + } + + + private: + const Tensor m_tensor; +}; + +template struct CoeffLoader { + enum { + DirectOffsets = true + }; + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE CoeffLoader(const Tensor& tensor) : m_data(tensor.data()) {} + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void offsetBuffer(typename Tensor::Index offset) { + m_data += offset; + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE typename Tensor::Scalar coeff(typename Tensor::Index index) const { return loadConstant(m_data+index); } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + typename Tensor::PacketReturnType packet(typename Tensor::Index index) const + { + return internal::ploadt_ro(m_data + index); + } + private: + typedef typename Tensor::Scalar Scalar; + const Scalar* m_data; +}; + +template +class SimpleTensorContractionMapper { + public: + EIGEN_DEVICE_FUNC + SimpleTensorContractionMapper(const Tensor& tensor, + const nocontract_t& nocontract_strides, + const nocontract_t& ij_strides, + const contract_t& contract_strides, + const contract_t& k_strides) : + m_tensor(tensor), + m_nocontract_strides(nocontract_strides), + m_ij_strides(ij_strides), + m_contract_strides(contract_strides), + m_k_strides(k_strides) { } + + enum { + DirectOffsets = CoeffLoader::DirectOffsets + }; + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void offsetBuffer(typename Tensor::Index offset) { + m_tensor.offsetBuffer(offset); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE void prefetch(Index /*i*/) { } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar operator()(Index row) const { + // column major assumption + return operator()(row, 0); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar operator()(Index row, Index col) const { + return m_tensor.coeff(computeIndex(row, col)); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index computeIndex(Index row, Index col) const { + const bool left = (side == Lhs); + Index nocontract_val = left ? row : col; + Index linidx = 0; + for (int i = static_cast(array_size::value) - 1; i > 0; i--) { + const Index idx = nocontract_val / m_ij_strides[i]; + linidx += idx * m_nocontract_strides[i]; + nocontract_val -= idx * m_ij_strides[i]; + } + if (array_size::value > array_size::value) { + if (side == Lhs && inner_dim_contiguous) { + eigen_assert(m_nocontract_strides[0] == 1); + linidx += nocontract_val; + } else { + linidx += nocontract_val * m_nocontract_strides[0]; + } + } + + Index contract_val = left ? col : row; + if(array_size::value > 0) { + for (int i = static_cast(array_size::value) - 1; i > 0; i--) { + const Index idx = contract_val / m_k_strides[i]; + linidx += idx * m_contract_strides[i]; + contract_val -= idx * m_k_strides[i]; + } + + if (side == Rhs && inner_dim_contiguous) { + eigen_assert(m_contract_strides[0] == 1); + linidx += contract_val; + } else { + linidx += contract_val * m_contract_strides[0]; + } + } + + return linidx; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE IndexPair computeIndexPair(Index row, Index col, const Index distance) const { + const bool left = (side == Lhs); + Index nocontract_val[2] = {left ? row : col, left ? row + distance : col}; + Index linidx[2] = {0, 0}; + if (array_size::value > array_size::value) { + for (int i = static_cast(array_size::value) - 1; i > 0; i--) { + const Index idx0 = nocontract_val[0] / m_ij_strides[i]; + const Index idx1 = nocontract_val[1] / m_ij_strides[i]; + linidx[0] += idx0 * m_nocontract_strides[i]; + linidx[1] += idx1 * m_nocontract_strides[i]; + nocontract_val[0] -= idx0 * m_ij_strides[i]; + nocontract_val[1] -= idx1 * m_ij_strides[i]; + } + if (side == Lhs && inner_dim_contiguous) { + eigen_assert(m_nocontract_strides[0] == 1); + linidx[0] += nocontract_val[0]; + linidx[1] += nocontract_val[1]; + } else { + linidx[0] += nocontract_val[0] * m_nocontract_strides[0]; + linidx[1] += nocontract_val[1] * m_nocontract_strides[0]; + } + } + + Index contract_val[2] = {left ? col : row, left ? col : row + distance}; + if (array_size::value> 0) { + for (int i = static_cast(array_size::value) - 1; i > 0; i--) { + const Index idx0 = contract_val[0] / m_k_strides[i]; + const Index idx1 = contract_val[1] / m_k_strides[i]; + linidx[0] += idx0 * m_contract_strides[i]; + linidx[1] += idx1 * m_contract_strides[i]; + contract_val[0] -= idx0 * m_k_strides[i]; + contract_val[1] -= idx1 * m_k_strides[i]; + } + + if (side == Rhs && inner_dim_contiguous) { + eigen_assert(m_contract_strides[0] == 1); + linidx[0] += contract_val[0]; + linidx[1] += contract_val[1]; + } else { + linidx[0] += contract_val[0] * m_contract_strides[0]; + linidx[1] += contract_val[1] * m_contract_strides[0]; + } + } + return IndexPair(linidx[0], linidx[1]); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index firstAligned(Index size) const { + // Only claim alignment when we can compute the actual stride (ie when we're + // dealing with the lhs with inner_dim_contiguous. This is because the + // matrix-vector product relies on the stride when dealing with aligned inputs. + return (Alignment == Aligned) && (side == Lhs) && inner_dim_contiguous ? 0 : size; + } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index stride() const { + return ((side == Lhs) && inner_dim_contiguous && array_size::value > 0) ? m_contract_strides[0] : 1; + } + + protected: + CoeffLoader m_tensor; + const nocontract_t m_nocontract_strides; + const nocontract_t m_ij_strides; + const contract_t m_contract_strides; + const contract_t m_k_strides; +}; + + +template +class BaseTensorContractionMapper : public SimpleTensorContractionMapper +{ + public: + typedef SimpleTensorContractionMapper ParentMapper; + + EIGEN_DEVICE_FUNC + BaseTensorContractionMapper(const Tensor& tensor, + const nocontract_t& nocontract_strides, + const nocontract_t& ij_strides, + const contract_t& contract_strides, + const contract_t& k_strides) : + ParentMapper(tensor, nocontract_strides, ij_strides, contract_strides, k_strides) { } + + typedef typename Tensor::PacketReturnType Packet; + typedef typename unpacket_traits::half HalfPacket; + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Packet loadPacket(Index i, Index j) const { + // whole method makes column major assumption + + // don't need to add offsets for now (because operator handles that) + // current code assumes packet size must be a multiple of 2 + EIGEN_STATIC_ASSERT(packet_size % 2 == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + + if (Tensor::PacketAccess && inner_dim_contiguous && !inner_dim_reordered) { + const Index index = this->computeIndex(i, j); + eigen_assert(this->computeIndex(i+packet_size-1, j) == index + packet_size-1); + return this->m_tensor.template packet(index); + } + + const IndexPair indexPair = this->computeIndexPair(i, j, packet_size - 1); + const Index first = indexPair.first; + const Index last = indexPair.second; + + // We can always do optimized packet reads from left hand side right now, because + // the vertical matrix dimension on the left hand side is never contracting. + // On the right hand side we need to check if the contracting dimensions may have + // been shuffled first. + if (Tensor::PacketAccess && + (side == Lhs || internal::array_size::value <= 1 || !inner_dim_reordered) && + (last - first) == (packet_size - 1)) { + + return this->m_tensor.template packet(first); + } + + EIGEN_ALIGN_MAX Scalar data[packet_size]; + + data[0] = this->m_tensor.coeff(first); + for (Index k = 1; k < packet_size - 1; k += 2) { + const IndexPair internal_pair = this->computeIndexPair(i + k, j, 1); + data[k] = this->m_tensor.coeff(internal_pair.first); + data[k + 1] = this->m_tensor.coeff(internal_pair.second); + } + data[packet_size - 1] = this->m_tensor.coeff(last); + + return pload(data); + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE HalfPacket loadHalfPacket(Index i, Index j) const { + // whole method makes column major assumption + + // don't need to add offsets for now (because operator handles that) + const Index half_packet_size = unpacket_traits::size; + if (half_packet_size == packet_size) { + return loadPacket(i, j); + } + EIGEN_ALIGN_MAX Scalar data[half_packet_size]; + for (Index k = 0; k < half_packet_size; k++) { + data[k] = operator()(i + k, j); + } + return pload(data); + } +}; + + +template +class BaseTensorContractionMapper : public SimpleTensorContractionMapper +{ + public: + typedef SimpleTensorContractionMapper ParentMapper; + + EIGEN_DEVICE_FUNC + BaseTensorContractionMapper(const Tensor& tensor, + const nocontract_t& nocontract_strides, + const nocontract_t& ij_strides, + const contract_t& contract_strides, + const contract_t& k_strides) : + ParentMapper(tensor, nocontract_strides, ij_strides, contract_strides, k_strides) { } + + typedef typename Tensor::PacketReturnType Packet; + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Packet loadPacket(Index i, Index j) const { + EIGEN_ALIGN_MAX Scalar data[1]; + data[0] = this->m_tensor.coeff(this->computeIndex(i, j)); + return pload(data); + } + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Packet loadHalfPacket(Index i, Index j) const { + return loadPacket(i, j); + } +}; + + +template +class TensorContractionSubMapper { + public: + typedef typename Tensor::PacketReturnType Packet; + typedef typename unpacket_traits::half HalfPacket; + + typedef BaseTensorContractionMapper ParentMapper; + typedef TensorContractionSubMapper Self; + typedef Self LinearMapper; + + enum { + // We can use direct offsets iff the parent mapper supports then and we can compute the strides. + // TODO: we should also enable direct offsets for the Rhs case. + UseDirectOffsets = ParentMapper::DirectOffsets && (side == Lhs) && inner_dim_contiguous && (array_size::value > 0) + }; + + EIGEN_DEVICE_FUNC TensorContractionSubMapper(const ParentMapper& base_mapper, Index vert_offset, Index horiz_offset) + : m_base_mapper(base_mapper), m_vert_offset(vert_offset), m_horiz_offset(horiz_offset) { + // Bake the offsets into the buffer used by the base mapper whenever possible. This avoids the need to recompute + // this offset every time we attempt to access a coefficient. + if (UseDirectOffsets) { + Index stride = m_base_mapper.stride(); + m_base_mapper.offsetBuffer(vert_offset + horiz_offset * stride); + } + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar operator()(Index i) const { + if (UseDirectOffsets) { + return m_base_mapper(i, 0); + } + return m_base_mapper(i + m_vert_offset, m_horiz_offset); + } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar operator()(Index i, Index j) const { + if (UseDirectOffsets) { + return m_base_mapper(i, j); + } + return m_base_mapper(i + m_vert_offset, j + m_horiz_offset); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i) const { + if (UseDirectOffsets) { + return m_base_mapper.template loadPacket(i, 0); + } + return m_base_mapper.template loadPacket(i + m_vert_offset, m_horiz_offset); + } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i, Index j) const { + if (UseDirectOffsets) { + return m_base_mapper.template loadPacket(i, j); + } + return m_base_mapper.template loadPacket(i + m_vert_offset, j + m_horiz_offset); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE HalfPacket loadHalfPacket(Index i) const { + if (UseDirectOffsets) { + return m_base_mapper.template loadHalfPacket(i, 0); + } + return m_base_mapper.template loadHalfPacket(i + m_vert_offset, m_horiz_offset); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, Packet p) const { + if (UseDirectOffsets) { + m_base_mapper.storePacket(i, 0, p); + } + m_base_mapper.storePacket(i + m_vert_offset, m_horiz_offset, p); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const { + if (UseDirectOffsets) { + return LinearMapper(m_base_mapper, i, j); + } + return LinearMapper(m_base_mapper, i + m_vert_offset, j + m_horiz_offset); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i) const { + EIGEN_STATIC_ASSERT((internal::is_same::value), YOU_MADE_A_PROGRAMMING_MISTAKE); + const int ActualAlignment = (AlignmentType == Aligned) && (Alignment == Aligned) ? Aligned : Unaligned; + if (UseDirectOffsets) { + return m_base_mapper.template loadPacket(i, 0); + } + return m_base_mapper.template loadPacket(i + m_vert_offset, m_horiz_offset); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool aligned(Index) const { + return false; + } + + private: + ParentMapper m_base_mapper; + const Index m_vert_offset; + const Index m_horiz_offset; +}; + + +template +class TensorContractionInputMapper + : public BaseTensorContractionMapper { + + public: + typedef Scalar_ Scalar; + typedef BaseTensorContractionMapper Base; + typedef TensorContractionSubMapper SubMapper; + typedef SubMapper VectorMapper; + + EIGEN_DEVICE_FUNC TensorContractionInputMapper(const Tensor& tensor, + const nocontract_t& nocontract_strides, + const nocontract_t& ij_strides, + const contract_t& contract_strides, + const contract_t& k_strides) + : Base(tensor, nocontract_strides, ij_strides, contract_strides, k_strides) { } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE SubMapper getSubMapper(Index i, Index j) const { + return SubMapper(*this, i, j); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE VectorMapper getVectorMapper(Index i, Index j) const { + return VectorMapper(*this, i, j); + } +}; + + + +} // end namespace internal +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_MAPPER_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h new file mode 100644 index 0000000000..ee16cde9b1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h @@ -0,0 +1,1052 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_THREAD_POOL_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_THREAD_POOL_H + +// evaluator for thread pool device +#ifdef EIGEN_USE_THREADS + +namespace Eigen { + +#ifdef EIGEN_USE_SIMPLE_THREAD_POOL +namespace internal { + +template +struct packLhsArg { + LhsScalar* blockA; + const LhsMapper& lhs; + const Index m_start; + const Index k_start; + const Index mc; + const Index kc; +}; + +template +struct packRhsAndKernelArg { + const MaxSizeVector* blockAs; + RhsScalar* blockB; + const RhsMapper& rhs; + OutputMapper& output; + const Index m; + const Index k; + const Index n; + const Index mc; + const Index kc; + const Index nc; + const Index num_threads; + const Index num_blockAs; + const Index max_m; + const Index k_block_idx; + const Index m_block_idx; + const Index n_block_idx; + const Index m_blocks; + const Index n_blocks; + MaxSizeVector* kernel_notifications; + const MaxSizeVector* lhs_notifications; + const bool need_to_pack; +}; + +} // end namespace internal +#endif // EIGEN_USE_SIMPLE_THREAD_POOL + +template +struct TensorEvaluator, ThreadPoolDevice> : + public TensorContractionEvaluatorBase, ThreadPoolDevice> > { + + typedef ThreadPoolDevice Device; + + typedef TensorEvaluator, Device> Self; + typedef TensorContractionEvaluatorBase Base; + + typedef TensorContractionOp XprType; + typedef typename internal::remove_const::type Scalar; + typedef typename XprType::Index Index; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + enum { + Layout = TensorEvaluator::Layout, + }; + + // Most of the code is assuming that both input tensors are ColMajor. If the + // inputs are RowMajor, we will "cheat" by swapping the LHS and RHS: + // If we want to compute A * B = C, where A is LHS and B is RHS, the code + // will pretend B is LHS and A is RHS. + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), LeftArgType, RightArgType>::type EvalLeftArgType; + typedef typename internal::conditional< + static_cast(Layout) == static_cast(ColMajor), RightArgType, LeftArgType>::type EvalRightArgType; + + static const int LDims = + internal::array_size::Dimensions>::value; + static const int RDims = + internal::array_size::Dimensions>::value; + static const int ContractDims = internal::array_size::value; + + typedef array left_dim_mapper_t; + typedef array right_dim_mapper_t; + + typedef array contract_t; + typedef array left_nocontract_t; + typedef array right_nocontract_t; + + static const int NumDims = LDims + RDims - 2 * ContractDims; + + typedef DSizes Dimensions; + + // typedefs needed in evalTo + typedef typename internal::remove_const::type LhsScalar; + typedef typename internal::remove_const::type RhsScalar; + typedef typename internal::gebp_traits Traits; + + typedef TensorEvaluator LeftEvaluator; + typedef TensorEvaluator RightEvaluator; + + TensorEvaluator(const XprType& op, const Device& device) : + Base(op, device) {} + +#ifndef EIGEN_USE_SIMPLE_THREAD_POOL + template + void evalProduct(Scalar* buffer) const { + typedef + typename internal::remove_const::type + LhsScalar; + typedef + typename internal::remove_const::type + RhsScalar; + typedef typename internal::gebp_traits Traits; + typedef TensorEvaluator LeftEvaluator; + typedef TensorEvaluator RightEvaluator; + typedef internal::TensorContractionInputMapper< + LhsScalar, Index, internal::Lhs, LeftEvaluator, left_nocontract_t, + contract_t, internal::packet_traits::size, + lhs_inner_dim_contiguous, false, Unaligned> + LhsMapper; + typedef internal::TensorContractionInputMapper< + RhsScalar, Index, internal::Rhs, RightEvaluator, right_nocontract_t, + contract_t, internal::packet_traits::size, + rhs_inner_dim_contiguous, rhs_inner_dim_reordered, Unaligned> + RhsMapper; + typedef internal::blas_data_mapper OutputMapper; + typedef internal::gemm_pack_lhs + LhsPacker; + typedef internal::gemm_pack_rhs< + RhsScalar, Index, typename RhsMapper::SubMapper, Traits::nr, ColMajor> + RhsPacker; + typedef internal::gebp_kernel + GebpKernel; + + const Index m = this->m_i_size; + const Index n = this->m_j_size; + const Index k = this->m_k_size; + if (m == 0 || n == 0 || k == 0) return; + + // Compute a set of algorithm parameters: + // - kernel block sizes (bm, bn, bk) + // - task grain sizes (number of kernels executed per task: gm, gn) + // - number of threads + // - sharding by row/column + // - parallel packing or first lhs then rhs + // and some derived parameters: + // - number of tasks (nm, nn, nk) + // - number of kernels (nm0, nn0) + // Unfortunately, all these parameters are tightly interdependent. + // So in some cases we first compute approximate values, then compute other + // values based on these approximations and then refine the approximations. + + // There are lots of heuristics here. There is some reasoning behind them, + // but ultimately they are just tuned on contraction benchmarks for + // different input configurations, thread counts and instruction sets. + // So feel free to question any of them. + + // Compute whether we want to shard by row or by column. + // This is a first approximation, it will be refined later. Since we don't + // know number of threads yet we use 2, because what's we are most + // interested in at this point is whether it makes sense to use + // parallelization at all or not. + bool shard_by_col = shardByCol(m, n, 2); + + // First approximation of kernel blocking sizes. + // Again, we don't know number of threads yet, so we use 2. + Index bm, bn, bk; + if (shard_by_col) { + internal::TensorContractionBlocking + blocking(k, m, n, 2); + bm = blocking.mc(); + bn = blocking.nc(); + bk = blocking.kc(); + } else { + internal::TensorContractionBlocking + blocking(k, m, n, 2); + bm = blocking.mc(); + bn = blocking.nc(); + bk = blocking.kc(); + } + + // Compute optimal number of threads. + // Note: we use bk instead of k here because we are interested in amount of + // _parallelizable_ computations, and computations are not parallelizable + // across k dimension. + const TensorOpCost cost = + contractionCost(m, n, bm, bn, bk, shard_by_col, false); + int num_threads = TensorCostModel::numThreads( + static_cast(n) * m, cost, this->m_device.numThreads()); + + // TODO(dvyukov): this is a stop-gap to prevent regressions while the cost + // model is not tuned. Remove this when the cost model is tuned. + if (n == 1) num_threads = 1; + + if (num_threads == 1) { + // The single-threaded algorithm should be faster in this case. + if (n == 1) + this->template evalGemv(buffer); + else + this->template evalGemm(buffer); + return; + } + + // Now that we know number of threads, recalculate sharding and blocking. + shard_by_col = shardByCol(m, n, num_threads); + if (shard_by_col) { + internal::TensorContractionBlocking + blocking(k, m, n, num_threads); + bm = blocking.mc(); + bn = blocking.nc(); + bk = blocking.kc(); + } else { + internal::TensorContractionBlocking + blocking(k, m, n, num_threads); + bm = blocking.mc(); + bn = blocking.nc(); + bk = blocking.kc(); + } + + // Number of kernels for each dimension. + Index nm0 = divup(m, bm); + Index nn0 = divup(n, bn); + Index nk = divup(k, bk); + + // Calculate task grain size (number of kernels executed per task). + // This task size coarsening serves two purposes: + // 1. It reduces per-task overheads including synchronization overheads. + // 2. It allows to use caches better (reuse the same packed rhs in several + // consecutive kernels). + Index gm = 1; + Index gn = 1; + // If we are sharding by column, then we prefer to reduce rows first. + if (shard_by_col) { + gm = coarsenM(m, n, bm, bn, bk, gn, num_threads, shard_by_col); + gn = coarsenN(m, n, bm, bn, bk, gm, num_threads, shard_by_col); + } else { + gn = coarsenN(m, n, bm, bn, bk, gm, num_threads, shard_by_col); + gm = coarsenM(m, n, bm, bn, bk, gn, num_threads, shard_by_col); + } + // Number of tasks in each dimension. + Index nm = divup(nm0, gm); + Index nn = divup(nn0, gn); + + // Last by not least, decide whether we want to issue both lhs and rhs + // packing in parallel; or issue lhs packing first, and then issue rhs + // packing when lhs packing completes (for !shard_by_col lhs and rhs are + // swapped). Parallel packing allows more parallelism (for both packing and + // kernels), while sequential packing provides better locality (once + // a thread finishes rhs packing it proceed to kernels with that rhs). + // First, we are interested in parallel packing if there are few tasks. + bool parallel_pack = num_threads >= nm * nn; + // Also do parallel packing if all data fits into L2$. + if (m * bk * Index(sizeof(LhsScalar)) + n * bk * Index(sizeof(RhsScalar)) <= + l2CacheSize() * num_threads) + parallel_pack = true; + // But don't do it if we will use each rhs only once. Locality seems to be + // more important in this case. + if ((shard_by_col ? nm : nn) == 1) parallel_pack = false; + + LhsMapper lhs(this->m_leftImpl, this->m_left_nocontract_strides, + this->m_i_strides, this->m_left_contracting_strides, + this->m_k_strides); + + RhsMapper rhs(this->m_rightImpl, this->m_right_nocontract_strides, + this->m_j_strides, this->m_right_contracting_strides, + this->m_k_strides); + + Context(this->m_device, num_threads, lhs, rhs, buffer, m, n, + k, bm, bn, bk, nm, nn, nk, gm, gn, nm0, nn0, + shard_by_col, parallel_pack) + .run(); + } + + // Context coordinates a single parallel gemm operation. + template + class Context { + public: + Context(const Device& device, int num_threads, LhsMapper& lhs, + RhsMapper& rhs, Scalar* buffer, Index tm, Index tn, Index tk, Index bm, + Index bn, Index bk, Index nm, Index nn, Index nk, Index gm, + Index gn, Index nm0, Index nn0, bool shard_by_col, + bool parallel_pack) + : device_(device), + lhs_(lhs), + rhs_(rhs), + buffer_(buffer), + output_(buffer, tm), + num_threads_(num_threads), + shard_by_col_(shard_by_col), + parallel_pack_(parallel_pack), + m_(tm), + n_(tn), + k_(tk), + bm_(bm), + bn_(bn), + bk_(bk), + nm_(nm), + nn_(nn), + nk_(nk), + gm_(gm), + gn_(gn), + nm0_(nm0), + nn0_(nn0) + { + for (Index x = 0; x < P; x++) { + // Normal number of notifications for k slice switch is + // nm_ + nn_ + nm_ * nn_. However, first P - 1 slices will receive only + // nm_ + nn_ notifications, because they will not receive notifications + // from preceeding kernels. + state_switch_[x] = + x == 0 + ? 1 + : (parallel_pack_ ? nn_ + nm_ : (shard_by_col_ ? nn_ : nm_)) + + (x == P - 1 ? nm_ * nn_ : 0); + state_packing_ready_[x] = + parallel_pack_ ? 0 : (shard_by_col_ ? nm_ : nn_); + state_kernel_[x] = new std::atomic*[nm_]; + for (Index m = 0; m < nm_; m++) { + state_kernel_[x][m] = new std::atomic[nn_]; + // Kernels generally receive 3 notifications (previous kernel + 2 + // packing), but the first slice won't get notifications from previous + // kernels. + for (Index n = 0; n < nn_; n++) + state_kernel_[x][m][n].store( + (x == 0 ? 0 : 1) + (parallel_pack_ ? 2 : 1), + std::memory_order_relaxed); + } + } + + // Allocate memory for packed rhs/lhs matrices. + size_t align = numext::maxi(EIGEN_MAX_ALIGN_BYTES, 1); + size_t lhs_size = + divup(bm_ * bk_ * sizeof(LhsScalar), align) * align; + size_t rhs_size = + divup(bn_ * bk_ * sizeof(RhsScalar), align) * align; + packed_mem_ = static_cast(internal::aligned_malloc( + (nm0_ * lhs_size + nn0_ * rhs_size) * std::min(nk_, P - 1))); + char* mem = static_cast(packed_mem_); + for (Index x = 0; x < numext::mini(nk_, P - 1); x++) { + packed_lhs_[x].resize(nm0_); + for (Index m = 0; m < nm0_; m++) { + packed_lhs_[x][m] = reinterpret_cast(mem); + mem += lhs_size; + } + packed_rhs_[x].resize(nn0_); + for (Index n = 0; n < nn0_; n++) { + packed_rhs_[x][n] = reinterpret_cast(mem); + mem += rhs_size; + } + } + } + + ~Context() { + for (Index x = 0; x < P; x++) { + for (Index m = 0; m < nm_; m++) delete[] state_kernel_[x][m]; + delete[] state_kernel_[x]; + } + internal::aligned_free(packed_mem_); + } + + void run() { + // Kick off packing of the first slice. + signal_switch(0, 1); + // Wait for overall completion. + // TODO(dvyukov): this wait can lead to deadlock. + // If nthreads contractions are concurrently submitted from worker + // threads, this wait will block all worker threads and the system will + // deadlock. + done_.Wait(); + } + + private: + Notification done_; + const Device& device_; + LhsMapper& lhs_; + RhsMapper& rhs_; + Scalar* const buffer_; + OutputMapper output_; + const int num_threads_; + const bool shard_by_col_; + const bool parallel_pack_; + // Matrix sizes. + const Index m_; + const Index n_; + const Index k_; + // Block sizes. + const Index bm_; + const Index bn_; + const Index bk_; + // Number of tasks. + const Index nm_; + const Index nn_; + const Index nk_; + // Task grain sizes (number of kernels executed per task). + const Index gm_; + const Index gn_; + // Number of blocks (this is different from ni_/nn_ because of task size + // coarsening). + const Index nm0_; + const Index nn0_; + + // Parallelization strategy. + // + // Blocks related to the same k block can run in parallel because they write + // to different output blocks. So we parallelize within k slices, this + // gives us parallelism level of m x n. Before we can start any kernels + // related to k-th slice, we need to issue m lhs packing tasks and n rhs + // packing tasks. + // + // However, there is a bottleneck when we are finishing kernels for k-th + // slice (at the very end there is only 1 runnable kernel). To mitigate this + // bottleneck we allow kernels from k-th and k+1-th slices to run in + // parallel. Note that (m, n, k) and (m, n, k+1) kernels write to the same + // output block, so they must not run in parallel. + // + // This gives us the following dependency graph. + // On each k slice we have m x n kernel tasks, m lhs paking tasks and n rhs + // packing tasks. + // Kernel (m, n, k) can start when: + // - kernel (m, n, k-1) has finished + // - lhs packing (m, k) has finished + // - rhs packing (n, k) has finished + // Lhs/rhs packing can start when: + // - all k-1 packing has finished (artificially imposed to limit amount of + // parallel packing) + // + // On top of that we limit runnable tasks to two consecutive k slices. + // This is done to limit amount of memory we need for packed lhs/rhs + // (for each k slice we need m*bk + n*bk memory in packed_lhs_/packed_rhs_). + // + // state_switch_ tracks when we are ready to switch to the next k slice. + // state_kernel_[m][n] tracks when we are ready to kick off kernel (m, n). + // These variable are rolling over 3 consecutive k slices: first two we are + // actively executing + one to track completion of kernels in the second + // slice. + static const Index P = 3; + void* packed_mem_; + std::vector packed_lhs_[P - 1]; + std::vector packed_rhs_[P - 1]; + std::atomic** state_kernel_[P]; + // state_switch_ is frequently modified by worker threads, while other + // fields are read-only after constructor. Let's move it to a separate cache + // line to reduce cache-coherency traffic. + char pad_[128]; + std::atomic state_packing_ready_[P]; + std::atomic state_switch_[P]; + + void pack_lhs(Index m, Index k) { + const Index mend = m * gm_ + gm(m); + for (Index m1 = m * gm_; m1 < mend; m1++) + LhsPacker()(packed_lhs_[k % (P - 1)][m1], + lhs_.getSubMapper(m1 * bm_, k * bk_), bk(k), bm(m1)); + + if (!parallel_pack_ && shard_by_col_) { + signal_packing(k); + } else { + signal_switch(k + 1); + for (Index n = nn_ - 1; n >= 0; n--) signal_kernel(m, n, k, n == 0); + } + } + + void pack_rhs(Index n, Index k) { + const Index nend = n * gn_ + gn(n); + for (Index n1 = n * gn_; n1 < nend; n1++) { + if (k == 0) { + // Zero the output memory in parallel. + // On 10000x2x10000 mm zeroing can easily take half of time. + // Zero (bn x m) row. Safe to do here because all kernels that will + // write to this memory depend on completion of this task. + // Note: don't call device_.memset() here. device_.memset() blocks on + // thread pool worker thread, which can lead to underutilization and + // deadlocks. + memset(buffer_ + n1 * bn_ * m_, 0, bn(n1) * m_ * sizeof(Scalar)); + } + RhsPacker()(packed_rhs_[k % (P - 1)][n1], + rhs_.getSubMapper(k * bk_, n1 * bn_), bk(k), bn(n1)); + } + + if (parallel_pack_ || shard_by_col_) { + signal_switch(k + 1); + for (Index m = nm_ - 1; m >= 0; m--) signal_kernel(m, n, k, m == 0); + } else { + signal_packing(k); + } + } + + void kernel(Index m, Index n, Index k) { + // Note: order of iteration matters here. Iteration over m is innermost + // because we want to reuse the same packed rhs in consequetive tasks + // (rhs fits into L2$ while lhs only into L3$). + const Index nend = n * gn_ + gn(n); + const Index mend = m * gm_ + gm(m); + if (shard_by_col_) { + for (Index n1 = n * gn_; n1 < nend; n1++) { + for (Index m1 = m * gm_; m1 < mend; m1++) + GebpKernel()(output_.getSubMapper(m1 * bm_, n1 * bn_), + packed_lhs_[k % (P - 1)][m1], + packed_rhs_[k % (P - 1)][n1], bm(m1), bk(k), bn(n1), + Scalar(1), -1, -1, 0, 0); + } + } else { + for (Index m1 = m * gm_; m1 < mend; m1++) + for (Index n1 = n * gn_; n1 < nend; n1++) { + GebpKernel()(output_.getSubMapper(m1 * bm_, n1 * bn_), + packed_lhs_[k % (P - 1)][m1], + packed_rhs_[k % (P - 1)][n1], bm(m1), bk(k), bn(n1), + Scalar(1), -1, -1, 0, 0); + } + } + signal_kernel(m, n, k + 1, false); + signal_switch(k + 2); + } + + void signal_packing(Index k) { + eigen_assert(!parallel_pack_); + Index s = state_packing_ready_[k % P].fetch_sub(1); + eigen_assert(s > 0); + if (s != 1) return; + state_packing_ready_[k % P] = shard_by_col_ ? nm_ : nn_; + enqueue_packing(k, shard_by_col_); + } + + void signal_kernel(Index m, Index n, Index k, bool sync) { + std::atomic* state = &state_kernel_[k % P][m][n]; + Index s = state->load(); + eigen_assert(s > 0); + if (s != 1 && state->fetch_sub(1) != 1) return; + state->store(parallel_pack_ ? 3 : 2, std::memory_order_relaxed); + if (sync) + kernel(m, n, k); + else + device_.enqueueNoNotification([=]() { kernel(m, n, k); }); + } + + void signal_switch(Index k, Index v = 1) { + Index s = state_switch_[k % P].fetch_sub(v); + eigen_assert(s >= v); + if (s != v) return; + + // Ready to switch to the next k slice. + // Reset counter for the next iteration. + state_switch_[k % P] = + (parallel_pack_ ? nm_ + nn_ : (shard_by_col_ ? nn_ : nm_)) + + nm_ * nn_; + if (k < nk_) { + // Issue lhs/rhs packing. Their completion will in turn kick off + // kernels. + if (parallel_pack_) { + enqueue_packing(k, !shard_by_col_); + enqueue_packing(k, shard_by_col_); + } else if (shard_by_col_) { + enqueue_packing(k, false); + } else { + enqueue_packing(k, true); + } + + // Termination handling. + // Because kernel completion signals k + 2 switch, we need to finish nk + // + 2 slices without issuing any tasks on nk + 1 slice. So here we + // pretend that all nk + 1 packing tasks just finish instantly; so that + // nk + 2 switch only waits for completion of nk kernels. + } else if (k == nk_) { + signal_switch(k + 1, + parallel_pack_ ? nm_ + nn_ : (shard_by_col_ ? nn_ : nm_)); + } else { + done_.Notify(); + } + } + + // Enqueue all rhs/lhs packing for k-th slice. + void enqueue_packing(Index k, bool rhs) { + enqueue_packing_helper(0, rhs ? nn_ : nm_, k, rhs); + } + + void enqueue_packing_helper(Index start, Index end, Index k, bool rhs) { + if (end - start == 1) { + if (rhs) + pack_rhs(start, k); + else + pack_lhs(start, k); + } else { + Index mid = (start + end) / 2; + device_.enqueueNoNotification( + [=]() { enqueue_packing_helper(mid, end, k, rhs); }); + device_.enqueueNoNotification( + [=]() { enqueue_packing_helper(start, mid, k, rhs); }); + } + } + + // Block sizes with accounting for potentially incomplete last block. + Index bm(Index m) const { return m + 1 < nm0_ ? bm_ : m_ + bm_ - bm_ * nm0_; } + Index bn(Index n) const { return n + 1 < nn0_ ? bn_ : n_ + bn_ - bn_ * nn0_; } + Index bk(Index k) const { return k + 1 < nk_ ? bk_ : k_ + bk_ - bk_ * nk_; } + // Task grain sizes accounting for potentially incomplete last task. + Index gm(Index m) const { return m + 1 < nm_ ? gm_ : nm0_ + gm_ - gm_ * nm_; } + Index gn(Index n) const { return n + 1 < nn_ ? gn_ : nn0_ + gn_ - gn_ * nn_; } + + Context(const Context&) = delete; + void operator=(const Context&) = delete; + }; + + // Decide whether we want to shard m x n contraction by columns or by rows. + static bool shardByCol(Index m, Index n, Index num_threads) { + // Note: we are comparing both n and m against Traits::nr, it is not + // a mistake. We are trying to figure out how both n and m will fit into + // the main sharding dimension. + + // Sharding by column is the default + // ... unless there is enough data for vectorization over rows + if (m / num_threads >= Traits::nr && + // and not enough data for vectorization over columns + (n / num_threads < Traits::nr || + // ... or barely enough data for vectorization over columns, + // but it is not evenly dividable across threads + (n / num_threads < 4 * Traits::nr && + (n % (num_threads * Traits::nr)) != 0 && + // ... and it is evenly dividable across threads for rows + ((m % (num_threads * Traits::nr)) == 0 || + // .. or it is not evenly dividable for both dimensions but + // there is much more data over rows so that corner effects are + // mitigated. + (m / n >= 6))))) + return false; + // Wait, or if matrices are just substantially prolonged over the other + // dimension. + if (n / num_threads < 16 * Traits::nr && m > n * 32) return false; + return true; + } + + Index coarsenM(Index m, Index n, Index bm, Index bn, Index bk, Index gn, + int num_threads, bool shard_by_col) const { + Index gm = 1; + Index gm1 = 1; + Index nm0 = divup(m, bm); + Index nm1 = nm0; + for (;;) { + // Find the next candidate for m grain size. It needs to result in + // different number of blocks. E.g. if we have 10 kernels, we want to try + // 5 and 10, but not 6, 7, 8 and 9. + while (gm1 <= nm0 && nm1 == divup(nm0, gm1)) gm1++; + if (gm1 > nm0) break; + // Check the candidate. + int res = checkGrain(m, n, bm, bn, bk, gm1, gn, gm, gn, num_threads, + shard_by_col); + if (res < 0) break; + nm1 = divup(nm0, gm1); + if (res == 0) continue; + // Commit new grain size. + gm = gm1; + } + return gm; + } + + Index coarsenN(Index m, Index n, Index bm, Index bn, Index bk, Index gm, + int num_threads, bool shard_by_col) const { + Index gn = 1; + Index gn1 = 1; + Index nn0 = divup(n, bn); + Index nn1 = nn0; + for (;;) { + while (gn1 <= nn0 && nn1 == divup(nn0, gn1)) gn1++; + if (gn1 > nn0) break; + int res = checkGrain(m, n, bm, bn, bk, gm, gn1, gm, gn, num_threads, + shard_by_col); + if (res < 0) break; + nn1 = divup(nn0, gn1); + if (res == 0) continue; + gn = gn1; + } + return gn; + } + + // checkGrain checks whether grain (gm, gn) is suitable and is better than + // (oldgm, oldgn). + int checkGrain(Index m, Index n, Index bm, Index bn, Index bk, Index gm, + Index gn, Index oldgm, Index oldgn, int num_threads, + bool shard_by_col) const { + const TensorOpCost cost = + contractionCost(bm * gm, bn * gn, bm, bn, bk, shard_by_col, true); + double taskSize = TensorCostModel::taskSize( + static_cast(bm) * gm * bn * gn, cost); + // If the task is too small, then we agree on it regardless of anything + // else. Otherwise synchronization overheads will dominate. + if (taskSize < 1) return 1; + // If it is too large, then we reject it and all larger tasks. + if (taskSize > 2) return -1; + // Now we are in presumably good task size range. + // The main deciding factor here is parallelism. Consider that we have 12 + // kernels and 4 threads. Grains of 2, 3 and 4 all yield good task sizes. + // But 2/4 yield 6/3 tasks, which gives us parallelism of 0.75 (at most 3/4 + // of cores will be busy). While grain size 3 gives us 4 tasks, which gives + // us parallelism of 1 (we can load all cores). + Index nm0 = divup(m, bm); + Index nn0 = divup(n, bn); + Index new_tasks = divup(nm0, gm) * divup(nn0, gn); + double new_parallelism = static_cast(new_tasks) / + (divup(new_tasks, num_threads) * num_threads); + Index old_tasks = divup(nm0, oldgm) * divup(nn0, oldgn); + double old_parallelism = static_cast(old_tasks) / + (divup(old_tasks, num_threads) * num_threads); + if (new_parallelism > old_parallelism || new_parallelism == 1) return 1; + return 0; + } + +#else // EIGEN_USE_SIMPLE_THREAD_POOL + + template + void evalProduct(Scalar* buffer) const { + if (this->m_j_size == 1) { + this->template evalGemv(buffer); + return; + } + + evalGemm(buffer); + } + + template + void evalGemm(Scalar* buffer) const { + // columns in left side, rows in right side + const Index k = this->m_k_size; + + // rows in left side + const Index m = this->m_i_size; + + // columns in right side + const Index n = this->m_j_size; + + // zero out the result buffer (which must be of size at least m * n * sizeof(Scalar) + this->m_device.memset(buffer, 0, m * n * sizeof(Scalar)); + + + const int lhs_packet_size = internal::unpacket_traits::size; + const int rhs_packet_size = internal::unpacket_traits::size; + + typedef internal::TensorContractionInputMapper LhsMapper; + + typedef internal::TensorContractionInputMapper RhsMapper; + + typedef internal::blas_data_mapper OutputMapper; + + // TODO: packing could be faster sometimes if we supported row major tensor mappers + typedef internal::gemm_pack_lhs LhsPacker; + typedef internal::gemm_pack_rhs RhsPacker; + + // TODO: replace false, false with conjugate values? + typedef internal::gebp_kernel GebpKernel; + + typedef internal::packLhsArg packLArg; + typedef internal::packRhsAndKernelArg packRKArg; + + // initialize data mappers + LhsMapper lhs(this->m_leftImpl, this->m_left_nocontract_strides, this->m_i_strides, + this->m_left_contracting_strides, this->m_k_strides); + + RhsMapper rhs(this->m_rightImpl, this->m_right_nocontract_strides, this->m_j_strides, + this->m_right_contracting_strides, this->m_k_strides); + + OutputMapper output(buffer, m); + + // compute block sizes (which depend on number of threads) + const Index num_threads = this->m_device.numThreads(); + internal::TensorContractionBlocking blocking(k, m, n, num_threads); + Index mc = blocking.mc(); + Index nc = blocking.nc(); + Index kc = blocking.kc(); + eigen_assert(mc <= m); + eigen_assert(nc <= n); + eigen_assert(kc <= k); + +#define CEIL_DIV(a, b) (((a) + (b) - 1) / (b)) + const Index k_blocks = CEIL_DIV(k, kc); + const Index n_blocks = CEIL_DIV(n, nc); + const Index m_blocks = CEIL_DIV(m, mc); + const Index sizeA = mc * kc; + const Index sizeB = kc * nc; + + /* cout << "m: " << m << " n: " << n << " k: " << k << endl; + cout << "mc: " << mc << " nc: " << nc << " kc: " << kc << endl; + cout << "m_blocks: " << m_blocks << " n_blocks: " << n_blocks << " k_blocks: " << k_blocks << endl; + cout << "num threads: " << num_threads << endl; + */ + + // note: m_device.allocate should return 16 byte aligned pointers, but if blockA and blockB + // aren't 16 byte aligned segfaults will happen due to SIMD instructions + // note: You can get away with allocating just a single blockA and offsets and meet the + // the alignment requirements with the assumption that + // (Traits::mr * sizeof(ResScalar)) % 16 == 0 + const Index numBlockAs = numext::mini(num_threads, m_blocks); + MaxSizeVector blockAs(num_threads); + for (int i = 0; i < num_threads; i++) { + blockAs.push_back(static_cast(this->m_device.allocate(sizeA * sizeof(LhsScalar)))); + } + + // To circumvent alignment issues, I'm just going to separately allocate the memory for each thread + // TODO: is this too much memory to allocate? This simplifies coding a lot, but is wasteful. + // Other options: (1) reuse memory when a thread finishes. con: tricky + // (2) allocate block B memory in each thread. con: overhead + MaxSizeVector blockBs(n_blocks); + for (int i = 0; i < n_blocks; i++) { + blockBs.push_back(static_cast(this->m_device.allocate(sizeB * sizeof(RhsScalar)))); + } + + // lhs_notifications starts with all null Notifications + MaxSizeVector lhs_notifications(num_threads, nullptr); + + // this should really be numBlockAs * n_blocks; + const Index num_kernel_notifications = num_threads * n_blocks; + MaxSizeVector kernel_notifications(num_kernel_notifications, + nullptr); + + for (Index k_block_idx = 0; k_block_idx < k_blocks; k_block_idx++) { + const Index k_start = k_block_idx * kc; + // make sure we don't overshoot right edge of left matrix + const Index actual_kc = numext::mini(k_start + kc, k) - k_start; + + for (Index m_block_idx = 0; m_block_idx < m_blocks; m_block_idx += numBlockAs) { + const Index num_blocks = numext::mini(m_blocks-m_block_idx, numBlockAs); + + for (Index mt_block_idx = m_block_idx; mt_block_idx < m_block_idx+num_blocks; mt_block_idx++) { + const Index m_start = mt_block_idx * mc; + const Index actual_mc = numext::mini(m_start + mc, m) - m_start; + eigen_assert(actual_mc > 0); + + Index blockAId = (k_block_idx * m_blocks + mt_block_idx) % num_threads; + + for (int i = 0; i < n_blocks; ++i) { + Index notification_id = (blockAId * n_blocks + i); + // Wait for any current kernels using this slot to complete + // before using it. + if (kernel_notifications[notification_id]) { + wait_until_ready(kernel_notifications[notification_id]); + delete kernel_notifications[notification_id]; + } + kernel_notifications[notification_id] = new Notification(); + } + const packLArg arg = { + blockAs[blockAId], // blockA + lhs, // lhs + m_start, // m + k_start, // k + actual_mc, // mc + actual_kc, // kc + }; + + // Delete any existing notification since we may be + // replacing it. The algorithm should ensure that there are + // no existing waiters on this notification. + delete lhs_notifications[blockAId]; + lhs_notifications[blockAId] = + this->m_device.enqueue(&Self::packLhs, arg); + } + + // now start kernels. + const Index m_base_start = m_block_idx * mc; + const bool need_to_pack = m_block_idx == 0; + + for (Index n_block_idx = 0; n_block_idx < n_blocks; n_block_idx++) { + const Index n_start = n_block_idx * nc; + const Index actual_nc = numext::mini(n_start + nc, n) - n_start; + + // first make sure the previous kernels are all done before overwriting rhs. Also wait if + // we're going to start new k. In both cases need_to_pack is true. + if (need_to_pack) { + for (Index i = num_blocks; i < num_threads; ++i) { + Index blockAId = (k_block_idx * m_blocks + i + m_block_idx) % num_threads; + Index future_id = (blockAId * n_blocks + n_block_idx); + wait_until_ready(kernel_notifications[future_id]); + } + } + + packRKArg arg = { + &blockAs, // blockA + blockBs[n_block_idx], // blockB + rhs, // rhs + output, // output + m_base_start, // m + k_start, // k + n_start, // n + mc, // mc + actual_kc, // kc + actual_nc, // nc + num_threads, + numBlockAs, + m, + k_block_idx, + m_block_idx, + n_block_idx, // n_block_idx + m_blocks, // m_blocks + n_blocks, // n_blocks + &kernel_notifications, // kernel notifications + &lhs_notifications, // lhs notifications + need_to_pack, // need_to_pack + }; + + // We asynchronously kick off this function, which ends up + // notifying the appropriate kernel_notifications objects, + // which this thread waits on before exiting. + this->m_device.enqueueNoNotification(&Self::packRhsAndKernel, arg); + } + } + } + + // Make sure all the kernels are done. + for (size_t i = 0; i < kernel_notifications.size(); ++i) { + wait_until_ready(kernel_notifications[i]); + delete kernel_notifications[i]; + } + + // No need to wait for lhs notifications since they should have + // already been waited on. Just clean them up. + for (size_t i = 0; i < lhs_notifications.size(); ++i) { + delete lhs_notifications[i]; + } + + // deallocate all of the memory for both A and B's + for (size_t i = 0; i < blockAs.size(); i++) { + this->m_device.deallocate(blockAs[i]); + } + for (size_t i = 0; i < blockBs.size(); i++) { + this->m_device.deallocate(blockBs[i]); + } + +#undef CEIL_DIV + } + + /* + * Packs a LHS block of size (mt, kc) starting at lhs(m, k). Before packing + * the LHS block, check that all of the kernels that worked on the same + * mt_block_idx in the previous m_block are done. + */ + template + static void packLhs(const packLArg arg) { + // perform actual packing + LhsPacker pack_lhs; + pack_lhs(arg.blockA, arg.lhs.getSubMapper(arg.m_start, arg.k_start), arg.kc, arg.mc); + } + + /* + * Packs a RHS block of size (kc, nc) starting at (k, n) after checking that + * all kernels in the previous block are done. + * Then for each LHS future, we wait on the future and then call GEBP + * on the area packed by the future (which starts at + * blockA + future_idx * mt * kc) on the LHS and with the full packed + * RHS block. + * The output of this GEBP is written to output(m + i * mt, n). + */ + template + static void packRhsAndKernel(packRKArg arg) { + if (arg.need_to_pack) { + RhsPacker pack_rhs; + pack_rhs(arg.blockB, arg.rhs.getSubMapper(arg.k, arg.n), arg.kc, arg.nc); + } + + GebpKernel gebp; + for (Index mt_block_idx = 0; mt_block_idx < arg.num_blockAs; mt_block_idx++) { + const Index m_base_start = arg.m + arg.mc*mt_block_idx; + if (m_base_start < arg.max_m) { + Index blockAId = (arg.k_block_idx * arg.m_blocks + mt_block_idx + arg.m_block_idx) % arg.num_threads; + wait_until_ready((*arg.lhs_notifications)[blockAId]); + const Index actual_mc = numext::mini(m_base_start + arg.mc, arg.max_m) - m_base_start; + gebp(arg.output.getSubMapper(m_base_start, arg.n), + (*arg.blockAs)[blockAId], arg.blockB, + actual_mc, arg.kc, arg.nc, Scalar(1), -1, -1, 0, 0); + + // Notify that the kernel is done. + const Index set_idx = blockAId * arg.n_blocks + arg.n_block_idx; + (*arg.kernel_notifications)[set_idx]->Notify(); + } + } + } +#endif // EIGEN_USE_SIMPLE_THREAD_POOL + + TensorOpCost contractionCost(Index m, Index n, Index bm, Index bn, Index bk, + bool shard_by_col, bool prepacked) const { + const int packed_size = std::min(PacketType::size, + PacketType::size); + const int output_packet_size = internal::unpacket_traits::size; + const double kd = static_cast(bk); + // Peak VFMA bandwidth is 0.5. However if we have not enough data for + // vectorization bandwidth drops. The 4.0 and 2.0 bandwidth is determined + // experimentally. + double computeBandwidth = bk == 1 ? 4.0 : + (shard_by_col ? bn : bm) < Traits::nr || + (shard_by_col ? bm : bn) < Traits::mr ? 2.0 : 0.5; +#ifndef EIGEN_VECTORIZE_FMA + // Bandwidth of all of VFMA/MULPS/ADDPS is 0.5 on latest Intel processors. + // However for MULPS/ADDPS we have dependent sequence of 2 such instructions, + // so overall bandwidth is 1.0. + if (computeBandwidth == 0.5) computeBandwidth = 1.0; +#endif + // Computations. + TensorOpCost cost = TensorOpCost(0, 0, kd * computeBandwidth, true, packed_size); + // Output stores. + cost += TensorOpCost(0, sizeof(CoeffReturnType), 0, true, output_packet_size); + if (prepacked) { + // Packing and kernels are executed in different tasks. When we calculate + // task grain size we look only at kernel cost assuming that kernel + // is more expensive than packing. + return cost; + } + // Lhs/rhs loads + computations. + TensorOpCost lhsCost = this->m_leftImpl.costPerCoeff(true) * (kd / n); + TensorOpCost rhsCost = this->m_rightImpl.costPerCoeff(true) * (kd / m); + // Lhs packing memory cost does not contribute considerably to overall + // execution time because lhs is prefetched early and accessed sequentially. + if (shard_by_col) + lhsCost.dropMemoryCost(); + else + rhsCost.dropMemoryCost(); + return cost + lhsCost + rhsCost; + } +}; + +} // end namespace Eigen + +#endif // EIGEN_USE_THREADS +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_THREAD_POOL_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConversion.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConversion.h new file mode 100644 index 0000000000..860a6949a9 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConversion.h @@ -0,0 +1,279 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONVERSION_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONVERSION_H + +namespace Eigen { + +/** \class TensorConversionOp + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor conversion class. This class makes it possible to vectorize + * type casting operations when the number of scalars per packet in the source + * and the destination type differ + */ +namespace internal { +template +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs are different. + typedef TargetType Scalar; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = traits::NumDimensions; + static const int Layout = traits::Layout; + enum { Flags = 0 }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorConversionOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorConversionOp type; +}; + +} // end namespace internal + + +template +struct PacketConverter { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + PacketConverter(const TensorEvaluator& impl) + : m_impl(impl) {} + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket packet(Index index) const { + return internal::pcast(m_impl.template packet(index)); + } + + private: + const TensorEvaluator& m_impl; +}; + + +template +struct PacketConverter { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + PacketConverter(const TensorEvaluator& impl) + : m_impl(impl) {} + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket packet(Index index) const { + const int SrcPacketSize = internal::unpacket_traits::size; + + SrcPacket src1 = m_impl.template packet(index); + SrcPacket src2 = m_impl.template packet(index + SrcPacketSize); + TgtPacket result = internal::pcast(src1, src2); + return result; + } + + private: + const TensorEvaluator& m_impl; +}; + +template +struct PacketConverter { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + PacketConverter(const TensorEvaluator& impl) + : m_impl(impl) {} + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket packet(Index index) const { + const int SrcPacketSize = internal::unpacket_traits::size; + + SrcPacket src1 = m_impl.template packet(index); + SrcPacket src2 = m_impl.template packet(index + SrcPacketSize); + SrcPacket src3 = m_impl.template packet(index + 2 * SrcPacketSize); + SrcPacket src4 = m_impl.template packet(index + 3 * SrcPacketSize); + TgtPacket result = internal::pcast(src1, src2, src3, src4); + return result; + } + + private: + const TensorEvaluator& m_impl; +}; + +template +struct PacketConverter { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + PacketConverter(const TensorEvaluator& impl) + : m_impl(impl), m_maxIndex(impl.dimensions().TotalSize()) {} + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TgtPacket packet(Index index) const { + const int SrcPacketSize = internal::unpacket_traits::size; + // Only call m_impl.packet() when we have direct access to the underlying data. This + // ensures that we don't compute the subexpression twice. We may however load some + // coefficients twice, but in practice this doesn't negatively impact performance. + if (m_impl.data() && (index + SrcPacketSize < m_maxIndex)) { + // Force unaligned memory loads since we can't ensure alignment anymore + return internal::pcast(m_impl.template packet(index)); + } else { + const int TgtPacketSize = internal::unpacket_traits::size; + typedef typename internal::unpacket_traits::type SrcType; + typedef typename internal::unpacket_traits::type TgtType; + internal::scalar_cast_op converter; + EIGEN_ALIGN_MAX typename internal::unpacket_traits::type values[TgtPacketSize]; + for (int i = 0; i < TgtPacketSize; ++i) { + values[i] = converter(m_impl.coeff(index+i)); + } + TgtPacket rslt = internal::pload(values); + return rslt; + } + } + + private: + const TensorEvaluator& m_impl; + const typename TensorEvaluator::Index m_maxIndex; +}; + +template +class TensorConversionOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + typedef typename internal::nested::type Nested; + typedef Scalar CoeffReturnType; + typedef typename NumTraits::Real RealScalar; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorConversionOp(const XprType& xpr) + : m_xpr(xpr) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; +}; + +template struct ConversionSubExprEval { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(Eval& impl, Scalar*) { + impl.evalSubExprsIfNeeded(NULL); + return true; + } +}; + +template struct ConversionSubExprEval { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(Eval& impl, Scalar* data) { + return impl.evalSubExprsIfNeeded(data); + } +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorConversionOp XprType; + typedef typename XprType::Index Index; + typedef typename TensorEvaluator::Dimensions Dimensions; + typedef TargetType Scalar; + typedef TargetType CoeffReturnType; + typedef typename internal::remove_all::Scalar>::type SrcType; + typedef typename PacketType::type PacketReturnType; + typedef typename PacketType::type PacketSourceType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = true, + Layout = TensorEvaluator::Layout, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device) + { + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_impl.dimensions(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* data) + { + return ConversionSubExprEval::value, TensorEvaluator, Scalar>::run(m_impl, data); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() + { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + internal::scalar_cast_op converter; + return converter(m_impl.coeff(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + const bool Vectorizable = TensorEvaluator::PacketAccess & + internal::type_casting_traits::VectorizedCast; + return PacketConv::run(m_impl, index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + const double cast_cost = TensorOpCost::CastCost(); + if (vectorized) { + const double SrcCoeffRatio = + internal::type_casting_traits::SrcCoeffRatio; + const double TgtCoeffRatio = + internal::type_casting_traits::TgtCoeffRatio; + return m_impl.costPerCoeff(vectorized) * (SrcCoeffRatio / PacketSize) + + TensorOpCost(0, 0, TgtCoeffRatio * (cast_cost / PacketSize)); + } else { + return m_impl.costPerCoeff(vectorized) + TensorOpCost(0, 0, cast_cost); + } + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + protected: + template + struct PacketConv { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType run(const TensorEvaluator& impl, Index index) { + internal::scalar_cast_op converter; + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + for (int i = 0; i < PacketSize; ++i) { + values[i] = converter(impl.coeff(index+i)); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + }; + + template + struct PacketConv { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType run(const TensorEvaluator& impl, Index index) { + const int SrcCoeffRatio = internal::type_casting_traits::SrcCoeffRatio; + const int TgtCoeffRatio = internal::type_casting_traits::TgtCoeffRatio; + PacketConverter, PacketSourceType, PacketReturnType, + SrcCoeffRatio, TgtCoeffRatio> converter(impl); + return converter.template packet(index); + } + }; + + TensorEvaluator m_impl; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONVERSION_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConvolution.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConvolution.h new file mode 100644 index 0000000000..abdf742c6e --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConvolution.h @@ -0,0 +1,1104 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CONVOLUTION_H +#define EIGEN_CXX11_TENSOR_TENSOR_CONVOLUTION_H + +namespace Eigen { + +/** \class TensorConvolution + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor convolution class. + * + * + */ +namespace internal { + +template +class IndexMapper { + public: + IndexMapper(const InputDims& input_dims, const array& kernel_dims, + const array& indices) { + + array dimensions = input_dims; + for (int i = 0; i < NumKernelDims; ++i) { + const Index index = indices[i]; + const Index input_dim = input_dims[index]; + const Index kernel_dim = kernel_dims[i]; + const Index result_dim = input_dim - kernel_dim + 1; + dimensions[index] = result_dim; + } + + array inputStrides; + array outputStrides; + if (static_cast(Layout) == static_cast(ColMajor)) { + inputStrides[0] = 1; + outputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + inputStrides[i] = inputStrides[i-1] * input_dims[i-1]; + outputStrides[i] = outputStrides[i-1] * dimensions[i-1]; + } + } else { + inputStrides[NumDims - 1] = 1; + outputStrides[NumDims - 1] = 1; + for (int i = static_cast(NumDims) - 2; i >= 0; --i) { + inputStrides[i] = inputStrides[i + 1] * input_dims[i + 1]; + outputStrides[i] = outputStrides[i + 1] * dimensions[i + 1]; + } + } + + array cudaInputDimensions; + array cudaOutputDimensions; + array tmp = dimensions; + array ordering; + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + for (int i = 0; i < NumKernelDims; ++i) { + const Index index = i + offset; + ordering[index] = indices[i]; + tmp[indices[i]] = -1; + cudaInputDimensions[index] = input_dims[indices[i]]; + cudaOutputDimensions[index] = dimensions[indices[i]]; + } + + int written = static_cast(Layout) == static_cast(ColMajor) + ? NumKernelDims + : 0; + for (int i = 0; i < NumDims; ++i) { + if (tmp[i] >= 0) { + ordering[written] = i; + cudaInputDimensions[written] = input_dims[i]; + cudaOutputDimensions[written] = dimensions[i]; + ++written; + } + } + + for (int i = 0; i < NumDims; ++i) { + m_inputStrides[i] = inputStrides[ordering[i]]; + m_outputStrides[i] = outputStrides[ordering[i]]; + } + + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < NumDims; ++i) { + if (i > NumKernelDims) { + m_cudaInputStrides[i] = + m_cudaInputStrides[i - 1] * cudaInputDimensions[i - 1]; + m_cudaOutputStrides[i] = + m_cudaOutputStrides[i - 1] * cudaOutputDimensions[i - 1]; + } else { + m_cudaInputStrides[i] = 1; + m_cudaOutputStrides[i] = 1; + } + } + } else { + for (int i = NumDims - 1; i >= 0; --i) { + if (i + 1 < offset) { + m_cudaInputStrides[i] = + m_cudaInputStrides[i + 1] * cudaInputDimensions[i + 1]; + m_cudaOutputStrides[i] = + m_cudaOutputStrides[i + 1] * cudaOutputDimensions[i + 1]; + } else { + m_cudaInputStrides[i] = 1; + m_cudaOutputStrides[i] = 1; + } + } + } + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaInputPlaneToTensorInputOffset(Index p) const { + Index inputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int d = NumDims - 1; d > NumKernelDims; --d) { + const Index idx = p / m_cudaInputStrides[d]; + inputIndex += idx * m_inputStrides[d]; + p -= idx * m_cudaInputStrides[d]; + } + inputIndex += p * m_inputStrides[NumKernelDims]; + } else { + std::ptrdiff_t limit = 0; + if (NumKernelDims < NumDims) { + limit = NumDims - NumKernelDims - 1; + } + for (int d = 0; d < limit; ++d) { + const Index idx = p / m_cudaInputStrides[d]; + inputIndex += idx * m_inputStrides[d]; + p -= idx * m_cudaInputStrides[d]; + } + inputIndex += p * m_inputStrides[limit]; + } + return inputIndex; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaOutputPlaneToTensorOutputOffset(Index p) const { + Index outputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int d = NumDims - 1; d > NumKernelDims; --d) { + const Index idx = p / m_cudaOutputStrides[d]; + outputIndex += idx * m_outputStrides[d]; + p -= idx * m_cudaOutputStrides[d]; + } + outputIndex += p * m_outputStrides[NumKernelDims]; + } else { + std::ptrdiff_t limit = 0; + if (NumKernelDims < NumDims) { + limit = NumDims - NumKernelDims - 1; + } + for (int d = 0; d < limit; ++d) { + const Index idx = p / m_cudaOutputStrides[d]; + outputIndex += idx * m_outputStrides[d]; + p -= idx * m_cudaOutputStrides[d]; + } + outputIndex += p * m_outputStrides[limit]; + } + return outputIndex; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaInputKernelToTensorInputOffset(Index i) const { + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + return i * m_inputStrides[offset]; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaOutputKernelToTensorOutputOffset(Index i) const { + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + return i * m_outputStrides[offset]; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaInputKernelToTensorInputOffset(Index i, Index j) const { + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + return i * m_inputStrides[offset] + j * m_inputStrides[offset + 1]; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaOutputKernelToTensorOutputOffset(Index i, Index j) const { + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + return i * m_outputStrides[offset] + j * m_outputStrides[offset + 1]; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaInputKernelToTensorInputOffset(Index i, Index j, Index k) const { + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + return i * m_inputStrides[offset] + j * m_inputStrides[offset + 1] + + k * m_inputStrides[offset + 2]; + } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Index mapCudaOutputKernelToTensorOutputOffset(Index i, Index j, Index k) const { + const size_t offset = static_cast(Layout) == static_cast(ColMajor) + ? 0 + : NumDims - NumKernelDims; + return i * m_outputStrides[offset] + j * m_outputStrides[offset + 1] + + k * m_outputStrides[offset + 2]; + } + + private: + static const int NumDims = internal::array_size::value; + array m_inputStrides; + array m_outputStrides; + array m_cudaInputStrides; + array m_cudaOutputStrides; +}; + + + +template +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs are different. + typedef typename promote_storage_type::ret Scalar; + typedef typename promote_storage_type::StorageKind, + typename traits::StorageKind>::ret StorageKind; + typedef typename promote_index_type::Index, + typename traits::Index>::type Index; + typedef typename InputXprType::Nested LhsNested; + typedef typename KernelXprType::Nested RhsNested; + typedef typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _RhsNested; + static const int NumDimensions = traits::NumDimensions; + static const int Layout = traits::Layout; + + enum { + Flags = 0 + }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorConvolutionOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorConvolutionOp type; +}; + +} // end namespace internal + + + +template +class TensorConvolutionOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::promote_storage_type::ret CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorConvolutionOp(const InputXprType& input, const KernelXprType& kernel, const Indices& dims) + : m_input_xpr(input), m_kernel_xpr(kernel), m_indices(dims) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const Indices& indices() const { return m_indices; } + + /** \returns the nested expressions */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const typename internal::remove_all::type& + inputExpression() const { return m_input_xpr; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const typename internal::remove_all::type& + kernelExpression() const { return m_kernel_xpr; } + + protected: + typename InputXprType::Nested m_input_xpr; + typename KernelXprType::Nested m_kernel_xpr; + const Indices m_indices; +}; + + +template +struct TensorEvaluator, Device> +{ + typedef TensorConvolutionOp XprType; + + static const int NumDims = internal::array_size::Dimensions>::value; + static const int NumKernelDims = internal::array_size::value; + typedef typename XprType::Index Index; + typedef DSizes Dimensions; + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = TensorEvaluator::IsAligned & TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_inputImpl(op.inputExpression(), device), m_kernelImpl(op.kernelExpression(), device), m_kernelArg(op.kernelExpression()), m_kernel(NULL), m_local_kernel(false), m_device(device) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout)), YOU_MADE_A_PROGRAMMING_MISTAKE); + + const typename TensorEvaluator::Dimensions& input_dims = m_inputImpl.dimensions(); + const typename TensorEvaluator::Dimensions& kernel_dims = m_kernelImpl.dimensions(); + + if (static_cast(Layout) == static_cast(ColMajor)) { + m_inputStride[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_inputStride[i] = m_inputStride[i - 1] * input_dims[i - 1]; + } + } else { + m_inputStride[NumDims - 1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_inputStride[i] = m_inputStride[i + 1] * input_dims[i + 1]; + } + } + + m_dimensions = m_inputImpl.dimensions(); + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < NumKernelDims; ++i) { + const Index index = op.indices()[i]; + const Index input_dim = input_dims[index]; + const Index kernel_dim = kernel_dims[i]; + const Index result_dim = input_dim - kernel_dim + 1; + m_dimensions[index] = result_dim; + if (i > 0) { + m_kernelStride[i] = m_kernelStride[i - 1] * kernel_dims[i - 1]; + } else { + m_kernelStride[0] = 1; + } + m_indexStride[i] = m_inputStride[index]; + } + + m_outputStride[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_outputStride[i] = m_outputStride[i - 1] * m_dimensions[i - 1]; + } + } else { + for (int i = NumKernelDims - 1; i >= 0; --i) { + const Index index = op.indices()[i]; + const Index input_dim = input_dims[index]; + const Index kernel_dim = kernel_dims[i]; + const Index result_dim = input_dim - kernel_dim + 1; + m_dimensions[index] = result_dim; + if (i < NumKernelDims - 1) { + m_kernelStride[i] = m_kernelStride[i + 1] * kernel_dims[i + 1]; + } else { + m_kernelStride[NumKernelDims - 1] = 1; + } + m_indexStride[i] = m_inputStride[index]; + } + + m_outputStride[NumDims - 1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_outputStride[i] = m_outputStride[i + 1] * m_dimensions[i + 1]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar*) { + m_inputImpl.evalSubExprsIfNeeded(NULL); + preloadKernel(); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_inputImpl.cleanup(); + if (m_local_kernel) { + m_device.deallocate((void*)m_kernel); + m_local_kernel = false; + } + m_kernel = NULL; + } + + void evalTo(typename XprType::Scalar* buffer) { + evalSubExprsIfNeeded(NULL); + for (int i = 0; i < dimensions().TotalSize(); ++i) { + buffer[i] += coeff(i); + } + cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + CoeffReturnType result = CoeffReturnType(0); + convolve(firstInput(index), 0, NumKernelDims-1, result); + return result; + } + + template + EIGEN_DEVICE_FUNC PacketReturnType packet(const Index index) const + { + Index indices[2] = {index, index+PacketSize-1}; + Index startInputs[2] = {0, 0}; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx0 = indices[0] / m_outputStride[i]; + const Index idx1 = indices[1] / m_outputStride[i]; + startInputs[0] += idx0 * m_inputStride[i]; + startInputs[1] += idx1 * m_inputStride[i]; + indices[0] -= idx0 * m_outputStride[i]; + indices[1] -= idx1 * m_outputStride[i]; + } + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx0 = indices[0] / m_outputStride[i]; + const Index idx1 = indices[1] / m_outputStride[i]; + startInputs[0] += idx0 * m_inputStride[i]; + startInputs[1] += idx1 * m_inputStride[i]; + indices[0] -= idx0 * m_outputStride[i]; + indices[1] -= idx1 * m_outputStride[i]; + } + } + startInputs[0] += indices[0]; + startInputs[1] += indices[1]; + + if (startInputs[1]-startInputs[0] == PacketSize-1) { + PacketReturnType result = internal::pset1(0); + convolvePacket(startInputs[0], 0, NumKernelDims-1, result); + return result; + } else { + EIGEN_ALIGN_MAX Scalar data[PacketSize]; + data[0] = Scalar(0); + convolve(startInputs[0], 0, NumKernelDims-1, data[0]); + for (int i = 1; i < PacketSize-1; ++i) { + data[i] = Scalar(0); + convolve(firstInput(index+i), 0, NumKernelDims-1, data[i]); + } + data[PacketSize-1] = Scalar(0); + convolve(startInputs[1], 0, NumKernelDims-1, data[PacketSize-1]); + return internal::pload(data); + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + const double kernel_size = m_kernelImpl.dimensions().TotalSize(); + // We ignore the use of fused multiply-add. + const double convolve_compute_cost = + TensorOpCost::AddCost() + TensorOpCost::MulCost(); + const double firstIndex_compute_cost = + NumDims * + (2 * TensorOpCost::AddCost() + 2 * TensorOpCost::MulCost() + + TensorOpCost::DivCost()); + return TensorOpCost(0, 0, firstIndex_compute_cost, vectorized, PacketSize) + + kernel_size * (m_inputImpl.costPerCoeff(vectorized) + + m_kernelImpl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, convolve_compute_cost, vectorized, + PacketSize)); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + private: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index firstInput(Index index) const { + Index startInput = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_outputStride[i]; + startInput += idx * m_inputStride[i]; + index -= idx * m_outputStride[i]; + } + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_outputStride[i]; + startInput += idx * m_inputStride[i]; + index -= idx * m_outputStride[i]; + } + } + startInput += index; + return startInput; + } + + EIGEN_DEVICE_FUNC void convolve(Index firstIndex, Index firstKernel, int DimIndex, CoeffReturnType& accum) const { + for (int j = 0; j < m_kernelImpl.dimensions()[DimIndex]; ++j) { + const Index input = firstIndex + j * m_indexStride[DimIndex]; + const Index kernel = firstKernel + j * m_kernelStride[DimIndex]; + if (DimIndex > 0) { + convolve(input, kernel, DimIndex-1, accum); + } else { + accum += m_inputImpl.coeff(input) * m_kernel[kernel]; + } + } + } + + template + EIGEN_DEVICE_FUNC void convolvePacket(Index firstIndex, Index firstKernel, int DimIndex, Packet& accum) const { + for (int j = 0; j < m_kernelImpl.dimensions()[DimIndex]; ++j) { + const Index input = firstIndex + j * m_indexStride[DimIndex]; + const Index kernel = firstKernel + j * m_kernelStride[DimIndex]; + if (DimIndex > 0) { + convolvePacket(input, kernel, DimIndex-1, accum); + } else { + accum = internal::pmadd(m_inputImpl.template packet(input), internal::pset1(m_kernel[kernel]), accum); + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void preloadKernel() { + // Don't make a local copy of the kernel unless we have to (i.e. it's an + // expression that needs to be evaluated) + const Scalar* in_place = m_kernelImpl.data(); + if (in_place) { + m_kernel = in_place; + m_local_kernel = false; + } else { + size_t kernel_sz = m_kernelImpl.dimensions().TotalSize() * sizeof(Scalar); + Scalar* local = (Scalar*)m_device.allocate(kernel_sz); + typedef TensorEvalToOp EvalTo; + EvalTo evalToTmp(local, m_kernelArg); + const bool PacketAccess = internal::IsVectorizable::value; + internal::TensorExecutor::run(evalToTmp, m_device); + + m_kernel = local; + m_local_kernel = true; + } + } + + array m_inputStride; + array m_outputStride; + + array m_indexStride; + array m_kernelStride; + TensorEvaluator m_inputImpl; + TensorEvaluator m_kernelImpl; + Dimensions m_dimensions; + + KernelArgType m_kernelArg; + const Scalar* m_kernel; + bool m_local_kernel; + const Device& m_device; +}; + + + + +// Use an optimized implementation of the evaluation code for GPUs whenever possible. +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) + +template +struct GetKernelSize { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int operator() (const int /*kernelSize*/) const { + return StaticKernelSize; + } +}; +template <> +struct GetKernelSize { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int operator() (const int kernelSize) const { + return kernelSize; + } +}; + +template +__global__ void EigenConvolutionKernel1D( + InputEvaluator eval, + const internal::IndexMapper + indexMapper, + const float* __restrict kernel, const int numPlanes, const int numX, + const int maxX, const int kernelSize, float* buffer) { + extern __shared__ float s[]; + + const int first_x = blockIdx.x * maxX; + const int last_x = (first_x + maxX < numX ? first_x + maxX : numX) - 1; + const int num_x_input = last_x - first_x + GetKernelSize()(kernelSize); + const int num_x_output = last_x - first_x + 1; + + const int first_plane = blockIdx.y * blockDim.y; + const int plane_stride = blockDim.y * gridDim.y; + + for (int p = first_plane + threadIdx.y; p < numPlanes; p += plane_stride) { + // Load inputs to shared memory + const int plane_input_offset = indexMapper.mapCudaInputPlaneToTensorInputOffset(p); + const int plane_kernel_offset = threadIdx.y * num_x_input; + #pragma unroll + for (int i = threadIdx.x; i < num_x_input; i += blockDim.x) { + const int tensor_index = plane_input_offset + indexMapper.mapCudaInputKernelToTensorInputOffset(i+first_x); + s[i + plane_kernel_offset] = eval.coeff(tensor_index); + } + + __syncthreads(); + + // Compute the convolution + const int plane_output_offset = indexMapper.mapCudaOutputPlaneToTensorOutputOffset(p); + + #pragma unroll + for (int i = threadIdx.x; i < num_x_output; i += blockDim.x) { + const int kernel_offset = plane_kernel_offset + i; + float result = 0.0f; + #pragma unroll + for (int k = 0; k < GetKernelSize()(kernelSize); ++k) { + result += s[k + kernel_offset] * kernel[k]; + } + const int tensor_index = plane_output_offset + indexMapper.mapCudaOutputKernelToTensorOutputOffset(i+first_x); + buffer[tensor_index] = result; + } + __syncthreads(); + } +}; + +template +__global__ void EigenConvolutionKernel2D( + InputEvaluator eval, + const internal::IndexMapper + indexMapper, + const float* __restrict kernel, const int numPlanes, const int numX, + const int maxX, const int numY, const int maxY, const int kernelSizeX, + const int kernelSizeY, float* buffer) { + extern __shared__ float s[]; + + const int first_x = blockIdx.x * maxX; + const int last_x = (first_x + maxX < numX ? first_x + maxX : numX) - 1; + const int num_x_input = last_x - first_x + GetKernelSize()(kernelSizeX); + const int num_x_output = last_x - first_x + 1; + + const int first_y = blockIdx.y * maxY; + const int last_y = (first_y + maxY < numY ? first_y + maxY : numY) - 1; + const int num_y_input = last_y - first_y + GetKernelSize()(kernelSizeY); + const int num_y_output = last_y - first_y + 1; + + const int first_plane = blockIdx.z * blockDim.z; + const int plane_stride = blockDim.z * gridDim.z; + + for (int p = first_plane + threadIdx.z; p < numPlanes; p += plane_stride) { + + const int plane_input_offset = indexMapper.mapCudaInputPlaneToTensorInputOffset(p); + const int plane_kernel_offset = threadIdx.z * num_y_input; + + // Load inputs to shared memory + #pragma unroll + for (int j = threadIdx.y; j < num_y_input; j += blockDim.y) { + const int input_offset = num_x_input * (j + plane_kernel_offset); + #pragma unroll + for (int i = threadIdx.x; i < num_x_input; i += blockDim.x) { + const int tensor_index = plane_input_offset + indexMapper.mapCudaInputKernelToTensorInputOffset(i+first_x, j+first_y); + s[i + input_offset] = eval.coeff(tensor_index); + } + } + + __syncthreads(); + + // Convolution + const int plane_output_offset = indexMapper.mapCudaOutputPlaneToTensorOutputOffset(p); + + #pragma unroll + for (int j = threadIdx.y; j < num_y_output; j += blockDim.y) { + #pragma unroll + for (int i = threadIdx.x; i < num_x_output; i += blockDim.x) { + float result = 0.0f; + #pragma unroll + for (int l = 0; l < GetKernelSize()(kernelSizeY); ++l) { + const int kernel_offset = kernelSizeX * l; + const int input_offset = i + num_x_input * (j + l + plane_kernel_offset); + #pragma unroll + for (int k = 0; k < GetKernelSize()(kernelSizeX); ++k) { + result += s[k + input_offset] * kernel[k + kernel_offset]; + } + } + const int tensor_index = plane_output_offset + indexMapper.mapCudaOutputKernelToTensorOutputOffset(i+first_x, j+first_y); + buffer[tensor_index] = result; + } + } + + __syncthreads(); + } +}; + +template +__global__ void EigenConvolutionKernel3D( + InputEvaluator eval, + const internal::IndexMapper + indexMapper, + const float* __restrict kernel, const size_t numPlanes, const size_t numX, + const size_t maxX, const size_t numY, const size_t maxY, const size_t numZ, + const size_t maxZ, const size_t kernelSizeX, const size_t kernelSizeY, + const size_t kernelSizeZ, float* buffer) { + extern __shared__ float s[]; + + // Load inputs to shared memory + const int first_x = blockIdx.x * maxX; + const int last_x = (first_x + maxX < numX ? first_x + maxX : numX) - 1; + const int num_x_input = last_x - first_x + kernelSizeX; + + const int first_y = blockIdx.y * maxY; + const int last_y = (first_y + maxY < numY ? first_y + maxY : numY) - 1; + const int num_y_input = last_y - first_y + kernelSizeY; + + const int first_z = blockIdx.z * maxZ; + const int last_z = (first_z + maxZ < numZ ? first_z + maxZ : numZ) - 1; + const int num_z_input = last_z - first_z + kernelSizeZ; + + for (int p = 0; p < numPlanes; ++p) { + + const int plane_input_offset = indexMapper.mapCudaInputPlaneToTensorInputOffset(p); + const int plane_kernel_offset = 0; + + for (int k = threadIdx.z; k < num_z_input; k += blockDim.z) { + for (int j = threadIdx.y; j < num_y_input; j += blockDim.y) { + for (int i = threadIdx.x; i < num_x_input; i += blockDim.x) { + const int tensor_index = plane_input_offset + indexMapper.mapCudaInputKernelToTensorInputOffset(i+first_x, j+first_y, k+first_z); + s[i + num_x_input * (j + num_y_input * (k + plane_kernel_offset))] = eval.coeff(tensor_index); + } + } + } + + __syncthreads(); + + // Convolution + const int num_z_output = last_z - first_z + 1; + const int num_y_output = last_y - first_y + 1; + const int num_x_output = last_x - first_x + 1; + const int plane_output_offset = indexMapper.mapCudaOutputPlaneToTensorOutputOffset(p); + + for (int k = threadIdx.z; k < num_z_output; k += blockDim.z) { + for (int j = threadIdx.y; j < num_y_output; j += blockDim.y) { + for (int i = threadIdx.x; i < num_x_output; i += blockDim.x) { + float result = 0.0f; + for (int n = 0; n < kernelSizeZ; ++n) { + for (int m = 0; m < kernelSizeY; ++m) { + for (int l = 0; l < kernelSizeX; ++l) { + result += s[i + l + num_x_input * (j + m + num_y_input * (k + n + plane_kernel_offset))] * kernel[l + kernelSizeX * (m + kernelSizeY * n)]; + } + } + } + const int tensor_index = plane_output_offset + indexMapper.mapCudaOutputKernelToTensorOutputOffset(i+first_x, j+first_y, k+first_z); + buffer[tensor_index] = result; + } + } + } + __syncthreads(); + } +}; + + + +template +struct TensorEvaluator, GpuDevice> +{ + typedef TensorConvolutionOp XprType; + + static const int NumDims = internal::array_size::Dimensions>::value; + static const int NumKernelDims = internal::array_size::value; + typedef typename XprType::Index Index; + typedef DSizes Dimensions; + typedef typename TensorEvaluator::Dimensions KernelDimensions; + + enum { + IsAligned = TensorEvaluator::IsAligned & TensorEvaluator::IsAligned, + PacketAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const GpuDevice& device) + : m_inputImpl(op.inputExpression(), device), m_kernelArg(op.kernelExpression()), m_kernelImpl(op.kernelExpression(), device), m_indices(op.indices()), m_buf(NULL), m_kernel(NULL), m_local_kernel(false), m_device(device) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout)), YOU_MADE_A_PROGRAMMING_MISTAKE); + + const typename TensorEvaluator::Dimensions& input_dims = m_inputImpl.dimensions(); + const typename TensorEvaluator::Dimensions& kernel_dims = m_kernelImpl.dimensions(); + + m_dimensions = m_inputImpl.dimensions(); + for (int i = 0; i < NumKernelDims; ++i) { + const Index index = op.indices()[i]; + const Index input_dim = input_dims[index]; + const Index kernel_dim = kernel_dims[i]; + const Index result_dim = input_dim - kernel_dim + 1; + m_dimensions[index] = result_dim; + } + } + + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef typename InputArgType::Scalar Scalar; + static const int PacketSize = internal::unpacket_traits::size; + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* data) { + preloadKernel(); + m_inputImpl.evalSubExprsIfNeeded(NULL); + if (data) { + executeEval(data); + return false; + } else { + m_buf = (Scalar*)m_device.allocate(dimensions().TotalSize() * sizeof(Scalar)); + executeEval(m_buf); + return true; + } + } + + EIGEN_STRONG_INLINE void cleanup() { + m_inputImpl.cleanup(); + if (m_buf) { + m_device.deallocate(m_buf); + m_buf = NULL; + } + if (m_local_kernel) { + m_device.deallocate((void*)m_kernel); + m_local_kernel = false; + } + m_kernel = NULL; + } + + EIGEN_STRONG_INLINE void preloadKernel() { + // Don't make a local copy of the kernel unless we have to (i.e. it's an + // expression that needs to be evaluated) + const Scalar* in_place = m_kernelImpl.data(); + if (in_place) { + m_kernel = in_place; + m_local_kernel = false; + } else { + size_t kernel_sz = m_kernelImpl.dimensions().TotalSize() * sizeof(Scalar); + Scalar* local = (Scalar*)m_device.allocate(kernel_sz); + typedef TensorEvalToOp EvalTo; + EvalTo evalToTmp(local, m_kernelArg); + const bool PacketAccess = internal::IsVectorizable::value; + internal::TensorExecutor::run(evalToTmp, m_device); + + m_kernel = local; + m_local_kernel = true; + } + } + + static unsigned int ceil(unsigned int num, unsigned int denom) { + const unsigned int rounded_toward_zero = num / denom; + if (num > rounded_toward_zero * denom) { + return rounded_toward_zero + 1; + } + return rounded_toward_zero; + } + + void executeEval(Scalar* data) const { + typedef typename TensorEvaluator::Dimensions InputDims; + + const int maxSharedMem = m_device.sharedMemPerBlock(); + const int maxThreadsPerBlock = m_device.maxCudaThreadsPerBlock(); + const int maxBlocksPerProcessor = m_device.maxCudaThreadsPerMultiProcessor() / maxThreadsPerBlock; + const int numMultiProcessors = m_device.getNumCudaMultiProcessors(); + const int warpSize = 32; + + switch (NumKernelDims) { + case 1: { + const int kernel_size = m_kernelImpl.dimensions().TotalSize(); + + const int numX = dimensions()[m_indices[0]]; + const int numP = dimensions().TotalSize() / numX; + int maxX; + dim3 block_size; + + const int single_stride_dim = + static_cast(Layout) == static_cast(ColMajor) + ? 0 + : m_inputImpl.dimensions().rank() - 1; + if (m_indices[0] == single_stride_dim) { + // Maximum the reuse + const int inner_dim = ((maxSharedMem / (sizeof(Scalar)) - kernel_size + 1 + 31) / 32) * 32; + maxX = numext::mini(inner_dim, numX); + const int maxP = numext::mini(maxSharedMem / ((kernel_size - 1 + maxX) * sizeof(Scalar)), numP); + block_size.x = numext::mini(maxThreadsPerBlock, maxX); + block_size.y = numext::mini(maxThreadsPerBlock / block_size.x, maxP); + } + else { + // Read as much as possible alongside the inner most dimension, that is the plane + const int inner_dim = maxSharedMem / ((warpSize + kernel_size) * sizeof(Scalar)); + const int maxP = numext::mini(inner_dim, numP); + maxX = numext::mini(maxSharedMem / (inner_dim * sizeof(Scalar)) - kernel_size + 1, numX); + + block_size.x = numext::mini(warpSize, maxX); + block_size.y = numext::mini(maxThreadsPerBlock/block_size.x, maxP); + } + + const int shared_mem = block_size.y * (maxX + kernel_size - 1) * sizeof(Scalar); + assert(shared_mem <= maxSharedMem); + + const int num_x_blocks = ceil(numX, maxX); + const int blocksPerProcessor = numext::mini(maxBlocksPerProcessor, maxSharedMem / shared_mem); + const int num_y_blocks = ceil(numMultiProcessors * blocksPerProcessor, num_x_blocks); + + dim3 num_blocks(num_x_blocks, numext::mini(num_y_blocks, ceil(numP, block_size.y))); + + + //cout << "launching 1D kernel with block_size.x: " << block_size.x << " block_size.y: " << block_size.y << " num_blocks.x: " << num_blocks.x << " num_blocks.y: " << num_blocks.y << " maxX: " << maxX << " shared_mem: " << shared_mem << " in stream " << m_device.stream() << endl; + + const array indices(m_indices[0]); + const array kernel_dims(m_kernelImpl.dimensions()[0]); + internal::IndexMapper indexMapper( + m_inputImpl.dimensions(), kernel_dims, indices); + switch(kernel_size) { + case 4: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel1D, Index, InputDims, 4>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, 4, data); + break; + } + case 7: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel1D, Index, InputDims, 7>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, 7, data); + break; + } + default: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel1D, Index, InputDims, Dynamic>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, kernel_size, data); + } + } + break; + } + + case 2: { + const int idxX = + static_cast(Layout) == static_cast(ColMajor) ? 0 : 1; + const int idxY = + static_cast(Layout) == static_cast(ColMajor) ? 1 : 0; + const int kernel_size_x = m_kernelImpl.dimensions()[idxX]; + const int kernel_size_y = m_kernelImpl.dimensions()[idxY]; + + const int numX = dimensions()[m_indices[idxX]]; + const int numY = dimensions()[m_indices[idxY]]; + const int numP = dimensions().TotalSize() / (numX*numY); + + const float scaling_factor = sqrtf(static_cast(maxSharedMem) / (sizeof(Scalar) * kernel_size_y * kernel_size_x)); + + // Snap maxX to warp size + int inner_dim = ((static_cast(scaling_factor * kernel_size_x) - kernel_size_x + 1 + 32) / 32) * 32; + const int maxX = numext::mini(inner_dim, numX); + const int maxY = numext::mini(maxSharedMem / (sizeof(Scalar) * (maxX + kernel_size_x - 1)) - kernel_size_y + 1, numY); + const int maxP = numext::mini(maxSharedMem / ((kernel_size_x - 1 + maxX) * (kernel_size_y - 1 + maxY) * sizeof(Scalar)), numP); + + dim3 block_size; + block_size.x = numext::mini(1024, maxX); + block_size.y = numext::mini(1024/block_size.x, maxY); + block_size.z = numext::mini(1024/(block_size.x*block_size.y), maxP); + + const int shared_mem = block_size.z * (maxX + kernel_size_x - 1) * (maxY + kernel_size_y - 1) * sizeof(Scalar); + assert(shared_mem <= maxSharedMem); + + const int num_x_blocks = ceil(numX, maxX); + const int num_y_blocks = ceil(numY, maxY); + const int blocksPerProcessor = numext::mini(maxBlocksPerProcessor, maxSharedMem / shared_mem); + const int num_z_blocks = ceil(numMultiProcessors * blocksPerProcessor, num_x_blocks * num_y_blocks); + + dim3 num_blocks(num_x_blocks, num_y_blocks, numext::mini(num_z_blocks, ceil(numP, block_size.z))); + + + //cout << "launching 2D kernel with block_size.x: " << block_size.x << " block_size.y: " << block_size.y << " block_size.z: " << block_size.z << " num_blocks.x: " << num_blocks.x << " num_blocks.y: " << num_blocks.y << " num_blocks.z: " << num_blocks.z << " maxX: " << maxX << " maxY: " << maxY << " maxP: " << maxP << " shared_mem: " << shared_mem << " in stream " << m_device.stream() << endl; + + const array indices(m_indices[idxX], m_indices[idxY]); + const array kernel_dims(m_kernelImpl.dimensions()[idxX], + m_kernelImpl.dimensions()[idxY]); + internal::IndexMapper indexMapper( + m_inputImpl.dimensions(), kernel_dims, indices); + switch (kernel_size_x) { + case 4: { + switch (kernel_size_y) { + case 7: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel2D, Index, InputDims, 4, 7>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, numY, maxY, 4, 7, data); + break; + } + default: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel2D, Index, InputDims, 4, Dynamic>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, numY, maxY, 4, kernel_size_y, data); + break; + } + } + break; + } + case 7: { + switch (kernel_size_y) { + case 4: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel2D, Index, InputDims, 7, 4>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, numY, maxY, 7, 4, data); + break; + } + default: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel2D, Index, InputDims, 7, Dynamic>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, numY, maxY, 7, kernel_size_y, data); + break; + } + } + break; + } + default: { + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel2D, Index, InputDims, Dynamic, Dynamic>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, numY, maxY, kernel_size_x, kernel_size_y, data); + break; + } + } + break; + } + + case 3: { + const int idxX = + static_cast(Layout) == static_cast(ColMajor) ? 0 : 2; + const int idxY = + static_cast(Layout) == static_cast(ColMajor) ? 1 : 1; + const int idxZ = + static_cast(Layout) == static_cast(ColMajor) ? 2 : 0; + + const int kernel_size_x = m_kernelImpl.dimensions()[idxX]; + const int kernel_size_y = m_kernelImpl.dimensions()[idxY]; + const int kernel_size_z = m_kernelImpl.dimensions()[idxZ]; + + const int numX = dimensions()[m_indices[idxX]]; + const int numY = dimensions()[m_indices[idxY]]; + const int numZ = dimensions()[m_indices[idxZ]]; + const int numP = dimensions().TotalSize() / (numX*numY*numZ); + + const int maxX = numext::mini(128, numext::mini(maxSharedMem / (sizeof(Scalar) * kernel_size_y * kernel_size_z) - kernel_size_x + 1, numX)); + const int maxY = numext::mini(128, numext::mini(maxSharedMem / (sizeof(Scalar) * (maxX + kernel_size_x - 1) * kernel_size_z) - kernel_size_y + 1, numY)); + const int maxZ = numext::mini(128, numext::mini(maxSharedMem / (sizeof(Scalar) * (maxX + kernel_size_x - 1) * (maxY + kernel_size_y - 1)) - kernel_size_z + 1, numZ)); + + dim3 block_size; + block_size.x = numext::mini(32, maxX); + block_size.y = numext::mini(32, maxY); + block_size.z = numext::mini(1024/(block_size.x*block_size.y), maxZ); + dim3 num_blocks(ceil(numX, maxX), ceil(numY, maxY), ceil(numZ, maxZ)); + + const int shared_mem = (maxX + kernel_size_x - 1) * (maxY + kernel_size_y - 1) * (maxZ + kernel_size_z - 1) * sizeof(Scalar); + assert(shared_mem <= maxSharedMem); + + //cout << "launching 3D kernel with block_size.x: " << block_size.x << " block_size.y: " << block_size.y << " block_size.z: " << block_size.z << " num_blocks.x: " << num_blocks.x << " num_blocks.y: " << num_blocks.y << " num_blocks.z: " << num_blocks.z << " shared_mem: " << shared_mem << " in stream " << m_device.stream() << endl; + const array indices(m_indices[idxX], m_indices[idxY], + m_indices[idxZ]); + const array kernel_dims(m_kernelImpl.dimensions()[idxX], + m_kernelImpl.dimensions()[idxY], + m_kernelImpl.dimensions()[idxZ]); + internal::IndexMapper indexMapper( + m_inputImpl.dimensions(), kernel_dims, indices); + + LAUNCH_CUDA_KERNEL((EigenConvolutionKernel3D, Index, InputDims>), num_blocks, block_size, shared_mem, m_device, m_inputImpl, indexMapper, m_kernel, numP, numX, maxX, numY, maxY, numZ, maxZ, kernel_size_x, kernel_size_y, kernel_size_z, data); + break; + } + + default: { + EIGEN_STATIC_ASSERT((NumKernelDims >= 1 && NumKernelDims <= 3), THIS_METHOD_IS_ONLY_FOR_OBJECTS_OF_A_SPECIFIC_SIZE); + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + eigen_assert(m_buf); + eigen_assert(index < m_dimensions.TotalSize()); + return m_buf[index]; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(const Index index) const + { + eigen_assert(m_buf); + eigen_assert(index < m_dimensions.TotalSize()); + return internal::ploadt(m_buf+index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + // TODO(rmlarsen): FIXME: For now, this is just a copy of the CPU cost + // model. + const double kernel_size = m_kernelImpl.dimensions().TotalSize(); + // We ignore the use of fused multiply-add. + const double convolve_compute_cost = + TensorOpCost::AddCost() + TensorOpCost::MulCost(); + const double firstIndex_compute_cost = + NumDims * + (2 * TensorOpCost::AddCost() + 2 * TensorOpCost::MulCost() + + TensorOpCost::DivCost()); + return TensorOpCost(0, 0, firstIndex_compute_cost, vectorized, PacketSize) + + kernel_size * (m_inputImpl.costPerCoeff(vectorized) + + m_kernelImpl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, convolve_compute_cost, vectorized, + PacketSize)); + } + + private: + // No assignment (copies are needed by the kernels) + TensorEvaluator& operator = (const TensorEvaluator&); + + TensorEvaluator m_inputImpl; + TensorEvaluator m_kernelImpl; + KernelArgType m_kernelArg; + Indices m_indices; + Dimensions m_dimensions; + Scalar* m_buf; + const Scalar* m_kernel; + bool m_local_kernel; + + const GpuDevice& m_device; +}; +#endif + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CONVOLUTION_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCostModel.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCostModel.h new file mode 100644 index 0000000000..83c449cf19 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCostModel.h @@ -0,0 +1,212 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Rasmus Munk Larsen +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_COST_MODEL_H +#define EIGEN_CXX11_TENSOR_TENSOR_COST_MODEL_H + +namespace Eigen { + +/** \class TensorEvaluator + * \ingroup CXX11_Tensor_Module + * + * \brief A cost model used to limit the number of threads used for evaluating + * tensor expression. + * + */ + +// Class storing the cost of evaluating a tensor expression in terms of the +// estimated number of operand bytes loads, bytes stored, and compute cycles. +class TensorOpCost { + public: + // TODO(rmlarsen): Fix the scalar op costs in Eigen proper. Even a simple + // model based on minimal reciprocal throughput numbers from Intel or + // Agner Fog's tables would be better than what is there now. + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int MulCost() { + return internal::functor_traits< + internal::scalar_product_op >::Cost; + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int AddCost() { + return internal::functor_traits >::Cost; + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int DivCost() { + return internal::functor_traits< + internal::scalar_quotient_op >::Cost; + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int ModCost() { + return internal::functor_traits >::Cost; + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int CastCost() { + return internal::functor_traits< + internal::scalar_cast_op >::Cost; + } + + EIGEN_DEVICE_FUNC + TensorOpCost() : bytes_loaded_(0), bytes_stored_(0), compute_cycles_(0) {} + EIGEN_DEVICE_FUNC + TensorOpCost(double bytes_loaded, double bytes_stored, double compute_cycles) + : bytes_loaded_(bytes_loaded), + bytes_stored_(bytes_stored), + compute_cycles_(compute_cycles) {} + + EIGEN_DEVICE_FUNC + TensorOpCost(double bytes_loaded, double bytes_stored, double compute_cycles, + bool vectorized, double packet_size) + : bytes_loaded_(bytes_loaded), + bytes_stored_(bytes_stored), + compute_cycles_(vectorized ? compute_cycles / packet_size + : compute_cycles) { + eigen_assert(bytes_loaded >= 0 && (numext::isfinite)(bytes_loaded)); + eigen_assert(bytes_stored >= 0 && (numext::isfinite)(bytes_stored)); + eigen_assert(compute_cycles >= 0 && (numext::isfinite)(compute_cycles)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double bytes_loaded() const { + return bytes_loaded_; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double bytes_stored() const { + return bytes_stored_; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double compute_cycles() const { + return compute_cycles_; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double total_cost( + double load_cost, double store_cost, double compute_cost) const { + return load_cost * bytes_loaded_ + store_cost * bytes_stored_ + + compute_cost * compute_cycles_; + } + + // Drop memory access component. Intended for cases when memory accesses are + // sequential or are completely masked by computations. + EIGEN_DEVICE_FUNC void dropMemoryCost() { + bytes_loaded_ = 0; + bytes_stored_ = 0; + } + + // TODO(rmlarsen): Define min in terms of total cost, not elementwise. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost cwiseMin( + const TensorOpCost& rhs) const { + double bytes_loaded = numext::mini(bytes_loaded_, rhs.bytes_loaded()); + double bytes_stored = numext::mini(bytes_stored_, rhs.bytes_stored()); + double compute_cycles = numext::mini(compute_cycles_, rhs.compute_cycles()); + return TensorOpCost(bytes_loaded, bytes_stored, compute_cycles); + } + + // TODO(rmlarsen): Define max in terms of total cost, not elementwise. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost cwiseMax( + const TensorOpCost& rhs) const { + double bytes_loaded = numext::maxi(bytes_loaded_, rhs.bytes_loaded()); + double bytes_stored = numext::maxi(bytes_stored_, rhs.bytes_stored()); + double compute_cycles = numext::maxi(compute_cycles_, rhs.compute_cycles()); + return TensorOpCost(bytes_loaded, bytes_stored, compute_cycles); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost& operator+=( + const TensorOpCost& rhs) { + bytes_loaded_ += rhs.bytes_loaded(); + bytes_stored_ += rhs.bytes_stored(); + compute_cycles_ += rhs.compute_cycles(); + return *this; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost& operator*=(double rhs) { + bytes_loaded_ *= rhs; + bytes_stored_ *= rhs; + compute_cycles_ *= rhs; + return *this; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE friend TensorOpCost operator+( + TensorOpCost lhs, const TensorOpCost& rhs) { + lhs += rhs; + return lhs; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE friend TensorOpCost operator*( + TensorOpCost lhs, double rhs) { + lhs *= rhs; + return lhs; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE friend TensorOpCost operator*( + double lhs, TensorOpCost rhs) { + rhs *= lhs; + return rhs; + } + + friend std::ostream& operator<<(std::ostream& os, const TensorOpCost& tc) { + return os << "[bytes_loaded = " << tc.bytes_loaded() + << ", bytes_stored = " << tc.bytes_stored() + << ", compute_cycles = " << tc.compute_cycles() << "]"; + } + + private: + double bytes_loaded_; + double bytes_stored_; + double compute_cycles_; +}; + +// TODO(rmlarsen): Implement a policy that chooses an "optimal" number of theads +// in [1:max_threads] instead of just switching multi-threading off for small +// work units. +template +class TensorCostModel { + public: + // Scaling from Eigen compute cost to device cycles. + static const int kDeviceCyclesPerComputeCycle = 1; + + // Costs in device cycles. + static const int kStartupCycles = 100000; + static const int kPerThreadCycles = 100000; + static const int kTaskSize = 40000; + + // Returns the number of threads in [1:max_threads] to use for + // evaluating an expression with the given output size and cost per + // coefficient. + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int numThreads( + double output_size, const TensorOpCost& cost_per_coeff, int max_threads) { + double cost = totalCost(output_size, cost_per_coeff); + int threads = (cost - kStartupCycles) / kPerThreadCycles + 0.9; + return numext::mini(max_threads, numext::maxi(1, threads)); + } + + // taskSize assesses parallel task size. + // Value of 1.0 means ideal parallel task size. Values < 1.0 mean that task + // granularity needs to be increased to mitigate parallelization overheads. + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double taskSize( + double output_size, const TensorOpCost& cost_per_coeff) { + return totalCost(output_size, cost_per_coeff) / kTaskSize; + } + + private: + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double totalCost( + double output_size, const TensorOpCost& cost_per_coeff) { + // Cost of memory fetches from L2 cache. 64 is typical cache line size. + // 11 is L2 cache latency on Haswell. + // We don't know whether data is in L1, L2 or L3. But we are most interested + // in single-threaded computational time around 100us-10ms (smaller time + // is too small for parallelization, larger time is not intersting + // either because we are probably using all available threads already). + // And for the target time range, L2 seems to be what matters. Data set + // fitting into L1 is too small to take noticeable time. Data set fitting + // only into L3 presumably will take more than 10ms to load and process. + const double kLoadCycles = 1.0 / 64 * 11; + const double kStoreCycles = 1.0 / 64 * 11; + // Scaling from Eigen compute cost to device cycles. + return output_size * + cost_per_coeff.total_cost(kLoadCycles, kStoreCycles, + kDeviceCyclesPerComputeCycle); + } +}; + +} // namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_COST_MODEL_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCustomOp.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCustomOp.h new file mode 100644 index 0000000000..e020d076f0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCustomOp.h @@ -0,0 +1,313 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_CUSTOM_OP_H +#define EIGEN_CXX11_TENSOR_TENSOR_CUSTOM_OP_H + +namespace Eigen { + +/** \class TensorCustomUnaryOp + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor custom class. + * + * + */ +namespace internal { +template +struct traits > +{ + typedef typename XprType::Scalar Scalar; + typedef typename XprType::StorageKind StorageKind; + typedef typename XprType::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = traits::NumDimensions; + static const int Layout = traits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorCustomUnaryOp& type; +}; + +template +struct nested > +{ + typedef TensorCustomUnaryOp type; +}; + +} // end namespace internal + + + +template +class TensorCustomUnaryOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename internal::nested::type Nested; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorCustomUnaryOp(const XprType& expr, const CustomUnaryFunc& func) + : m_expr(expr), m_func(func) {} + + EIGEN_DEVICE_FUNC + const CustomUnaryFunc& func() const { return m_func; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_expr; } + + protected: + typename XprType::Nested m_expr; + const CustomUnaryFunc m_func; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorCustomUnaryOp ArgType; + typedef typename internal::traits::Index Index; + static const int NumDims = internal::traits::NumDimensions; + typedef DSizes Dimensions; + typedef typename internal::remove_const::type Scalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = (internal::packet_traits::size > 1), + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const ArgType& op, const Device& device) + : m_op(op), m_device(device), m_result(NULL) + { + m_dimensions = op.func().dimensions(op.expression()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* data) { + if (data) { + evalTo(data); + return false; + } else { + m_result = static_cast( + m_device.allocate(dimensions().TotalSize() * sizeof(Scalar))); + evalTo(m_result); + return true; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + if (m_result != NULL) { + m_device.deallocate(m_result); + m_result = NULL; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_result[index]; + } + + template + EIGEN_DEVICE_FUNC PacketReturnType packet(Index index) const { + return internal::ploadt(m_result + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + // TODO(rmlarsen): Extend CustomOp API to return its cost estimate. + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return m_result; } + + protected: + EIGEN_DEVICE_FUNC void evalTo(Scalar* data) { + TensorMap > result( + data, m_dimensions); + m_op.func().eval(m_op.expression(), result, m_device); + } + + Dimensions m_dimensions; + const ArgType m_op; + const Device& m_device; + CoeffReturnType* m_result; +}; + + + +/** \class TensorCustomBinaryOp + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor custom class. + * + * + */ +namespace internal { +template +struct traits > +{ + typedef typename internal::promote_storage_type::ret Scalar; + typedef typename internal::promote_storage_type::ret CoeffReturnType; + typedef typename promote_storage_type::StorageKind, + typename traits::StorageKind>::ret StorageKind; + typedef typename promote_index_type::Index, + typename traits::Index>::type Index; + typedef typename LhsXprType::Nested LhsNested; + typedef typename RhsXprType::Nested RhsNested; + typedef typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _RhsNested; + static const int NumDimensions = traits::NumDimensions; + static const int Layout = traits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorCustomBinaryOp& type; +}; + +template +struct nested > +{ + typedef TensorCustomBinaryOp type; +}; + +} // end namespace internal + + + +template +class TensorCustomBinaryOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::traits::CoeffReturnType CoeffReturnType; + typedef typename internal::nested::type Nested; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorCustomBinaryOp(const LhsXprType& lhs, const RhsXprType& rhs, const CustomBinaryFunc& func) + + : m_lhs_xpr(lhs), m_rhs_xpr(rhs), m_func(func) {} + + EIGEN_DEVICE_FUNC + const CustomBinaryFunc& func() const { return m_func; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + lhsExpression() const { return m_lhs_xpr; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + rhsExpression() const { return m_rhs_xpr; } + + protected: + typename LhsXprType::Nested m_lhs_xpr; + typename RhsXprType::Nested m_rhs_xpr; + const CustomBinaryFunc m_func; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorCustomBinaryOp XprType; + typedef typename internal::traits::Index Index; + static const int NumDims = internal::traits::NumDimensions; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = (internal::packet_traits::size > 1), + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_op(op), m_device(device), m_result(NULL) + { + m_dimensions = op.func().dimensions(op.lhsExpression(), op.rhsExpression()); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* data) { + if (data) { + evalTo(data); + return false; + } else { + m_result = static_cast(m_device.allocate(dimensions().TotalSize() * sizeof(Scalar))); + evalTo(m_result); + return true; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + if (m_result != NULL) { + m_device.deallocate(m_result); + m_result = NULL; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + return m_result[index]; + } + + template + EIGEN_DEVICE_FUNC PacketReturnType packet(Index index) const { + return internal::ploadt(m_result + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + // TODO(rmlarsen): Extend CustomOp API to return its cost estimate. + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return m_result; } + + protected: + EIGEN_DEVICE_FUNC void evalTo(Scalar* data) { + TensorMap > result(data, m_dimensions); + m_op.func().eval(m_op.lhsExpression(), m_op.rhsExpression(), result, m_device); + } + + Dimensions m_dimensions; + const XprType m_op; + const Device& m_device; + CoeffReturnType* m_result; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_CUSTOM_OP_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDevice.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDevice.h new file mode 100644 index 0000000000..29e50a3b2d --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDevice.h @@ -0,0 +1,68 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_DEVICE_H +#define EIGEN_CXX11_TENSOR_TENSOR_DEVICE_H + +namespace Eigen { + +/** \class TensorDevice + * \ingroup CXX11_Tensor_Module + * + * \brief Pseudo expression providing an operator = that will evaluate its argument + * on the specified computing 'device' (GPU, thread pool, ...) + * + * Example: + * C.device(EIGEN_GPU) = A + B; + * + * Todo: operator *= and /=. + */ + +template class TensorDevice { + public: + TensorDevice(const DeviceType& device, ExpressionType& expression) : m_device(device), m_expression(expression) {} + + template + EIGEN_STRONG_INLINE TensorDevice& operator=(const OtherDerived& other) { + typedef TensorAssignOp Assign; + Assign assign(m_expression, other); + internal::TensorExecutor::run(assign, m_device); + return *this; + } + + template + EIGEN_STRONG_INLINE TensorDevice& operator+=(const OtherDerived& other) { + typedef typename OtherDerived::Scalar Scalar; + typedef TensorCwiseBinaryOp, const ExpressionType, const OtherDerived> Sum; + Sum sum(m_expression, other); + typedef TensorAssignOp Assign; + Assign assign(m_expression, sum); + internal::TensorExecutor::run(assign, m_device); + return *this; + } + + template + EIGEN_STRONG_INLINE TensorDevice& operator-=(const OtherDerived& other) { + typedef typename OtherDerived::Scalar Scalar; + typedef TensorCwiseBinaryOp, const ExpressionType, const OtherDerived> Difference; + Difference difference(m_expression, other); + typedef TensorAssignOp Assign; + Assign assign(m_expression, difference); + internal::TensorExecutor::run(assign, m_device); + return *this; + } + + protected: + const DeviceType& m_device; + ExpressionType& m_expression; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DEVICE_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h new file mode 100644 index 0000000000..4f5767bc7f --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h @@ -0,0 +1,337 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#if defined(EIGEN_USE_GPU) && !defined(EIGEN_CXX11_TENSOR_TENSOR_DEVICE_CUDA_H) +#define EIGEN_CXX11_TENSOR_TENSOR_DEVICE_CUDA_H + +namespace Eigen { + +static const int kCudaScratchSize = 1024; + +// This defines an interface that GPUDevice can take to use +// CUDA streams underneath. +class StreamInterface { + public: + virtual ~StreamInterface() {} + + virtual const cudaStream_t& stream() const = 0; + virtual const cudaDeviceProp& deviceProperties() const = 0; + + // Allocate memory on the actual device where the computation will run + virtual void* allocate(size_t num_bytes) const = 0; + virtual void deallocate(void* buffer) const = 0; + + // Return a scratchpad buffer of size 1k + virtual void* scratchpad() const = 0; + + // Return a semaphore. The semaphore is initially initialized to 0, and + // each kernel using it is responsible for resetting to 0 upon completion + // to maintain the invariant that the semaphore is always equal to 0 upon + // each kernel start. + virtual unsigned int* semaphore() const = 0; +}; + +static cudaDeviceProp* m_deviceProperties; +static bool m_devicePropInitialized = false; + +static void initializeDeviceProp() { + if (!m_devicePropInitialized) { + // Attempts to ensure proper behavior in the case of multiple threads + // calling this function simultaneously. This would be trivial to + // implement if we could use std::mutex, but unfortunately mutex don't + // compile with nvcc, so we resort to atomics and thread fences instead. + // Note that if the caller uses a compiler that doesn't support c++11 we + // can't ensure that the initialization is thread safe. +#if __cplusplus >= 201103L + static std::atomic first(true); + if (first.exchange(false)) { +#else + static bool first = true; + if (first) { + first = false; +#endif + // We're the first thread to reach this point. + int num_devices; + cudaError_t status = cudaGetDeviceCount(&num_devices); + if (status != cudaSuccess) { + std::cerr << "Failed to get the number of CUDA devices: " + << cudaGetErrorString(status) + << std::endl; + assert(status == cudaSuccess); + } + m_deviceProperties = new cudaDeviceProp[num_devices]; + for (int i = 0; i < num_devices; ++i) { + status = cudaGetDeviceProperties(&m_deviceProperties[i], i); + if (status != cudaSuccess) { + std::cerr << "Failed to initialize CUDA device #" + << i + << ": " + << cudaGetErrorString(status) + << std::endl; + assert(status == cudaSuccess); + } + } + +#if __cplusplus >= 201103L + std::atomic_thread_fence(std::memory_order_release); +#endif + m_devicePropInitialized = true; + } else { + // Wait for the other thread to inititialize the properties. + while (!m_devicePropInitialized) { +#if __cplusplus >= 201103L + std::atomic_thread_fence(std::memory_order_acquire); +#endif + sleep(1); + } + } + } +} + +static const cudaStream_t default_stream = cudaStreamDefault; + +class CudaStreamDevice : public StreamInterface { + public: + // Use the default stream on the current device + CudaStreamDevice() : stream_(&default_stream), scratch_(NULL), semaphore_(NULL) { + cudaGetDevice(&device_); + initializeDeviceProp(); + } + // Use the default stream on the specified device + CudaStreamDevice(int device) : stream_(&default_stream), device_(device), scratch_(NULL), semaphore_(NULL) { + initializeDeviceProp(); + } + // Use the specified stream. Note that it's the + // caller responsibility to ensure that the stream can run on + // the specified device. If no device is specified the code + // assumes that the stream is associated to the current gpu device. + CudaStreamDevice(const cudaStream_t* stream, int device = -1) + : stream_(stream), device_(device), scratch_(NULL), semaphore_(NULL) { + if (device < 0) { + cudaGetDevice(&device_); + } else { + int num_devices; + cudaError_t err = cudaGetDeviceCount(&num_devices); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); + assert(device < num_devices); + device_ = device; + } + initializeDeviceProp(); + } + + virtual ~CudaStreamDevice() { + if (scratch_) { + deallocate(scratch_); + } + } + + const cudaStream_t& stream() const { return *stream_; } + const cudaDeviceProp& deviceProperties() const { + return m_deviceProperties[device_]; + } + virtual void* allocate(size_t num_bytes) const { + cudaError_t err = cudaSetDevice(device_); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); + void* result; + err = cudaMalloc(&result, num_bytes); + assert(err == cudaSuccess); + assert(result != NULL); + return result; + } + virtual void deallocate(void* buffer) const { + cudaError_t err = cudaSetDevice(device_); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); + assert(buffer != NULL); + err = cudaFree(buffer); + assert(err == cudaSuccess); + } + + virtual void* scratchpad() const { + if (scratch_ == NULL) { + scratch_ = allocate(kCudaScratchSize + sizeof(unsigned int)); + } + return scratch_; + } + + virtual unsigned int* semaphore() const { + if (semaphore_ == NULL) { + char* scratch = static_cast(scratchpad()) + kCudaScratchSize; + semaphore_ = reinterpret_cast(scratch); + cudaError_t err = cudaMemsetAsync(semaphore_, 0, sizeof(unsigned int), *stream_); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); + } + return semaphore_; + } + + private: + const cudaStream_t* stream_; + int device_; + mutable void* scratch_; + mutable unsigned int* semaphore_; +}; + +struct GpuDevice { + // The StreamInterface is not owned: the caller is + // responsible for its initialization and eventual destruction. + explicit GpuDevice(const StreamInterface* stream) : stream_(stream), max_blocks_(INT_MAX) { + eigen_assert(stream); + } + explicit GpuDevice(const StreamInterface* stream, int num_blocks) : stream_(stream), max_blocks_(num_blocks) { + eigen_assert(stream); + } + // TODO(bsteiner): This is an internal API, we should not expose it. + EIGEN_STRONG_INLINE const cudaStream_t& stream() const { + return stream_->stream(); + } + + EIGEN_STRONG_INLINE void* allocate(size_t num_bytes) const { + return stream_->allocate(num_bytes); + } + + EIGEN_STRONG_INLINE void deallocate(void* buffer) const { + stream_->deallocate(buffer); + } + + EIGEN_STRONG_INLINE void* scratchpad() const { + return stream_->scratchpad(); + } + + EIGEN_STRONG_INLINE unsigned int* semaphore() const { + return stream_->semaphore(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpy(void* dst, const void* src, size_t n) const { +#ifndef __CUDA_ARCH__ + cudaError_t err = cudaMemcpyAsync(dst, src, n, cudaMemcpyDeviceToDevice, + stream_->stream()); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); +#else + eigen_assert(false && "The default device should be used instead to generate kernel code"); +#endif + } + + EIGEN_STRONG_INLINE void memcpyHostToDevice(void* dst, const void* src, size_t n) const { + cudaError_t err = + cudaMemcpyAsync(dst, src, n, cudaMemcpyHostToDevice, stream_->stream()); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); + } + + EIGEN_STRONG_INLINE void memcpyDeviceToHost(void* dst, const void* src, size_t n) const { + cudaError_t err = + cudaMemcpyAsync(dst, src, n, cudaMemcpyDeviceToHost, stream_->stream()); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memset(void* buffer, int c, size_t n) const { +#ifndef __CUDA_ARCH__ + cudaError_t err = cudaMemsetAsync(buffer, c, n, stream_->stream()); + EIGEN_UNUSED_VARIABLE(err) + assert(err == cudaSuccess); +#else + eigen_assert(false && "The default device should be used instead to generate kernel code"); +#endif + } + + EIGEN_STRONG_INLINE size_t numThreads() const { + // FIXME + return 32; + } + + EIGEN_STRONG_INLINE size_t firstLevelCacheSize() const { + // FIXME + return 48*1024; + } + + EIGEN_STRONG_INLINE size_t lastLevelCacheSize() const { + // We won't try to take advantage of the l2 cache for the time being, and + // there is no l3 cache on cuda devices. + return firstLevelCacheSize(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void synchronize() const { +#if defined(__CUDACC__) && !defined(__CUDA_ARCH__) + cudaError_t err = cudaStreamSynchronize(stream_->stream()); + if (err != cudaSuccess) { + std::cerr << "Error detected in CUDA stream: " + << cudaGetErrorString(err) + << std::endl; + assert(err == cudaSuccess); + } +#else + assert(false && "The default device should be used instead to generate kernel code"); +#endif + } + + EIGEN_STRONG_INLINE int getNumCudaMultiProcessors() const { + return stream_->deviceProperties().multiProcessorCount; + } + EIGEN_STRONG_INLINE int maxCudaThreadsPerBlock() const { + return stream_->deviceProperties().maxThreadsPerBlock; + } + EIGEN_STRONG_INLINE int maxCudaThreadsPerMultiProcessor() const { + return stream_->deviceProperties().maxThreadsPerMultiProcessor; + } + EIGEN_STRONG_INLINE int sharedMemPerBlock() const { + return stream_->deviceProperties().sharedMemPerBlock; + } + EIGEN_STRONG_INLINE int majorDeviceVersion() const { + return stream_->deviceProperties().major; + } + EIGEN_STRONG_INLINE int minorDeviceVersion() const { + return stream_->deviceProperties().minor; + } + + EIGEN_STRONG_INLINE int maxBlocks() const { + return max_blocks_; + } + + // This function checks if the CUDA runtime recorded an error for the + // underlying stream device. + inline bool ok() const { +#ifdef __CUDACC__ + cudaError_t error = cudaStreamQuery(stream_->stream()); + return (error == cudaSuccess) || (error == cudaErrorNotReady); +#else + return false; +#endif + } + + private: + const StreamInterface* stream_; + int max_blocks_; +}; + +#define LAUNCH_CUDA_KERNEL(kernel, gridsize, blocksize, sharedmem, device, ...) \ + (kernel) <<< (gridsize), (blocksize), (sharedmem), (device).stream() >>> (__VA_ARGS__); \ + assert(cudaGetLastError() == cudaSuccess); + + +// FIXME: Should be device and kernel specific. +#ifdef __CUDACC__ +static EIGEN_DEVICE_FUNC inline void setCudaSharedMemConfig(cudaSharedMemConfig config) { +#ifndef __CUDA_ARCH__ + cudaError_t status = cudaDeviceSetSharedMemConfig(config); + EIGEN_UNUSED_VARIABLE(status) + assert(status == cudaSuccess); +#else + EIGEN_UNUSED_VARIABLE(config) +#endif +} +#endif + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DEVICE_CUDA_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceDefault.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceDefault.h new file mode 100644 index 0000000000..9d141395b7 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceDefault.h @@ -0,0 +1,81 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_DEVICE_DEFAULT_H +#define EIGEN_CXX11_TENSOR_TENSOR_DEVICE_DEFAULT_H + + +namespace Eigen { + +// Default device for the machine (typically a single cpu core) +struct DefaultDevice { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void* allocate(size_t num_bytes) const { + return internal::aligned_malloc(num_bytes); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void deallocate(void* buffer) const { + internal::aligned_free(buffer); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpy(void* dst, const void* src, size_t n) const { + ::memcpy(dst, src, n); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpyHostToDevice(void* dst, const void* src, size_t n) const { + memcpy(dst, src, n); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpyDeviceToHost(void* dst, const void* src, size_t n) const { + memcpy(dst, src, n); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memset(void* buffer, int c, size_t n) const { + ::memset(buffer, c, n); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t numThreads() const { +#ifndef __CUDA_ARCH__ + // Running on the host CPU + return 1; +#else + // Running on a CUDA device + return 32; +#endif + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t firstLevelCacheSize() const { +#ifndef __CUDA_ARCH__ + // Running on the host CPU + return l1CacheSize(); +#else + // Running on a CUDA device, return the amount of shared memory available. + return 48*1024; +#endif + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t lastLevelCacheSize() const { +#ifndef __CUDA_ARCH__ + // Running single threaded on the host CPU + return l3CacheSize(); +#else + // Running on a CUDA device + return firstLevelCacheSize(); +#endif + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int majorDeviceVersion() const { +#ifndef __CUDA_ARCH__ + // Running single threaded on the host CPU + // Should return an enum that encodes the ISA supported by the CPU + return 1; +#else + // Running on a CUDA device + return __CUDA_ARCH__ / 100; +#endif + } +}; + +} // namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DEVICE_DEFAULT_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceSycl.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceSycl.h new file mode 100644 index 0000000000..7c039890e2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceSycl.h @@ -0,0 +1,122 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Mehdi Goli Codeplay Software Ltd. +// Ralph Potter Codeplay Software Ltd. +// Luke Iwanski Codeplay Software Ltd. +// Contact: +// Copyright (C) 2016 Benoit Steiner + +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#if defined(EIGEN_USE_SYCL) && !defined(EIGEN_CXX11_TENSOR_TENSOR_DEVICE_SYCL_H) +#define EIGEN_CXX11_TENSOR_TENSOR_DEVICE_SYCL_H + +namespace Eigen { +struct SyclDevice { + /// class members + /// sycl queue + mutable cl::sycl::queue m_queue; + /// std::map is the container used to make sure that we create only one buffer + /// per pointer. The lifespan of the buffer now depends on the lifespan of SyclDevice. + /// If a non-read-only pointer is needed to be accessed on the host we should manually deallocate it. + mutable std::map> buffer_map; + /// creating device by using selector + template SyclDevice(dev_Selector s) + : +#ifdef EIGEN_EXCEPTIONS + m_queue(cl::sycl::queue(s, [=](cl::sycl::exception_list l) { + for (const auto& e : l) { + try { + std::rethrow_exception(e); + } catch (cl::sycl::exception e) { + std::cout << e.what() << std::endl; + } + } + })) +#else + m_queue(cl::sycl::queue(s)) +#endif + {} + // destructor + ~SyclDevice() { deallocate_all(); } + + template void deallocate(T *p) const { + auto it = buffer_map.find(p); + if (it != buffer_map.end()) { + buffer_map.erase(it); + internal::aligned_free(p); + } + } + void deallocate_all() const { + std::map>::iterator it=buffer_map.begin(); + while (it!=buffer_map.end()) { + auto p=it->first; + buffer_map.erase(it); + internal::aligned_free(const_cast(p)); + it=buffer_map.begin(); + } + buffer_map.clear(); + } + + /// creation of sycl accessor for a buffer. This function first tries to find + /// the buffer in the buffer_map. If found it gets the accessor from it, if not, + ///the function then adds an entry by creating a sycl buffer for that particular pointer. + template inline cl::sycl::accessor + get_sycl_accessor(size_t num_bytes, cl::sycl::handler &cgh, const T * ptr) const { + return (get_sycl_buffer(num_bytes, ptr)->template get_access(cgh)); + } + + template inline std::pair>::iterator,bool> add_sycl_buffer(const T *ptr, size_t num_bytes) const { + using Type = cl::sycl::buffer; + std::pair>::iterator,bool> ret = buffer_map.insert(std::pair>(ptr, std::shared_ptr(new Type(cl::sycl::range<1>(num_bytes)), + [](void *dataMem) { delete static_cast(dataMem); }))); + (static_cast(buffer_map.at(ptr).get()))->set_final_data(nullptr); + return ret; + } + + template inline cl::sycl::buffer* get_sycl_buffer(size_t num_bytes,const T * ptr) const { + return static_cast*>(add_sycl_buffer(ptr, num_bytes).first->second.get()); + } + + /// allocating memory on the cpu + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void *allocate(size_t) const { + return internal::aligned_malloc(8); + } + + // some runtime conditions that can be applied here + bool isDeviceSuitable() const { return true; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpy(void *dst, const void *src, size_t n) const { + ::memcpy(dst, src, n); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpyHostToDevice(T *dst, const T *src, size_t n) const { + auto host_acc= (static_cast*>(add_sycl_buffer(dst, n).first->second.get()))-> template get_access(); + memcpy(host_acc.get_pointer(), src, n); + } + /// whith the current implementation of sycl, the data is copied twice from device to host. This will be fixed soon. + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memcpyDeviceToHost(T *dst, const T *src, size_t n) const { + auto it = buffer_map.find(src); + if (it != buffer_map.end()) { + auto host_acc= (static_cast*>(it->second.get()))-> template get_access(); + memcpy(dst,host_acc.get_pointer(), n); + } else{ + eigen_assert("no device memory found. The memory might be destroyed before creation"); + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void memset(void *buffer, int c, size_t n) const { + ::memset(buffer, c, n); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int majorDeviceVersion() const { + return 1; + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DEVICE_SYCL_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h new file mode 100644 index 0000000000..069680a117 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h @@ -0,0 +1,279 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#if defined(EIGEN_USE_THREADS) && !defined(EIGEN_CXX11_TENSOR_TENSOR_DEVICE_THREAD_POOL_H) +#define EIGEN_CXX11_TENSOR_TENSOR_DEVICE_THREAD_POOL_H + +namespace Eigen { + +// Use the SimpleThreadPool by default. We'll switch to the new non blocking +// thread pool later. +#ifndef EIGEN_USE_SIMPLE_THREAD_POOL +template using ThreadPoolTempl = NonBlockingThreadPoolTempl; +typedef NonBlockingThreadPool ThreadPool; +#else +template using ThreadPoolTempl = SimpleThreadPoolTempl; +typedef SimpleThreadPool ThreadPool; +#endif + + +// Barrier is an object that allows one or more threads to wait until +// Notify has been called a specified number of times. +class Barrier { + public: + Barrier(unsigned int count) : state_(count << 1), notified_(false) { + eigen_assert(((count << 1) >> 1) == count); + } + ~Barrier() { + eigen_assert((state_>>1) == 0); + } + + void Notify() { + unsigned int v = state_.fetch_sub(2, std::memory_order_acq_rel) - 2; + if (v != 1) { + eigen_assert(((v + 2) & ~1) != 0); + return; // either count has not dropped to 0, or waiter is not waiting + } + std::unique_lock l(mu_); + eigen_assert(!notified_); + notified_ = true; + cv_.notify_all(); + } + + void Wait() { + unsigned int v = state_.fetch_or(1, std::memory_order_acq_rel); + if ((v >> 1) == 0) return; + std::unique_lock l(mu_); + while (!notified_) { + cv_.wait(l); + } + } + + private: + std::mutex mu_; + std::condition_variable cv_; + std::atomic state_; // low bit is waiter flag + bool notified_; +}; + + +// Notification is an object that allows a user to to wait for another +// thread to signal a notification that an event has occurred. +// +// Multiple threads can wait on the same Notification object, +// but only one caller must call Notify() on the object. +struct Notification : Barrier { + Notification() : Barrier(1) {}; +}; + + +// Runs an arbitrary function and then calls Notify() on the passed in +// Notification. +template struct FunctionWrapperWithNotification +{ + static void run(Notification* n, Function f, Args... args) { + f(args...); + if (n) { + n->Notify(); + } + } +}; + +template struct FunctionWrapperWithBarrier +{ + static void run(Barrier* b, Function f, Args... args) { + f(args...); + if (b) { + b->Notify(); + } + } +}; + +template +static EIGEN_STRONG_INLINE void wait_until_ready(SyncType* n) { + if (n) { + n->Wait(); + } +} + + +// Build a thread pool device on top the an existing pool of threads. +struct ThreadPoolDevice { + // The ownership of the thread pool remains with the caller. + ThreadPoolDevice(ThreadPoolInterface* pool, int num_cores) : pool_(pool), num_threads_(num_cores) { } + + EIGEN_STRONG_INLINE void* allocate(size_t num_bytes) const { + return internal::aligned_malloc(num_bytes); + } + + EIGEN_STRONG_INLINE void deallocate(void* buffer) const { + internal::aligned_free(buffer); + } + + EIGEN_STRONG_INLINE void memcpy(void* dst, const void* src, size_t n) const { + ::memcpy(dst, src, n); + } + EIGEN_STRONG_INLINE void memcpyHostToDevice(void* dst, const void* src, size_t n) const { + memcpy(dst, src, n); + } + EIGEN_STRONG_INLINE void memcpyDeviceToHost(void* dst, const void* src, size_t n) const { + memcpy(dst, src, n); + } + + EIGEN_STRONG_INLINE void memset(void* buffer, int c, size_t n) const { + ::memset(buffer, c, n); + } + + EIGEN_STRONG_INLINE int numThreads() const { + return num_threads_; + } + + EIGEN_STRONG_INLINE size_t firstLevelCacheSize() const { + return l1CacheSize(); + } + + EIGEN_STRONG_INLINE size_t lastLevelCacheSize() const { + // The l3 cache size is shared between all the cores. + return l3CacheSize() / num_threads_; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int majorDeviceVersion() const { + // Should return an enum that encodes the ISA supported by the CPU + return 1; + } + + template + EIGEN_STRONG_INLINE Notification* enqueue(Function&& f, Args&&... args) const { + Notification* n = new Notification(); + pool_->Schedule(std::bind(&FunctionWrapperWithNotification::run, n, f, args...)); + return n; + } + + template + EIGEN_STRONG_INLINE void enqueue_with_barrier(Barrier* b, + Function&& f, + Args&&... args) const { + pool_->Schedule(std::bind( + &FunctionWrapperWithBarrier::run, b, f, args...)); + } + + template + EIGEN_STRONG_INLINE void enqueueNoNotification(Function&& f, Args&&... args) const { + pool_->Schedule(std::bind(f, args...)); + } + + // Returns a logical thread index between 0 and pool_->NumThreads() - 1 if + // called from one of the threads in pool_. Returns -1 otherwise. + EIGEN_STRONG_INLINE int currentThreadId() const { + return pool_->CurrentThreadId(); + } + + // parallelFor executes f with [0, n) arguments in parallel and waits for + // completion. F accepts a half-open interval [first, last). + // Block size is choosen based on the iteration cost and resulting parallel + // efficiency. If block_align is not nullptr, it is called to round up the + // block size. + void parallelFor(Index n, const TensorOpCost& cost, + std::function block_align, + std::function f) const { + typedef TensorCostModel CostModel; + if (n <= 1 || numThreads() == 1 || + CostModel::numThreads(n, cost, static_cast(numThreads())) == 1) { + f(0, n); + return; + } + + // Calculate block size based on (1) the iteration cost and (2) parallel + // efficiency. We want blocks to be not too small to mitigate + // parallelization overheads; not too large to mitigate tail + // effect and potential load imbalance and we also want number + // of blocks to be evenly dividable across threads. + + double block_size_f = 1.0 / CostModel::taskSize(1, cost); + Index block_size = numext::mini(n, numext::maxi(1, block_size_f)); + const Index max_block_size = + numext::mini(n, numext::maxi(1, 2 * block_size_f)); + if (block_align) { + Index new_block_size = block_align(block_size); + eigen_assert(new_block_size >= block_size); + block_size = numext::mini(n, new_block_size); + } + Index block_count = divup(n, block_size); + // Calculate parallel efficiency as fraction of total CPU time used for + // computations: + double max_efficiency = + static_cast(block_count) / + (divup(block_count, numThreads()) * numThreads()); + // Now try to increase block size up to max_block_size as long as it + // doesn't decrease parallel efficiency. + for (Index prev_block_count = block_count; prev_block_count > 1;) { + // This is the next block size that divides size into a smaller number + // of blocks than the current block_size. + Index coarser_block_size = divup(n, prev_block_count - 1); + if (block_align) { + Index new_block_size = block_align(coarser_block_size); + eigen_assert(new_block_size >= coarser_block_size); + coarser_block_size = numext::mini(n, new_block_size); + } + if (coarser_block_size > max_block_size) { + break; // Reached max block size. Stop. + } + // Recalculate parallel efficiency. + const Index coarser_block_count = divup(n, coarser_block_size); + eigen_assert(coarser_block_count < prev_block_count); + prev_block_count = coarser_block_count; + const double coarser_efficiency = + static_cast(coarser_block_count) / + (divup(coarser_block_count, numThreads()) * numThreads()); + if (coarser_efficiency + 0.01 >= max_efficiency) { + // Taking it. + block_size = coarser_block_size; + block_count = coarser_block_count; + if (max_efficiency < coarser_efficiency) { + max_efficiency = coarser_efficiency; + } + } + } + + // Recursively divide size into halves until we reach block_size. + // Division code rounds mid to block_size, so we are guaranteed to get + // block_count leaves that do actual computations. + Barrier barrier(static_cast(block_count)); + std::function handleRange; + handleRange = [=, &handleRange, &barrier, &f](Index first, Index last) { + if (last - first <= block_size) { + // Single block or less, execute directly. + f(first, last); + barrier.Notify(); + return; + } + // Split into halves and submit to the pool. + Index mid = first + divup((last - first) / 2, block_size) * block_size; + pool_->Schedule([=, &handleRange]() { handleRange(mid, last); }); + pool_->Schedule([=, &handleRange]() { handleRange(first, mid); }); + }; + handleRange(0, n); + barrier.Wait(); + } + + // Convenience wrapper for parallelFor that does not align blocks. + void parallelFor(Index n, const TensorOpCost& cost, + std::function f) const { + parallelFor(n, cost, nullptr, std::move(f)); + } + + private: + ThreadPoolInterface* pool_; + int num_threads_; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DEVICE_THREAD_POOL_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensionList.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensionList.h new file mode 100644 index 0000000000..1a30e45fb1 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensionList.h @@ -0,0 +1,236 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_DIMENSION_LIST_H +#define EIGEN_CXX11_TENSOR_TENSOR_DIMENSION_LIST_H + +namespace Eigen { + +/** \internal + * + * \class TensorDimensionList + * \ingroup CXX11_Tensor_Module + * + * \brief Special case of tensor index list used to list all the dimensions of a tensor of rank n. + * + * \sa Tensor + */ + +template struct DimensionList { + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE + const Index operator[] (const Index i) const { return i; } +}; + +namespace internal { + +template struct array_size > { + static const size_t value = Rank; +}; +template struct array_size > { + static const size_t value = Rank; +}; + +template const Index array_get(DimensionList&) { + return n; +} +template const Index array_get(const DimensionList&) { + return n; +} + + +#if EIGEN_HAS_CONSTEXPR +template +struct index_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex) { + return true; + } +}; +template +struct index_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex) { + return true; + } +}; + +template +struct all_indices_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return true; + } +}; +template +struct all_indices_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return true; + } +}; + +template +struct indices_statically_known_to_increase_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return true; + } +}; +template +struct indices_statically_known_to_increase_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return true; + } +}; + +template +struct index_statically_eq_impl > { + static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i == value; + } +}; +template +struct index_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i == value; + } +}; + +template +struct index_statically_ne_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i != value; + } +}; +template +struct index_statically_ne_impl > { + static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i != value; + } +}; + +template +struct index_statically_gt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i > value; + } +}; +template +struct index_statically_gt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i > value; + } +}; + +template +struct index_statically_lt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i < value; + } +}; +template +struct index_statically_lt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return i < value; + } +}; + +#else +template +struct index_known_statically_impl > { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE bool run(const DenseIndex) { + return true; + } +}; +template +struct index_known_statically_impl > { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE bool run(const DenseIndex) { + return true; + } +}; + +template +struct all_indices_known_statically_impl > { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE bool run() { + return true; + } +}; +template +struct all_indices_known_statically_impl > { + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE bool run() { + return true; + } +}; + +template +struct indices_statically_known_to_increase_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run() { + return true; + } +}; +template +struct indices_statically_known_to_increase_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run() { + return true; + } +}; + +template +struct index_statically_eq_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; +template +struct index_statically_eq_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; + +template +struct index_statically_ne_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex){ + return false; + } +}; +template +struct index_statically_ne_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; + +template +struct index_statically_gt_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; +template +struct index_statically_gt_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; + +template +struct index_statically_lt_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; +template +struct index_statically_lt_impl > { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex, const DenseIndex) { + return false; + } +}; +#endif + +} // end namespace internal +} // end namespace Eigen + + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DIMENSION_LIST_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h new file mode 100644 index 0000000000..b24cdebf1e --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h @@ -0,0 +1,428 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_DIMENSIONS_H +#define EIGEN_CXX11_TENSOR_TENSOR_DIMENSIONS_H + + +namespace Eigen { + +/** \internal + * + * \class TensorDimensions + * \ingroup CXX11_Tensor_Module + * + * \brief Set of classes used to encode and store the dimensions of a Tensor. + * + * The Sizes class encodes as part of the type the number of dimensions and the + * sizes corresponding to each dimension. It uses no storage space since it is + * entirely known at compile time. + * The DSizes class is its dynamic sibling: the number of dimensions is known + * at compile time but the sizes are set during execution. + * + * \sa Tensor + */ + +// Boilerplate code +namespace internal { + +template struct dget { + static const std::size_t value = get::value; +}; + + +template +struct fixed_size_tensor_index_linearization_helper +{ + template EIGEN_DEVICE_FUNC + static inline Index run(array const& indices, + const Dimensions& dimensions) + { + return array_get(indices) + + dget::value * + fixed_size_tensor_index_linearization_helper::run(indices, dimensions); + } +}; + +template +struct fixed_size_tensor_index_linearization_helper +{ + template EIGEN_DEVICE_FUNC + static inline Index run(array const&, const Dimensions&) + { + return 0; + } +}; + +template +struct fixed_size_tensor_index_extraction_helper +{ + template EIGEN_DEVICE_FUNC + static inline Index run(const Index index, + const Dimensions& dimensions) + { + const Index mult = (index == n-1) ? 1 : 0; + return array_get(dimensions) * mult + + fixed_size_tensor_index_extraction_helper::run(index, dimensions); + } +}; + +template +struct fixed_size_tensor_index_extraction_helper +{ + template EIGEN_DEVICE_FUNC + static inline Index run(const Index, + const Dimensions&) + { + return 0; + } + }; + +} // end namespace internal + + +// Fixed size +#ifndef EIGEN_EMULATE_CXX11_META_H +template +struct Sizes : internal::numeric_list { + typedef internal::numeric_list Base; + static const std::ptrdiff_t total_size = internal::arg_prod(Indices...); + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::ptrdiff_t rank() const { + return Base::count; + } + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::ptrdiff_t TotalSize() { + return internal::arg_prod(Indices...); + } + + EIGEN_DEVICE_FUNC Sizes() { } + template + explicit EIGEN_DEVICE_FUNC Sizes(const array& /*indices*/) { + // todo: add assertion + } +#if EIGEN_HAS_VARIADIC_TEMPLATES + template EIGEN_DEVICE_FUNC Sizes(DenseIndex...) { } + explicit EIGEN_DEVICE_FUNC Sizes(std::initializer_list /*l*/) { + // todo: add assertion + } +#endif + + template Sizes& operator = (const T& /*other*/) { + // add assertion failure if the size of other is different + return *this; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::ptrdiff_t operator[] (const std::size_t index) const { + return internal::fixed_size_tensor_index_extraction_helper::run(index, *this); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + size_t IndexOfColMajor(const array& indices) const { + return internal::fixed_size_tensor_index_linearization_helper::run(indices, *static_cast(this)); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + size_t IndexOfRowMajor(const array& indices) const { + return internal::fixed_size_tensor_index_linearization_helper::run(indices, *static_cast(this)); + } +}; + +namespace internal { +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::ptrdiff_t array_prod(const Sizes&) { + return Sizes::total_size; +} +} + +#else + +template +struct non_zero_size { + typedef internal::type2val type; +}; +template <> +struct non_zero_size<0> { + typedef internal::null_type type; +}; + +template struct Sizes { + typedef typename internal::make_type_list::type, typename non_zero_size::type, typename non_zero_size::type, typename non_zero_size::type, typename non_zero_size::type >::type Base; + static const size_t count = Base::count; + static const std::size_t total_size = internal::arg_prod::value; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t rank() const { + return count; + } + + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t TotalSize() { + return internal::arg_prod::value; + } + + Sizes() { } + template + explicit Sizes(const array& /*indices*/) { + // todo: add assertion + } + template Sizes& operator = (const T& /*other*/) { + // add assertion failure if the size of other is different + return *this; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template Sizes(DenseIndex... /*indices*/) { } + explicit Sizes(std::initializer_list) { + // todo: add assertion + } +#else + EIGEN_DEVICE_FUNC explicit Sizes(const DenseIndex) { + } + EIGEN_DEVICE_FUNC Sizes(const DenseIndex, const DenseIndex) { + } + EIGEN_DEVICE_FUNC Sizes(const DenseIndex, const DenseIndex, const DenseIndex) { + } + EIGEN_DEVICE_FUNC Sizes(const DenseIndex, const DenseIndex, const DenseIndex, const DenseIndex) { + } + EIGEN_DEVICE_FUNC Sizes(const DenseIndex, const DenseIndex, const DenseIndex, const DenseIndex, const DenseIndex) { + } +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex operator[] (const int index) const { + switch (index) { + case 0: + return internal::get<0, Base>::value; + case 1: + return internal::get<1, Base>::value; + case 2: + return internal::get<2, Base>::value; + case 3: + return internal::get<3, Base>::value; + case 4: + return internal::get<4, Base>::value; + default: + eigen_assert(false && "index overflow"); + return static_cast(-1); + } + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + size_t IndexOfColMajor(const array& indices) const { + return internal::fixed_size_tensor_index_linearization_helper::run(indices, *reinterpret_cast(this)); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + size_t IndexOfRowMajor(const array& indices) const { + return internal::fixed_size_tensor_index_linearization_helper::run(indices, *reinterpret_cast(this)); + } +}; + +namespace internal { +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::size_t array_prod(const Sizes&) { + return Sizes::total_size; +} +} + +#endif + +// Boilerplate +namespace internal { +template +struct tensor_index_linearization_helper +{ + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index run(array const& indices, array const& dimensions) + { + return array_get(indices) + + array_get(dimensions) * + tensor_index_linearization_helper::run(indices, dimensions); + } +}; + +template +struct tensor_index_linearization_helper +{ + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index run(array const& indices, array const&) + { + return array_get(indices); + } +}; +} // end namespace internal + + + +// Dynamic size +template +struct DSizes : array { + typedef array Base; + static const int count = NumDims; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE size_t rank() const { + return NumDims; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex TotalSize() const { + return (NumDims == 0) ? 1 : internal::array_prod(*static_cast(this)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DSizes() { + for (int i = 0 ; i < NumDims; ++i) { + (*this)[i] = 0; + } + } + EIGEN_DEVICE_FUNC explicit DSizes(const array& a) : Base(a) { } + + EIGEN_DEVICE_FUNC explicit DSizes(const DenseIndex i0) { + eigen_assert(NumDims == 1); + (*this)[0] = i0; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE explicit DSizes(DenseIndex firstDimension, DenseIndex secondDimension, IndexTypes... otherDimensions) : Base({{firstDimension, secondDimension, otherDimensions...}}) { + EIGEN_STATIC_ASSERT(sizeof...(otherDimensions) + 2 == NumDims, YOU_MADE_A_PROGRAMMING_MISTAKE) + } +#else + EIGEN_DEVICE_FUNC DSizes(const DenseIndex i0, const DenseIndex i1) { + eigen_assert(NumDims == 2); + (*this)[0] = i0; + (*this)[1] = i1; + } + EIGEN_DEVICE_FUNC DSizes(const DenseIndex i0, const DenseIndex i1, const DenseIndex i2) { + eigen_assert(NumDims == 3); + (*this)[0] = i0; + (*this)[1] = i1; + (*this)[2] = i2; + } + EIGEN_DEVICE_FUNC DSizes(const DenseIndex i0, const DenseIndex i1, const DenseIndex i2, const DenseIndex i3) { + eigen_assert(NumDims == 4); + (*this)[0] = i0; + (*this)[1] = i1; + (*this)[2] = i2; + (*this)[3] = i3; + } + EIGEN_DEVICE_FUNC DSizes(const DenseIndex i0, const DenseIndex i1, const DenseIndex i2, const DenseIndex i3, const DenseIndex i4) { + eigen_assert(NumDims == 5); + (*this)[0] = i0; + (*this)[1] = i1; + (*this)[2] = i2; + (*this)[3] = i3; + (*this)[4] = i4; + } +#endif + + EIGEN_DEVICE_FUNC DSizes& operator = (const array& other) { + *static_cast(this) = other; + return *this; + } + + // A constexpr would be so much better here + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex IndexOfColMajor(const array& indices) const { + return internal::tensor_index_linearization_helper::run(indices, *static_cast(this)); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex IndexOfRowMajor(const array& indices) const { + return internal::tensor_index_linearization_helper::run(indices, *static_cast(this)); + } +}; + + + + +// Boilerplate +namespace internal { +template +struct tensor_vsize_index_linearization_helper +{ + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index run(array const& indices, std::vector const& dimensions) + { + return array_get(indices) + + array_get(dimensions) * + tensor_vsize_index_linearization_helper::run(indices, dimensions); + } +}; + +template +struct tensor_vsize_index_linearization_helper +{ + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Index run(array const& indices, std::vector const&) + { + return array_get(indices); + } +}; +} // end namespace internal + + +namespace internal { + +template struct array_size > { + static const size_t value = NumDims; +}; +template struct array_size > { + static const size_t value = NumDims; +}; +#ifndef EIGEN_EMULATE_CXX11_META_H +template struct array_size > { +static const std::ptrdiff_t value = Sizes::count; +}; +template struct array_size > { +static const std::ptrdiff_t value = Sizes::count; +}; +template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::ptrdiff_t array_get(const Sizes&) { + return get >::value; +} +template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::ptrdiff_t array_get(const Sizes<>&) { + eigen_assert(false && "should never be called"); + return -1; +} +#else +template struct array_size > { + static const size_t value = Sizes::count; +}; +template struct array_size > { + static const size_t value = Sizes::count; +}; +template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::size_t array_get(const Sizes&) { + return get::Base>::value; +} + +#endif + + +template +struct sizes_match_below_dim { + static EIGEN_DEVICE_FUNC inline bool run(Dims1&, Dims2&) { + return false; + } +}; +template +struct sizes_match_below_dim { + static EIGEN_DEVICE_FUNC inline bool run(Dims1& dims1, Dims2& dims2) { + return (array_get(dims1) == array_get(dims2)) & + sizes_match_below_dim::run(dims1, dims2); + } +}; +template +struct sizes_match_below_dim { + static EIGEN_DEVICE_FUNC inline bool run(Dims1&, Dims2&) { + return true; + } +}; + +} // end namespace internal + + +template +EIGEN_DEVICE_FUNC bool dimensions_match(Dims1& dims1, Dims2& dims2) { + return internal::sizes_match_below_dim::value, internal::array_size::value>::run(dims1, dims2); +} + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_DIMENSIONS_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvalTo.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvalTo.h new file mode 100644 index 0000000000..06987132b6 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvalTo.h @@ -0,0 +1,181 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_EVAL_TO_H +#define EIGEN_CXX11_TENSOR_TENSOR_EVAL_TO_H + +namespace Eigen { + +/** \class TensorForcedEval + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor reshaping class. + * + * + */ +namespace internal { +template class MakePointer_> +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs are different. + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; + + enum { + Flags = 0 + }; + template + struct MakePointer { + // Intermediate typedef to workaround MSVC issue. + typedef MakePointer_ MakePointerT; + typedef typename MakePointerT::Type Type; + }; +}; + +template class MakePointer_> +struct eval, Eigen::Dense> +{ + typedef const TensorEvalToOp& type; +}; + +template class MakePointer_> +struct nested, 1, typename eval >::type> +{ + typedef TensorEvalToOp type; +}; + +} // end namespace internal + + + + +template class MakePointer_> +class TensorEvalToOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename MakePointer_::Type PointerType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvalToOp(PointerType buffer, const XprType& expr) + : m_xpr(expr), m_buffer(buffer) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC PointerType buffer() const { return m_buffer; } + + protected: + typename XprType::Nested m_xpr; + PointerType m_buffer; +}; + + + +template class MakePointer_> +struct TensorEvaluator, Device> +{ + typedef TensorEvalToOp XprType; + typedef typename ArgType::Scalar Scalar; + typedef typename TensorEvaluator::Dimensions Dimensions; + typedef typename XprType::Index Index; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = true + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_device(device), + m_buffer(op.buffer()), m_op(op), m_expression(op.expression()) + { } + + // Used for accessor extraction in SYCL Managed TensorMap: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const XprType& op() const { + return m_op; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ~TensorEvaluator() { + } + + typedef typename internal::traits >::template MakePointer::Type DevicePointer; + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const { return m_impl.dimensions(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(DevicePointer scalar) { + EIGEN_UNUSED_VARIABLE(scalar); + eigen_assert(scalar == NULL); + return m_impl.evalSubExprsIfNeeded(m_buffer); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalScalar(Index i) { + m_buffer[i] = m_impl.coeff(i); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalPacket(Index i) { + internal::pstoret(m_buffer + i, m_impl.template packet::IsAligned ? Aligned : Unaligned>(i)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_buffer[index]; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return internal::ploadt(m_buffer + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + // We assume that evalPacket or evalScalar is called to perform the + // assignment and account for the cost of the write here. + return m_impl.costPerCoeff(vectorized) + + TensorOpCost(0, sizeof(CoeffReturnType), 0, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC DevicePointer data() const { return m_buffer; } + ArgType expression() const { return m_expression; } + + /// required by sycl in order to extract the accessor + const TensorEvaluator& impl() const { return m_impl; } + /// added for sycl in order to construct the buffer from the sycl device + const Device& device() const{return m_device;} + + private: + TensorEvaluator m_impl; + const Device& m_device; + DevicePointer m_buffer; + const XprType& m_op; + const ArgType m_expression; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_EVAL_TO_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h new file mode 100644 index 0000000000..834ce07df5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h @@ -0,0 +1,633 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_EVALUATOR_H +#define EIGEN_CXX11_TENSOR_TENSOR_EVALUATOR_H + +namespace Eigen { + +/** \class TensorEvaluator + * \ingroup CXX11_Tensor_Module + * + * \brief The tensor evaluator classes. + * + * These classes are responsible for the evaluation of the tensor expression. + * + * TODO: add support for more types of expressions, in particular expressions + * leading to lvalues (slicing, reshaping, etc...) + */ + +// Generic evaluator +template +struct TensorEvaluator +{ + typedef typename Derived::Index Index; + typedef typename Derived::Scalar Scalar; + typedef typename Derived::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef typename Derived::Dimensions Dimensions; + + // NumDimensions is -1 for variable dim tensors + static const int NumCoords = internal::traits::NumDimensions > 0 ? + internal::traits::NumDimensions : 0; + + enum { + IsAligned = Derived::IsAligned, + PacketAccess = (internal::unpacket_traits::size > 1), + Layout = Derived::Layout, + CoordAccess = NumCoords > 0, + RawAccess = true + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const Derived& m, const Device& device) + : m_data(const_cast::template MakePointer::Type>(m.data())), m_dims(m.dimensions()), m_device(device), m_impl(m) + { } + + // Used for accessor extraction in SYCL Managed TensorMap: + const Derived& derived() const { return m_impl; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dims; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* dest) { + if (dest) { + m_device.memcpy((void*)dest, m_data, sizeof(Scalar) * m_dims.TotalSize()); + return false; + } + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + eigen_assert(m_data); + return m_data[index]; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) { + eigen_assert(m_data); + return m_data[index]; + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + PacketReturnType packet(Index index) const + { + return internal::ploadt(m_data + index); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketReturnType& x) + { + return internal::pstoret(m_data + index, x); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(const array& coords) const { + eigen_assert(m_data); + if (static_cast(Layout) == static_cast(ColMajor)) { + return m_data[m_dims.IndexOfColMajor(coords)]; + } else { + return m_data[m_dims.IndexOfRowMajor(coords)]; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(const array& coords) { + eigen_assert(m_data); + if (static_cast(Layout) == static_cast(ColMajor)) { + return m_data[m_dims.IndexOfColMajor(coords)]; + } else { + return m_data[m_dims.IndexOfRowMajor(coords)]; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, + internal::unpacket_traits::size); + } + + EIGEN_DEVICE_FUNC typename internal::traits::template MakePointer::Type data() const { return m_data; } + + /// required by sycl in order to construct sycl buffer from raw pointer + const Device& device() const{return m_device;} + + protected: + typename internal::traits::template MakePointer::Type m_data; + Dimensions m_dims; + const Device& m_device; + const Derived& m_impl; +}; + +namespace { +template EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T loadConstant(const T* address) { + return *address; +} +// Use the texture cache on CUDA devices whenever possible +#if defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 350 +template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +float loadConstant(const float* address) { + return __ldg(address); +} +template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +double loadConstant(const double* address) { + return __ldg(address); +} +template <> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +Eigen::half loadConstant(const Eigen::half* address) { + return Eigen::half(half_impl::raw_uint16_to_half(__ldg(&address->x))); +} +#endif +} + + +// Default evaluator for rvalues +template +struct TensorEvaluator +{ + typedef typename Derived::Index Index; + typedef typename Derived::Scalar Scalar; + typedef typename Derived::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef typename Derived::Dimensions Dimensions; + + // NumDimensions is -1 for variable dim tensors + static const int NumCoords = internal::traits::NumDimensions > 0 ? + internal::traits::NumDimensions : 0; + + enum { + IsAligned = Derived::IsAligned, + PacketAccess = (internal::unpacket_traits::size > 1), + Layout = Derived::Layout, + CoordAccess = NumCoords > 0, + RawAccess = true + }; + + // Used for accessor extraction in SYCL Managed TensorMap: + const Derived& derived() const { return m_impl; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const Derived& m, const Device& device) + : m_data(m.data()), m_dims(m.dimensions()), m_device(device), m_impl(m) + { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dims; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* data) { + if (!NumTraits::type>::RequireInitialization && data) { + m_device.memcpy((void*)data, m_data, m_dims.TotalSize() * sizeof(Scalar)); + return false; + } + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { + eigen_assert(m_data); + return loadConstant(m_data+index); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + PacketReturnType packet(Index index) const + { + return internal::ploadt_ro(m_data + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(const array& coords) const { + eigen_assert(m_data); + const Index index = (static_cast(Layout) == static_cast(ColMajor)) ? m_dims.IndexOfColMajor(coords) + : m_dims.IndexOfRowMajor(coords); + return loadConstant(m_data+index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, + internal::unpacket_traits::size); + } + + EIGEN_DEVICE_FUNC typename internal::traits::template MakePointer::Type data() const { return m_data; } + + /// added for sycl in order to construct the buffer from the sycl device + const Device& device() const{return m_device;} + + protected: + typename internal::traits::template MakePointer::Type m_data; + Dimensions m_dims; + const Device& m_device; + const Derived& m_impl; +}; + + + + +// -------------------- CwiseNullaryOp -------------------- + +template +struct TensorEvaluator, Device> +{ + typedef TensorCwiseNullaryOp XprType; + + enum { + IsAligned = true, + PacketAccess = internal::functor_traits::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC + TensorEvaluator(const XprType& op, const Device& device) + : m_functor(op.functor()), m_argImpl(op.nestedExpression(), device), m_wrapper() + { } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename internal::traits::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + typedef typename TensorEvaluator::Dimensions Dimensions; + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const { return m_argImpl.dimensions(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType*) { return true; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { } + + EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index index) const + { + return m_wrapper(m_functor, index); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return m_wrapper.template packetOp(m_functor, index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, + internal::unpacket_traits::size); + } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return NULL; } + + /// required by sycl in order to extract the accessor + const TensorEvaluator& impl() const { return m_argImpl; } + /// required by sycl in order to extract the accessor + NullaryOp functor() const { return m_functor; } + + + private: + const NullaryOp m_functor; + TensorEvaluator m_argImpl; + const internal::nullary_wrapper m_wrapper; +}; + + + +// -------------------- CwiseUnaryOp -------------------- + +template +struct TensorEvaluator, Device> +{ + typedef TensorCwiseUnaryOp XprType; + + enum { + IsAligned = TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess & internal::functor_traits::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) + : m_functor(op.functor()), + m_argImpl(op.nestedExpression(), device) + { } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename internal::traits::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + typedef typename TensorEvaluator::Dimensions Dimensions; + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const { return m_argImpl.dimensions(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar*) { + m_argImpl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_argImpl.cleanup(); + } + + EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index index) const + { + return m_functor(m_argImpl.coeff(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return m_functor.packetOp(m_argImpl.template packet(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + const double functor_cost = internal::functor_traits::Cost; + return m_argImpl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, functor_cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return NULL; } + + /// required by sycl in order to extract the accessor + const TensorEvaluator & impl() const { return m_argImpl; } + /// added for sycl in order to construct the buffer from sycl device + UnaryOp functor() const { return m_functor; } + + + private: + const UnaryOp m_functor; + TensorEvaluator m_argImpl; +}; + + +// -------------------- CwiseBinaryOp -------------------- + +template +struct TensorEvaluator, Device> +{ + typedef TensorCwiseBinaryOp XprType; + + enum { + IsAligned = TensorEvaluator::IsAligned & TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess & + internal::functor_traits::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) + : m_functor(op.functor()), + m_leftImpl(op.lhsExpression(), device), + m_rightImpl(op.rhsExpression(), device) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout) || internal::traits::NumDimensions <= 1), YOU_MADE_A_PROGRAMMING_MISTAKE); + eigen_assert(dimensions_match(m_leftImpl.dimensions(), m_rightImpl.dimensions())); + } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename internal::traits::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + typedef typename TensorEvaluator::Dimensions Dimensions; + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const + { + // TODO: use right impl instead if right impl dimensions are known at compile time. + return m_leftImpl.dimensions(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType*) { + m_leftImpl.evalSubExprsIfNeeded(NULL); + m_rightImpl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_leftImpl.cleanup(); + m_rightImpl.cleanup(); + } + + EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index index) const + { + return m_functor(m_leftImpl.coeff(index), m_rightImpl.coeff(index)); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return m_functor.packetOp(m_leftImpl.template packet(index), m_rightImpl.template packet(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + const double functor_cost = internal::functor_traits::Cost; + return m_leftImpl.costPerCoeff(vectorized) + + m_rightImpl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, functor_cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return NULL; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& left_impl() const { return m_leftImpl; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& right_impl() const { return m_rightImpl; } + /// required by sycl in order to extract the accessor + BinaryOp functor() const { return m_functor; } + + private: + const BinaryOp m_functor; + TensorEvaluator m_leftImpl; + TensorEvaluator m_rightImpl; +}; + +// -------------------- CwiseTernaryOp -------------------- + +template +struct TensorEvaluator, Device> +{ + typedef TensorCwiseTernaryOp XprType; + + enum { + IsAligned = TensorEvaluator::IsAligned & TensorEvaluator::IsAligned & TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess & + internal::functor_traits::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) + : m_functor(op.functor()), + m_arg1Impl(op.arg1Expression(), device), + m_arg2Impl(op.arg2Expression(), device), + m_arg3Impl(op.arg3Expression(), device) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout) || internal::traits::NumDimensions <= 1), YOU_MADE_A_PROGRAMMING_MISTAKE); + + EIGEN_STATIC_ASSERT((internal::is_same::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + EIGEN_STATIC_ASSERT((internal::is_same::StorageKind, + typename internal::traits::StorageKind>::value), + STORAGE_KIND_MUST_MATCH) + EIGEN_STATIC_ASSERT((internal::is_same::Index, + typename internal::traits::Index>::value), + STORAGE_INDEX_MUST_MATCH) + EIGEN_STATIC_ASSERT((internal::is_same::Index, + typename internal::traits::Index>::value), + STORAGE_INDEX_MUST_MATCH) + + eigen_assert(dimensions_match(m_arg1Impl.dimensions(), m_arg2Impl.dimensions()) && dimensions_match(m_arg1Impl.dimensions(), m_arg3Impl.dimensions())); + } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename internal::traits::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + typedef typename TensorEvaluator::Dimensions Dimensions; + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const + { + // TODO: use arg2 or arg3 dimensions if they are known at compile time. + return m_arg1Impl.dimensions(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType*) { + m_arg1Impl.evalSubExprsIfNeeded(NULL); + m_arg2Impl.evalSubExprsIfNeeded(NULL); + m_arg3Impl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_arg1Impl.cleanup(); + m_arg2Impl.cleanup(); + m_arg3Impl.cleanup(); + } + + EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index index) const + { + return m_functor(m_arg1Impl.coeff(index), m_arg2Impl.coeff(index), m_arg3Impl.coeff(index)); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return m_functor.packetOp(m_arg1Impl.template packet(index), + m_arg2Impl.template packet(index), + m_arg3Impl.template packet(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + const double functor_cost = internal::functor_traits::Cost; + return m_arg1Impl.costPerCoeff(vectorized) + + m_arg2Impl.costPerCoeff(vectorized) + + m_arg3Impl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, functor_cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC CoeffReturnType* data() const { return NULL; } + + /// required by sycl in order to extract the accessor + const TensorEvaluator & arg1Impl() const { return m_arg1Impl; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& arg2Impl() const { return m_arg2Impl; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& arg3Impl() const { return m_arg3Impl; } + + private: + const TernaryOp m_functor; + TensorEvaluator m_arg1Impl; + TensorEvaluator m_arg2Impl; + TensorEvaluator m_arg3Impl; +}; + + +// -------------------- SelectOp -------------------- + +template +struct TensorEvaluator, Device> +{ + typedef TensorSelectOp XprType; + typedef typename XprType::Scalar Scalar; + + enum { + IsAligned = TensorEvaluator::IsAligned & TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess & TensorEvaluator::PacketAccess & + internal::packet_traits::HasBlend, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) + : m_condImpl(op.ifExpression(), device), + m_thenImpl(op.thenExpression(), device), + m_elseImpl(op.elseExpression(), device) + { + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout)), YOU_MADE_A_PROGRAMMING_MISTAKE); + EIGEN_STATIC_ASSERT((static_cast(TensorEvaluator::Layout) == static_cast(TensorEvaluator::Layout)), YOU_MADE_A_PROGRAMMING_MISTAKE); + eigen_assert(dimensions_match(m_condImpl.dimensions(), m_thenImpl.dimensions())); + eigen_assert(dimensions_match(m_thenImpl.dimensions(), m_elseImpl.dimensions())); + } + + typedef typename XprType::Index Index; + typedef typename internal::traits::Scalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + typedef typename TensorEvaluator::Dimensions Dimensions; + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const + { + // TODO: use then or else impl instead if they happen to be known at compile time. + return m_condImpl.dimensions(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType*) { + m_condImpl.evalSubExprsIfNeeded(NULL); + m_thenImpl.evalSubExprsIfNeeded(NULL); + m_elseImpl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_condImpl.cleanup(); + m_thenImpl.cleanup(); + m_elseImpl.cleanup(); + } + + EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index index) const + { + return m_condImpl.coeff(index) ? m_thenImpl.coeff(index) : m_elseImpl.coeff(index); + } + template + EIGEN_DEVICE_FUNC PacketReturnType packet(Index index) const + { + internal::Selector select; + for (Index i = 0; i < PacketSize; ++i) { + select.select[i] = m_condImpl.coeff(index+i); + } + return internal::pblend(select, + m_thenImpl.template packet(index), + m_elseImpl.template packet(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + return m_condImpl.costPerCoeff(vectorized) + + m_thenImpl.costPerCoeff(vectorized) + .cwiseMax(m_elseImpl.costPerCoeff(vectorized)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType* data() const { return NULL; } + /// required by sycl in order to extract the accessor + const TensorEvaluator & cond_impl() const { return m_condImpl; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& then_impl() const { return m_thenImpl; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& else_impl() const { return m_elseImpl; } + + private: + TensorEvaluator m_condImpl; + TensorEvaluator m_thenImpl; + TensorEvaluator m_elseImpl; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_EVALUATOR_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExecutor.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExecutor.h new file mode 100644 index 0000000000..f01d77c0a0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExecutor.h @@ -0,0 +1,288 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_EXECUTOR_H +#define EIGEN_CXX11_TENSOR_TENSOR_EXECUTOR_H + +namespace Eigen { + +/** \class TensorExecutor + * \ingroup CXX11_Tensor_Module + * + * \brief The tensor executor class. + * + * This class is responsible for launch the evaluation of the expression on + * the specified computing device. + */ +namespace internal { + +// Default strategy: the expression is evaluated with a single cpu thread. +template +class TensorExecutor +{ + public: + typedef typename Expression::Index Index; + EIGEN_DEVICE_FUNC + static inline void run(const Expression& expr, const Device& device = Device()) + { + TensorEvaluator evaluator(expr, device); + const bool needs_assign = evaluator.evalSubExprsIfNeeded(NULL); + if (needs_assign) + { + const Index size = array_prod(evaluator.dimensions()); + for (Index i = 0; i < size; ++i) { + evaluator.evalScalar(i); + } + } + evaluator.cleanup(); + } +}; + + +template +class TensorExecutor +{ + public: + typedef typename Expression::Index Index; + EIGEN_DEVICE_FUNC + static inline void run(const Expression& expr, const DefaultDevice& device = DefaultDevice()) + { + TensorEvaluator evaluator(expr, device); + const bool needs_assign = evaluator.evalSubExprsIfNeeded(NULL); + if (needs_assign) + { + const Index size = array_prod(evaluator.dimensions()); + const int PacketSize = unpacket_traits::PacketReturnType>::size; + // Give the compiler a strong hint to unroll the loop. But don't insist + // on unrolling, because if the function is expensive the compiler should not + // unroll the loop at the expense of inlining. + const Index UnrolledSize = (size / (4 * PacketSize)) * 4 * PacketSize; + for (Index i = 0; i < UnrolledSize; i += 4*PacketSize) { + for (Index j = 0; j < 4; j++) { + evaluator.evalPacket(i + j * PacketSize); + } + } + const Index VectorizedSize = (size / PacketSize) * PacketSize; + for (Index i = UnrolledSize; i < VectorizedSize; i += PacketSize) { + evaluator.evalPacket(i); + } + for (Index i = VectorizedSize; i < size; ++i) { + evaluator.evalScalar(i); + } + } + evaluator.cleanup(); + } +}; + + + +// Multicore strategy: the index space is partitioned and each partition is executed on a single core +#ifdef EIGEN_USE_THREADS +template +struct EvalRange { + static void run(Evaluator* evaluator_in, const Index first, const Index last) { + Evaluator evaluator = *evaluator_in; + eigen_assert(last >= first); + for (Index i = first; i < last; ++i) { + evaluator.evalScalar(i); + } + } + + static Index alignBlockSize(Index size) { + return size; + } +}; + +template +struct EvalRange { + static const int PacketSize = unpacket_traits::size; + + static void run(Evaluator* evaluator_in, const Index first, const Index last) { + Evaluator evaluator = *evaluator_in; + eigen_assert(last >= first); + Index i = first; + if (last - first >= PacketSize) { + eigen_assert(first % PacketSize == 0); + Index last_chunk_offset = last - 4 * PacketSize; + // Give the compiler a strong hint to unroll the loop. But don't insist + // on unrolling, because if the function is expensive the compiler should not + // unroll the loop at the expense of inlining. + for (; i <= last_chunk_offset; i += 4*PacketSize) { + for (Index j = 0; j < 4; j++) { + evaluator.evalPacket(i + j * PacketSize); + } + } + last_chunk_offset = last - PacketSize; + for (; i <= last_chunk_offset; i += PacketSize) { + evaluator.evalPacket(i); + } + } + for (; i < last; ++i) { + evaluator.evalScalar(i); + } + } + + static Index alignBlockSize(Index size) { + // Align block size to packet size and account for unrolling in run above. + if (size >= 16 * PacketSize) { + return (size + 4 * PacketSize - 1) & ~(4 * PacketSize - 1); + } + // Aligning to 4 * PacketSize would increase block size by more than 25%. + return (size + PacketSize - 1) & ~(PacketSize - 1); + } +}; + +template +class TensorExecutor { + public: + typedef typename Expression::Index Index; + static inline void run(const Expression& expr, const ThreadPoolDevice& device) + { + typedef TensorEvaluator Evaluator; + Evaluator evaluator(expr, device); + const bool needs_assign = evaluator.evalSubExprsIfNeeded(NULL); + if (needs_assign) + { + const Index size = array_prod(evaluator.dimensions()); +#if !defined(EIGEN_USE_SIMPLE_THREAD_POOL) + device.parallelFor(size, evaluator.costPerCoeff(Vectorizable), + EvalRange::alignBlockSize, + [&evaluator](Index first, Index last) { + EvalRange::run(&evaluator, first, last); + }); +#else + size_t num_threads = device.numThreads(); + if (num_threads > 1) { + num_threads = TensorCostModel::numThreads( + size, evaluator.costPerCoeff(Vectorizable), num_threads); + } + if (num_threads == 1) { + EvalRange::run(&evaluator, 0, size); + } else { + const Index PacketSize = Vectorizable ? unpacket_traits::size : 1; + Index blocksz = std::ceil(static_cast(size)/num_threads) + PacketSize - 1; + const Index blocksize = numext::maxi(PacketSize, (blocksz - (blocksz % PacketSize))); + const Index numblocks = size / blocksize; + + Barrier barrier(numblocks); + for (int i = 0; i < numblocks; ++i) { + device.enqueue_with_barrier( + &barrier, &EvalRange::run, + &evaluator, i * blocksize, (i + 1) * blocksize); + } + if (numblocks * blocksize < size) { + EvalRange::run( + &evaluator, numblocks * blocksize, size); + } + barrier.Wait(); + } +#endif // defined(!EIGEN_USE_SIMPLE_THREAD_POOL) + } + evaluator.cleanup(); + } +}; +#endif // EIGEN_USE_THREADS + + +// GPU: the evaluation of the expression is offloaded to a GPU. +#if defined(EIGEN_USE_GPU) + +template +class TensorExecutor { + public: + typedef typename Expression::Index Index; + static void run(const Expression& expr, const GpuDevice& device); +}; + + +#if defined(__CUDACC__) +template +struct EigenMetaKernelEval { + static __device__ EIGEN_ALWAYS_INLINE + void run(Evaluator& eval, Index first, Index last, Index step_size) { + for (Index i = first; i < last; i += step_size) { + eval.evalScalar(i); + } + } +}; + +template +struct EigenMetaKernelEval { + static __device__ EIGEN_ALWAYS_INLINE + void run(Evaluator& eval, Index first, Index last, Index step_size) { + const Index PacketSize = unpacket_traits::size; + const Index vectorized_size = (last / PacketSize) * PacketSize; + const Index vectorized_step_size = step_size * PacketSize; + + // Use the vector path + for (Index i = first * PacketSize; i < vectorized_size; + i += vectorized_step_size) { + eval.evalPacket(i); + } + for (Index i = vectorized_size + first; i < last; i += step_size) { + eval.evalScalar(i); + } + } +}; + +template +__global__ void +__launch_bounds__(1024) +EigenMetaKernel(Evaluator eval, Index size) { + + const Index first_index = blockIdx.x * blockDim.x + threadIdx.x; + const Index step_size = blockDim.x * gridDim.x; + + const bool vectorizable = Evaluator::PacketAccess & Evaluator::IsAligned; + EigenMetaKernelEval::run(eval, first_index, size, step_size); +} + +/*static*/ +template +inline void TensorExecutor::run( + const Expression& expr, const GpuDevice& device) { + TensorEvaluator evaluator(expr, device); + const bool needs_assign = evaluator.evalSubExprsIfNeeded(NULL); + if (needs_assign) { + const int block_size = device.maxCudaThreadsPerBlock(); + const int max_blocks = device.getNumCudaMultiProcessors() * + device.maxCudaThreadsPerMultiProcessor() / block_size; + const Index size = array_prod(evaluator.dimensions()); + // Create a least one block to ensure we won't crash when tensorflow calls with tensors of size 0. + const int num_blocks = numext::maxi(numext::mini(max_blocks, divup(size, block_size)), 1); + + LAUNCH_CUDA_KERNEL( + (EigenMetaKernel, Index>), + num_blocks, block_size, 0, device, evaluator, size); + } + evaluator.cleanup(); +} + +#endif // __CUDACC__ +#endif // EIGEN_USE_GPU + +// SYCL Executor policy +#ifdef EIGEN_USE_SYCL + +template +class TensorExecutor { +public: + static inline void run(const Expression &expr, const SyclDevice &device) { + // call TensorSYCL module + TensorSycl::run(expr, device); + } +}; + +#endif + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_EXECUTOR_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExpr.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExpr.h new file mode 100644 index 0000000000..85dfc7a69f --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExpr.h @@ -0,0 +1,371 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_EXPR_H +#define EIGEN_CXX11_TENSOR_TENSOR_EXPR_H + +namespace Eigen { + +/** \class TensorExpr + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor expression classes. + * + * The TensorCwiseNullaryOp class applies a nullary operators to an expression. + * This is typically used to generate constants. + * + * The TensorCwiseUnaryOp class represents an expression where a unary operator + * (e.g. cwiseSqrt) is applied to an expression. + * + * The TensorCwiseBinaryOp class represents an expression where a binary + * operator (e.g. addition) is applied to a lhs and a rhs expression. + * + */ +namespace internal { +template +struct traits > + : traits +{ + typedef traits XprTraits; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::Nested XprTypeNested; + typedef typename remove_reference::type _XprTypeNested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; + + enum { + Flags = 0 + }; +}; + +} // end namespace internal + + + +template +class TensorCwiseNullaryOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef TensorCwiseNullaryOp Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorCwiseNullaryOp(const XprType& xpr, const NullaryOp& func = NullaryOp()) + : m_xpr(xpr), m_functor(func) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + nestedExpression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + const NullaryOp& functor() const { return m_functor; } + + protected: + typename XprType::Nested m_xpr; + const NullaryOp m_functor; +}; + + + +namespace internal { +template +struct traits > + : traits +{ + // TODO(phli): Add InputScalar, InputPacket. Check references to + // current Scalar/Packet to see if the intent is Input or Output. + typedef typename result_of::type Scalar; + typedef traits XprTraits; + typedef typename XprType::Nested XprTypeNested; + typedef typename remove_reference::type _XprTypeNested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorCwiseUnaryOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorCwiseUnaryOp type; +}; + +} // end namespace internal + + + +template +class TensorCwiseUnaryOp : public TensorBase, ReadOnlyAccessors> +{ + public: + // TODO(phli): Add InputScalar, InputPacket. Check references to + // current Scalar/Packet to see if the intent is Input or Output. + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef Scalar CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorCwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp()) + : m_xpr(xpr), m_functor(func) {} + + EIGEN_DEVICE_FUNC + const UnaryOp& functor() const { return m_functor; } + + /** \returns the nested expression */ + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + nestedExpression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const UnaryOp m_functor; +}; + + +namespace internal { +template +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs + // are different. + // TODO(phli): Add Lhs/RhsScalar, Lhs/RhsPacket. Check references to + // current Scalar/Packet to see if the intent is Inputs or Output. + typedef typename result_of< + BinaryOp(typename LhsXprType::Scalar, + typename RhsXprType::Scalar)>::type Scalar; + typedef traits XprTraits; + typedef typename promote_storage_type< + typename traits::StorageKind, + typename traits::StorageKind>::ret StorageKind; + typedef typename promote_index_type< + typename traits::Index, + typename traits::Index>::type Index; + typedef typename LhsXprType::Nested LhsNested; + typedef typename RhsXprType::Nested RhsNested; + typedef typename remove_reference::type _LhsNested; + typedef typename remove_reference::type _RhsNested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; + + enum { + Flags = 0 + }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorCwiseBinaryOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorCwiseBinaryOp type; +}; + +} // end namespace internal + + + +template +class TensorCwiseBinaryOp : public TensorBase, ReadOnlyAccessors> +{ + public: + // TODO(phli): Add Lhs/RhsScalar, Lhs/RhsPacket. Check references to + // current Scalar/Packet to see if the intent is Inputs or Output. + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef Scalar CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorCwiseBinaryOp(const LhsXprType& lhs, const RhsXprType& rhs, const BinaryOp& func = BinaryOp()) + : m_lhs_xpr(lhs), m_rhs_xpr(rhs), m_functor(func) {} + + EIGEN_DEVICE_FUNC + const BinaryOp& functor() const { return m_functor; } + + /** \returns the nested expressions */ + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + lhsExpression() const { return m_lhs_xpr; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + rhsExpression() const { return m_rhs_xpr; } + + protected: + typename LhsXprType::Nested m_lhs_xpr; + typename RhsXprType::Nested m_rhs_xpr; + const BinaryOp m_functor; +}; + + +namespace internal { +template +struct traits > +{ + // Type promotion to handle the case where the types of the args are different. + typedef typename result_of< + TernaryOp(typename Arg1XprType::Scalar, + typename Arg2XprType::Scalar, + typename Arg3XprType::Scalar)>::type Scalar; + typedef traits XprTraits; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::Index Index; + typedef typename Arg1XprType::Nested Arg1Nested; + typedef typename Arg2XprType::Nested Arg2Nested; + typedef typename Arg3XprType::Nested Arg3Nested; + typedef typename remove_reference::type _Arg1Nested; + typedef typename remove_reference::type _Arg2Nested; + typedef typename remove_reference::type _Arg3Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; + + enum { + Flags = 0 + }; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorCwiseTernaryOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorCwiseTernaryOp type; +}; + +} // end namespace internal + + + +template +class TensorCwiseTernaryOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef Scalar CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorCwiseTernaryOp(const Arg1XprType& arg1, const Arg2XprType& arg2, const Arg3XprType& arg3, const TernaryOp& func = TernaryOp()) + : m_arg1_xpr(arg1), m_arg2_xpr(arg2), m_arg3_xpr(arg3), m_functor(func) {} + + EIGEN_DEVICE_FUNC + const TernaryOp& functor() const { return m_functor; } + + /** \returns the nested expressions */ + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + arg1Expression() const { return m_arg1_xpr; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + arg2Expression() const { return m_arg2_xpr; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + arg3Expression() const { return m_arg3_xpr; } + + protected: + typename Arg1XprType::Nested m_arg1_xpr; + typename Arg2XprType::Nested m_arg2_xpr; + typename Arg3XprType::Nested m_arg3_xpr; + const TernaryOp m_functor; +}; + + +namespace internal { +template +struct traits > + : traits +{ + typedef typename traits::Scalar Scalar; + typedef traits XprTraits; + typedef typename promote_storage_type::StorageKind, + typename traits::StorageKind>::ret StorageKind; + typedef typename promote_index_type::Index, + typename traits::Index>::type Index; + typedef typename IfXprType::Nested IfNested; + typedef typename ThenXprType::Nested ThenNested; + typedef typename ElseXprType::Nested ElseNested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorSelectOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorSelectOp type; +}; + +} // end namespace internal + + +template +class TensorSelectOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::promote_storage_type::ret CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC + TensorSelectOp(const IfXprType& a_condition, + const ThenXprType& a_then, + const ElseXprType& a_else) + : m_condition(a_condition), m_then(a_then), m_else(a_else) + { } + + EIGEN_DEVICE_FUNC + const IfXprType& ifExpression() const { return m_condition; } + + EIGEN_DEVICE_FUNC + const ThenXprType& thenExpression() const { return m_then; } + + EIGEN_DEVICE_FUNC + const ElseXprType& elseExpression() const { return m_else; } + + protected: + typename IfXprType::Nested m_condition; + typename ThenXprType::Nested m_then; + typename ElseXprType::Nested m_else; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_EXPR_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFFT.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFFT.h new file mode 100644 index 0000000000..08eb5595a2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFFT.h @@ -0,0 +1,651 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Jianwei Cui +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_FFT_H +#define EIGEN_CXX11_TENSOR_TENSOR_FFT_H + +// This code requires the ability to initialize arrays of constant +// values directly inside a class. +#if __cplusplus >= 201103L || EIGEN_COMP_MSVC >= 1900 + +namespace Eigen { + +/** \class TensorFFT + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor FFT class. + * + * TODO: + * Vectorize the Cooley Tukey and the Bluestein algorithm + * Add support for multithreaded evaluation + * Improve the performance on GPU + */ + +template struct MakeComplex { + template + EIGEN_DEVICE_FUNC + T operator() (const T& val) const { return val; } +}; + +template <> struct MakeComplex { + template + EIGEN_DEVICE_FUNC + std::complex operator() (const T& val) const { return std::complex(val, 0); } +}; + +template <> struct MakeComplex { + template + EIGEN_DEVICE_FUNC + std::complex operator() (const std::complex& val) const { return val; } +}; + +template struct PartOf { + template T operator() (const T& val) const { return val; } +}; + +template <> struct PartOf { + template T operator() (const std::complex& val) const { return val.real(); } +}; + +template <> struct PartOf { + template T operator() (const std::complex& val) const { return val.imag(); } +}; + +namespace internal { +template +struct traits > : public traits { + typedef traits XprTraits; + typedef typename NumTraits::Real RealScalar; + typedef typename std::complex ComplexScalar; + typedef typename XprTraits::Scalar InputScalar; + typedef typename conditional::type OutputScalar; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> { + typedef const TensorFFTOp& type; +}; + +template +struct nested, 1, typename eval >::type> { + typedef TensorFFTOp type; +}; + +} // end namespace internal + +template +class TensorFFTOp : public TensorBase, ReadOnlyAccessors> { + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename std::complex ComplexScalar; + typedef typename internal::conditional::type OutputScalar; + typedef OutputScalar CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorFFTOp(const XprType& expr, const FFT& fft) + : m_xpr(expr), m_fft(fft) {} + + EIGEN_DEVICE_FUNC + const FFT& fft() const { return m_fft; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& expression() const { + return m_xpr; + } + + protected: + typename XprType::Nested m_xpr; + const FFT m_fft; +}; + +// Eval as rvalue +template +struct TensorEvaluator, Device> { + typedef TensorFFTOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::Dimensions>::value; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename std::complex ComplexScalar; + typedef typename TensorEvaluator::Dimensions InputDimensions; + typedef internal::traits XprTraits; + typedef typename XprTraits::Scalar InputScalar; + typedef typename internal::conditional::type OutputScalar; + typedef OutputScalar CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = true, + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) : m_fft(op.fft()), m_impl(op.expression(), device), m_data(NULL), m_device(device) { + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + for (int i = 0; i < NumDims; ++i) { + eigen_assert(input_dims[i] > 0); + m_dimensions[i] = input_dims[i]; + } + + if (static_cast(Layout) == static_cast(ColMajor)) { + m_strides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_strides[i] = m_strides[i - 1] * m_dimensions[i - 1]; + } + } else { + m_strides[NumDims - 1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_strides[i] = m_strides[i + 1] * m_dimensions[i + 1]; + } + } + m_size = m_dimensions.TotalSize(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { + return m_dimensions; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(OutputScalar* data) { + m_impl.evalSubExprsIfNeeded(NULL); + if (data) { + evalToBuf(data); + return false; + } else { + m_data = (CoeffReturnType*)m_device.allocate(sizeof(CoeffReturnType) * m_size); + evalToBuf(m_data); + return true; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + if (m_data) { + m_device.deallocate(m_data); + m_data = NULL; + } + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE CoeffReturnType coeff(Index index) const { + return m_data[index]; + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketReturnType + packet(Index index) const { + return internal::ploadt(m_data + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return m_data; } + + + private: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalToBuf(OutputScalar* data) { + const bool write_to_out = internal::is_same::value; + ComplexScalar* buf = write_to_out ? (ComplexScalar*)data : (ComplexScalar*)m_device.allocate(sizeof(ComplexScalar) * m_size); + + for (Index i = 0; i < m_size; ++i) { + buf[i] = MakeComplex::value>()(m_impl.coeff(i)); + } + + for (size_t i = 0; i < m_fft.size(); ++i) { + Index dim = m_fft[i]; + eigen_assert(dim >= 0 && dim < NumDims); + Index line_len = m_dimensions[dim]; + eigen_assert(line_len >= 1); + ComplexScalar* line_buf = (ComplexScalar*)m_device.allocate(sizeof(ComplexScalar) * line_len); + const bool is_power_of_two = isPowerOfTwo(line_len); + const Index good_composite = is_power_of_two ? 0 : findGoodComposite(line_len); + const Index log_len = is_power_of_two ? getLog2(line_len) : getLog2(good_composite); + + ComplexScalar* a = is_power_of_two ? NULL : (ComplexScalar*)m_device.allocate(sizeof(ComplexScalar) * good_composite); + ComplexScalar* b = is_power_of_two ? NULL : (ComplexScalar*)m_device.allocate(sizeof(ComplexScalar) * good_composite); + ComplexScalar* pos_j_base_powered = is_power_of_two ? NULL : (ComplexScalar*)m_device.allocate(sizeof(ComplexScalar) * (line_len + 1)); + if (!is_power_of_two) { + // Compute twiddle factors + // t_n = exp(sqrt(-1) * pi * n^2 / line_len) + // for n = 0, 1,..., line_len-1. + // For n > 2 we use the recurrence t_n = t_{n-1}^2 / t_{n-2} * t_1^2 + pos_j_base_powered[0] = ComplexScalar(1, 0); + if (line_len > 1) { + const RealScalar pi_over_len(EIGEN_PI / line_len); + const ComplexScalar pos_j_base = ComplexScalar( + std::cos(pi_over_len), std::sin(pi_over_len)); + pos_j_base_powered[1] = pos_j_base; + if (line_len > 2) { + const ComplexScalar pos_j_base_sq = pos_j_base * pos_j_base; + for (int j = 2; j < line_len + 1; ++j) { + pos_j_base_powered[j] = pos_j_base_powered[j - 1] * + pos_j_base_powered[j - 1] / + pos_j_base_powered[j - 2] * pos_j_base_sq; + } + } + } + } + + for (Index partial_index = 0; partial_index < m_size / line_len; ++partial_index) { + const Index base_offset = getBaseOffsetFromIndex(partial_index, dim); + + // get data into line_buf + const Index stride = m_strides[dim]; + if (stride == 1) { + memcpy(line_buf, &buf[base_offset], line_len*sizeof(ComplexScalar)); + } else { + Index offset = base_offset; + for (int j = 0; j < line_len; ++j, offset += stride) { + line_buf[j] = buf[offset]; + } + } + + // processs the line + if (is_power_of_two) { + processDataLineCooleyTukey(line_buf, line_len, log_len); + } + else { + processDataLineBluestein(line_buf, line_len, good_composite, log_len, a, b, pos_j_base_powered); + } + + // write back + if (FFTDir == FFT_FORWARD && stride == 1) { + memcpy(&buf[base_offset], line_buf, line_len*sizeof(ComplexScalar)); + } else { + Index offset = base_offset; + const ComplexScalar div_factor = ComplexScalar(1.0 / line_len, 0); + for (int j = 0; j < line_len; ++j, offset += stride) { + buf[offset] = (FFTDir == FFT_FORWARD) ? line_buf[j] : line_buf[j] * div_factor; + } + } + } + m_device.deallocate(line_buf); + if (!is_power_of_two) { + m_device.deallocate(a); + m_device.deallocate(b); + m_device.deallocate(pos_j_base_powered); + } + } + + if(!write_to_out) { + for (Index i = 0; i < m_size; ++i) { + data[i] = PartOf()(buf[i]); + } + m_device.deallocate(buf); + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static bool isPowerOfTwo(Index x) { + eigen_assert(x > 0); + return !(x & (x - 1)); + } + + // The composite number for padding, used in Bluestein's FFT algorithm + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static Index findGoodComposite(Index n) { + Index i = 2; + while (i < 2 * n - 1) i *= 2; + return i; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static Index getLog2(Index m) { + Index log2m = 0; + while (m >>= 1) log2m++; + return log2m; + } + + // Call Cooley Tukey algorithm directly, data length must be power of 2 + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void processDataLineCooleyTukey(ComplexScalar* line_buf, Index line_len, Index log_len) { + eigen_assert(isPowerOfTwo(line_len)); + scramble_FFT(line_buf, line_len); + compute_1D_Butterfly(line_buf, line_len, log_len); + } + + // Call Bluestein's FFT algorithm, m is a good composite number greater than (2 * n - 1), used as the padding length + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void processDataLineBluestein(ComplexScalar* line_buf, Index line_len, Index good_composite, Index log_len, ComplexScalar* a, ComplexScalar* b, const ComplexScalar* pos_j_base_powered) { + Index n = line_len; + Index m = good_composite; + ComplexScalar* data = line_buf; + + for (Index i = 0; i < n; ++i) { + if(FFTDir == FFT_FORWARD) { + a[i] = data[i] * numext::conj(pos_j_base_powered[i]); + } + else { + a[i] = data[i] * pos_j_base_powered[i]; + } + } + for (Index i = n; i < m; ++i) { + a[i] = ComplexScalar(0, 0); + } + + for (Index i = 0; i < n; ++i) { + if(FFTDir == FFT_FORWARD) { + b[i] = pos_j_base_powered[i]; + } + else { + b[i] = numext::conj(pos_j_base_powered[i]); + } + } + for (Index i = n; i < m - n; ++i) { + b[i] = ComplexScalar(0, 0); + } + for (Index i = m - n; i < m; ++i) { + if(FFTDir == FFT_FORWARD) { + b[i] = pos_j_base_powered[m-i]; + } + else { + b[i] = numext::conj(pos_j_base_powered[m-i]); + } + } + + scramble_FFT(a, m); + compute_1D_Butterfly(a, m, log_len); + + scramble_FFT(b, m); + compute_1D_Butterfly(b, m, log_len); + + for (Index i = 0; i < m; ++i) { + a[i] *= b[i]; + } + + scramble_FFT(a, m); + compute_1D_Butterfly(a, m, log_len); + + //Do the scaling after ifft + for (Index i = 0; i < m; ++i) { + a[i] /= m; + } + + for (Index i = 0; i < n; ++i) { + if(FFTDir == FFT_FORWARD) { + data[i] = a[i] * numext::conj(pos_j_base_powered[i]); + } + else { + data[i] = a[i] * pos_j_base_powered[i]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE static void scramble_FFT(ComplexScalar* data, Index n) { + eigen_assert(isPowerOfTwo(n)); + Index j = 1; + for (Index i = 1; i < n; ++i){ + if (j > i) { + std::swap(data[j-1], data[i-1]); + } + Index m = n >> 1; + while (m >= 2 && j > m) { + j -= m; + m >>= 1; + } + j += m; + } + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void butterfly_2(ComplexScalar* data) { + ComplexScalar tmp = data[1]; + data[1] = data[0] - data[1]; + data[0] += tmp; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void butterfly_4(ComplexScalar* data) { + ComplexScalar tmp[4]; + tmp[0] = data[0] + data[1]; + tmp[1] = data[0] - data[1]; + tmp[2] = data[2] + data[3]; + if (Dir == FFT_FORWARD) { + tmp[3] = ComplexScalar(0.0, -1.0) * (data[2] - data[3]); + } else { + tmp[3] = ComplexScalar(0.0, 1.0) * (data[2] - data[3]); + } + data[0] = tmp[0] + tmp[2]; + data[1] = tmp[1] + tmp[3]; + data[2] = tmp[0] - tmp[2]; + data[3] = tmp[1] - tmp[3]; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void butterfly_8(ComplexScalar* data) { + ComplexScalar tmp_1[8]; + ComplexScalar tmp_2[8]; + + tmp_1[0] = data[0] + data[1]; + tmp_1[1] = data[0] - data[1]; + tmp_1[2] = data[2] + data[3]; + if (Dir == FFT_FORWARD) { + tmp_1[3] = (data[2] - data[3]) * ComplexScalar(0, -1); + } else { + tmp_1[3] = (data[2] - data[3]) * ComplexScalar(0, 1); + } + tmp_1[4] = data[4] + data[5]; + tmp_1[5] = data[4] - data[5]; + tmp_1[6] = data[6] + data[7]; + if (Dir == FFT_FORWARD) { + tmp_1[7] = (data[6] - data[7]) * ComplexScalar(0, -1); + } else { + tmp_1[7] = (data[6] - data[7]) * ComplexScalar(0, 1); + } + tmp_2[0] = tmp_1[0] + tmp_1[2]; + tmp_2[1] = tmp_1[1] + tmp_1[3]; + tmp_2[2] = tmp_1[0] - tmp_1[2]; + tmp_2[3] = tmp_1[1] - tmp_1[3]; + tmp_2[4] = tmp_1[4] + tmp_1[6]; +// SQRT2DIV2 = sqrt(2)/2 +#define SQRT2DIV2 0.7071067811865476 + if (Dir == FFT_FORWARD) { + tmp_2[5] = (tmp_1[5] + tmp_1[7]) * ComplexScalar(SQRT2DIV2, -SQRT2DIV2); + tmp_2[6] = (tmp_1[4] - tmp_1[6]) * ComplexScalar(0, -1); + tmp_2[7] = (tmp_1[5] - tmp_1[7]) * ComplexScalar(-SQRT2DIV2, -SQRT2DIV2); + } else { + tmp_2[5] = (tmp_1[5] + tmp_1[7]) * ComplexScalar(SQRT2DIV2, SQRT2DIV2); + tmp_2[6] = (tmp_1[4] - tmp_1[6]) * ComplexScalar(0, 1); + tmp_2[7] = (tmp_1[5] - tmp_1[7]) * ComplexScalar(-SQRT2DIV2, SQRT2DIV2); + } + data[0] = tmp_2[0] + tmp_2[4]; + data[1] = tmp_2[1] + tmp_2[5]; + data[2] = tmp_2[2] + tmp_2[6]; + data[3] = tmp_2[3] + tmp_2[7]; + data[4] = tmp_2[0] - tmp_2[4]; + data[5] = tmp_2[1] - tmp_2[5]; + data[6] = tmp_2[2] - tmp_2[6]; + data[7] = tmp_2[3] - tmp_2[7]; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void butterfly_1D_merge( + ComplexScalar* data, Index n, Index n_power_of_2) { + // Original code: + // RealScalar wtemp = std::sin(M_PI/n); + // RealScalar wpi = -std::sin(2 * M_PI/n); + const RealScalar wtemp = m_sin_PI_div_n_LUT[n_power_of_2]; + const RealScalar wpi = (Dir == FFT_FORWARD) + ? m_minus_sin_2_PI_div_n_LUT[n_power_of_2] + : -m_minus_sin_2_PI_div_n_LUT[n_power_of_2]; + + const ComplexScalar wp(wtemp, wpi); + const ComplexScalar wp_one = wp + ComplexScalar(1, 0); + const ComplexScalar wp_one_2 = wp_one * wp_one; + const ComplexScalar wp_one_3 = wp_one_2 * wp_one; + const ComplexScalar wp_one_4 = wp_one_3 * wp_one; + const Index n2 = n / 2; + ComplexScalar w(1.0, 0.0); + for (Index i = 0; i < n2; i += 4) { + ComplexScalar temp0(data[i + n2] * w); + ComplexScalar temp1(data[i + 1 + n2] * w * wp_one); + ComplexScalar temp2(data[i + 2 + n2] * w * wp_one_2); + ComplexScalar temp3(data[i + 3 + n2] * w * wp_one_3); + w = w * wp_one_4; + + data[i + n2] = data[i] - temp0; + data[i] += temp0; + + data[i + 1 + n2] = data[i + 1] - temp1; + data[i + 1] += temp1; + + data[i + 2 + n2] = data[i + 2] - temp2; + data[i + 2] += temp2; + + data[i + 3 + n2] = data[i + 3] - temp3; + data[i + 3] += temp3; + } + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void compute_1D_Butterfly( + ComplexScalar* data, Index n, Index n_power_of_2) { + eigen_assert(isPowerOfTwo(n)); + if (n > 8) { + compute_1D_Butterfly(data, n / 2, n_power_of_2 - 1); + compute_1D_Butterfly(data + n / 2, n / 2, n_power_of_2 - 1); + butterfly_1D_merge(data, n, n_power_of_2); + } else if (n == 8) { + butterfly_8(data); + } else if (n == 4) { + butterfly_4(data); + } else if (n == 2) { + butterfly_2(data); + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index getBaseOffsetFromIndex(Index index, Index omitted_dim) const { + Index result = 0; + + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > omitted_dim; --i) { + const Index partial_m_stride = m_strides[i] / m_dimensions[omitted_dim]; + const Index idx = index / partial_m_stride; + index -= idx * partial_m_stride; + result += idx * m_strides[i]; + } + result += index; + } + else { + for (Index i = 0; i < omitted_dim; ++i) { + const Index partial_m_stride = m_strides[i] / m_dimensions[omitted_dim]; + const Index idx = index / partial_m_stride; + index -= idx * partial_m_stride; + result += idx * m_strides[i]; + } + result += index; + } + // Value of index_coords[omitted_dim] is not determined to this step + return result; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index getIndexFromOffset(Index base, Index omitted_dim, Index offset) const { + Index result = base + offset * m_strides[omitted_dim] ; + return result; + } + + protected: + Index m_size; + const FFT& m_fft; + Dimensions m_dimensions; + array m_strides; + TensorEvaluator m_impl; + CoeffReturnType* m_data; + const Device& m_device; + + // This will support a maximum FFT size of 2^32 for each dimension + // m_sin_PI_div_n_LUT[i] = (-2) * std::sin(M_PI / std::pow(2,i)) ^ 2; + const RealScalar m_sin_PI_div_n_LUT[32] = { + RealScalar(0.0), + RealScalar(-2), + RealScalar(-0.999999999999999), + RealScalar(-0.292893218813453), + RealScalar(-0.0761204674887130), + RealScalar(-0.0192147195967696), + RealScalar(-0.00481527332780311), + RealScalar(-0.00120454379482761), + RealScalar(-3.01181303795779e-04), + RealScalar(-7.52981608554592e-05), + RealScalar(-1.88247173988574e-05), + RealScalar(-4.70619042382852e-06), + RealScalar(-1.17654829809007e-06), + RealScalar(-2.94137117780840e-07), + RealScalar(-7.35342821488550e-08), + RealScalar(-1.83835707061916e-08), + RealScalar(-4.59589268710903e-09), + RealScalar(-1.14897317243732e-09), + RealScalar(-2.87243293150586e-10), + RealScalar( -7.18108232902250e-11), + RealScalar(-1.79527058227174e-11), + RealScalar(-4.48817645568941e-12), + RealScalar(-1.12204411392298e-12), + RealScalar(-2.80511028480785e-13), + RealScalar(-7.01277571201985e-14), + RealScalar(-1.75319392800498e-14), + RealScalar(-4.38298482001247e-15), + RealScalar(-1.09574620500312e-15), + RealScalar(-2.73936551250781e-16), + RealScalar(-6.84841378126949e-17), + RealScalar(-1.71210344531737e-17), + RealScalar(-4.28025861329343e-18) + }; + + // m_minus_sin_2_PI_div_n_LUT[i] = -std::sin(2 * M_PI / std::pow(2,i)); + const RealScalar m_minus_sin_2_PI_div_n_LUT[32] = { + RealScalar(0.0), + RealScalar(0.0), + RealScalar(-1.00000000000000e+00), + RealScalar(-7.07106781186547e-01), + RealScalar(-3.82683432365090e-01), + RealScalar(-1.95090322016128e-01), + RealScalar(-9.80171403295606e-02), + RealScalar(-4.90676743274180e-02), + RealScalar(-2.45412285229123e-02), + RealScalar(-1.22715382857199e-02), + RealScalar(-6.13588464915448e-03), + RealScalar(-3.06795676296598e-03), + RealScalar(-1.53398018628477e-03), + RealScalar(-7.66990318742704e-04), + RealScalar(-3.83495187571396e-04), + RealScalar(-1.91747597310703e-04), + RealScalar(-9.58737990959773e-05), + RealScalar(-4.79368996030669e-05), + RealScalar(-2.39684498084182e-05), + RealScalar(-1.19842249050697e-05), + RealScalar(-5.99211245264243e-06), + RealScalar(-2.99605622633466e-06), + RealScalar(-1.49802811316901e-06), + RealScalar(-7.49014056584716e-07), + RealScalar(-3.74507028292384e-07), + RealScalar(-1.87253514146195e-07), + RealScalar(-9.36267570730981e-08), + RealScalar(-4.68133785365491e-08), + RealScalar(-2.34066892682746e-08), + RealScalar(-1.17033446341373e-08), + RealScalar(-5.85167231706864e-09), + RealScalar(-2.92583615853432e-09) + }; +}; + +} // end namespace Eigen + +#endif // EIGEN_HAS_CONSTEXPR + + +#endif // EIGEN_CXX11_TENSOR_TENSOR_FFT_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFixedSize.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFixedSize.h new file mode 100644 index 0000000000..fcee5f60d0 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFixedSize.h @@ -0,0 +1,389 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_FIXED_SIZE_H +#define EIGEN_CXX11_TENSOR_TENSOR_FIXED_SIZE_H + +namespace Eigen { + +/** \class TensorFixedSize + * \ingroup CXX11_Tensor_Module + * + * \brief The fixed sized version of the tensor class. + * + * The fixed sized equivalent of + * Eigen::Tensor t(3, 5, 7); + * is + * Eigen::TensorFixedSize> t; + */ + +template +class TensorFixedSize : public TensorBase > +{ + public: + typedef TensorFixedSize Self; + typedef TensorBase > Base; + typedef typename Eigen::internal::nested::type Nested; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + typedef Scalar_ Scalar; + typedef typename NumTraits::Real RealScalar; + typedef typename Base::CoeffReturnType CoeffReturnType; + + static const int Options = Options_; + + enum { + IsAligned = bool(EIGEN_MAX_ALIGN_BYTES>0), + Layout = Options_ & RowMajor ? RowMajor : ColMajor, + CoordAccess = true, + RawAccess = true + }; + + typedef Dimensions_ Dimensions; + static const std::size_t NumIndices = Dimensions::count; + + protected: + TensorStorage m_storage; + + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rank() const { return NumIndices; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index dimension(std::size_t n) const { return m_storage.dimensions()[n]; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_storage.dimensions(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const { return m_storage.size(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar *data() { return m_storage.data(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar *data() const { return m_storage.data(); } + + // This makes EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED + // work, because that uses base().coeffRef() - and we don't yet + // implement a similar class hierarchy + inline Self& base() { return *this; } + inline const Self& base() const { return *this; } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& coeff(Index firstIndex, IndexTypes... otherIndices) const + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return coeff(array{{firstIndex, otherIndices...}}); + } +#endif + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& coeff(const array& indices) const + { + eigen_internal_assert(checkIndexRange(indices)); + return m_storage.data()[linearizedIndex(indices)]; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const + { + eigen_internal_assert(index >= 0 && index < size()); + return m_storage.data()[index]; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& coeff() const + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return m_storage.data()[0]; + } + + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& coeffRef(Index firstIndex, IndexTypes... otherIndices) + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return coeffRef(array{{firstIndex, otherIndices...}}); + } +#endif + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& coeffRef(const array& indices) + { + eigen_internal_assert(checkIndexRange(indices)); + return m_storage.data()[linearizedIndex(indices)]; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) + { + eigen_internal_assert(index >= 0 && index < size()); + return m_storage.data()[index]; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& coeffRef() + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return m_storage.data()[0]; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar& operator()(Index firstIndex, IndexTypes... otherIndices) const + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return this->operator()(array{{firstIndex, otherIndices...}}); + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1) const + { + if (Options&RowMajor) { + const Index index = i1 + i0 * m_storage.dimensions()[1]; + return m_storage.data()[index]; + } else { + const Index index = i0 + i1 * m_storage.dimensions()[0]; + return m_storage.data()[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2) const + { + if (Options&RowMajor) { + const Index index = i2 + m_storage.dimensions()[2] * (i1 + m_storage.dimensions()[1] * i0); + return m_storage.data()[index]; + } else { + const Index index = i0 + m_storage.dimensions()[0] * (i1 + m_storage.dimensions()[1] * i2); + return m_storage.data()[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3) const + { + if (Options&RowMajor) { + const Index index = i3 + m_storage.dimensions()[3] * (i2 + m_storage.dimensions()[2] * (i1 + m_storage.dimensions()[1] * i0)); + return m_storage.data()[index]; + } else { + const Index index = i0 + m_storage.dimensions()[0] * (i1 + m_storage.dimensions()[1] * (i2 + m_storage.dimensions()[2] * i3)); + return m_storage.data()[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) const + { + if (Options&RowMajor) { + const Index index = i4 + m_storage.dimensions()[4] * (i3 + m_storage.dimensions()[3] * (i2 + m_storage.dimensions()[2] * (i1 + m_storage.dimensions()[1] * i0))); + return m_storage.data()[index]; + } else { + const Index index = i0 + m_storage.dimensions()[0] * (i1 + m_storage.dimensions()[1] * (i2 + m_storage.dimensions()[2] * (i3 + m_storage.dimensions()[3] * i4))); + return m_storage.data()[index]; + } + } +#endif + + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(const array& indices) const + { + eigen_assert(checkIndexRange(indices)); + return coeff(indices); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index index) const + { + eigen_internal_assert(index >= 0 && index < size()); + return coeff(index); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()() const + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return coeff(); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator[](Index index) const + { + // The bracket operator is only for vectors, use the parenthesis operator instead. + EIGEN_STATIC_ASSERT(NumIndices == 1, YOU_MADE_A_PROGRAMMING_MISTAKE); + return coeff(index); + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar& operator()(Index firstIndex, IndexTypes... otherIndices) + { + // The number of indices used to access a tensor coefficient must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + return operator()(array{{firstIndex, otherIndices...}}); + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1) + { + if (Options&RowMajor) { + const Index index = i1 + i0 * m_storage.dimensions()[1]; + return m_storage.data()[index]; + } else { + const Index index = i0 + i1 * m_storage.dimensions()[0]; + return m_storage.data()[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2) + { + if (Options&RowMajor) { + const Index index = i2 + m_storage.dimensions()[2] * (i1 + m_storage.dimensions()[1] * i0); + return m_storage.data()[index]; + } else { + const Index index = i0 + m_storage.dimensions()[0] * (i1 + m_storage.dimensions()[1] * i2); + return m_storage.data()[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2, Index i3) + { + if (Options&RowMajor) { + const Index index = i3 + m_storage.dimensions()[3] * (i2 + m_storage.dimensions()[2] * (i1 + m_storage.dimensions()[1] * i0)); + return m_storage.data()[index]; + } else { + const Index index = i0 + m_storage.dimensions()[0] * (i1 + m_storage.dimensions()[1] * (i2 + m_storage.dimensions()[2] * i3)); + return m_storage.data()[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) + { + if (Options&RowMajor) { + const Index index = i4 + m_storage.dimensions()[4] * (i3 + m_storage.dimensions()[3] * (i2 + m_storage.dimensions()[2] * (i1 + m_storage.dimensions()[1] * i0))); + return m_storage.data()[index]; + } else { + const Index index = i0 + m_storage.dimensions()[0] * (i1 + m_storage.dimensions()[1] * (i2 + m_storage.dimensions()[2] * (i3 + m_storage.dimensions()[3] * i4))); + return m_storage.data()[index]; + } + } +#endif + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(const array& indices) + { + eigen_assert(checkIndexRange(indices)); + return coeffRef(indices); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index index) + { + eigen_assert(index >= 0 && index < size()); + return coeffRef(index); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()() + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE); + return coeffRef(); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator[](Index index) + { + // The bracket operator is only for vectors, use the parenthesis operator instead + EIGEN_STATIC_ASSERT(NumIndices == 1, YOU_MADE_A_PROGRAMMING_MISTAKE) + return coeffRef(index); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorFixedSize() + : m_storage() + { + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorFixedSize(const Self& other) + : m_storage(other.m_storage) + { + } + +#if EIGEN_HAS_RVALUE_REFERENCES + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorFixedSize(Self&& other) + : m_storage(other.m_storage) + { + } +#endif + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorFixedSize(const TensorBase& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other.derived()); + internal::TensorExecutor::run(assign, DefaultDevice()); + } + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorFixedSize(const TensorBase& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other.derived()); + internal::TensorExecutor::run(assign, DefaultDevice()); + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorFixedSize& operator=(const TensorFixedSize& other) + { + // FIXME: check that the dimensions of other match the dimensions of *this. + // Unfortunately this isn't possible yet when the rhs is an expression. + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorFixedSize& operator=(const OtherDerived& other) + { + // FIXME: check that the dimensions of other match the dimensions of *this. + // Unfortunately this isn't possible yet when the rhs is an expression. + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + protected: + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE bool checkIndexRange(const array& /*indices*/) const + { + using internal::array_apply_and_reduce; + using internal::array_zip_and_reduce; + using internal::greater_equal_zero_op; + using internal::logical_and_op; + using internal::lesser_op; + + return true; + // check whether the indices are all >= 0 + /* array_apply_and_reduce(indices) && + // check whether the indices fit in the dimensions + array_zip_and_reduce(indices, m_storage.dimensions());*/ + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index linearizedIndex(const array& indices) const + { + if (Options&RowMajor) { + return m_storage.dimensions().IndexOfRowMajor(indices); + } else { + return m_storage.dimensions().IndexOfColMajor(indices); + } + } +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_FIXED_SIZE_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h new file mode 100644 index 0000000000..bbd5eb3743 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h @@ -0,0 +1,167 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_FORCED_EVAL_H +#define EIGEN_CXX11_TENSOR_TENSOR_FORCED_EVAL_H + +namespace Eigen { + +/** \class TensorForcedEval + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor reshaping class. + * + * + */ +/// template class MakePointer_ is added to convert the host pointer to the device pointer. +/// It is added due to the fact that for our device compiler T* is not allowed. +/// If we wanted to use the same Evaluator functions we have to convert that type to our pointer T. +/// This is done through our MakePointer_ class. By default the Type in the MakePointer_ is T* . +/// Therefore, by adding the default value, we managed to convert the type and it does not break any +/// existing code as its default value is T*. +namespace internal { +template class MakePointer_> +struct traits > +{ + // Type promotion to handle the case where the types of the lhs and the rhs are different. + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename traits::StorageKind StorageKind; + typedef typename traits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; + + enum { + Flags = 0 + }; + template struct MakePointer { + // Intermediate typedef to workaround MSVC issue. + typedef MakePointer_ MakePointerT; + typedef typename MakePointerT::Type Type; + }; +}; + +template class MakePointer_> +struct eval, Eigen::Dense> +{ + typedef const TensorForcedEvalOp& type; +}; + +template class MakePointer_> +struct nested, 1, typename eval >::type> +{ + typedef TensorForcedEvalOp type; +}; + +} // end namespace internal + + + +template class MakePointer_> +class TensorForcedEvalOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorForcedEvalOp(const XprType& expr) + : m_xpr(expr) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; +}; + + +template class MakePointer_> +struct TensorEvaluator, Device> +{ + typedef TensorForcedEvalOp XprType; + typedef typename ArgType::Scalar Scalar; + typedef typename TensorEvaluator::Dimensions Dimensions; + typedef typename XprType::Index Index; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = true, + PacketAccess = (PacketSize > 1), + Layout = TensorEvaluator::Layout, + RawAccess = true + }; + + EIGEN_DEVICE_FUNC TensorEvaluator(const XprType& op, const Device& device) + /// op_ is used for sycl + : m_impl(op.expression(), device), m_op(op.expression()), m_device(device), m_buffer(NULL) + { } + + EIGEN_DEVICE_FUNC const Dimensions& dimensions() const { return m_impl.dimensions(); } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType*) { + const Index numValues = internal::array_prod(m_impl.dimensions()); + m_buffer = (CoeffReturnType*)m_device.allocate(numValues * sizeof(CoeffReturnType)); + // Should initialize the memory in case we're dealing with non POD types. + if (NumTraits::RequireInitialization) { + for (Index i = 0; i < numValues; ++i) { + new(m_buffer+i) CoeffReturnType(); + } + } + typedef TensorEvalToOp< const typename internal::remove_const::type > EvalTo; + EvalTo evalToTmp(m_buffer, m_op); + const bool PacketAccess = internal::IsVectorizable::value; + internal::TensorExecutor::type, PacketAccess>::run(evalToTmp, m_device); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_device.deallocate(m_buffer); + m_buffer = NULL; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_buffer[index]; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return internal::ploadt(m_buffer + index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC typename MakePointer::Type data() const { return m_buffer; } + + /// required by sycl in order to extract the sycl accessor + const TensorEvaluator& impl() { return m_impl; } + /// used by sycl in order to build the sycl buffer + const Device& device() const{return m_device;} + private: + TensorEvaluator m_impl; + const ArgType m_op; + const Device& m_device; + typename MakePointer::Type m_buffer; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_FORCED_EVAL_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForwardDeclarations.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForwardDeclarations.h new file mode 100644 index 0000000000..52b803d7f2 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForwardDeclarations.h @@ -0,0 +1,109 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_FORWARD_DECLARATIONS_H +#define EIGEN_CXX11_TENSOR_TENSOR_FORWARD_DECLARATIONS_H + +namespace Eigen { + +// MakePointer class is used as a container of the adress space of the pointer +// on the host and on the device. From the host side it generates the T* pointer +// and when EIGEN_USE_SYCL is used it construct a buffer with a map_allocator to +// T* m_data on the host. It is always called on the device. +// Specialisation of MakePointer class for creating the sycl buffer with +// map_allocator. +template struct MakePointer { + typedef T* Type; +}; + +template class MakePointer_ = MakePointer> class TensorMap; +template class Tensor; +template class TensorFixedSize; +template class TensorRef; +template class TensorBase; + +template class TensorCwiseNullaryOp; +template class TensorCwiseUnaryOp; +template class TensorCwiseBinaryOp; +template class TensorCwiseTernaryOp; +template class TensorSelectOp; +template class MakePointer_ = MakePointer > class TensorReductionOp; +template class TensorIndexTupleOp; +template class TensorTupleReducerOp; +template class TensorConcatenationOp; +template class TensorContractionOp; +template class TensorConversionOp; +template class TensorConvolutionOp; +template class TensorFFTOp; +template class TensorPatchOp; +template class TensorImagePatchOp; +template class TensorVolumePatchOp; +template class TensorBroadcastingOp; +template class TensorChippingOp; +template class TensorReshapingOp; +template class TensorLayoutSwapOp; +template class TensorSlicingOp; +template class TensorReverseOp; +template class TensorPaddingOp; +template class TensorShufflingOp; +template class TensorStridingOp; +template class TensorStridingSlicingOp; +template class TensorInflationOp; +template class TensorGeneratorOp; +template class TensorAssignOp; +template class TensorScanOp; + +template class TensorCustomUnaryOp; +template class TensorCustomBinaryOp; + +template class MakePointer_ = MakePointer> class TensorEvalToOp; +template class MakePointer_ = MakePointer> class TensorForcedEvalOp; + +template class TensorDevice; +template struct TensorEvaluator; + +struct DefaultDevice; +struct ThreadPoolDevice; +struct GpuDevice; +struct SyclDevice; + +enum FFTResultType { + RealPart = 0, + ImagPart = 1, + BothParts = 2 +}; + +enum FFTDirection { + FFT_FORWARD = 0, + FFT_REVERSE = 1 +}; + + +namespace internal { + +template +struct IsVectorizable { + static const bool value = TensorEvaluator::PacketAccess; +}; + +template +struct IsVectorizable { + static const bool value = TensorEvaluator::PacketAccess && + TensorEvaluator::IsAligned; +}; + +template ::value> +class TensorExecutor; + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_FORWARD_DECLARATIONS_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFunctors.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFunctors.h new file mode 100644 index 0000000000..d73f6dc683 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFunctors.h @@ -0,0 +1,489 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_FUNCTORS_H +#define EIGEN_CXX11_TENSOR_TENSOR_FUNCTORS_H + +namespace Eigen { +namespace internal { + + +/** \internal + * \brief Template functor to compute the modulo between an array and a scalar. + */ +template +struct scalar_mod_op { + EIGEN_DEVICE_FUNC scalar_mod_op(const Scalar& divisor) : m_divisor(divisor) {} + EIGEN_DEVICE_FUNC inline Scalar operator() (const Scalar& a) const { return a % m_divisor; } + const Scalar m_divisor; +}; +template +struct functor_traits > +{ enum { Cost = scalar_div_cost::value, PacketAccess = false }; }; + + +/** \internal + * \brief Template functor to compute the modulo between 2 arrays. + */ +template +struct scalar_mod2_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_mod2_op); + EIGEN_DEVICE_FUNC inline Scalar operator() (const Scalar& a, const Scalar& b) const { return a % b; } +}; +template +struct functor_traits > +{ enum { Cost = scalar_div_cost::value, PacketAccess = false }; }; + +template +struct scalar_fmod_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_fmod_op); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar + operator()(const Scalar& a, const Scalar& b) const { + return numext::fmod(a, b); + } +}; +template +struct functor_traits > { + enum { Cost = 13, // Reciprocal throughput of FPREM on Haswell. + PacketAccess = false }; +}; + + +/** \internal + * \brief Template functor to compute the sigmoid of a scalar + * \sa class CwiseUnaryOp, ArrayBase::sigmoid() + */ +template +struct scalar_sigmoid_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_sigmoid_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const { + const T one = T(1); + return one / (one + numext::exp(-x)); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Packet packetOp(const Packet& x) const { + const Packet one = pset1(T(1)); + return pdiv(one, padd(one, pexp(pnegate(x)))); + } +}; + +template +struct functor_traits > { + enum { + Cost = NumTraits::AddCost * 2 + NumTraits::MulCost * 6, + PacketAccess = packet_traits::HasAdd && packet_traits::HasDiv && + packet_traits::HasNegate && packet_traits::HasExp + }; +}; + + +template +struct reducer_traits { + enum { + Cost = 1, + PacketAccess = false + }; +}; + +// Standard reduction functors +template struct SumReducer +{ + static const bool PacketAccess = packet_traits::HasAdd; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T t, T* accum) const { + internal::scalar_sum_op sum_op; + *accum = sum_op(*accum, t); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reducePacket(const Packet& p, Packet* accum) const { + (*accum) = padd(*accum, p); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + internal::scalar_cast_op conv; + return conv(0); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet initializePacket() const { + return pset1(initialize()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T accum) const { + return accum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet finalizePacket(const Packet& vaccum) const { + return vaccum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalizeBoth(const T saccum, const Packet& vaccum) const { + internal::scalar_sum_op sum_op; + return sum_op(saccum, predux(vaccum)); + } +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::AddCost, + PacketAccess = PacketType::HasAdd + }; +}; + + +template struct MeanReducer +{ + static const bool PacketAccess = packet_traits::HasAdd && !NumTraits::IsInteger; + static const bool IsStateful = true; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + MeanReducer() : scalarCount_(0), packetCount_(0) { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T t, T* accum) { + internal::scalar_sum_op sum_op; + *accum = sum_op(*accum, t); + scalarCount_++; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reducePacket(const Packet& p, Packet* accum) { + (*accum) = padd(*accum, p); + packetCount_++; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + internal::scalar_cast_op conv; + return conv(0); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet initializePacket() const { + return pset1(initialize()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T accum) const { + return accum / scalarCount_; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet finalizePacket(const Packet& vaccum) const { + return pdiv(vaccum, pset1(packetCount_)); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalizeBoth(const T saccum, const Packet& vaccum) const { + internal::scalar_sum_op sum_op; + return sum_op(saccum, predux(vaccum)) / (scalarCount_ + packetCount_ * unpacket_traits::size); + } + + protected: + DenseIndex scalarCount_; + DenseIndex packetCount_; +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::AddCost, + PacketAccess = PacketType::HasAdd + }; +}; + + +template +struct MinMaxBottomValue { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T bottom_value() { + return Eigen::NumTraits::lowest(); + } +}; +template +struct MinMaxBottomValue { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T bottom_value() { + return -Eigen::NumTraits::infinity(); + } +}; +template +struct MinMaxBottomValue { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T bottom_value() { + return Eigen::NumTraits::highest(); + } +}; +template +struct MinMaxBottomValue { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T bottom_value() { + return Eigen::NumTraits::infinity(); + } +}; + + +template struct MaxReducer +{ + static const bool PacketAccess = packet_traits::HasMax; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T t, T* accum) const { + if (t > *accum) { *accum = t; } + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reducePacket(const Packet& p, Packet* accum) const { + (*accum) = pmax(*accum, p); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + return MinMaxBottomValue::IsInteger>::bottom_value(); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet initializePacket() const { + return pset1(initialize()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T accum) const { + return accum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet finalizePacket(const Packet& vaccum) const { + return vaccum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalizeBoth(const T saccum, const Packet& vaccum) const { + return numext::maxi(saccum, predux_max(vaccum)); + } +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::AddCost, + PacketAccess = PacketType::HasMax + }; +}; + + +template struct MinReducer +{ + static const bool PacketAccess = packet_traits::HasMin; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T t, T* accum) const { + if (t < *accum) { *accum = t; } + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reducePacket(const Packet& p, Packet* accum) const { + (*accum) = pmin(*accum, p); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + return MinMaxBottomValue::IsInteger>::bottom_value(); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet initializePacket() const { + return pset1(initialize()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T accum) const { + return accum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet finalizePacket(const Packet& vaccum) const { + return vaccum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalizeBoth(const T saccum, const Packet& vaccum) const { + return numext::mini(saccum, predux_min(vaccum)); + } +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::AddCost, + PacketAccess = PacketType::HasMin + }; +}; + + +template struct ProdReducer +{ + static const bool PacketAccess = packet_traits::HasMul; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T t, T* accum) const { + internal::scalar_product_op prod_op; + (*accum) = prod_op(*accum, t); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reducePacket(const Packet& p, Packet* accum) const { + (*accum) = pmul(*accum, p); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + internal::scalar_cast_op conv; + return conv(1); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet initializePacket() const { + return pset1(initialize()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T accum) const { + return accum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet finalizePacket(const Packet& vaccum) const { + return vaccum; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalizeBoth(const T saccum, const Packet& vaccum) const { + internal::scalar_product_op prod_op; + return prod_op(saccum, predux_mul(vaccum)); + } +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::MulCost, + PacketAccess = PacketType::HasMul + }; +}; + + +struct AndReducer +{ + static const bool PacketAccess = false; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(bool t, bool* accum) const { + *accum = *accum && t; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool initialize() const { + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool finalize(bool accum) const { + return accum; + } +}; + +template +struct reducer_traits { + enum { + Cost = 1, + PacketAccess = false + }; +}; + + +struct OrReducer { + static const bool PacketAccess = false; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(bool t, bool* accum) const { + *accum = *accum || t; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool initialize() const { + return false; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool finalize(bool accum) const { + return accum; + } +}; + +template +struct reducer_traits { + enum { + Cost = 1, + PacketAccess = false + }; +}; + + +// Argmin/Argmax reducers +template struct ArgMaxTupleReducer +{ + static const bool PacketAccess = false; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T t, T* accum) const { + if (t.second > accum->second) { *accum = t; } + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + return T(0, NumTraits::lowest()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T& accum) const { + return accum; + } +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::AddCost, + PacketAccess = false + }; +}; + + +template struct ArgMinTupleReducer +{ + static const bool PacketAccess = false; + static const bool IsStateful = false; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const T& t, T* accum) const { + if (t.second < accum->second) { *accum = t; } + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T initialize() const { + return T(0, NumTraits::highest()); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T finalize(const T& accum) const { + return accum; + } +}; + +template +struct reducer_traits, Device> { + enum { + Cost = NumTraits::AddCost, + PacketAccess = false + }; +}; + + +template +class GaussianGenerator { + public: + static const bool PacketAccess = false; + + EIGEN_DEVICE_FUNC GaussianGenerator(const array& means, + const array& std_devs) + : m_means(means) + { + for (size_t i = 0; i < NumDims; ++i) { + m_two_sigmas[i] = std_devs[i] * std_devs[i] * 2; + } + } + + EIGEN_DEVICE_FUNC T operator()(const array& coordinates) const { + T tmp = T(0); + for (size_t i = 0; i < NumDims; ++i) { + T offset = coordinates[i] - m_means[i]; + tmp += offset * offset / m_two_sigmas[i]; + } + return numext::exp(-tmp); + } + + private: + array m_means; + array m_two_sigmas; +}; + +template +struct functor_traits > { + enum { + Cost = NumDims * (2 * NumTraits::AddCost + NumTraits::MulCost + + functor_traits >::Cost) + + functor_traits >::Cost, + PacketAccess = GaussianGenerator::PacketAccess + }; +}; + +} // end namespace internal +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_FUNCTORS_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h new file mode 100644 index 0000000000..eb1d4934ed --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h @@ -0,0 +1,185 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_GENERATOR_H +#define EIGEN_CXX11_TENSOR_TENSOR_GENERATOR_H + +namespace Eigen { + +/** \class TensorGenerator + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor generator class. + * + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorGeneratorOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorGeneratorOp type; +}; + +} // end namespace internal + + + +template +class TensorGeneratorOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorGeneratorOp(const XprType& expr, const Generator& generator) + : m_xpr(expr), m_generator(generator) {} + + EIGEN_DEVICE_FUNC + const Generator& generator() const { return m_generator; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const Generator m_generator; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorGeneratorOp XprType; + typedef typename XprType::Index Index; + typedef typename TensorEvaluator::Dimensions Dimensions; + static const int NumDims = internal::array_size::value; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + enum { + IsAligned = false, + PacketAccess = (internal::unpacket_traits::size > 1), + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_generator(op.generator()) + { + TensorEvaluator impl(op.expression(), device); + m_dimensions = impl.dimensions(); + + if (static_cast(Layout) == static_cast(ColMajor)) { + m_strides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_strides[i] = m_strides[i - 1] * m_dimensions[i - 1]; + } + } else { + m_strides[NumDims - 1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_strides[i] = m_strides[i + 1] * m_dimensions[i + 1]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + array coords; + extract_coordinates(index, coords); + return m_generator(coords); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + const int packetSize = internal::unpacket_traits::size; + EIGEN_STATIC_ASSERT((packetSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+packetSize-1 < dimensions().TotalSize()); + + EIGEN_ALIGN_MAX typename internal::remove_const::type values[packetSize]; + for (int i = 0; i < packetSize; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool) const { + // TODO(rmlarsen): This is just a placeholder. Define interface to make + // generators return their cost. + return TensorOpCost(0, 0, TensorOpCost::AddCost() + + TensorOpCost::MulCost()); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void extract_coordinates(Index index, array& coords) const { + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_strides[i]; + index -= idx * m_strides[i]; + coords[i] = idx; + } + coords[0] = index; + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_strides[i]; + index -= idx * m_strides[i]; + coords[i] = idx; + } + coords[NumDims-1] = index; + } + } + + Dimensions m_dimensions; + array m_strides; + Generator m_generator; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_GENERATOR_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGlobalFunctions.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGlobalFunctions.h new file mode 100644 index 0000000000..665b861cfd --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGlobalFunctions.h @@ -0,0 +1,33 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Eugene Brevdo +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H +#define EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H + +namespace Eigen { + +/** \cpp11 \returns an expression of the coefficient-wise betainc(\a x, \a a, \a b) to the given tensors. + * + * This function computes the regularized incomplete beta function (integral). + * + */ +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const + TensorCwiseTernaryOp, + const ADerived, const BDerived, const XDerived> + betainc(const ADerived& a, const BDerived& b, const XDerived& x) { + return TensorCwiseTernaryOp< + internal::scalar_betainc_op, const ADerived, + const BDerived, const XDerived>( + a, b, x, internal::scalar_betainc_op()); +} + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIO.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIO.h new file mode 100644 index 0000000000..a901c5dd45 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIO.h @@ -0,0 +1,79 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_IO_H +#define EIGEN_CXX11_TENSOR_TENSOR_IO_H + +namespace Eigen { + +namespace internal { + +// Print the tensor as a 2d matrix +template +struct TensorPrinter { + static void run (std::ostream& os, const Tensor& tensor) { + typedef typename internal::remove_const::type Scalar; + typedef typename Tensor::Index Index; + const Index total_size = internal::array_prod(tensor.dimensions()); + if (total_size > 0) { + const Index first_dim = Eigen::internal::array_get<0>(tensor.dimensions()); + static const int layout = Tensor::Layout; + Map > matrix(const_cast(tensor.data()), first_dim, total_size/first_dim); + os << matrix; + } + } +}; + + +// Print the tensor as a vector +template +struct TensorPrinter { + static void run (std::ostream& os, const Tensor& tensor) { + typedef typename internal::remove_const::type Scalar; + typedef typename Tensor::Index Index; + const Index total_size = internal::array_prod(tensor.dimensions()); + if (total_size > 0) { + Map > array(const_cast(tensor.data()), total_size); + os << array; + } + } +}; + + +// Print the tensor as a scalar +template +struct TensorPrinter { + static void run (std::ostream& os, const Tensor& tensor) { + os << tensor.coeff(0); + } +}; +} + +template +std::ostream& operator << (std::ostream& os, const TensorBase& expr) { + typedef TensorEvaluator, DefaultDevice> Evaluator; + typedef typename Evaluator::Dimensions Dimensions; + + // Evaluate the expression if needed + TensorForcedEvalOp eval = expr.eval(); + Evaluator tensor(eval, DefaultDevice()); + tensor.evalSubExprsIfNeeded(NULL); + + // Print the result + static const int rank = internal::array_size::value; + internal::TensorPrinter::run(os, tensor); + + // Cleanup. + tensor.cleanup(); + return os; +} + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_IO_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorImagePatch.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorImagePatch.h new file mode 100644 index 0000000000..566856ed20 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorImagePatch.h @@ -0,0 +1,509 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_IMAGE_PATCH_H +#define EIGEN_CXX11_TENSOR_TENSOR_IMAGE_PATCH_H + +namespace Eigen { + +/** \class TensorImagePatch + * \ingroup CXX11_Tensor_Module + * + * \brief Patch extraction specialized for image processing. + * This assumes that the input has a least 3 dimensions ordered as follow: + * 1st dimension: channels (of size d) + * 2nd dimension: rows (of size r) + * 3rd dimension: columns (of size c) + * There can be additional dimensions such as time (for video) or batch (for + * bulk processing after the first 3. + * Calling the image patch code with patch_rows and patch_cols is equivalent + * to calling the regular patch extraction code with parameters d, patch_rows, + * patch_cols, and 1 for all the additional dimensions. + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename internal::remove_const::type Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions + 1; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorImagePatchOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorImagePatchOp type; +}; + +} // end namespace internal + +template +class TensorImagePatchOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorImagePatchOp(const XprType& expr, DenseIndex patch_rows, DenseIndex patch_cols, + DenseIndex row_strides, DenseIndex col_strides, + DenseIndex in_row_strides, DenseIndex in_col_strides, + DenseIndex row_inflate_strides, DenseIndex col_inflate_strides, + PaddingType padding_type, Scalar padding_value) + : m_xpr(expr), m_patch_rows(patch_rows), m_patch_cols(patch_cols), + m_row_strides(row_strides), m_col_strides(col_strides), + m_in_row_strides(in_row_strides), m_in_col_strides(in_col_strides), + m_row_inflate_strides(row_inflate_strides), m_col_inflate_strides(col_inflate_strides), + m_padding_explicit(false), m_padding_top(0), m_padding_bottom(0), m_padding_left(0), m_padding_right(0), + m_padding_type(padding_type), m_padding_value(padding_value) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorImagePatchOp(const XprType& expr, DenseIndex patch_rows, DenseIndex patch_cols, + DenseIndex row_strides, DenseIndex col_strides, + DenseIndex in_row_strides, DenseIndex in_col_strides, + DenseIndex row_inflate_strides, DenseIndex col_inflate_strides, + DenseIndex padding_top, DenseIndex padding_bottom, + DenseIndex padding_left, DenseIndex padding_right, + Scalar padding_value) + : m_xpr(expr), m_patch_rows(patch_rows), m_patch_cols(patch_cols), + m_row_strides(row_strides), m_col_strides(col_strides), + m_in_row_strides(in_row_strides), m_in_col_strides(in_col_strides), + m_row_inflate_strides(row_inflate_strides), m_col_inflate_strides(col_inflate_strides), + m_padding_explicit(true), m_padding_top(padding_top), m_padding_bottom(padding_bottom), + m_padding_left(padding_left), m_padding_right(padding_right), + m_padding_type(PADDING_VALID), m_padding_value(padding_value) {} + + EIGEN_DEVICE_FUNC + DenseIndex patch_rows() const { return m_patch_rows; } + EIGEN_DEVICE_FUNC + DenseIndex patch_cols() const { return m_patch_cols; } + EIGEN_DEVICE_FUNC + DenseIndex row_strides() const { return m_row_strides; } + EIGEN_DEVICE_FUNC + DenseIndex col_strides() const { return m_col_strides; } + EIGEN_DEVICE_FUNC + DenseIndex in_row_strides() const { return m_in_row_strides; } + EIGEN_DEVICE_FUNC + DenseIndex in_col_strides() const { return m_in_col_strides; } + EIGEN_DEVICE_FUNC + DenseIndex row_inflate_strides() const { return m_row_inflate_strides; } + EIGEN_DEVICE_FUNC + DenseIndex col_inflate_strides() const { return m_col_inflate_strides; } + EIGEN_DEVICE_FUNC + bool padding_explicit() const { return m_padding_explicit; } + EIGEN_DEVICE_FUNC + DenseIndex padding_top() const { return m_padding_top; } + EIGEN_DEVICE_FUNC + DenseIndex padding_bottom() const { return m_padding_bottom; } + EIGEN_DEVICE_FUNC + DenseIndex padding_left() const { return m_padding_left; } + EIGEN_DEVICE_FUNC + DenseIndex padding_right() const { return m_padding_right; } + EIGEN_DEVICE_FUNC + PaddingType padding_type() const { return m_padding_type; } + EIGEN_DEVICE_FUNC + Scalar padding_value() const { return m_padding_value; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const DenseIndex m_patch_rows; + const DenseIndex m_patch_cols; + const DenseIndex m_row_strides; + const DenseIndex m_col_strides; + const DenseIndex m_in_row_strides; + const DenseIndex m_in_col_strides; + const DenseIndex m_row_inflate_strides; + const DenseIndex m_col_inflate_strides; + const bool m_padding_explicit; + const DenseIndex m_padding_top; + const DenseIndex m_padding_bottom; + const DenseIndex m_padding_left; + const DenseIndex m_padding_right; + const PaddingType m_padding_type; + const Scalar m_padding_value; +}; + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorImagePatchOp XprType; + typedef typename XprType::Index Index; + static const int NumInputDims = internal::array_size::Dimensions>::value; + static const int NumDims = NumInputDims + 1; + typedef DSizes Dimensions; + typedef typename internal::remove_const::type Scalar; + typedef TensorEvaluator, + Device> Self; + typedef TensorEvaluator Impl; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device) + { + EIGEN_STATIC_ASSERT((NumDims >= 4), YOU_MADE_A_PROGRAMMING_MISTAKE); + + m_paddingValue = op.padding_value(); + + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + + // Caches a few variables. + if (static_cast(Layout) == static_cast(ColMajor)) { + m_inputDepth = input_dims[0]; + m_inputRows = input_dims[1]; + m_inputCols = input_dims[2]; + } else { + m_inputDepth = input_dims[NumInputDims-1]; + m_inputRows = input_dims[NumInputDims-2]; + m_inputCols = input_dims[NumInputDims-3]; + } + + m_row_strides = op.row_strides(); + m_col_strides = op.col_strides(); + + // Input strides and effective input/patch size + m_in_row_strides = op.in_row_strides(); + m_in_col_strides = op.in_col_strides(); + m_row_inflate_strides = op.row_inflate_strides(); + m_col_inflate_strides = op.col_inflate_strides(); + // The "effective" input rows and input cols are the input rows and cols + // after inflating them with zeros. + // For examples, a 2x3 matrix with row_inflate_strides and + // col_inflate_strides of 2 comes from: + // A B C + // D E F + // + // to a matrix is 3 x 5: + // + // A . B . C + // . . . . . + // D . E . F + + m_input_rows_eff = (m_inputRows - 1) * m_row_inflate_strides + 1; + m_input_cols_eff = (m_inputCols - 1) * m_col_inflate_strides + 1; + m_patch_rows_eff = op.patch_rows() + (op.patch_rows() - 1) * (m_in_row_strides - 1); + m_patch_cols_eff = op.patch_cols() + (op.patch_cols() - 1) * (m_in_col_strides - 1); + + if (op.padding_explicit()) { + m_outputRows = numext::ceil((m_input_rows_eff + op.padding_top() + op.padding_bottom() - m_patch_rows_eff + 1.f) / static_cast(m_row_strides)); + m_outputCols = numext::ceil((m_input_cols_eff + op.padding_left() + op.padding_right() - m_patch_cols_eff + 1.f) / static_cast(m_col_strides)); + m_rowPaddingTop = op.padding_top(); + m_colPaddingLeft = op.padding_left(); + } else { + // Computing padding from the type + switch (op.padding_type()) { + case PADDING_VALID: + m_outputRows = numext::ceil((m_input_rows_eff - m_patch_rows_eff + 1.f) / static_cast(m_row_strides)); + m_outputCols = numext::ceil((m_input_cols_eff - m_patch_cols_eff + 1.f) / static_cast(m_col_strides)); + // Calculate the padding + m_rowPaddingTop = numext::maxi(0, ((m_outputRows - 1) * m_row_strides + m_patch_rows_eff - m_input_rows_eff) / 2); + m_colPaddingLeft = numext::maxi(0, ((m_outputCols - 1) * m_col_strides + m_patch_cols_eff - m_input_cols_eff) / 2); + break; + case PADDING_SAME: + m_outputRows = numext::ceil(m_input_rows_eff / static_cast(m_row_strides)); + m_outputCols = numext::ceil(m_input_cols_eff / static_cast(m_col_strides)); + // Calculate the padding + m_rowPaddingTop = ((m_outputRows - 1) * m_row_strides + m_patch_rows_eff - m_input_rows_eff) / 2; + m_colPaddingLeft = ((m_outputCols - 1) * m_col_strides + m_patch_cols_eff - m_input_cols_eff) / 2; + break; + default: + eigen_assert(false && "unexpected padding"); + } + } + eigen_assert(m_outputRows > 0); + eigen_assert(m_outputCols > 0); + + // Dimensions for result of extraction. + if (static_cast(Layout) == static_cast(ColMajor)) { + // ColMajor + // 0: depth + // 1: patch_rows + // 2: patch_cols + // 3: number of patches + // 4 and beyond: anything else (such as batch). + m_dimensions[0] = input_dims[0]; + m_dimensions[1] = op.patch_rows(); + m_dimensions[2] = op.patch_cols(); + m_dimensions[3] = m_outputRows * m_outputCols; + for (int i = 4; i < NumDims; ++i) { + m_dimensions[i] = input_dims[i-1]; + } + } else { + // RowMajor + // NumDims-1: depth + // NumDims-2: patch_rows + // NumDims-3: patch_cols + // NumDims-4: number of patches + // NumDims-5 and beyond: anything else (such as batch). + m_dimensions[NumDims-1] = input_dims[NumInputDims-1]; + m_dimensions[NumDims-2] = op.patch_rows(); + m_dimensions[NumDims-3] = op.patch_cols(); + m_dimensions[NumDims-4] = m_outputRows * m_outputCols; + for (int i = NumDims-5; i >= 0; --i) { + m_dimensions[i] = input_dims[i]; + } + } + + // Strides for moving the patch in various dimensions. + if (static_cast(Layout) == static_cast(ColMajor)) { + m_colStride = m_dimensions[1]; + m_patchStride = m_colStride * m_dimensions[2] * m_dimensions[0]; + m_otherStride = m_patchStride * m_dimensions[3]; + } else { + m_colStride = m_dimensions[NumDims-2]; + m_patchStride = m_colStride * m_dimensions[NumDims-3] * m_dimensions[NumDims-1]; + m_otherStride = m_patchStride * m_dimensions[NumDims-4]; + } + + // Strides for navigating through the input tensor. + m_rowInputStride = m_inputDepth; + m_colInputStride = m_inputDepth * m_inputRows; + m_patchInputStride = m_inputDepth * m_inputRows * m_inputCols; + + // Fast representations of different variables. + m_fastOtherStride = internal::TensorIntDivisor(m_otherStride); + m_fastPatchStride = internal::TensorIntDivisor(m_patchStride); + m_fastColStride = internal::TensorIntDivisor(m_colStride); + m_fastInflateRowStride = internal::TensorIntDivisor(m_row_inflate_strides); + m_fastInflateColStride = internal::TensorIntDivisor(m_col_inflate_strides); + m_fastInputColsEff = internal::TensorIntDivisor(m_input_cols_eff); + + // Number of patches in the width dimension. + m_fastOutputRows = internal::TensorIntDivisor(m_outputRows); + if (static_cast(Layout) == static_cast(ColMajor)) { + m_fastOutputDepth = internal::TensorIntDivisor(m_dimensions[0]); + } else { + m_fastOutputDepth = internal::TensorIntDivisor(m_dimensions[NumDims-1]); + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + // Patch index corresponding to the passed in index. + const Index patchIndex = index / m_fastPatchStride; + // Find the offset of the element wrt the location of the first element. + const Index patchOffset = (index - patchIndex * m_patchStride) / m_fastOutputDepth; + + // Other ways to index this element. + const Index otherIndex = (NumDims == 4) ? 0 : index / m_fastOtherStride; + const Index patch2DIndex = (NumDims == 4) ? patchIndex : (index - otherIndex * m_otherStride) / m_fastPatchStride; + + // Calculate col index in the input original tensor. + const Index colIndex = patch2DIndex / m_fastOutputRows; + const Index colOffset = patchOffset / m_fastColStride; + const Index inputCol = colIndex * m_col_strides + colOffset * m_in_col_strides - m_colPaddingLeft; + const Index origInputCol = (m_col_inflate_strides == 1) ? inputCol : ((inputCol >= 0) ? (inputCol / m_fastInflateColStride) : 0); + if (inputCol < 0 || inputCol >= m_input_cols_eff || + ((m_col_inflate_strides != 1) && (inputCol != origInputCol * m_col_inflate_strides))) { + return Scalar(m_paddingValue); + } + + // Calculate row index in the original input tensor. + const Index rowIndex = patch2DIndex - colIndex * m_outputRows; + const Index rowOffset = patchOffset - colOffset * m_colStride; + const Index inputRow = rowIndex * m_row_strides + rowOffset * m_in_row_strides - m_rowPaddingTop; + const Index origInputRow = (m_row_inflate_strides == 1) ? inputRow : ((inputRow >= 0) ? (inputRow / m_fastInflateRowStride) : 0); + if (inputRow < 0 || inputRow >= m_input_rows_eff || + ((m_row_inflate_strides != 1) && (inputRow != origInputRow * m_row_inflate_strides))) { + return Scalar(m_paddingValue); + } + + const int depth_index = static_cast(Layout) == static_cast(ColMajor) ? 0 : NumDims - 1; + const Index depth = index - (index / m_fastOutputDepth) * m_dimensions[depth_index]; + + const Index inputIndex = depth + origInputRow * m_rowInputStride + origInputCol * m_colInputStride + otherIndex * m_patchInputStride; + return m_impl.coeff(inputIndex); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + if (m_in_row_strides != 1 || m_in_col_strides != 1 || m_row_inflate_strides != 1 || m_col_inflate_strides != 1) { + return packetWithPossibleZero(index); + } + + const Index indices[2] = {index, index + PacketSize - 1}; + const Index patchIndex = indices[0] / m_fastPatchStride; + if (patchIndex != indices[1] / m_fastPatchStride) { + return packetWithPossibleZero(index); + } + const Index otherIndex = (NumDims == 4) ? 0 : indices[0] / m_fastOtherStride; + eigen_assert(otherIndex == indices[1] / m_fastOtherStride); + + // Find the offset of the element wrt the location of the first element. + const Index patchOffsets[2] = {(indices[0] - patchIndex * m_patchStride) / m_fastOutputDepth, + (indices[1] - patchIndex * m_patchStride) / m_fastOutputDepth}; + + const Index patch2DIndex = (NumDims == 4) ? patchIndex : (indices[0] - otherIndex * m_otherStride) / m_fastPatchStride; + eigen_assert(patch2DIndex == (indices[1] - otherIndex * m_otherStride) / m_fastPatchStride); + + const Index colIndex = patch2DIndex / m_fastOutputRows; + const Index colOffsets[2] = {patchOffsets[0] / m_fastColStride, patchOffsets[1] / m_fastColStride}; + + // Calculate col indices in the original input tensor. + const Index inputCols[2] = {colIndex * m_col_strides + colOffsets[0] - + m_colPaddingLeft, colIndex * m_col_strides + colOffsets[1] - m_colPaddingLeft}; + if (inputCols[1] < 0 || inputCols[0] >= m_inputCols) { + return internal::pset1(Scalar(m_paddingValue)); + } + + if (inputCols[0] == inputCols[1]) { + const Index rowIndex = patch2DIndex - colIndex * m_outputRows; + const Index rowOffsets[2] = {patchOffsets[0] - colOffsets[0]*m_colStride, patchOffsets[1] - colOffsets[1]*m_colStride}; + eigen_assert(rowOffsets[0] <= rowOffsets[1]); + // Calculate col indices in the original input tensor. + const Index inputRows[2] = {rowIndex * m_row_strides + rowOffsets[0] - + m_rowPaddingTop, rowIndex * m_row_strides + rowOffsets[1] - m_rowPaddingTop}; + + if (inputRows[1] < 0 || inputRows[0] >= m_inputRows) { + return internal::pset1(Scalar(m_paddingValue)); + } + + if (inputRows[0] >= 0 && inputRows[1] < m_inputRows) { + // no padding + const int depth_index = static_cast(Layout) == static_cast(ColMajor) ? 0 : NumDims - 1; + const Index depth = index - (index / m_fastOutputDepth) * m_dimensions[depth_index]; + const Index inputIndex = depth + inputRows[0] * m_rowInputStride + inputCols[0] * m_colInputStride + otherIndex * m_patchInputStride; + return m_impl.template packet(inputIndex); + } + } + + return packetWithPossibleZero(index); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + const TensorEvaluator& impl() const { return m_impl; } + + Index rowPaddingTop() const { return m_rowPaddingTop; } + Index colPaddingLeft() const { return m_colPaddingLeft; } + Index outputRows() const { return m_outputRows; } + Index outputCols() const { return m_outputCols; } + Index userRowStride() const { return m_row_strides; } + Index userColStride() const { return m_col_strides; } + Index userInRowStride() const { return m_in_row_strides; } + Index userInColStride() const { return m_in_col_strides; } + Index rowInflateStride() const { return m_row_inflate_strides; } + Index colInflateStride() const { return m_col_inflate_strides; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost + costPerCoeff(bool vectorized) const { + // We conservatively estimate the cost for the code path where the computed + // index is inside the original image and + // TensorEvaluator::CoordAccess is false. + const double compute_cost = 3 * TensorOpCost::DivCost() + + 6 * TensorOpCost::MulCost() + + 8 * TensorOpCost::MulCost(); + return m_impl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, compute_cost, vectorized, PacketSize); + } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packetWithPossibleZero(Index index) const + { + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + for (int i = 0; i < PacketSize; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + + Dimensions m_dimensions; + + Index m_otherStride; + Index m_patchStride; + Index m_colStride; + Index m_row_strides; + Index m_col_strides; + + Index m_in_row_strides; + Index m_in_col_strides; + Index m_row_inflate_strides; + Index m_col_inflate_strides; + + Index m_input_rows_eff; + Index m_input_cols_eff; + Index m_patch_rows_eff; + Index m_patch_cols_eff; + + internal::TensorIntDivisor m_fastOtherStride; + internal::TensorIntDivisor m_fastPatchStride; + internal::TensorIntDivisor m_fastColStride; + internal::TensorIntDivisor m_fastInflateRowStride; + internal::TensorIntDivisor m_fastInflateColStride; + internal::TensorIntDivisor m_fastInputColsEff; + + Index m_rowInputStride; + Index m_colInputStride; + Index m_patchInputStride; + + Index m_inputDepth; + Index m_inputRows; + Index m_inputCols; + + Index m_outputRows; + Index m_outputCols; + + Index m_rowPaddingTop; + Index m_colPaddingLeft; + + internal::TensorIntDivisor m_fastOutputRows; + internal::TensorIntDivisor m_fastOutputDepth; + + Scalar m_paddingValue; + + TensorEvaluator m_impl; +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_IMAGE_PATCH_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIndexList.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIndexList.h new file mode 100644 index 0000000000..3209fecd34 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIndexList.h @@ -0,0 +1,725 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_INDEX_LIST_H +#define EIGEN_CXX11_TENSOR_TENSOR_INDEX_LIST_H + + +#if EIGEN_HAS_CONSTEXPR && EIGEN_HAS_VARIADIC_TEMPLATES + +#define EIGEN_HAS_INDEX_LIST + +namespace Eigen { + +/** \internal + * + * \class TensorIndexList + * \ingroup CXX11_Tensor_Module + * + * \brief Set of classes used to encode a set of Tensor dimensions/indices. + * + * The indices in the list can be known at compile time or at runtime. A mix + * of static and dynamic indices can also be provided if needed. The tensor + * code will attempt to take advantage of the indices that are known at + * compile time to optimize the code it generates. + * + * This functionality requires a c++11 compliant compiler. If your compiler + * is older you need to use arrays of indices instead. + * + * Several examples are provided in the cxx11_tensor_index_list.cpp file. + * + * \sa Tensor + */ + +template +struct type2index { + static const DenseIndex value = n; + EIGEN_DEVICE_FUNC constexpr operator DenseIndex() const { return n; } + EIGEN_DEVICE_FUNC void set(DenseIndex val) { + eigen_assert(val == n); + } +}; + +// This can be used with IndexPairList to get compile-time constant pairs, +// such as IndexPairList, type2indexpair<3,4>>(). +template +struct type2indexpair { + static const DenseIndex first = f; + static const DenseIndex second = s; + + constexpr EIGEN_DEVICE_FUNC operator IndexPair() const { + return IndexPair(f, s); + } + + EIGEN_DEVICE_FUNC void set(const IndexPair& val) { + eigen_assert(val.first == f); + eigen_assert(val.second == s); + } +}; + + +template struct NumTraits > +{ + typedef DenseIndex Real; + enum { + IsComplex = 0, + RequireInitialization = false, + ReadCost = 1, + AddCost = 1, + MulCost = 1 + }; + + EIGEN_DEVICE_FUNC static inline Real epsilon() { return 0; } + EIGEN_DEVICE_FUNC static inline Real dummy_precision() { return 0; } + EIGEN_DEVICE_FUNC static inline Real highest() { return n; } + EIGEN_DEVICE_FUNC static inline Real lowest() { return n; } +}; + +namespace internal { +template +EIGEN_DEVICE_FUNC void update_value(T& val, DenseIndex new_val) { + val = new_val; +} +template +EIGEN_DEVICE_FUNC void update_value(type2index& val, DenseIndex new_val) { + val.set(new_val); +} + +template +EIGEN_DEVICE_FUNC void update_value(T& val, IndexPair new_val) { + val = new_val; +} +template +EIGEN_DEVICE_FUNC void update_value(type2indexpair& val, IndexPair new_val) { + val.set(new_val); +} + + +template +struct is_compile_time_constant { + static constexpr bool value = false; +}; + +template +struct is_compile_time_constant > { + static constexpr bool value = true; +}; +template +struct is_compile_time_constant > { + static constexpr bool value = true; +}; +template +struct is_compile_time_constant& > { + static constexpr bool value = true; +}; +template +struct is_compile_time_constant& > { + static constexpr bool value = true; +}; + +template +struct is_compile_time_constant > { + static constexpr bool value = true; +}; +template +struct is_compile_time_constant > { + static constexpr bool value = true; +}; +template +struct is_compile_time_constant& > { + static constexpr bool value = true; +}; +template +struct is_compile_time_constant& > { + static constexpr bool value = true; +}; + + +template +struct IndexTuple; + +template +struct IndexTuple { + EIGEN_DEVICE_FUNC constexpr IndexTuple() : head(), others() { } + EIGEN_DEVICE_FUNC constexpr IndexTuple(const T& v, const O... o) : head(v), others(o...) { } + + constexpr static int count = 1 + sizeof...(O); + T head; + IndexTuple others; + typedef T Head; + typedef IndexTuple Other; +}; + +template + struct IndexTuple { + EIGEN_DEVICE_FUNC constexpr IndexTuple() : head() { } + EIGEN_DEVICE_FUNC constexpr IndexTuple(const T& v) : head(v) { } + + constexpr static int count = 1; + T head; + typedef T Head; +}; + + +template +struct IndexTupleExtractor; + +template +struct IndexTupleExtractor { + + typedef typename IndexTupleExtractor::ValType ValType; + + EIGEN_DEVICE_FUNC static constexpr ValType& get_val(IndexTuple& val) { + return IndexTupleExtractor::get_val(val.others); + } + + EIGEN_DEVICE_FUNC static constexpr const ValType& get_val(const IndexTuple& val) { + return IndexTupleExtractor::get_val(val.others); + } + template + EIGEN_DEVICE_FUNC static void set_val(IndexTuple& val, V& new_val) { + IndexTupleExtractor::set_val(val.others, new_val); + } + +}; + +template + struct IndexTupleExtractor<0, T, O...> { + + typedef T ValType; + + EIGEN_DEVICE_FUNC static constexpr ValType& get_val(IndexTuple& val) { + return val.head; + } + EIGEN_DEVICE_FUNC static constexpr const ValType& get_val(const IndexTuple& val) { + return val.head; + } + template + EIGEN_DEVICE_FUNC static void set_val(IndexTuple& val, V& new_val) { + val.head = new_val; + } +}; + + + +template +EIGEN_DEVICE_FUNC constexpr typename IndexTupleExtractor::ValType& array_get(IndexTuple& tuple) { + return IndexTupleExtractor::get_val(tuple); +} +template +EIGEN_DEVICE_FUNC constexpr const typename IndexTupleExtractor::ValType& array_get(const IndexTuple& tuple) { + return IndexTupleExtractor::get_val(tuple); +} +template + struct array_size > { + static const size_t value = IndexTuple::count; +}; +template + struct array_size > { + static const size_t value = IndexTuple::count; +}; + + + + +template +struct tuple_coeff { + template + EIGEN_DEVICE_FUNC static constexpr ValueT get(const DenseIndex i, const IndexTuple& t) { + // return array_get(t) * (i == Idx) + tuple_coeff::get(i, t) * (i != Idx); + return (i == Idx ? array_get(t) : tuple_coeff::get(i, t)); + } + template + EIGEN_DEVICE_FUNC static void set(const DenseIndex i, IndexTuple& t, const ValueT& value) { + if (i == Idx) { + update_value(array_get(t), value); + } else { + tuple_coeff::set(i, t, value); + } + } + + template + EIGEN_DEVICE_FUNC static constexpr bool value_known_statically(const DenseIndex i, const IndexTuple& t) { + return ((i == Idx) & is_compile_time_constant::ValType>::value) || + tuple_coeff::value_known_statically(i, t); + } + + template + EIGEN_DEVICE_FUNC static constexpr bool values_up_to_known_statically(const IndexTuple& t) { + return is_compile_time_constant::ValType>::value && + tuple_coeff::values_up_to_known_statically(t); + } + + template + EIGEN_DEVICE_FUNC static constexpr bool values_up_to_statically_known_to_increase(const IndexTuple& t) { + return is_compile_time_constant::ValType>::value && + is_compile_time_constant::ValType>::value && + array_get(t) > array_get(t) && + tuple_coeff::values_up_to_statically_known_to_increase(t); + } +}; + +template +struct tuple_coeff<0, ValueT> { + template + EIGEN_DEVICE_FUNC static constexpr ValueT get(const DenseIndex /*i*/, const IndexTuple& t) { + // eigen_assert (i == 0); // gcc fails to compile assertions in constexpr + return array_get<0>(t)/* * (i == 0)*/; + } + template + EIGEN_DEVICE_FUNC static void set(const DenseIndex i, IndexTuple& t, const ValueT value) { + eigen_assert (i == 0); + update_value(array_get<0>(t), value); + } + template + EIGEN_DEVICE_FUNC static constexpr bool value_known_statically(const DenseIndex i, const IndexTuple&) { + return is_compile_time_constant::ValType>::value & (i == 0); + } + + template + EIGEN_DEVICE_FUNC static constexpr bool values_up_to_known_statically(const IndexTuple&) { + return is_compile_time_constant::ValType>::value; + } + + template + EIGEN_DEVICE_FUNC static constexpr bool values_up_to_statically_known_to_increase(const IndexTuple&) { + return true; + } +}; +} // namespace internal + + + +template +struct IndexList : internal::IndexTuple { + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC constexpr DenseIndex operator[] (const DenseIndex i) const { + return internal::tuple_coeff >::value-1, DenseIndex>::get(i, *this); + } + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC constexpr DenseIndex get(const DenseIndex i) const { + return internal::tuple_coeff >::value-1, DenseIndex>::get(i, *this); + } + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC void set(const DenseIndex i, const DenseIndex value) { + return internal::tuple_coeff >::value-1, DenseIndex>::set(i, *this, value); + } + + EIGEN_DEVICE_FUNC constexpr IndexList(const internal::IndexTuple& other) : internal::IndexTuple(other) { } + EIGEN_DEVICE_FUNC constexpr IndexList(FirstType& first, OtherTypes... other) : internal::IndexTuple(first, other...) { } + EIGEN_DEVICE_FUNC constexpr IndexList() : internal::IndexTuple() { } + + EIGEN_DEVICE_FUNC constexpr bool value_known_statically(const DenseIndex i) const { + return internal::tuple_coeff >::value-1, DenseIndex>::value_known_statically(i, *this); + } + EIGEN_DEVICE_FUNC constexpr bool all_values_known_statically() const { + return internal::tuple_coeff >::value-1, DenseIndex>::values_up_to_known_statically(*this); + } + + EIGEN_DEVICE_FUNC constexpr bool values_statically_known_to_increase() const { + return internal::tuple_coeff >::value-1, DenseIndex>::values_up_to_statically_known_to_increase(*this); + } +}; + + +template +constexpr IndexList make_index_list(FirstType val1, OtherTypes... other_vals) { + return IndexList(val1, other_vals...); +} + + +template +struct IndexPairList : internal::IndexTuple { + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC constexpr IndexPair operator[] (const DenseIndex i) const { + return internal::tuple_coeff >::value-1, IndexPair>::get(i, *this); + } + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC void set(const DenseIndex i, const IndexPair value) { + return internal::tuple_coeff>::value-1, IndexPair >::set(i, *this, value); + } + + EIGEN_DEVICE_FUNC constexpr IndexPairList(const internal::IndexTuple& other) : internal::IndexTuple(other) { } + EIGEN_DEVICE_FUNC constexpr IndexPairList() : internal::IndexTuple() { } + + EIGEN_DEVICE_FUNC constexpr bool value_known_statically(const DenseIndex i) const { + return internal::tuple_coeff >::value-1, DenseIndex>::value_known_statically(i, *this); + } +}; + +namespace internal { + +template size_t array_prod(const IndexList& sizes) { + size_t result = 1; + for (int i = 0; i < array_size >::value; ++i) { + result *= sizes[i]; + } + return result; +} + +template struct array_size > { + static const size_t value = array_size >::value; +}; +template struct array_size > { + static const size_t value = array_size >::value; +}; + +template struct array_size > { + static const size_t value = std::tuple_size >::value; +}; +template struct array_size > { + static const size_t value = std::tuple_size >::value; +}; + +template EIGEN_DEVICE_FUNC constexpr DenseIndex array_get(IndexList& a) { + return IndexTupleExtractor::get_val(a); +} +template EIGEN_DEVICE_FUNC constexpr DenseIndex array_get(const IndexList& a) { + return IndexTupleExtractor::get_val(a); +} + +template +struct index_known_statically_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex) { + return false; + } +}; + +template +struct index_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i) { + return IndexList().value_known_statically(i); + } +}; + +template +struct index_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i) { + return IndexList().value_known_statically(i); + } +}; + + +template +struct all_indices_known_statically_impl { + static constexpr bool run() { + return false; + } +}; + +template +struct all_indices_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return IndexList().all_values_known_statically(); + } +}; + +template +struct all_indices_known_statically_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return IndexList().all_values_known_statically(); + } +}; + + +template +struct indices_statically_known_to_increase_impl { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return false; + } +}; + +template + struct indices_statically_known_to_increase_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return Eigen::IndexList().values_statically_known_to_increase(); + } +}; + +template + struct indices_statically_known_to_increase_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run() { + return Eigen::IndexList().values_statically_known_to_increase(); + } +}; + + +template +struct index_statically_eq_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) == value); + } +}; + +template +struct index_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) == value); + } +}; + + +template +struct index_statically_ne_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_ne_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) != value); + } +}; + +template +struct index_statically_ne_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) != value); + } +}; + + +template +struct index_statically_gt_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_gt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) > value); + } +}; + +template +struct index_statically_gt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) > value); + } +}; + + + +template +struct index_statically_lt_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_lt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) < value); + } +}; + +template +struct index_statically_lt_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexList().value_known_statically(i) & + (IndexList().get(i) < value); + } +}; + + + +template +struct index_pair_first_statically_eq_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_pair_first_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexPairList().value_known_statically(i) & + (IndexPairList().operator[](i).first == value); + } +}; + +template +struct index_pair_first_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexPairList().value_known_statically(i) & + (IndexPairList().operator[](i).first == value); + } +}; + + + +template +struct index_pair_second_statically_eq_impl { + EIGEN_DEVICE_FUNC static constexpr bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_pair_second_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexPairList().value_known_statically(i) & + (IndexPairList().operator[](i).second == value); + } +}; + +template +struct index_pair_second_statically_eq_impl > { + EIGEN_DEVICE_FUNC static constexpr bool run(const DenseIndex i, const DenseIndex value) { + return IndexPairList().value_known_statically(i) & + (IndexPairList().operator[](i).second == value); + } +}; + + +} // end namespace internal +} // end namespace Eigen + +#else + +namespace Eigen { +namespace internal { + +template +struct index_known_statically_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(const DenseIndex) { + return false; + } +}; + +template +struct all_indices_known_statically_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run() { + return false; + } +}; + +template +struct indices_statically_known_to_increase_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run() { + return false; + } +}; + +template +struct index_statically_eq_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_ne_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_gt_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_statically_lt_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_pair_first_statically_eq_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(DenseIndex, DenseIndex) { + return false; + } +}; + +template +struct index_pair_second_statically_eq_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool run(DenseIndex, DenseIndex) { + return false; + } +}; + + + +} // end namespace internal +} // end namespace Eigen + +#endif + + +namespace Eigen { +namespace internal { +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_known_statically(DenseIndex i) { + return index_known_statically_impl::run(i); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool all_indices_known_statically() { + return all_indices_known_statically_impl::run(); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool indices_statically_known_to_increase() { + return indices_statically_known_to_increase_impl::run(); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_statically_eq(DenseIndex i, DenseIndex value) { + return index_statically_eq_impl::run(i, value); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_statically_ne(DenseIndex i, DenseIndex value) { + return index_statically_ne_impl::run(i, value); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_statically_gt(DenseIndex i, DenseIndex value) { + return index_statically_gt_impl::run(i, value); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_statically_lt(DenseIndex i, DenseIndex value) { + return index_statically_lt_impl::run(i, value); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_pair_first_statically_eq(DenseIndex i, DenseIndex value) { + return index_pair_first_statically_eq_impl::run(i, value); +} + +template +static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bool index_pair_second_statically_eq(DenseIndex i, DenseIndex value) { + return index_pair_second_statically_eq_impl::run(i, value); +} + +} // end namespace internal +} // end namespace Eigen + + +#endif // EIGEN_CXX11_TENSOR_TENSOR_INDEX_LIST_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInflation.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInflation.h new file mode 100644 index 0000000000..f391fb9ee5 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInflation.h @@ -0,0 +1,229 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Ke Yang +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_INFLATION_H +#define EIGEN_CXX11_TENSOR_TENSOR_INFLATION_H + +namespace Eigen { + +/** \class TensorInflation + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor inflation class. + * + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorInflationOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorInflationOp type; +}; + +} // end namespace internal + +template +class TensorInflationOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorInflationOp(const XprType& expr, const Strides& strides) + : m_xpr(expr), m_strides(strides) {} + + EIGEN_DEVICE_FUNC + const Strides& strides() const { return m_strides; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const Strides m_strides; +}; + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorInflationOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::Dimensions>::value; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = /*TensorEvaluator::IsAligned*/ false, + PacketAccess = TensorEvaluator::PacketAccess, + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_strides(op.strides()) + { + m_dimensions = m_impl.dimensions(); + // Expand each dimension to the inflated dimension. + for (int i = 0; i < NumDims; ++i) { + m_dimensions[i] = (m_dimensions[i] - 1) * op.strides()[i] + 1; + } + + // Remember the strides for fast division. + for (int i = 0; i < NumDims; ++i) { + m_fastStrides[i] = internal::TensorIntDivisor(m_strides[i]); + } + + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + if (static_cast(Layout) == static_cast(ColMajor)) { + m_outputStrides[0] = 1; + m_inputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_outputStrides[i] = m_outputStrides[i-1] * m_dimensions[i-1]; + m_inputStrides[i] = m_inputStrides[i-1] * input_dims[i-1]; + } + } else { // RowMajor + m_outputStrides[NumDims-1] = 1; + m_inputStrides[NumDims-1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_outputStrides[i] = m_outputStrides[i+1] * m_dimensions[i+1]; + m_inputStrides[i] = m_inputStrides[i+1] * input_dims[i+1]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + // Computes the input index given the output index. Returns true if the output + // index doesn't fall into a hole. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool getInputIndex(Index index, Index* inputIndex) const + { + eigen_assert(index < dimensions().TotalSize()); + *inputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_outputStrides[i]; + if (idx != idx / m_fastStrides[i] * m_strides[i]) { + return false; + } + *inputIndex += idx / m_strides[i] * m_inputStrides[i]; + index -= idx * m_outputStrides[i]; + } + if (index != index / m_fastStrides[0] * m_strides[0]) { + return false; + } + *inputIndex += index / m_strides[0]; + return true; + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_outputStrides[i]; + if (idx != idx / m_fastStrides[i] * m_strides[i]) { + return false; + } + *inputIndex += idx / m_strides[i] * m_inputStrides[i]; + index -= idx * m_outputStrides[i]; + } + if (index != index / m_fastStrides[NumDims-1] * m_strides[NumDims-1]) { + return false; + } + *inputIndex += index / m_strides[NumDims - 1]; + } + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + Index inputIndex = 0; + if (getInputIndex(index, &inputIndex)) { + return m_impl.coeff(inputIndex); + } else { + return Scalar(0); + } + } + + // TODO(yangke): optimize this function so that we can detect and produce + // all-zero packets + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + for (int i = 0; i < PacketSize; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + const double compute_cost = NumDims * (3 * TensorOpCost::DivCost() + + 3 * TensorOpCost::MulCost() + + 2 * TensorOpCost::AddCost()); + const double input_size = m_impl.dimensions().TotalSize(); + const double output_size = m_dimensions.TotalSize(); + if (output_size == 0) + return TensorOpCost(); + return m_impl.costPerCoeff(vectorized) + + TensorOpCost(sizeof(CoeffReturnType) * input_size / output_size, 0, + compute_cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + protected: + Dimensions m_dimensions; + array m_outputStrides; + array m_inputStrides; + TensorEvaluator m_impl; + const Strides m_strides; + array, NumDims> m_fastStrides; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_INFLATION_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInitializer.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInitializer.h new file mode 100644 index 0000000000..33edc49e39 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInitializer.h @@ -0,0 +1,82 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_INITIALIZER_H +#define EIGEN_CXX11_TENSOR_TENSOR_INITIALIZER_H + +#if EIGEN_HAS_VARIADIC_TEMPLATES + +#include + +namespace Eigen { + +/** \class TensorInitializer + * \ingroup CXX11_Tensor_Module + * + * \brief Helper template to initialize Tensors from std::initializer_lists. + */ +namespace internal { + +template +struct Initializer { + typedef std::initializer_list< + typename Initializer::InitList> InitList; + + static void run(TensorEvaluator& tensor, + Eigen::array::Index, traits::NumDimensions>* indices, + const InitList& vals) { + int i = 0; + for (auto v : vals) { + (*indices)[traits::NumDimensions - N] = i++; + Initializer::run(tensor, indices, v); + } + } +}; + +template +struct Initializer { + typedef std::initializer_list::Scalar> InitList; + + static void run(TensorEvaluator& tensor, + Eigen::array::Index, traits::NumDimensions>* indices, + const InitList& vals) { + int i = 0; + // There is likely a faster way to do that than iterating. + for (auto v : vals) { + (*indices)[traits::NumDimensions - 1] = i++; + tensor.coeffRef(*indices) = v; + } + } +}; + +template +struct Initializer { + typedef typename traits::Scalar InitList; + + static void run(TensorEvaluator& tensor, + Eigen::array::Index, traits::NumDimensions>*, + const InitList& v) { + tensor.coeffRef(0) = v; + } +}; + + +template +void initialize_tensor(TensorEvaluator& tensor, + const typename Initializer::NumDimensions>::InitList& vals) { + Eigen::array::Index, traits::NumDimensions> indices; + Initializer::NumDimensions>::run(tensor, &indices, vals); +} + +} // namespace internal +} // namespace Eigen + +#endif // EIGEN_HAS_VARIADIC_TEMPLATES + +#endif // EIGEN_CXX11_TENSOR_TENSOR_INITIALIZER_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIntDiv.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIntDiv.h new file mode 100644 index 0000000000..ede3939c26 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIntDiv.h @@ -0,0 +1,253 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_INTDIV_H +#define EIGEN_CXX11_TENSOR_TENSOR_INTDIV_H + + +namespace Eigen { + +/** \internal + * + * \class TensorIntDiv + * \ingroup CXX11_Tensor_Module + * + * \brief Fast integer division by a constant. + * + * See the paper from Granlund and Montgomery for explanation. + * (at http://dx.doi.org/10.1145/773473.178249) + * + * \sa Tensor + */ + +namespace internal { + +namespace { + + // Note: result is undefined if val == 0 + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE + typename internal::enable_if::type count_leading_zeros(const T val) + { +#ifdef __CUDA_ARCH__ + return __clz(val); +#elif EIGEN_COMP_MSVC + unsigned long index; + _BitScanReverse(&index, val); + return 31 - index; +#else + EIGEN_STATIC_ASSERT(sizeof(unsigned long long) == 8, YOU_MADE_A_PROGRAMMING_MISTAKE); + return __builtin_clz(static_cast(val)); +#endif + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE + typename internal::enable_if::type count_leading_zeros(const T val) + { +#ifdef __CUDA_ARCH__ + return __clzll(val); +#elif EIGEN_COMP_MSVC && EIGEN_ARCH_x86_64 + unsigned long index; + _BitScanReverse64(&index, val); + return 63 - index; +#elif EIGEN_COMP_MSVC + // MSVC's _BitScanReverse64 is not available for 32bits builds. + unsigned int lo = (unsigned int)(val&0xffffffff); + unsigned int hi = (unsigned int)((val>>32)&0xffffffff); + int n; + if(hi==0) + n = 32 + count_leading_zeros(lo); + else + n = count_leading_zeros(hi); + return n; +#else + EIGEN_STATIC_ASSERT(sizeof(unsigned long long) == 8, YOU_MADE_A_PROGRAMMING_MISTAKE); + return __builtin_clzll(static_cast(val)); +#endif + } + + template + struct UnsignedTraits { + typedef typename conditional::type type; + }; + + template + struct DividerTraits { + typedef typename UnsignedTraits::type type; + static const int N = sizeof(T) * 8; + }; + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE uint32_t muluh(const uint32_t a, const T b) { +#if defined(__CUDA_ARCH__) + return __umulhi(a, b); +#else + return (static_cast(a) * b) >> 32; +#endif + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE uint64_t muluh(const uint64_t a, const T b) { +#if defined(__CUDA_ARCH__) + return __umul64hi(a, b); +#elif defined(__SIZEOF_INT128__) + __uint128_t v = static_cast<__uint128_t>(a) * static_cast<__uint128_t>(b); + return static_cast(v >> 64); +#else + return (TensorUInt128, uint64_t>(a) * TensorUInt128, uint64_t>(b)).upper(); +#endif + } + + template + struct DividerHelper { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE uint32_t computeMultiplier(const int log_div, const T divider) { + EIGEN_STATIC_ASSERT(N == 32, YOU_MADE_A_PROGRAMMING_MISTAKE); + return static_cast((static_cast(1) << (N+log_div)) / divider - (static_cast(1) << N) + 1); + } + }; + + template + struct DividerHelper<64, T> { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE uint64_t computeMultiplier(const int log_div, const T divider) { +#if defined(__SIZEOF_INT128__) && !defined(__CUDA_ARCH__) + return static_cast((static_cast<__uint128_t>(1) << (64+log_div)) / static_cast<__uint128_t>(divider) - (static_cast<__uint128_t>(1) << 64) + 1); +#else + const uint64_t shift = 1ULL << log_div; + TensorUInt128 result = TensorUInt128 >(shift, 0) / TensorUInt128, uint64_t>(divider) + - TensorUInt128, static_val<0> >(1, 0) + + TensorUInt128, static_val<1> >(1); + return static_cast(result); +#endif + } + }; +} + + +template +struct TensorIntDivisor { + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorIntDivisor() { + multiplier = 0; + shift1 = 0; + shift2 = 0; + } + + // Must have 0 < divider < 2^31. This is relaxed to + // 0 < divider < 2^63 when using 64-bit indices on platforms that support + // the __uint128_t type. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorIntDivisor(const T divider) { + const int N = DividerTraits::N; + eigen_assert(static_cast::type>(divider) < NumTraits::highest()/2); + eigen_assert(divider > 0); + + // fast ln2 + const int leading_zeros = count_leading_zeros(static_cast(divider)); + int log_div = N - leading_zeros; + // if divider is a power of two then log_div is 1 more than it should be. + if ((static_cast::type>(1) << (log_div-1)) == static_cast::type>(divider)) + log_div--; + + multiplier = DividerHelper::computeMultiplier(log_div, divider); + shift1 = log_div > 1 ? 1 : log_div; + shift2 = log_div > 1 ? log_div-1 : 0; + } + + // Must have 0 <= numerator. On platforms that dont support the __uint128_t + // type numerator should also be less than 2^32-1. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T divide(const T numerator) const { + eigen_assert(static_cast::type>(numerator) < NumTraits::highest()/2); + //eigen_assert(numerator >= 0); // this is implicitly asserted by the line above + + UnsignedType t1 = muluh(multiplier, numerator); + UnsignedType t = (static_cast(numerator) - t1) >> shift1; + return (t1 + t) >> shift2; + } + + private: + typedef typename DividerTraits::type UnsignedType; + UnsignedType multiplier; + int32_t shift1; + int32_t shift2; +}; + + +// Optimized version for signed 32 bit integers. +// Derived from Hacker's Delight. +// Only works for divisors strictly greater than one +template <> +class TensorIntDivisor { + public: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorIntDivisor() { + magic = 0; + shift = 0; + } + // Must have 2 <= divider + EIGEN_DEVICE_FUNC TensorIntDivisor(int32_t divider) { + eigen_assert(divider >= 2); + calcMagic(divider); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE int divide(const int32_t n) const { +#ifdef __CUDA_ARCH__ + return (__umulhi(magic, n) >> shift); +#else + uint64_t v = static_cast(magic) * static_cast(n); + return (static_cast(v >> 32) >> shift); +#endif + } + +private: + // Compute the magic numbers. See Hacker's Delight section 10 for an in + // depth explanation. + EIGEN_DEVICE_FUNC void calcMagic(int32_t d) { + const unsigned two31 = 0x80000000; // 2**31. + unsigned ad = d; + unsigned t = two31 + (ad >> 31); + unsigned anc = t - 1 - t%ad; // Absolute value of nc. + int p = 31; // Init. p. + unsigned q1 = two31/anc; // Init. q1 = 2**p/|nc|. + unsigned r1 = two31 - q1*anc; // Init. r1 = rem(2**p, |nc|). + unsigned q2 = two31/ad; // Init. q2 = 2**p/|d|. + unsigned r2 = two31 - q2*ad; // Init. r2 = rem(2**p, |d|). + unsigned delta = 0; + do { + p = p + 1; + q1 = 2*q1; // Update q1 = 2**p/|nc|. + r1 = 2*r1; // Update r1 = rem(2**p, |nc|). + if (r1 >= anc) { // (Must be an unsigned + q1 = q1 + 1; // comparison here). + r1 = r1 - anc;} + q2 = 2*q2; // Update q2 = 2**p/|d|. + r2 = 2*r2; // Update r2 = rem(2**p, |d|). + if (r2 >= ad) { // (Must be an unsigned + q2 = q2 + 1; // comparison here). + r2 = r2 - ad;} + delta = ad - r2; + } while (q1 < delta || (q1 == delta && r1 == 0)); + + magic = (unsigned)(q2 + 1); + shift = p - 32; + } + + uint32_t magic; + int32_t shift; +}; + + +template +static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator / (const T& numerator, const TensorIntDivisor& divisor) { + return divisor.divide(numerator); +} + + +} // end namespace internal +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_INTDIV_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorLayoutSwap.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorLayoutSwap.h new file mode 100644 index 0000000000..cd0109ef44 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorLayoutSwap.h @@ -0,0 +1,209 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_LAYOUT_SWAP_H +#define EIGEN_CXX11_TENSOR_TENSOR_LAYOUT_SWAP_H + +namespace Eigen { + +/** \class TensorLayoutSwap + * \ingroup CXX11_Tensor_Module + * + * \brief Swap the layout from col-major to row-major, or row-major + * to col-major, and invert the order of the dimensions. + * + * Beware: the dimensions are reversed by this operation. If you want to + * preserve the ordering of the dimensions, you need to combine this + * operation with a shuffle. + * + * \example: + * Tensor input(2, 4); + * Tensor output = input.swap_layout(); + * eigen_assert(output.dimension(0) == 4); + * eigen_assert(output.dimension(1) == 2); + * + * array shuffle(1, 0); + * output = input.swap_layout().shuffle(shuffle); + * eigen_assert(output.dimension(0) == 2); + * eigen_assert(output.dimension(1) == 4); + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = traits::NumDimensions; + static const int Layout = (traits::Layout == ColMajor) ? RowMajor : ColMajor; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorLayoutSwapOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorLayoutSwapOp type; +}; + +} // end namespace internal + + + +template +class TensorLayoutSwapOp : public TensorBase, WriteAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorLayoutSwapOp(const XprType& expr) + : m_xpr(expr) {} + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorLayoutSwapOp& operator = (const TensorLayoutSwapOp& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorLayoutSwapOp& operator = (const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + protected: + typename XprType::Nested m_xpr; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorLayoutSwapOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::Dimensions>::value; + typedef DSizes Dimensions; + + enum { + IsAligned = TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = (static_cast(TensorEvaluator::Layout) == static_cast(ColMajor)) ? RowMajor : ColMajor, + CoordAccess = false, // to be implemented + RawAccess = TensorEvaluator::RawAccess + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device) + { + for(int i = 0; i < NumDims; ++i) { + m_dimensions[i] = m_impl.dimensions()[NumDims-1-i]; + } + } + + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* data) { + return m_impl.evalSubExprsIfNeeded(data); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_impl.coeff(index); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return m_impl.template packet(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return m_impl.costPerCoeff(vectorized); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return m_impl.data(); } + + const TensorEvaluator& impl() const { return m_impl; } + + protected: + TensorEvaluator m_impl; + Dimensions m_dimensions; +}; + + +// Eval as lvalue +template + struct TensorEvaluator, Device> + : public TensorEvaluator, Device> +{ + typedef TensorEvaluator, Device> Base; + typedef TensorLayoutSwapOp XprType; + + enum { + IsAligned = TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = (static_cast(TensorEvaluator::Layout) == static_cast(ColMajor)) ? RowMajor : ColMajor, + CoordAccess = false // to be implemented + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : Base(op, device) + { } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType& coeffRef(Index index) + { + return this->m_impl.coeffRef(index); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketReturnType& x) + { + this->m_impl.template writePacket(index, x); + } +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_LAYOUT_SWAP_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.h new file mode 100644 index 0000000000..ee0078bbcc --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.h @@ -0,0 +1,54 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_META_MACROS_H +#define EIGEN_CXX11_TENSOR_TENSOR_META_MACROS_H + + +/** use this macro in sfinae selection in templated functions + * + * template::value , int >::type = 0 + * > + * void foo(){} + * + * becomes => + * + * template::value ) + * > + * void foo(){} + */ + +// SFINAE requires variadic templates +#ifndef __CUDACC__ +#if EIGEN_HAS_VARIADIC_TEMPLATES + // SFINAE doesn't work for gcc <= 4.7 + #ifdef EIGEN_COMP_GNUC + #if EIGEN_GNUC_AT_LEAST(4,8) + #define EIGEN_HAS_SFINAE + #endif + #else + #define EIGEN_HAS_SFINAE + #endif +#endif +#endif + +#define EIGEN_SFINAE_ENABLE_IF( __condition__ ) \ + typename internal::enable_if< ( __condition__ ) , int >::type = 0 + + +#if EIGEN_HAS_CONSTEXPR +#define EIGEN_CONSTEXPR constexpr +#else +#define EIGEN_CONSTEXPR +#endif + + +#endif diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h new file mode 100644 index 0000000000..a8e55757e4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h @@ -0,0 +1,321 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_MAP_H +#define EIGEN_CXX11_TENSOR_TENSOR_MAP_H + +namespace Eigen { + +/** \class TensorMap + * \ingroup CXX11_Tensor_Module + * + * \brief A tensor expression mapping an existing array of data. + * + */ +/// template class MakePointer_ is added to convert the host pointer to the device pointer. +/// It is added due to the fact that for our device compiler T* is not allowed. +/// If we wanted to use the same Evaluator functions we have to convert that type to our pointer T. +/// This is done through our MakePointer_ class. By default the Type in the MakePointer_ is T* . +/// Therefore, by adding the default value, we managed to convert the type and it does not break any +/// existing code as its default value is T*. +template class MakePointer_> class TensorMap : public TensorBase > +{ + public: + typedef TensorMap Self; + typedef typename PlainObjectType::Base Base; + typedef typename Eigen::internal::nested::type Nested; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + typedef typename internal::traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef typename Base::CoeffReturnType CoeffReturnType; + + /* typedef typename internal::conditional< + bool(internal::is_lvalue::value), + Scalar *, + const Scalar *>::type + PointerType;*/ + typedef typename MakePointer_::Type PointerType; + typedef PointerType PointerArgType; + + static const int Options = Options_; + + static const Index NumIndices = PlainObjectType::NumIndices; + typedef typename PlainObjectType::Dimensions Dimensions; + + enum { + IsAligned = ((int(Options_)&Aligned)==Aligned), + Layout = PlainObjectType::Layout, + CoordAccess = true, + RawAccess = true + }; + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr) : m_data(dataPtr), m_dimensions() { + // The number of dimensions used to construct a tensor must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT((0 == NumIndices || NumIndices == Dynamic), YOU_MADE_A_PROGRAMMING_MISTAKE) + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, Index firstDimension, IndexTypes... otherDimensions) : m_data(dataPtr), m_dimensions(firstDimension, otherDimensions...) { + // The number of dimensions used to construct a tensor must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT((sizeof...(otherDimensions) + 1 == NumIndices || NumIndices == Dynamic), YOU_MADE_A_PROGRAMMING_MISTAKE) + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, Index firstDimension) : m_data(dataPtr), m_dimensions(firstDimension) { + // The number of dimensions used to construct a tensor must be equal to the rank of the tensor. + EIGEN_STATIC_ASSERT((1 == NumIndices || NumIndices == Dynamic), YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, Index dim1, Index dim2) : m_data(dataPtr), m_dimensions(dim1, dim2) { + EIGEN_STATIC_ASSERT(2 == NumIndices || NumIndices == Dynamic, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, Index dim1, Index dim2, Index dim3) : m_data(dataPtr), m_dimensions(dim1, dim2, dim3) { + EIGEN_STATIC_ASSERT(3 == NumIndices || NumIndices == Dynamic, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, Index dim1, Index dim2, Index dim3, Index dim4) : m_data(dataPtr), m_dimensions(dim1, dim2, dim3, dim4) { + EIGEN_STATIC_ASSERT(4 == NumIndices || NumIndices == Dynamic, YOU_MADE_A_PROGRAMMING_MISTAKE) + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, Index dim1, Index dim2, Index dim3, Index dim4, Index dim5) : m_data(dataPtr), m_dimensions(dim1, dim2, dim3, dim4, dim5) { + EIGEN_STATIC_ASSERT(5 == NumIndices || NumIndices == Dynamic, YOU_MADE_A_PROGRAMMING_MISTAKE) + } +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, const array& dimensions) + : m_data(dataPtr), m_dimensions(dimensions) + { } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorMap(PointerArgType dataPtr, const Dimensions& dimensions) + : m_data(dataPtr), m_dimensions(dimensions) + { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorMap(PlainObjectType& tensor) + : m_data(tensor.data()), m_dimensions(tensor.dimensions()) + { } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index rank() const { return m_dimensions.rank(); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index dimension(Index n) const { return m_dimensions[n]; } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Index size() const { return m_dimensions.TotalSize(); } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE PointerType data() { return m_data; } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const PointerType data() const { return m_data; } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(const array& indices) const + { + // eigen_assert(checkIndexRange(indices)); + if (PlainObjectType::Options&RowMajor) { + const Index index = m_dimensions.IndexOfRowMajor(indices); + return m_data[index]; + } else { + const Index index = m_dimensions.IndexOfColMajor(indices); + return m_data[index]; + } + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()() const + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE) + return m_data[0]; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index index) const + { + eigen_internal_assert(index >= 0 && index < size()); + return m_data[index]; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index firstIndex, Index secondIndex, IndexTypes... otherIndices) const + { + EIGEN_STATIC_ASSERT(sizeof...(otherIndices) + 2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE) + if (PlainObjectType::Options&RowMajor) { + const Index index = m_dimensions.IndexOfRowMajor(array{{firstIndex, secondIndex, otherIndices...}}); + return m_data[index]; + } else { + const Index index = m_dimensions.IndexOfColMajor(array{{firstIndex, secondIndex, otherIndices...}}); + return m_data[index]; + } + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1) const + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i1 + i0 * m_dimensions[1]; + return m_data[index]; + } else { + const Index index = i0 + i1 * m_dimensions[0]; + return m_data[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2) const + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i2 + m_dimensions[2] * (i1 + m_dimensions[1] * i0); + return m_data[index]; + } else { + const Index index = i0 + m_dimensions[0] * (i1 + m_dimensions[1] * i2); + return m_data[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3) const + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i3 + m_dimensions[3] * (i2 + m_dimensions[2] * (i1 + m_dimensions[1] * i0)); + return m_data[index]; + } else { + const Index index = i0 + m_dimensions[0] * (i1 + m_dimensions[1] * (i2 + m_dimensions[2] * i3)); + return m_data[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) const + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i4 + m_dimensions[4] * (i3 + m_dimensions[3] * (i2 + m_dimensions[2] * (i1 + m_dimensions[1] * i0))); + return m_data[index]; + } else { + const Index index = i0 + m_dimensions[0] * (i1 + m_dimensions[1] * (i2 + m_dimensions[2] * (i3 + m_dimensions[3] * i4))); + return m_data[index]; + } + } +#endif + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(const array& indices) + { + // eigen_assert(checkIndexRange(indices)); + if (PlainObjectType::Options&RowMajor) { + const Index index = m_dimensions.IndexOfRowMajor(indices); + return m_data[index]; + } else { + const Index index = m_dimensions.IndexOfColMajor(indices); + return m_data[index]; + } + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()() + { + EIGEN_STATIC_ASSERT(NumIndices == 0, YOU_MADE_A_PROGRAMMING_MISTAKE) + return m_data[0]; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index index) + { + eigen_internal_assert(index >= 0 && index < size()); + return m_data[index]; + } + +#if EIGEN_HAS_VARIADIC_TEMPLATES + template EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index firstIndex, Index secondIndex, IndexTypes... otherIndices) + { + static_assert(sizeof...(otherIndices) + 2 == NumIndices || NumIndices == Dynamic, "Number of indices used to access a tensor coefficient must be equal to the rank of the tensor."); + const std::size_t NumDims = sizeof...(otherIndices) + 2; + if (PlainObjectType::Options&RowMajor) { + const Index index = m_dimensions.IndexOfRowMajor(array{{firstIndex, secondIndex, otherIndices...}}); + return m_data[index]; + } else { + const Index index = m_dimensions.IndexOfColMajor(array{{firstIndex, secondIndex, otherIndices...}}); + return m_data[index]; + } + } +#else + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1) + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i1 + i0 * m_dimensions[1]; + return m_data[index]; + } else { + const Index index = i0 + i1 * m_dimensions[0]; + return m_data[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2) + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i2 + m_dimensions[2] * (i1 + m_dimensions[1] * i0); + return m_data[index]; + } else { + const Index index = i0 + m_dimensions[0] * (i1 + m_dimensions[1] * i2); + return m_data[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2, Index i3) + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i3 + m_dimensions[3] * (i2 + m_dimensions[2] * (i1 + m_dimensions[1] * i0)); + return m_data[index]; + } else { + const Index index = i0 + m_dimensions[0] * (i1 + m_dimensions[1] * (i2 + m_dimensions[2] * i3)); + return m_data[index]; + } + } + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) + { + if (PlainObjectType::Options&RowMajor) { + const Index index = i4 + m_dimensions[4] * (i3 + m_dimensions[3] * (i2 + m_dimensions[2] * (i1 + m_dimensions[1] * i0))); + return m_data[index]; + } else { + const Index index = i0 + m_dimensions[0] * (i1 + m_dimensions[1] * (i2 + m_dimensions[2] * (i3 + m_dimensions[3] * i4))); + return m_data[index]; + } + } +#endif + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Self& operator=(const Self& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Self& operator=(const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + private: + typename MakePointer_::Type m_data; + Dimensions m_dimensions; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_MAP_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMeta.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMeta.h new file mode 100644 index 0000000000..615559d445 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMeta.h @@ -0,0 +1,218 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_META_H +#define EIGEN_CXX11_TENSOR_TENSOR_META_H + +namespace Eigen { + +template struct Cond {}; + +template EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +const T1& choose(Cond, const T1& first, const T2&) { + return first; +} + +template EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +const T2& choose(Cond, const T1&, const T2& second) { + return second; +} + + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T divup(const X x, const Y y) { + return static_cast((x + y - 1) / y); +} + +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE +T divup(const T x, const T y) { + return static_cast((x + y - 1) / y); +} + +template struct max_n_1 { + static const size_t size = n; +}; +template <> struct max_n_1<0> { + static const size_t size = 1; +}; + + +// Default packet types +template +struct PacketType : internal::packet_traits { + typedef typename internal::packet_traits::type type; +}; + +// For CUDA packet types when using a GpuDevice +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) && defined(EIGEN_HAS_CUDA_FP16) +template <> +struct PacketType { + typedef half2 type; + static const int size = 2; + enum { + HasAdd = 1, + HasSub = 1, + HasMul = 1, + HasNegate = 1, + HasAbs = 1, + HasArg = 0, + HasAbs2 = 0, + HasMin = 1, + HasMax = 1, + HasConj = 0, + HasSetLinear = 0, + HasBlend = 0, + + HasDiv = 1, + HasSqrt = 1, + HasRsqrt = 1, + HasExp = 1, + HasLog = 1, + HasLog1p = 0, + HasLog10 = 0, + HasPow = 1, + }; +}; +#endif + +#if defined(EIGEN_USE_SYCL) +template + struct PacketType { + typedef T type; + static const int size = 1; + enum { + HasAdd = 0, + HasSub = 0, + HasMul = 0, + HasNegate = 0, + HasAbs = 0, + HasArg = 0, + HasAbs2 = 0, + HasMin = 0, + HasMax = 0, + HasConj = 0, + HasSetLinear = 0, + HasBlend = 0 + }; +}; +#endif + + +// Tuple mimics std::pair but works on e.g. nvcc. +template struct Tuple { + public: + U first; + V second; + + typedef U first_type; + typedef V second_type; + + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Tuple() : first(), second() {} + + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Tuple(const U& f, const V& s) : first(f), second(s) {} + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Tuple& operator= (const Tuple& rhs) { + if (&rhs == this) return *this; + first = rhs.first; + second = rhs.second; + return *this; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void swap(Tuple& rhs) { + using numext::swap; + swap(first, rhs.first); + swap(second, rhs.second); + } +}; + +template +EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +bool operator==(const Tuple& x, const Tuple& y) { + return (x.first == y.first && x.second == y.second); +} + +template +EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +bool operator!=(const Tuple& x, const Tuple& y) { + return !(x == y); +} + + +// Can't use std::pairs on cuda devices +template struct IndexPair { + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE IndexPair() : first(0), second(0) {} + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE IndexPair(Idx f, Idx s) : first(f), second(s) {} + + EIGEN_DEVICE_FUNC void set(IndexPair val) { + first = val.first; + second = val.second; + } + + Idx first; + Idx second; +}; + + +#ifdef EIGEN_HAS_SFINAE +namespace internal { + + template + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + array customIndices2Array(IndexType& idx, numeric_list) { + return { idx[Is]... }; + } + template + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + array customIndices2Array(IndexType&, numeric_list) { + return array(); + } + + /** Make an array (for index/dimensions) out of a custom index */ + template + EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + array customIndices2Array(IndexType& idx) { + return customIndices2Array(idx, typename gen_numeric_list::type{}); + } + + + template + struct is_base_of + { + + typedef char (&yes)[1]; + typedef char (&no)[2]; + + template + struct Host + { + operator BB*() const; + operator DD*(); + }; + + template + static yes check(D*, T); + static no check(B*, int); + + static const bool value = sizeof(check(Host(), int())) == sizeof(yes); + }; + +} +#endif + + + +} // namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_META_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMorphing.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMorphing.h new file mode 100644 index 0000000000..d34f1e328c --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMorphing.h @@ -0,0 +1,888 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_MORPHING_H +#define EIGEN_CXX11_TENSOR_TENSOR_MORPHING_H + +namespace Eigen { + +/** \class TensorReshaping + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor reshaping class. + * + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = array_size::value; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorReshapingOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorReshapingOp type; +}; + +} // end namespace internal + + + +template +class TensorReshapingOp : public TensorBase, WriteAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorReshapingOp(const XprType& expr, const NewDimensions& dims) + : m_xpr(expr), m_dims(dims) {} + + EIGEN_DEVICE_FUNC + const NewDimensions& dimensions() const { return m_dims; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorReshapingOp& operator = (const TensorReshapingOp& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorReshapingOp& operator = (const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + protected: + typename XprType::Nested m_xpr; + const NewDimensions m_dims; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorReshapingOp XprType; + typedef NewDimensions Dimensions; + + enum { + IsAligned = TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = TensorEvaluator::RawAccess + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_dimensions(op.dimensions()) + { + // The total size of the reshaped tensor must be equal to the total size + // of the input tensor. + eigen_assert(internal::array_prod(m_impl.dimensions()) == internal::array_prod(op.dimensions())); + } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* data) { + return m_impl.evalSubExprsIfNeeded(data); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_impl.coeff(index); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + return m_impl.template packet(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return m_impl.costPerCoeff(vectorized); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return const_cast(m_impl.data()); } + + EIGEN_DEVICE_FUNC const TensorEvaluator& impl() const { return m_impl; } + + protected: + TensorEvaluator m_impl; + NewDimensions m_dimensions; +}; + + +// Eval as lvalue +template + struct TensorEvaluator, Device> + : public TensorEvaluator, Device> + +{ + typedef TensorEvaluator, Device> Base; + typedef TensorReshapingOp XprType; + typedef NewDimensions Dimensions; + + enum { + IsAligned = TensorEvaluator::IsAligned, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = TensorEvaluator::RawAccess + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : Base(op, device) + { } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType& coeffRef(Index index) + { + return this->m_impl.coeffRef(index); + } + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketReturnType& x) + { + this->m_impl.template writePacket(index, x); + } +}; + + +/** \class TensorSlicing + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor slicing class. + * + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = array_size::value; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorSlicingOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorSlicingOp type; +}; + +} // end namespace internal + + + +template +class TensorSlicingOp : public TensorBase > +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorSlicingOp(const XprType& expr, const StartIndices& indices, const Sizes& sizes) + : m_xpr(expr), m_indices(indices), m_sizes(sizes) {} + + EIGEN_DEVICE_FUNC + const StartIndices& startIndices() const { return m_indices; } + EIGEN_DEVICE_FUNC + const Sizes& sizes() const { return m_sizes; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorSlicingOp& operator = (const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorSlicingOp& operator = (const TensorSlicingOp& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run(assign, DefaultDevice()); + return *this; + } + + + protected: + typename XprType::Nested m_xpr; + const StartIndices m_indices; + const Sizes m_sizes; +}; + + +// Fixme: figure out the exact threshold +namespace { +template struct MemcpyTriggerForSlicing { + EIGEN_DEVICE_FUNC MemcpyTriggerForSlicing(const Device& device) : threshold_(2 * device.numThreads()) { } + EIGEN_DEVICE_FUNC bool operator ()(Index val) const { return val > threshold_; } + + private: + Index threshold_; +}; + +// It is very expensive to start the memcpy kernel on GPU: we therefore only +// use it for large copies. +#ifdef EIGEN_USE_GPU +template struct MemcpyTriggerForSlicing { + EIGEN_DEVICE_FUNC MemcpyTriggerForSlicing(const GpuDevice&) { } + EIGEN_DEVICE_FUNC bool operator ()(Index val) const { return val > 4*1024*1024; } +}; +#endif +} + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorSlicingOp XprType; + static const int NumDims = internal::array_size::value; + + enum { + // Alignment can't be guaranteed at compile time since it depends on the + // slice offsets and sizes. + IsAligned = /*TensorEvaluator::IsAligned*/false, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_device(device), m_dimensions(op.sizes()), m_offsets(op.startIndices()) + { + for (std::size_t i = 0; i < internal::array_size::value; ++i) { + eigen_assert(m_impl.dimensions()[i] >= op.sizes()[i] + op.startIndices()[i]); + } + + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + const Sizes& output_dims = op.sizes(); + if (static_cast(Layout) == static_cast(ColMajor)) { + m_inputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_inputStrides[i] = m_inputStrides[i-1] * input_dims[i-1]; + } + + // Don't initialize m_fastOutputStrides[0] since it won't ever be accessed. + m_outputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_outputStrides[i] = m_outputStrides[i-1] * output_dims[i-1]; + m_fastOutputStrides[i] = internal::TensorIntDivisor(m_outputStrides[i]); + } + } else { + m_inputStrides[NumDims-1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_inputStrides[i] = m_inputStrides[i+1] * input_dims[i+1]; + } + + // Don't initialize m_fastOutputStrides[NumDims-1] since it won't ever be accessed. + m_outputStrides[NumDims-1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_outputStrides[i] = m_outputStrides[i+1] * output_dims[i+1]; + m_fastOutputStrides[i] = internal::TensorIntDivisor(m_outputStrides[i]); + } + } + } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef Sizes Dimensions; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType* data) { + m_impl.evalSubExprsIfNeeded(NULL); + if (!NumTraits::type>::RequireInitialization && data && m_impl.data()) { + Index contiguous_values = 1; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < NumDims; ++i) { + contiguous_values *= dimensions()[i]; + if (dimensions()[i] != m_impl.dimensions()[i]) { + break; + } + } + } else { + for (int i = NumDims-1; i >= 0; --i) { + contiguous_values *= dimensions()[i]; + if (dimensions()[i] != m_impl.dimensions()[i]) { + break; + } + } + } + // Use memcpy if it's going to be faster than using the regular evaluation. + const MemcpyTriggerForSlicing trigger(m_device); + if (trigger(contiguous_values)) { + Scalar* src = (Scalar*)m_impl.data(); + for (int i = 0; i < internal::array_prod(dimensions()); i += contiguous_values) { + Index offset = srcCoeff(i); + m_device.memcpy((void*)(data+i), src+offset, contiguous_values * sizeof(Scalar)); + } + return false; + } + } + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_impl.coeff(srcCoeff(index)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + const int packetSize = internal::unpacket_traits::size; + EIGEN_STATIC_ASSERT((packetSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+packetSize-1 < internal::array_prod(dimensions())); + + Index inputIndices[] = {0, 0}; + Index indices[] = {index, index + packetSize - 1}; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx0 = indices[0] / m_fastOutputStrides[i]; + const Index idx1 = indices[1] / m_fastOutputStrides[i]; + inputIndices[0] += (idx0 + m_offsets[i]) * m_inputStrides[i]; + inputIndices[1] += (idx1 + m_offsets[i]) * m_inputStrides[i]; + indices[0] -= idx0 * m_outputStrides[i]; + indices[1] -= idx1 * m_outputStrides[i]; + } + inputIndices[0] += (indices[0] + m_offsets[0]); + inputIndices[1] += (indices[1] + m_offsets[0]); + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx0 = indices[0] / m_fastOutputStrides[i]; + const Index idx1 = indices[1] / m_fastOutputStrides[i]; + inputIndices[0] += (idx0 + m_offsets[i]) * m_inputStrides[i]; + inputIndices[1] += (idx1 + m_offsets[i]) * m_inputStrides[i]; + indices[0] -= idx0 * m_outputStrides[i]; + indices[1] -= idx1 * m_outputStrides[i]; + } + inputIndices[0] += (indices[0] + m_offsets[NumDims-1]); + inputIndices[1] += (indices[1] + m_offsets[NumDims-1]); + } + if (inputIndices[1] - inputIndices[0] == packetSize - 1) { + PacketReturnType rslt = m_impl.template packet(inputIndices[0]); + return rslt; + } + else { + EIGEN_ALIGN_MAX typename internal::remove_const::type values[packetSize]; + values[0] = m_impl.coeff(inputIndices[0]); + values[packetSize-1] = m_impl.coeff(inputIndices[1]); + for (int i = 1; i < packetSize-1; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return m_impl.costPerCoeff(vectorized) + TensorOpCost(0, 0, NumDims); + } + + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar* data() const { + Scalar* result = m_impl.data(); + if (result) { + Index offset = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < NumDims; ++i) { + if (m_dimensions[i] != m_impl.dimensions()[i]) { + offset += m_offsets[i] * m_inputStrides[i]; + for (int j = i+1; j < NumDims; ++j) { + if (m_dimensions[j] > 1) { + return NULL; + } + offset += m_offsets[j] * m_inputStrides[j]; + } + break; + } + } + } else { + for (int i = NumDims - 1; i >= 0; --i) { + if (m_dimensions[i] != m_impl.dimensions()[i]) { + offset += m_offsets[i] * m_inputStrides[i]; + for (int j = i-1; j >= 0; --j) { + if (m_dimensions[j] > 1) { + return NULL; + } + offset += m_offsets[j] * m_inputStrides[j]; + } + break; + } + } + } + return result + offset; + } + return NULL; + } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index srcCoeff(Index index) const + { + Index inputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_fastOutputStrides[i]; + inputIndex += (idx + m_offsets[i]) * m_inputStrides[i]; + index -= idx * m_outputStrides[i]; + } + inputIndex += (index + m_offsets[0]); + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_fastOutputStrides[i]; + inputIndex += (idx + m_offsets[i]) * m_inputStrides[i]; + index -= idx * m_outputStrides[i]; + } + inputIndex += (index + m_offsets[NumDims-1]); + } + return inputIndex; + } + + array m_outputStrides; + array, NumDims> m_fastOutputStrides; + array m_inputStrides; + TensorEvaluator m_impl; + const Device& m_device; + Dimensions m_dimensions; + const StartIndices m_offsets; +}; + + +// Eval as lvalue +template +struct TensorEvaluator, Device> + : public TensorEvaluator, Device> +{ + typedef TensorEvaluator, Device> Base; + typedef TensorSlicingOp XprType; + static const int NumDims = internal::array_size::value; + + enum { + IsAligned = /*TensorEvaluator::IsAligned*/false, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : Base(op, device) + { } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef Sizes Dimensions; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType& coeffRef(Index index) + { + return this->m_impl.coeffRef(this->srcCoeff(index)); + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void writePacket(Index index, const PacketReturnType& x) + { + const int packetSize = internal::unpacket_traits::size; + Index inputIndices[] = {0, 0}; + Index indices[] = {index, index + packetSize - 1}; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx0 = indices[0] / this->m_fastOutputStrides[i]; + const Index idx1 = indices[1] / this->m_fastOutputStrides[i]; + inputIndices[0] += (idx0 + this->m_offsets[i]) * this->m_inputStrides[i]; + inputIndices[1] += (idx1 + this->m_offsets[i]) * this->m_inputStrides[i]; + indices[0] -= idx0 * this->m_outputStrides[i]; + indices[1] -= idx1 * this->m_outputStrides[i]; + } + inputIndices[0] += (indices[0] + this->m_offsets[0]); + inputIndices[1] += (indices[1] + this->m_offsets[0]); + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx0 = indices[0] / this->m_fastOutputStrides[i]; + const Index idx1 = indices[1] / this->m_fastOutputStrides[i]; + inputIndices[0] += (idx0 + this->m_offsets[i]) * this->m_inputStrides[i]; + inputIndices[1] += (idx1 + this->m_offsets[i]) * this->m_inputStrides[i]; + indices[0] -= idx0 * this->m_outputStrides[i]; + indices[1] -= idx1 * this->m_outputStrides[i]; + } + inputIndices[0] += (indices[0] + this->m_offsets[NumDims-1]); + inputIndices[1] += (indices[1] + this->m_offsets[NumDims-1]); + } + if (inputIndices[1] - inputIndices[0] == packetSize - 1) { + this->m_impl.template writePacket(inputIndices[0], x); + } + else { + EIGEN_ALIGN_MAX CoeffReturnType values[packetSize]; + internal::pstore(values, x); + this->m_impl.coeffRef(inputIndices[0]) = values[0]; + this->m_impl.coeffRef(inputIndices[1]) = values[packetSize-1]; + for (int i = 1; i < packetSize-1; ++i) { + this->coeffRef(index+i) = values[i]; + } + } + } +}; + + + +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = array_size::value; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorStridingSlicingOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorStridingSlicingOp type; +}; + +} // end namespace internal + + +template +class TensorStridingSlicingOp : public TensorBase > +{ + public: + typedef typename internal::traits::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename internal::nested::type Nested; + typedef typename internal::traits::StorageKind StorageKind; + typedef typename internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorStridingSlicingOp( + const XprType& expr, const StartIndices& startIndices, + const StopIndices& stopIndices, const Strides& strides) + : m_xpr(expr), m_startIndices(startIndices), m_stopIndices(stopIndices), + m_strides(strides) {} + + EIGEN_DEVICE_FUNC + const StartIndices& startIndices() const { return m_startIndices; } + EIGEN_DEVICE_FUNC + const StartIndices& stopIndices() const { return m_stopIndices; } + EIGEN_DEVICE_FUNC + const StartIndices& strides() const { return m_strides; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorStridingSlicingOp& operator = (const TensorStridingSlicingOp& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run( + assign, DefaultDevice()); + return *this; + } + + template + EIGEN_DEVICE_FUNC + EIGEN_STRONG_INLINE TensorStridingSlicingOp& operator = (const OtherDerived& other) + { + typedef TensorAssignOp Assign; + Assign assign(*this, other); + internal::TensorExecutor::run( + assign, DefaultDevice()); + return *this; + } + + protected: + typename XprType::Nested m_xpr; + const StartIndices m_startIndices; + const StopIndices m_stopIndices; + const Strides m_strides; +}; + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorStridingSlicingOp XprType; + static const int NumDims = internal::array_size::value; + + enum { + // Alignment can't be guaranteed at compile time since it depends on the + // slice offsets and sizes. + IsAligned = false, + PacketAccess = false, + BlockAccess = false, + Layout = TensorEvaluator::Layout, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_device(device), m_strides(op.strides()) + { + // Handle degenerate intervals by gracefully clamping and allowing m_dimensions to be zero + DSizes startIndicesClamped, stopIndicesClamped; + for (size_t i = 0; i < internal::array_size::value; ++i) { + eigen_assert(m_strides[i] != 0 && "0 stride is invalid"); + if(m_strides[i]>0){ + startIndicesClamped[i] = clamp(op.startIndices()[i], 0, m_impl.dimensions()[i]); + stopIndicesClamped[i] = clamp(op.stopIndices()[i], 0, m_impl.dimensions()[i]); + }else{ + /* implies m_strides[i]<0 by assert */ + startIndicesClamped[i] = clamp(op.startIndices()[i], -1, m_impl.dimensions()[i] - 1); + stopIndicesClamped[i] = clamp(op.stopIndices()[i], -1, m_impl.dimensions()[i] - 1); + } + m_startIndices[i] = startIndicesClamped[i]; + } + + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + + // check for degenerate intervals and compute output tensor shape + bool degenerate = false;; + for(int i = 0; i < NumDims; i++){ + Index interval = stopIndicesClamped[i] - startIndicesClamped[i]; + if(interval == 0 || ((interval<0) != (m_strides[i]<0))){ + m_dimensions[i] = 0; + degenerate = true; + }else{ + m_dimensions[i] = interval / m_strides[i] + + (interval % m_strides[i] != 0 ? 1 : 0); + eigen_assert(m_dimensions[i] >= 0); + } + } + Strides output_dims = m_dimensions; + + if (static_cast(Layout) == static_cast(ColMajor)) { + m_inputStrides[0] = m_strides[0]; + m_offsets[0] = startIndicesClamped[0]; + Index previousDimProduct = 1; + for (int i = 1; i < NumDims; ++i) { + previousDimProduct *= input_dims[i-1]; + m_inputStrides[i] = previousDimProduct * m_strides[i]; + m_offsets[i] = startIndicesClamped[i] * previousDimProduct; + } + + // Don't initialize m_fastOutputStrides[0] since it won't ever be accessed. + m_outputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_outputStrides[i] = m_outputStrides[i-1] * output_dims[i-1]; + // NOTE: if tensor is degenerate, we send 1 to prevent TensorIntDivisor constructor crash + m_fastOutputStrides[i] = internal::TensorIntDivisor(degenerate ? 1 : m_outputStrides[i]); + } + } else { + m_inputStrides[NumDims-1] = m_strides[NumDims-1]; + m_offsets[NumDims-1] = startIndicesClamped[NumDims-1]; + Index previousDimProduct = 1; + for (int i = NumDims - 2; i >= 0; --i) { + previousDimProduct *= input_dims[i+1]; + m_inputStrides[i] = previousDimProduct * m_strides[i]; + m_offsets[i] = startIndicesClamped[i] * previousDimProduct; + } + + m_outputStrides[NumDims-1] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_outputStrides[i] = m_outputStrides[i+1] * output_dims[i+1]; + // NOTE: if tensor is degenerate, we send 1 to prevent TensorIntDivisor constructor crash + m_fastOutputStrides[i] = internal::TensorIntDivisor(degenerate ? 1 : m_outputStrides[i]); + } + } + m_block_total_size_max = numext::maxi(static_cast(1), + device.lastLevelCacheSize() / + sizeof(Scalar)); + } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename internal::remove_const::type ScalarNonConst; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef Strides Dimensions; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(CoeffReturnType*) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + return m_impl.coeff(srcCoeff(index)); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + return m_impl.costPerCoeff(vectorized) + TensorOpCost(0, 0, NumDims); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar* data() const { + return NULL; + } + + protected: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index srcCoeff(Index index) const + { + Index inputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i >= 0; --i) { + const Index idx = index / m_fastOutputStrides[i]; + inputIndex += idx * m_inputStrides[i] + m_offsets[i]; + index -= idx * m_outputStrides[i]; + } + } else { + for (int i = 0; i < NumDims; ++i) { + const Index idx = index / m_fastOutputStrides[i]; + inputIndex += idx * m_inputStrides[i] + m_offsets[i]; + index -= idx * m_outputStrides[i]; + } + } + return inputIndex; + } + + static EIGEN_STRONG_INLINE Index clamp(Index value, Index min, Index max) { + return numext::maxi(min, numext::mini(max,value)); + } + + array m_outputStrides; + array, NumDims> m_fastOutputStrides; + array m_inputStrides; + TensorEvaluator m_impl; + const Device& m_device; + DSizes m_startIndices; // clamped startIndices + DSizes m_dimensions; + DSizes m_offsets; // offset in a flattened shape + const Strides m_strides; + std::size_t m_block_total_size_max; +}; + +// Eval as lvalue +template +struct TensorEvaluator, Device> + : public TensorEvaluator, Device> +{ + typedef TensorEvaluator, Device> Base; + typedef TensorStridingSlicingOp XprType; + static const int NumDims = internal::array_size::value; + + enum { + IsAligned = false, + PacketAccess = false, + BlockAccess = false, + Layout = TensorEvaluator::Layout, + CoordAccess = TensorEvaluator::CoordAccess, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : Base(op, device) + { } + + typedef typename XprType::Index Index; + typedef typename XprType::Scalar Scalar; + typedef typename internal::remove_const::type ScalarNonConst; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + typedef Strides Dimensions; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType& coeffRef(Index index) + { + return this->m_impl.coeffRef(this->srcCoeff(index)); + } +}; + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_MORPHING_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPadding.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPadding.h new file mode 100644 index 0000000000..647bcf1088 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPadding.h @@ -0,0 +1,397 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_PADDING_H +#define EIGEN_CXX11_TENSOR_TENSOR_PADDING_H + +namespace Eigen { + +/** \class TensorPadding + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor padding class. + * At the moment only padding with a constant value is supported. + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorPaddingOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorPaddingOp type; +}; + +} // end namespace internal + + + +template +class TensorPaddingOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorPaddingOp(const XprType& expr, const PaddingDimensions& padding_dims, const Scalar padding_value) + : m_xpr(expr), m_padding_dims(padding_dims), m_padding_value(padding_value) {} + + EIGEN_DEVICE_FUNC + const PaddingDimensions& padding() const { return m_padding_dims; } + EIGEN_DEVICE_FUNC + Scalar padding_value() const { return m_padding_value; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const PaddingDimensions m_padding_dims; + const Scalar m_padding_value; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorPaddingOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::value; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = true, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = true, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_padding(op.padding()), m_paddingValue(op.padding_value()) + { + // The padding op doesn't change the rank of the tensor. Directly padding a scalar would lead + // to a vector, which doesn't make sense. Instead one should reshape the scalar into a vector + // of 1 element first and then pad. + EIGEN_STATIC_ASSERT((NumDims > 0), YOU_MADE_A_PROGRAMMING_MISTAKE); + + // Compute dimensions + m_dimensions = m_impl.dimensions(); + for (int i = 0; i < NumDims; ++i) { + m_dimensions[i] += m_padding[i].first + m_padding[i].second; + } + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + if (static_cast(Layout) == static_cast(ColMajor)) { + m_inputStrides[0] = 1; + m_outputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_inputStrides[i] = m_inputStrides[i-1] * input_dims[i-1]; + m_outputStrides[i] = m_outputStrides[i-1] * m_dimensions[i-1]; + } + m_outputStrides[NumDims] = m_outputStrides[NumDims-1] * m_dimensions[NumDims-1]; + } else { + m_inputStrides[NumDims - 1] = 1; + m_outputStrides[NumDims] = 1; + for (int i = NumDims - 2; i >= 0; --i) { + m_inputStrides[i] = m_inputStrides[i+1] * input_dims[i+1]; + m_outputStrides[i+1] = m_outputStrides[i+2] * m_dimensions[i+1]; + } + m_outputStrides[0] = m_outputStrides[1] * m_dimensions[0]; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar*) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + eigen_assert(index < dimensions().TotalSize()); + Index inputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 1; i > 0; --i) { + const Index idx = index / m_outputStrides[i]; + if (isPaddingAtIndexForDim(idx, i)) { + return m_paddingValue; + } + inputIndex += (idx - m_padding[i].first) * m_inputStrides[i]; + index -= idx * m_outputStrides[i]; + } + if (isPaddingAtIndexForDim(index, 0)) { + return m_paddingValue; + } + inputIndex += (index - m_padding[0].first); + } else { + for (int i = 0; i < NumDims - 1; ++i) { + const Index idx = index / m_outputStrides[i+1]; + if (isPaddingAtIndexForDim(idx, i)) { + return m_paddingValue; + } + inputIndex += (idx - m_padding[i].first) * m_inputStrides[i]; + index -= idx * m_outputStrides[i+1]; + } + if (isPaddingAtIndexForDim(index, NumDims-1)) { + return m_paddingValue; + } + inputIndex += (index - m_padding[NumDims-1].first); + } + return m_impl.coeff(inputIndex); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + if (static_cast(Layout) == static_cast(ColMajor)) { + return packetColMajor(index); + } + return packetRowMajor(index); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + TensorOpCost cost = m_impl.costPerCoeff(vectorized); + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < NumDims; ++i) + updateCostPerDimension(cost, i, i == 0); + } else { + for (int i = NumDims - 1; i >= 0; --i) + updateCostPerDimension(cost, i, i == NumDims - 1); + } + return cost; + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + private: + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool isPaddingAtIndexForDim( + Index index, int dim_index) const { +#if defined(EIGEN_HAS_INDEX_LIST) + return (!internal::index_pair_first_statically_eq(dim_index, 0) && + index < m_padding[dim_index].first) || + (!internal::index_pair_second_statically_eq(dim_index, 0) && + index >= m_dimensions[dim_index] - m_padding[dim_index].second); +#else + return (index < m_padding[dim_index].first) || + (index >= m_dimensions[dim_index] - m_padding[dim_index].second); +#endif + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool isLeftPaddingCompileTimeZero( + int dim_index) const { +#if defined(EIGEN_HAS_INDEX_LIST) + return internal::index_pair_first_statically_eq(dim_index, 0); +#else + EIGEN_UNUSED_VARIABLE(dim_index); + return false; +#endif + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool isRightPaddingCompileTimeZero( + int dim_index) const { +#if defined(EIGEN_HAS_INDEX_LIST) + return internal::index_pair_second_statically_eq(dim_index, 0); +#else + EIGEN_UNUSED_VARIABLE(dim_index); + return false; +#endif + } + + + void updateCostPerDimension(TensorOpCost& cost, int i, bool first) const { + const double in = static_cast(m_impl.dimensions()[i]); + const double out = in + m_padding[i].first + m_padding[i].second; + if (out == 0) + return; + const double reduction = in / out; + cost *= reduction; + if (first) { + cost += TensorOpCost(0, 0, 2 * TensorOpCost::AddCost() + + reduction * (1 * TensorOpCost::AddCost())); + } else { + cost += TensorOpCost(0, 0, 2 * TensorOpCost::AddCost() + + 2 * TensorOpCost::MulCost() + + reduction * (2 * TensorOpCost::MulCost() + + 1 * TensorOpCost::DivCost())); + } + } + + protected: + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packetColMajor(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + const Index initialIndex = index; + Index inputIndex = 0; + for (int i = NumDims - 1; i > 0; --i) { + const Index first = index; + const Index last = index + PacketSize - 1; + const Index lastPaddedLeft = m_padding[i].first * m_outputStrides[i]; + const Index firstPaddedRight = (m_dimensions[i] - m_padding[i].second) * m_outputStrides[i]; + const Index lastPaddedRight = m_outputStrides[i+1]; + + if (!isLeftPaddingCompileTimeZero(i) && last < lastPaddedLeft) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if (!isRightPaddingCompileTimeZero(i) && first >= firstPaddedRight && last < lastPaddedRight) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if ((isLeftPaddingCompileTimeZero(i) && isRightPaddingCompileTimeZero(i)) || (first >= lastPaddedLeft && last < firstPaddedRight)) { + // all the coefficient are between the 2 padding zones. + const Index idx = index / m_outputStrides[i]; + inputIndex += (idx - m_padding[i].first) * m_inputStrides[i]; + index -= idx * m_outputStrides[i]; + } + else { + // Every other case + return packetWithPossibleZero(initialIndex); + } + } + + const Index last = index + PacketSize - 1; + const Index first = index; + const Index lastPaddedLeft = m_padding[0].first; + const Index firstPaddedRight = (m_dimensions[0] - m_padding[0].second); + const Index lastPaddedRight = m_outputStrides[1]; + + if (!isLeftPaddingCompileTimeZero(0) && last < lastPaddedLeft) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if (!isRightPaddingCompileTimeZero(0) && first >= firstPaddedRight && last < lastPaddedRight) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if ((isLeftPaddingCompileTimeZero(0) && isRightPaddingCompileTimeZero(0)) || (first >= lastPaddedLeft && last < firstPaddedRight)) { + // all the coefficient are between the 2 padding zones. + inputIndex += (index - m_padding[0].first); + return m_impl.template packet(inputIndex); + } + // Every other case + return packetWithPossibleZero(initialIndex); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packetRowMajor(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + const Index initialIndex = index; + Index inputIndex = 0; + + for (int i = 0; i < NumDims - 1; ++i) { + const Index first = index; + const Index last = index + PacketSize - 1; + const Index lastPaddedLeft = m_padding[i].first * m_outputStrides[i+1]; + const Index firstPaddedRight = (m_dimensions[i] - m_padding[i].second) * m_outputStrides[i+1]; + const Index lastPaddedRight = m_outputStrides[i]; + + if (!isLeftPaddingCompileTimeZero(i) && last < lastPaddedLeft) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if (!isRightPaddingCompileTimeZero(i) && first >= firstPaddedRight && last < lastPaddedRight) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if ((isLeftPaddingCompileTimeZero(i) && isRightPaddingCompileTimeZero(i)) || (first >= lastPaddedLeft && last < firstPaddedRight)) { + // all the coefficient are between the 2 padding zones. + const Index idx = index / m_outputStrides[i+1]; + inputIndex += (idx - m_padding[i].first) * m_inputStrides[i]; + index -= idx * m_outputStrides[i+1]; + } + else { + // Every other case + return packetWithPossibleZero(initialIndex); + } + } + + const Index last = index + PacketSize - 1; + const Index first = index; + const Index lastPaddedLeft = m_padding[NumDims-1].first; + const Index firstPaddedRight = (m_dimensions[NumDims-1] - m_padding[NumDims-1].second); + const Index lastPaddedRight = m_outputStrides[NumDims-1]; + + if (!isLeftPaddingCompileTimeZero(NumDims-1) && last < lastPaddedLeft) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if (!isRightPaddingCompileTimeZero(NumDims-1) && first >= firstPaddedRight && last < lastPaddedRight) { + // all the coefficient are in the padding zone. + return internal::pset1(m_paddingValue); + } + else if ((isLeftPaddingCompileTimeZero(NumDims-1) && isRightPaddingCompileTimeZero(NumDims-1)) || (first >= lastPaddedLeft && last < firstPaddedRight)) { + // all the coefficient are between the 2 padding zones. + inputIndex += (index - m_padding[NumDims-1].first); + return m_impl.template packet(inputIndex); + } + // Every other case + return packetWithPossibleZero(initialIndex); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packetWithPossibleZero(Index index) const + { + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + for (int i = 0; i < PacketSize; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + + Dimensions m_dimensions; + array m_outputStrides; + array m_inputStrides; + TensorEvaluator m_impl; + PaddingDimensions m_padding; + + Scalar m_paddingValue; +}; + + + + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_PADDING_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPatch.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPatch.h new file mode 100644 index 0000000000..886a254f66 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPatch.h @@ -0,0 +1,269 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_PATCH_H +#define EIGEN_CXX11_TENSOR_TENSOR_PATCH_H + +namespace Eigen { + +/** \class TensorPatch + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor patch class. + * + * + */ +namespace internal { +template +struct traits > : public traits +{ + typedef typename XprType::Scalar Scalar; + typedef traits XprTraits; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + typedef typename remove_reference::type _Nested; + static const int NumDimensions = XprTraits::NumDimensions + 1; + static const int Layout = XprTraits::Layout; +}; + +template +struct eval, Eigen::Dense> +{ + typedef const TensorPatchOp& type; +}; + +template +struct nested, 1, typename eval >::type> +{ + typedef TensorPatchOp type; +}; + +} // end namespace internal + + + +template +class TensorPatchOp : public TensorBase, ReadOnlyAccessors> +{ + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorPatchOp(const XprType& expr, const PatchDim& patch_dims) + : m_xpr(expr), m_patch_dims(patch_dims) {} + + EIGEN_DEVICE_FUNC + const PatchDim& patch_dims() const { return m_patch_dims; } + + EIGEN_DEVICE_FUNC + const typename internal::remove_all::type& + expression() const { return m_xpr; } + + protected: + typename XprType::Nested m_xpr; + const PatchDim m_patch_dims; +}; + + +// Eval as rvalue +template +struct TensorEvaluator, Device> +{ + typedef TensorPatchOp XprType; + typedef typename XprType::Index Index; + static const int NumDims = internal::array_size::Dimensions>::value + 1; + typedef DSizes Dimensions; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::CoeffReturnType CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + + enum { + IsAligned = false, + PacketAccess = TensorEvaluator::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, + RawAccess = false + }; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device) + { + Index num_patches = 1; + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + const PatchDim& patch_dims = op.patch_dims(); + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = 0; i < NumDims-1; ++i) { + m_dimensions[i] = patch_dims[i]; + num_patches *= (input_dims[i] - patch_dims[i] + 1); + } + m_dimensions[NumDims-1] = num_patches; + + m_inputStrides[0] = 1; + m_patchStrides[0] = 1; + for (int i = 1; i < NumDims-1; ++i) { + m_inputStrides[i] = m_inputStrides[i-1] * input_dims[i-1]; + m_patchStrides[i] = m_patchStrides[i-1] * (input_dims[i-1] - patch_dims[i-1] + 1); + } + m_outputStrides[0] = 1; + for (int i = 1; i < NumDims; ++i) { + m_outputStrides[i] = m_outputStrides[i-1] * m_dimensions[i-1]; + } + } else { + for (int i = 0; i < NumDims-1; ++i) { + m_dimensions[i+1] = patch_dims[i]; + num_patches *= (input_dims[i] - patch_dims[i] + 1); + } + m_dimensions[0] = num_patches; + + m_inputStrides[NumDims-2] = 1; + m_patchStrides[NumDims-2] = 1; + for (int i = NumDims-3; i >= 0; --i) { + m_inputStrides[i] = m_inputStrides[i+1] * input_dims[i+1]; + m_patchStrides[i] = m_patchStrides[i+1] * (input_dims[i+1] - patch_dims[i+1] + 1); + } + m_outputStrides[NumDims-1] = 1; + for (int i = NumDims-2; i >= 0; --i) { + m_outputStrides[i] = m_outputStrides[i+1] * m_dimensions[i+1]; + } + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool evalSubExprsIfNeeded(Scalar* /*data*/) { + m_impl.evalSubExprsIfNeeded(NULL); + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + Index output_stride_index = (static_cast(Layout) == static_cast(ColMajor)) ? NumDims - 1 : 0; + // Find the location of the first element of the patch. + Index patchIndex = index / m_outputStrides[output_stride_index]; + // Find the offset of the element wrt the location of the first element. + Index patchOffset = index - patchIndex * m_outputStrides[output_stride_index]; + Index inputIndex = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 2; i > 0; --i) { + const Index patchIdx = patchIndex / m_patchStrides[i]; + patchIndex -= patchIdx * m_patchStrides[i]; + const Index offsetIdx = patchOffset / m_outputStrides[i]; + patchOffset -= offsetIdx * m_outputStrides[i]; + inputIndex += (patchIdx + offsetIdx) * m_inputStrides[i]; + } + } else { + for (int i = 0; i < NumDims - 2; ++i) { + const Index patchIdx = patchIndex / m_patchStrides[i]; + patchIndex -= patchIdx * m_patchStrides[i]; + const Index offsetIdx = patchOffset / m_outputStrides[i+1]; + patchOffset -= offsetIdx * m_outputStrides[i+1]; + inputIndex += (patchIdx + offsetIdx) * m_inputStrides[i]; + } + } + inputIndex += (patchIndex + patchOffset); + return m_impl.coeff(inputIndex); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index+PacketSize-1 < dimensions().TotalSize()); + + Index output_stride_index = (static_cast(Layout) == static_cast(ColMajor)) ? NumDims - 1 : 0; + Index indices[2] = {index, index + PacketSize - 1}; + Index patchIndices[2] = {indices[0] / m_outputStrides[output_stride_index], + indices[1] / m_outputStrides[output_stride_index]}; + Index patchOffsets[2] = {indices[0] - patchIndices[0] * m_outputStrides[output_stride_index], + indices[1] - patchIndices[1] * m_outputStrides[output_stride_index]}; + + Index inputIndices[2] = {0, 0}; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumDims - 2; i > 0; --i) { + const Index patchIdx[2] = {patchIndices[0] / m_patchStrides[i], + patchIndices[1] / m_patchStrides[i]}; + patchIndices[0] -= patchIdx[0] * m_patchStrides[i]; + patchIndices[1] -= patchIdx[1] * m_patchStrides[i]; + + const Index offsetIdx[2] = {patchOffsets[0] / m_outputStrides[i], + patchOffsets[1] / m_outputStrides[i]}; + patchOffsets[0] -= offsetIdx[0] * m_outputStrides[i]; + patchOffsets[1] -= offsetIdx[1] * m_outputStrides[i]; + + inputIndices[0] += (patchIdx[0] + offsetIdx[0]) * m_inputStrides[i]; + inputIndices[1] += (patchIdx[1] + offsetIdx[1]) * m_inputStrides[i]; + } + } else { + for (int i = 0; i < NumDims - 2; ++i) { + const Index patchIdx[2] = {patchIndices[0] / m_patchStrides[i], + patchIndices[1] / m_patchStrides[i]}; + patchIndices[0] -= patchIdx[0] * m_patchStrides[i]; + patchIndices[1] -= patchIdx[1] * m_patchStrides[i]; + + const Index offsetIdx[2] = {patchOffsets[0] / m_outputStrides[i+1], + patchOffsets[1] / m_outputStrides[i+1]}; + patchOffsets[0] -= offsetIdx[0] * m_outputStrides[i+1]; + patchOffsets[1] -= offsetIdx[1] * m_outputStrides[i+1]; + + inputIndices[0] += (patchIdx[0] + offsetIdx[0]) * m_inputStrides[i]; + inputIndices[1] += (patchIdx[1] + offsetIdx[1]) * m_inputStrides[i]; + } + } + inputIndices[0] += (patchIndices[0] + patchOffsets[0]); + inputIndices[1] += (patchIndices[1] + patchOffsets[1]); + + if (inputIndices[1] - inputIndices[0] == PacketSize - 1) { + PacketReturnType rslt = m_impl.template packet(inputIndices[0]); + return rslt; + } + else { + EIGEN_ALIGN_MAX CoeffReturnType values[PacketSize]; + values[0] = m_impl.coeff(inputIndices[0]); + values[PacketSize-1] = m_impl.coeff(inputIndices[1]); + for (int i = 1; i < PacketSize-1; ++i) { + values[i] = coeff(index+i); + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + const double compute_cost = NumDims * (TensorOpCost::DivCost() + + TensorOpCost::MulCost() + + 2 * TensorOpCost::AddCost()); + return m_impl.costPerCoeff(vectorized) + + TensorOpCost(0, 0, compute_cost, vectorized, PacketSize); + } + + EIGEN_DEVICE_FUNC Scalar* data() const { return NULL; } + + protected: + Dimensions m_dimensions; + array m_outputStrides; + array m_inputStrides; + array m_patchStrides; + + TensorEvaluator m_impl; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_PATCH_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorRandom.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorRandom.h new file mode 100644 index 0000000000..1655a813e4 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorRandom.h @@ -0,0 +1,276 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2016 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_RANDOM_H +#define EIGEN_CXX11_TENSOR_TENSOR_RANDOM_H + +namespace Eigen { +namespace internal { + +namespace { + +EIGEN_DEVICE_FUNC uint64_t get_random_seed() { +#ifdef __CUDA_ARCH__ + // We don't support 3d kernels since we currently only use 1 and + // 2d kernels. + assert(threadIdx.z == 0); + return clock64() + + blockIdx.x * blockDim.x + threadIdx.x + + gridDim.x * blockDim.x * (blockIdx.y * blockDim.y + threadIdx.y); + +#elif defined _WIN32 + // Use the current time as a baseline. + SYSTEMTIME st; + GetSystemTime(&st); + int time = st.wSecond + 1000 * st.wMilliseconds; + // Mix in a random number to make sure that we get different seeds if + // we try to generate seeds faster than the clock resolution. + // We need 2 random values since the generator only generate 16 bits at + // a time (https://msdn.microsoft.com/en-us/library/398ax69y.aspx) + int rnd1 = ::rand(); + int rnd2 = ::rand(); + uint64_t rnd = (rnd1 | rnd2 << 16) ^ time; + return rnd; + +#elif defined __APPLE__ + // Same approach as for win32, except that the random number generator + // is better (// https://developer.apple.com/legacy/library/documentation/Darwin/Reference/ManPages/man3/random.3.html#//apple_ref/doc/man/3/random). + uint64_t rnd = ::random() ^ mach_absolute_time(); + return rnd; + +#else + // Augment the current time with pseudo random number generation + // to ensure that we get different seeds if we try to generate seeds + // faster than the clock resolution. + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + uint64_t rnd = ::random() ^ ts.tv_nsec; + return rnd; +#endif +} + +static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE unsigned PCG_XSH_RS_generator(uint64_t* state) { + // TODO: Unify with the implementation in the non blocking thread pool. + uint64_t current = *state; + // Update the internal state + *state = current * 6364136223846793005ULL + 0xda3e39cb94b95bdbULL; + // Generate the random output (using the PCG-XSH-RS scheme) + return static_cast((current ^ (current >> 22)) >> (22 + (current >> 61))); +} + +static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE uint64_t PCG_XSH_RS_state(uint64_t seed) { + seed = seed ? seed : get_random_seed(); + return seed * 6364136223846793005ULL + 0xda3e39cb94b95bdbULL; +} + +} // namespace + + +template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +T RandomToTypeUniform(uint64_t* state) { + unsigned rnd = PCG_XSH_RS_generator(state); + return static_cast(rnd); +} + + +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +Eigen::half RandomToTypeUniform(uint64_t* state) { + Eigen::half result; + // Generate 10 random bits for the mantissa + unsigned rnd = PCG_XSH_RS_generator(state); + result.x = static_cast(rnd & 0x3ffu); + // Set the exponent + result.x |= (static_cast(15) << 10); + // Return the final result + return result - Eigen::half(1.0f); +} + + +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +float RandomToTypeUniform(uint64_t* state) { + typedef union { + uint32_t raw; + float fp; + } internal; + internal result; + // Generate 23 random bits for the mantissa mantissa + const unsigned rnd = PCG_XSH_RS_generator(state); + result.raw = rnd & 0x7fffffu; + // Set the exponent + result.raw |= (static_cast(127) << 23); + // Return the final result + return result.fp - 1.0f; +} + +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +double RandomToTypeUniform(uint64_t* state) { + typedef union { + uint64_t raw; + double dp; + } internal; + internal result; + result.raw = 0; + // Generate 52 random bits for the mantissa + // First generate the upper 20 bits + unsigned rnd1 = PCG_XSH_RS_generator(state) & 0xfffffu; + // The generate the lower 32 bits + unsigned rnd2 = PCG_XSH_RS_generator(state); + result.raw = (static_cast(rnd1) << 32) | rnd2; + // Set the exponent + result.raw |= (static_cast(1023) << 52); + // Return the final result + return result.dp - 1.0; +} + +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +std::complex RandomToTypeUniform >(uint64_t* state) { + return std::complex(RandomToTypeUniform(state), + RandomToTypeUniform(state)); +} +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +std::complex RandomToTypeUniform >(uint64_t* state) { + return std::complex(RandomToTypeUniform(state), + RandomToTypeUniform(state)); +} + +template class UniformRandomGenerator { + public: + static const bool PacketAccess = true; + + // Uses the given "seed" if non-zero, otherwise uses a random seed. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE UniformRandomGenerator( + uint64_t seed = 0) { + m_state = PCG_XSH_RS_state(seed); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE UniformRandomGenerator( + const UniformRandomGenerator& other) { + m_state = other.m_state; + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + T operator()(Index i) const { + uint64_t local_state = m_state + i; + T result = RandomToTypeUniform(&local_state); + m_state = local_state; + return result; + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Packet packetOp(Index i) const { + const int packetSize = internal::unpacket_traits::size; + EIGEN_ALIGN_MAX T values[packetSize]; + uint64_t local_state = m_state + i; + for (int j = 0; j < packetSize; ++j) { + values[j] = RandomToTypeUniform(&local_state); + } + m_state = local_state; + return internal::pload(values); + } + + private: + mutable uint64_t m_state; +}; + +template +struct functor_traits > { + enum { + // Rough estimate for floating point, multiplied by ceil(sizeof(T) / sizeof(float)). + Cost = 12 * NumTraits::AddCost * + ((sizeof(Scalar) + sizeof(float) - 1) / sizeof(float)), + PacketAccess = UniformRandomGenerator::PacketAccess + }; +}; + + + +template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +T RandomToTypeNormal(uint64_t* state) { + // Use the ratio of uniform method to generate numbers following a normal + // distribution. See for example Numerical Recipes chapter 7.3.9 for the + // details. + T u, v, q; + do { + u = RandomToTypeUniform(state); + v = T(1.7156) * (RandomToTypeUniform(state) - T(0.5)); + const T x = u - T(0.449871); + const T y = numext::abs(v) + T(0.386595); + q = x*x + y * (T(0.196)*y - T(0.25472)*x); + } while (q > T(0.27597) && + (q > T(0.27846) || v*v > T(-4) * numext::log(u) * u*u)); + + return v/u; +} + +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +std::complex RandomToTypeNormal >(uint64_t* state) { + return std::complex(RandomToTypeNormal(state), + RandomToTypeNormal(state)); +} +template <> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +std::complex RandomToTypeNormal >(uint64_t* state) { + return std::complex(RandomToTypeNormal(state), + RandomToTypeNormal(state)); +} + + +template class NormalRandomGenerator { + public: + static const bool PacketAccess = true; + + // Uses the given "seed" if non-zero, otherwise uses a random seed. + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE NormalRandomGenerator(uint64_t seed = 0) { + m_state = PCG_XSH_RS_state(seed); + } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE NormalRandomGenerator( + const NormalRandomGenerator& other) { + m_state = other.m_state; + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + T operator()(Index i) const { + uint64_t local_state = m_state + i; + T result = RandomToTypeNormal(&local_state); + m_state = local_state; + return result; + } + + template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + Packet packetOp(Index i) const { + const int packetSize = internal::unpacket_traits::size; + EIGEN_ALIGN_MAX T values[packetSize]; + uint64_t local_state = m_state + i; + for (int j = 0; j < packetSize; ++j) { + values[j] = RandomToTypeNormal(&local_state); + } + m_state = local_state; + return internal::pload(values); + } + + private: + mutable uint64_t m_state; +}; + + +template +struct functor_traits > { + enum { + // On average, we need to generate about 3 random numbers + // 15 mul, 8 add, 1.5 logs + Cost = 3 * functor_traits >::Cost + + 15 * NumTraits::AddCost + 8 * NumTraits::AddCost + + 3 * functor_traits >::Cost / 2, + PacketAccess = NormalRandomGenerator::PacketAccess + }; +}; + + +} // end namespace internal +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_RANDOM_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReduction.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReduction.h new file mode 100644 index 0000000000..41d0d0022f --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReduction.h @@ -0,0 +1,781 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// Copyright (C) 2016 Mehdi Goli, Codeplay Software Ltd +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_REDUCTION_H +#define EIGEN_CXX11_TENSOR_TENSOR_REDUCTION_H + +namespace Eigen { + +/** \class TensorReduction + * \ingroup CXX11_Tensor_Module + * + * \brief Tensor reduction class. + * + */ + +namespace internal { + template class MakePointer_ > + struct traits > + : traits +{ + typedef traits XprTraits; + typedef typename XprTraits::Scalar Scalar; + typedef typename XprTraits::StorageKind StorageKind; + typedef typename XprTraits::Index Index; + typedef typename XprType::Nested Nested; + static const int NumDimensions = XprTraits::NumDimensions - array_size::value; + static const int Layout = XprTraits::Layout; + + template struct MakePointer { + // Intermediate typedef to workaround MSVC issue. + typedef MakePointer_ MakePointerT; + typedef typename MakePointerT::Type Type; + }; +}; + +template class MakePointer_> +struct eval, Eigen::Dense> +{ + typedef const TensorReductionOp& type; +}; + +template class MakePointer_> +struct nested, 1, typename eval >::type> +{ + typedef TensorReductionOp type; +}; + + +template struct DimInitializer { + template EIGEN_DEVICE_FUNC + static void run(const InputDims& input_dims, + const array::value>& reduced, + OutputDims* output_dims, ReducedDims* reduced_dims) { + const int NumInputDims = internal::array_size::value; + int outputIndex = 0; + int reduceIndex = 0; + for (int i = 0; i < NumInputDims; ++i) { + if (reduced[i]) { + (*reduced_dims)[reduceIndex] = input_dims[i]; + ++reduceIndex; + } else { + (*output_dims)[outputIndex] = input_dims[i]; + ++outputIndex; + } + } + } +}; + +template <> struct DimInitializer > { + template EIGEN_DEVICE_FUNC + static void run(const InputDims& input_dims, const array&, + Sizes<>*, array* reduced_dims) { + const int NumInputDims = internal::array_size::value; + for (int i = 0; i < NumInputDims; ++i) { + (*reduced_dims)[i] = input_dims[i]; + } + } +}; + + +template +struct are_inner_most_dims { + static const bool value = false; +}; +template +struct preserve_inner_most_dims { + static const bool value = false; +}; + +#if EIGEN_HAS_CONSTEXPR && EIGEN_HAS_VARIADIC_TEMPLATES +template +struct are_inner_most_dims{ + static const bool tmp1 = indices_statically_known_to_increase(); + static const bool tmp2 = index_statically_eq(0, 0); + static const bool tmp3 = index_statically_eq(array_size::value-1, array_size::value-1); + static const bool value = tmp1 & tmp2 & tmp3; +}; +template +struct are_inner_most_dims{ + static const bool tmp1 = indices_statically_known_to_increase(); + static const bool tmp2 = index_statically_eq(0, NumTensorDims - array_size::value); + static const bool tmp3 = index_statically_eq(array_size::value - 1, NumTensorDims - 1); + static const bool value = tmp1 & tmp2 & tmp3; + +}; +template +struct preserve_inner_most_dims{ + static const bool tmp1 = indices_statically_known_to_increase(); + static const bool tmp2 = index_statically_gt(0, 0); + static const bool value = tmp1 & tmp2; + +}; +template +struct preserve_inner_most_dims{ + static const bool tmp1 = indices_statically_known_to_increase(); + static const bool tmp2 = index_statically_lt(array_size::value - 1, NumTensorDims - 1); + static const bool value = tmp1 & tmp2; +}; +#endif + + +template +struct GenericDimReducer { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self& self, typename Self::Index firstIndex, Op& reducer, typename Self::CoeffReturnType* accum) { + EIGEN_STATIC_ASSERT((DimIndex > 0), YOU_MADE_A_PROGRAMMING_MISTAKE); + for (int j = 0; j < self.m_reducedDims[DimIndex]; ++j) { + const typename Self::Index input = firstIndex + j * self.m_reducedStrides[DimIndex]; + GenericDimReducer::reduce(self, input, reducer, accum); + } + } +}; +template +struct GenericDimReducer<0, Self, Op> { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self& self, typename Self::Index firstIndex, Op& reducer, typename Self::CoeffReturnType* accum) { + for (int j = 0; j < self.m_reducedDims[0]; ++j) { + const typename Self::Index input = firstIndex + j * self.m_reducedStrides[0]; + reducer.reduce(self.m_impl.coeff(input), accum); + } + } +}; +template +struct GenericDimReducer<-1, Self, Op> { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self& self, typename Self::Index index, Op& reducer, typename Self::CoeffReturnType* accum) { + reducer.reduce(self.m_impl.coeff(index), accum); + } +}; + +template +struct InnerMostDimReducer { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename Self::CoeffReturnType reduce(const Self& self, typename Self::Index firstIndex, typename Self::Index numValuesToReduce, Op& reducer) { + typename Self::CoeffReturnType accum = reducer.initialize(); + for (typename Self::Index j = 0; j < numValuesToReduce; ++j) { + reducer.reduce(self.m_impl.coeff(firstIndex + j), &accum); + } + return reducer.finalize(accum); + } +}; + +template +struct InnerMostDimReducer { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename Self::CoeffReturnType reduce(const Self& self, typename Self::Index firstIndex, typename Self::Index numValuesToReduce, Op& reducer) { + const int packetSize = internal::unpacket_traits::size; + const typename Self::Index VectorizedSize = (numValuesToReduce / packetSize) * packetSize; + typename Self::PacketReturnType p = reducer.template initializePacket(); + for (typename Self::Index j = 0; j < VectorizedSize; j += packetSize) { + reducer.reducePacket(self.m_impl.template packet(firstIndex + j), &p); + } + typename Self::CoeffReturnType accum = reducer.initialize(); + for (typename Self::Index j = VectorizedSize; j < numValuesToReduce; ++j) { + reducer.reduce(self.m_impl.coeff(firstIndex + j), &accum); + } + return reducer.finalizeBoth(accum, p); + } +}; + +template +struct InnerMostDimPreserver { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self&, typename Self::Index, Op&, typename Self::PacketReturnType*) { + eigen_assert(false && "should never be called"); + } +}; + +template +struct InnerMostDimPreserver { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self& self, typename Self::Index firstIndex, Op& reducer, typename Self::PacketReturnType* accum) { + EIGEN_STATIC_ASSERT((DimIndex > 0), YOU_MADE_A_PROGRAMMING_MISTAKE); + for (typename Self::Index j = 0; j < self.m_reducedDims[DimIndex]; ++j) { + const typename Self::Index input = firstIndex + j * self.m_reducedStrides[DimIndex]; + InnerMostDimPreserver::reduce(self, input, reducer, accum); + } + } +}; + +template +struct InnerMostDimPreserver<0, Self, Op, true> { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self& self, typename Self::Index firstIndex, Op& reducer, typename Self::PacketReturnType* accum) { + for (typename Self::Index j = 0; j < self.m_reducedDims[0]; ++j) { + const typename Self::Index input = firstIndex + j * self.m_reducedStrides[0]; + reducer.reducePacket(self.m_impl.template packet(input), accum); + } + } +}; +template +struct InnerMostDimPreserver<-1, Self, Op, true> { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void reduce(const Self&, typename Self::Index, Op&, typename Self::PacketReturnType*) { + eigen_assert(false && "should never be called"); + } +}; + +// Default full reducer +template +struct FullReducer { + static const bool HasOptimizedImplementation = false; + + static EIGEN_DEVICE_FUNC void run(const Self& self, Op& reducer, const Device&, typename Self::CoeffReturnType* output) { + const typename Self::Index num_coeffs = array_prod(self.m_impl.dimensions()); + *output = InnerMostDimReducer::reduce(self, 0, num_coeffs, reducer); + } +}; + + +#ifdef EIGEN_USE_THREADS +// Multithreaded full reducers +template +struct FullReducerShard { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(const Self& self, typename Self::Index firstIndex, + typename Self::Index numValuesToReduce, Op& reducer, + typename Self::CoeffReturnType* output) { + *output = InnerMostDimReducer::reduce( + self, firstIndex, numValuesToReduce, reducer); + } +}; + +// Multithreaded full reducer +template +struct FullReducer { + static const bool HasOptimizedImplementation = !Op::IsStateful; + static const int PacketSize = + unpacket_traits::size; + + // launch one reducer per thread and accumulate the result. + static void run(const Self& self, Op& reducer, const ThreadPoolDevice& device, + typename Self::CoeffReturnType* output) { + typedef typename Self::Index Index; + const Index num_coeffs = array_prod(self.m_impl.dimensions()); + if (num_coeffs == 0) { + *output = reducer.finalize(reducer.initialize()); + return; + } + const TensorOpCost cost = + self.m_impl.costPerCoeff(Vectorizable) + + TensorOpCost(0, 0, internal::functor_traits::Cost, Vectorizable, + PacketSize); + const int num_threads = TensorCostModel::numThreads( + num_coeffs, cost, device.numThreads()); + if (num_threads == 1) { + *output = + InnerMostDimReducer::reduce(self, 0, num_coeffs, reducer); + return; + } + const Index blocksize = + std::floor(static_cast(num_coeffs) / num_threads); + const Index numblocks = blocksize > 0 ? num_coeffs / blocksize : 0; + eigen_assert(num_coeffs >= numblocks * blocksize); + + Barrier barrier(internal::convert_index(numblocks)); + MaxSizeVector shards(numblocks, reducer.initialize()); + for (Index i = 0; i < numblocks; ++i) { + device.enqueue_with_barrier(&barrier, &FullReducerShard::run, + self, i * blocksize, blocksize, reducer, + &shards[i]); + } + typename Self::CoeffReturnType finalShard; + if (numblocks * blocksize < num_coeffs) { + finalShard = InnerMostDimReducer::reduce( + self, numblocks * blocksize, num_coeffs - numblocks * blocksize, + reducer); + } else { + finalShard = reducer.initialize(); + } + barrier.Wait(); + + for (Index i = 0; i < numblocks; ++i) { + reducer.reduce(shards[i], &finalShard); + } + *output = reducer.finalize(finalShard); + } +}; + +#endif + + +// Default inner reducer +template +struct InnerReducer { + static const bool HasOptimizedImplementation = false; + + EIGEN_DEVICE_FUNC static bool run(const Self&, Op&, const Device&, typename Self::CoeffReturnType*, typename Self::Index, typename Self::Index) { + eigen_assert(false && "Not implemented"); + return true; + } +}; + +// Default outer reducer +template +struct OuterReducer { + static const bool HasOptimizedImplementation = false; + + EIGEN_DEVICE_FUNC static bool run(const Self&, Op&, const Device&, typename Self::CoeffReturnType*, typename Self::Index, typename Self::Index) { + eigen_assert(false && "Not implemented"); + return true; + } +}; + + +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) +template +__global__ void FullReductionKernel(R, const S, I, typename S::CoeffReturnType*, unsigned int*); + + +#ifdef EIGEN_HAS_CUDA_FP16 +template +__global__ void ReductionInitFullReduxKernelHalfFloat(R, const S, I, half2*); +template +__global__ void FullReductionKernelHalfFloat(R, const S, I, half*, half2*); +template +__global__ void InnerReductionKernelHalfFloat(R, const S, I, I, half*); + +#endif + +template +__global__ void InnerReductionKernel(R, const S, I, I, typename S::CoeffReturnType*); + +template +__global__ void OuterReductionKernel(R, const S, I, I, typename S::CoeffReturnType*); +#endif + +} // end namespace internal + + +template class MakePointer_> +class TensorReductionOp : public TensorBase, ReadOnlyAccessors> { + public: + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename Eigen::internal::nested::type Nested; + typedef typename Eigen::internal::traits::StorageKind StorageKind; + typedef typename Eigen::internal::traits::Index Index; + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorReductionOp(const XprType& expr, const Dims& dims) : m_expr(expr), m_dims(dims) + { } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + TensorReductionOp(const XprType& expr, const Dims& dims, const Op& reducer) : m_expr(expr), m_dims(dims), m_reducer(reducer) + { } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const XprType& expression() const { return m_expr; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const Dims& dims() const { return m_dims; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + const Op& reducer() const { return m_reducer; } + + protected: + typename XprType::Nested m_expr; + const Dims m_dims; + const Op m_reducer; +}; + + +// Eval as rvalue +template class MakePointer_, typename Device> +struct TensorEvaluator, Device> +{ + typedef TensorReductionOp XprType; + typedef typename XprType::Index Index; + typedef ArgType ChildType; + typedef typename TensorEvaluator::Dimensions InputDimensions; + static const int NumInputDims = internal::array_size::value; + static const int NumReducedDims = internal::array_size::value; + static const int NumOutputDims = NumInputDims - NumReducedDims; + typedef typename internal::conditional, DSizes >::type Dimensions; + typedef typename XprType::Scalar Scalar; + typedef TensorEvaluator, Device> Self; + static const bool InputPacketAccess = TensorEvaluator::PacketAccess; + typedef typename internal::remove_const::type CoeffReturnType; + typedef typename PacketType::type PacketReturnType; + static const int PacketSize = internal::unpacket_traits::size; + + enum { + IsAligned = false, + PacketAccess = Self::InputPacketAccess && Op::PacketAccess, + Layout = TensorEvaluator::Layout, + CoordAccess = false, // to be implemented + RawAccess = false + }; + + static const bool ReducingInnerMostDims = internal::are_inner_most_dims::value; + static const bool PreservingInnerMostDims = internal::preserve_inner_most_dims::value; + static const bool RunningFullReduction = (NumOutputDims==0); + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorEvaluator(const XprType& op, const Device& device) + : m_impl(op.expression(), device), m_reducer(op.reducer()), m_result(NULL), m_device(device), m_xpr_dims(op.dims()) + { + EIGEN_STATIC_ASSERT((NumInputDims >= NumReducedDims), YOU_MADE_A_PROGRAMMING_MISTAKE); + EIGEN_STATIC_ASSERT((!ReducingInnerMostDims | !PreservingInnerMostDims | (NumReducedDims == NumInputDims)), + YOU_MADE_A_PROGRAMMING_MISTAKE); + + // Build the bitmap indicating if an input dimension is reduced or not. + for (int i = 0; i < NumInputDims; ++i) { + m_reduced[i] = false; + } + for (int i = 0; i < NumReducedDims; ++i) { + eigen_assert(op.dims()[i] >= 0); + eigen_assert(op.dims()[i] < NumInputDims); + m_reduced[op.dims()[i]] = true; + } + + const typename TensorEvaluator::Dimensions& input_dims = m_impl.dimensions(); + internal::DimInitializer::run(input_dims, m_reduced, &m_dimensions, &m_reducedDims); + + // Precompute output strides. + if (NumOutputDims > 0) { + if (static_cast(Layout) == static_cast(ColMajor)) { + m_outputStrides[0] = 1; + for (int i = 1; i < NumOutputDims; ++i) { + m_outputStrides[i] = m_outputStrides[i - 1] * m_dimensions[i - 1]; + } + } else { + m_outputStrides.back() = 1; + for (int i = NumOutputDims - 2; i >= 0; --i) { + m_outputStrides[i] = m_outputStrides[i + 1] * m_dimensions[i + 1]; + } + } + } + + // Precompute input strides. + if (NumInputDims > 0) { + array input_strides; + if (static_cast(Layout) == static_cast(ColMajor)) { + input_strides[0] = 1; + for (int i = 1; i < NumInputDims; ++i) { + input_strides[i] = input_strides[i-1] * input_dims[i-1]; + } + } else { + input_strides.back() = 1; + for (int i = NumInputDims - 2; i >= 0; --i) { + input_strides[i] = input_strides[i + 1] * input_dims[i + 1]; + } + } + + int outputIndex = 0; + int reduceIndex = 0; + for (int i = 0; i < NumInputDims; ++i) { + if (m_reduced[i]) { + m_reducedStrides[reduceIndex] = input_strides[i]; + ++reduceIndex; + } else { + m_preservedStrides[outputIndex] = input_strides[i]; + ++outputIndex; + } + } + } + + // Special case for full reductions + if (NumOutputDims == 0) { + m_preservedStrides[0] = internal::array_prod(input_dims); + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Dimensions& dimensions() const { return m_dimensions; } + + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool evalSubExprsIfNeeded(typename MakePointer_::Type data) { + m_impl.evalSubExprsIfNeeded(NULL); + + // Use the FullReducer if possible. + if ((RunningFullReduction && RunningOnSycl) ||(RunningFullReduction && + internal::FullReducer::HasOptimizedImplementation && + ((RunningOnGPU && (m_device.majorDeviceVersion() >= 3)) || + !RunningOnGPU))) { + bool need_assign = false; + if (!data) { + m_result = static_cast(m_device.allocate(sizeof(CoeffReturnType))); + data = m_result; + need_assign = true; + } + Op reducer(m_reducer); + internal::FullReducer::run(*this, reducer, m_device, data); + return need_assign; + } + else if(RunningOnSycl){ + const Index num_values_to_reduce = internal::array_prod(m_reducedDims); + const Index num_coeffs_to_preserve = internal::array_prod(m_dimensions); + if (!data) { + data = static_cast(m_device.allocate(sizeof(CoeffReturnType) * num_coeffs_to_preserve)); + m_result = data; + } + Op reducer(m_reducer); + internal::InnerReducer::run(*this, reducer, m_device, data, num_values_to_reduce, num_coeffs_to_preserve); + return (m_result != NULL); + } + + // Attempt to use an optimized reduction. + else if (RunningOnGPU && (m_device.majorDeviceVersion() >= 3)) { + bool reducing_inner_dims = true; + for (int i = 0; i < NumReducedDims; ++i) { + if (static_cast(Layout) == static_cast(ColMajor)) { + reducing_inner_dims &= m_reduced[i]; + } else { + reducing_inner_dims &= m_reduced[NumInputDims - 1 - i]; + } + } + if (internal::InnerReducer::HasOptimizedImplementation && + (reducing_inner_dims || ReducingInnerMostDims)) { + const Index num_values_to_reduce = internal::array_prod(m_reducedDims); + const Index num_coeffs_to_preserve = internal::array_prod(m_dimensions); + if (!data) { + if (num_coeffs_to_preserve < 1024 && num_values_to_reduce > num_coeffs_to_preserve && num_values_to_reduce > 128) { + data = static_cast(m_device.allocate(sizeof(CoeffReturnType) * num_coeffs_to_preserve)); + m_result = data; + } + else { + return true; + } + } + Op reducer(m_reducer); + if (internal::InnerReducer::run(*this, reducer, m_device, data, num_values_to_reduce, num_coeffs_to_preserve)) { + if (m_result) { + m_device.deallocate(m_result); + m_result = NULL; + } + return true; + } else { + return (m_result != NULL); + } + } + + bool preserving_inner_dims = true; + for (int i = 0; i < NumReducedDims; ++i) { + if (static_cast(Layout) == static_cast(ColMajor)) { + preserving_inner_dims &= m_reduced[NumInputDims - 1 - i]; + } else { + preserving_inner_dims &= m_reduced[i]; + } + } + if (internal::OuterReducer::HasOptimizedImplementation && + preserving_inner_dims) { + const Index num_values_to_reduce = internal::array_prod(m_reducedDims); + const Index num_coeffs_to_preserve = internal::array_prod(m_dimensions); + if (!data) { + if (num_coeffs_to_preserve < 1024 && num_values_to_reduce > num_coeffs_to_preserve && num_values_to_reduce > 32) { + data = static_cast(m_device.allocate(sizeof(CoeffReturnType) * num_coeffs_to_preserve)); + m_result = data; + } + else { + return true; + } + } + Op reducer(m_reducer); + if (internal::OuterReducer::run(*this, reducer, m_device, data, num_values_to_reduce, num_coeffs_to_preserve)) { + if (m_result) { + m_device.deallocate(m_result); + m_result = NULL; + } + return true; + } else { + return (m_result != NULL); + } + } + } + return true; + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void cleanup() { + m_impl.cleanup(); + if (m_result) { + m_device.deallocate(m_result); + m_result = NULL; + } + } + + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const + { + if ((RunningOnSycl || RunningFullReduction || RunningOnGPU) && m_result) { + return *(m_result + index); + } + Op reducer(m_reducer); + if (ReducingInnerMostDims || RunningFullReduction) { + const Index num_values_to_reduce = + (static_cast(Layout) == static_cast(ColMajor)) ? m_preservedStrides[0] : m_preservedStrides[NumPreservedStrides - 1]; + return internal::InnerMostDimReducer::reduce(*this, firstInput(index), + num_values_to_reduce, reducer); + } else { + typename Self::CoeffReturnType accum = reducer.initialize(); + internal::GenericDimReducer::reduce(*this, firstInput(index), reducer, &accum); + return reducer.finalize(accum); + } + } + + // TODO(bsteiner): provide a more efficient implementation. + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketReturnType packet(Index index) const + { + EIGEN_STATIC_ASSERT((PacketSize > 1), YOU_MADE_A_PROGRAMMING_MISTAKE) + eigen_assert(index + PacketSize - 1 < Index(internal::array_prod(dimensions()))); + + if (RunningOnGPU && m_result) { + return internal::pload(m_result + index); + } + + EIGEN_ALIGN_MAX typename internal::remove_const::type values[PacketSize]; + if (ReducingInnerMostDims) { + const Index num_values_to_reduce = + (static_cast(Layout) == static_cast(ColMajor)) ? m_preservedStrides[0] : m_preservedStrides[NumPreservedStrides - 1]; + const Index firstIndex = firstInput(index); + for (Index i = 0; i < PacketSize; ++i) { + Op reducer(m_reducer); + values[i] = internal::InnerMostDimReducer::reduce(*this, firstIndex + i * num_values_to_reduce, + num_values_to_reduce, reducer); + } + } else if (PreservingInnerMostDims) { + const Index firstIndex = firstInput(index); + const int innermost_dim = (static_cast(Layout) == static_cast(ColMajor)) ? 0 : NumOutputDims - 1; + // TBD: extend this the the n innermost dimensions that we preserve. + if (((firstIndex % m_dimensions[innermost_dim]) + PacketSize - 1) < m_dimensions[innermost_dim]) { + Op reducer(m_reducer); + typename Self::PacketReturnType accum = reducer.template initializePacket(); + internal::InnerMostDimPreserver::reduce(*this, firstIndex, reducer, &accum); + return reducer.finalizePacket(accum); + } else { + for (int i = 0; i < PacketSize; ++i) { + values[i] = coeff(index + i); + } + } + } else { + for (int i = 0; i < PacketSize; ++i) { + values[i] = coeff(index + i); + } + } + PacketReturnType rslt = internal::pload(values); + return rslt; + } + + // Must be called after evalSubExprsIfNeeded(). + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorOpCost costPerCoeff(bool vectorized) const { + if (RunningFullReduction && m_result) { + return TensorOpCost(sizeof(CoeffReturnType), 0, 0, vectorized, PacketSize); + } else { + const Index num_values_to_reduce = internal::array_prod(m_reducedDims); + const double compute_cost = num_values_to_reduce * internal::functor_traits::Cost; + return m_impl.costPerCoeff(vectorized) * num_values_to_reduce + + TensorOpCost(0, 0, compute_cost, vectorized, PacketSize); + } + } + + EIGEN_DEVICE_FUNC typename MakePointer_::Type data() const { return m_result; } + /// required by sycl in order to extract the accessor + const TensorEvaluator& impl() const { return m_impl; } + /// added for sycl in order to construct the buffer from the sycl device + const Device& device() const{return m_device;} + /// added for sycl in order to re-construct the reduction eval on the device for the sub-kernel + const Dims& xprDims() const {return m_xpr_dims;} + + + private: + template friend struct internal::GenericDimReducer; + template friend struct internal::InnerMostDimReducer; + template friend struct internal::InnerMostDimPreserver; + template friend struct internal::FullReducer; +#ifdef EIGEN_USE_THREADS + template friend struct internal::FullReducerShard; +#endif +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) + template friend void internal::FullReductionKernel(R, const S, I, typename S::CoeffReturnType*, unsigned int*); +#ifdef EIGEN_HAS_CUDA_FP16 + template friend void internal::ReductionInitFullReduxKernelHalfFloat(R, const S, I, half2*); + template friend void internal::FullReductionKernelHalfFloat(R, const S, I, half*, half2*); + template friend void internal::InnerReductionKernelHalfFloat(R, const S, I, I, half*); +#endif + template friend void internal::InnerReductionKernel(R, const S, I, I, typename S::CoeffReturnType*); + + template friend void internal::OuterReductionKernel(R, const S, I, I, typename S::CoeffReturnType*); +#endif + + template friend struct internal::InnerReducer; + + // Returns the Index in the input tensor of the first value that needs to be + // used to compute the reduction at output index "index". + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index firstInput(Index index) const { + if (ReducingInnerMostDims) { + if (static_cast(Layout) == static_cast(ColMajor)) { + return index * m_preservedStrides[0]; + } else { + return index * m_preservedStrides[NumPreservedStrides - 1]; + } + } + // TBD: optimize the case where we preserve the innermost dimensions. + Index startInput = 0; + if (static_cast(Layout) == static_cast(ColMajor)) { + for (int i = NumOutputDims - 1; i > 0; --i) { + // This is index_i in the output tensor. + const Index idx = index / m_outputStrides[i]; + startInput += idx * m_preservedStrides[i]; + index -= idx * m_outputStrides[i]; + } + if (PreservingInnerMostDims) { + eigen_assert(m_preservedStrides[0] == 1); + startInput += index; + } else { + startInput += index * m_preservedStrides[0]; + } + } else { + for (int i = 0; i < NumOutputDims - 1; ++i) { + // This is index_i in the output tensor. + const Index idx = index / m_outputStrides[i]; + startInput += idx * m_preservedStrides[i]; + index -= idx * m_outputStrides[i]; + } + if (PreservingInnerMostDims) { + eigen_assert(m_preservedStrides[NumPreservedStrides - 1] == 1); + startInput += index; + } else { + startInput += index * m_preservedStrides[NumPreservedStrides - 1]; + } + } + return startInput; + } + + // Bitmap indicating if an input dimension is reduced or not. + array m_reduced; + // Dimensions of the output of the operation. + Dimensions m_dimensions; + // Precomputed strides for the output tensor. + array m_outputStrides; + // Subset of strides of the input tensor for the non-reduced dimensions. + // Indexed by output dimensions. + static const int NumPreservedStrides = max_n_1::size; + array m_preservedStrides; + + // Subset of strides of the input tensor for the reduced dimensions. + // Indexed by reduced dimensions. + array m_reducedStrides; + // Size of the input dimensions that are reduced. + // Indexed by reduced dimensions. + array m_reducedDims; + + // Evaluator for the input expression. + TensorEvaluator m_impl; + + // Operation to apply for computing the reduction. + Op m_reducer; + + // For full reductions +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) + static const bool RunningOnGPU = internal::is_same::value; + static const bool RunningOnSycl = false; +#elif defined(EIGEN_USE_SYCL) +static const bool RunningOnSycl = internal::is_same::type, Eigen::SyclDevice>::value; +static const bool RunningOnGPU = false; +#else + static const bool RunningOnGPU = false; + static const bool RunningOnSycl = false; +#endif + typename MakePointer_::Type m_result; + + const Device& m_device; + const Dims& m_xpr_dims; +}; + +} // end namespace Eigen + +#endif // EIGEN_CXX11_TENSOR_TENSOR_REDUCTION_H diff --git a/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h new file mode 100644 index 0000000000..65638b6a84 --- /dev/null +++ b/ground/gcs/src/libs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h @@ -0,0 +1,750 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2014 Benoit Steiner +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_CXX11_TENSOR_TENSOR_REDUCTION_CUDA_H +#define EIGEN_CXX11_TENSOR_TENSOR_REDUCTION_CUDA_H + +namespace Eigen { +namespace internal { + + +#if defined(EIGEN_USE_GPU) && defined(__CUDACC__) +// Full reducers for GPU, don't vectorize for now + +// Reducer function that enables multiple cuda thread to safely accumulate at the same +// output address. It basically reads the current value of the output variable, and +// attempts to update it with the new value. If in the meantime another cuda thread +// updated the content of the output address it will try again. +template +__device__ EIGEN_ALWAYS_INLINE void atomicReduce(T* output, T accum, R& reducer) { +#if __CUDA_ARCH__ >= 300 + if (sizeof(T) == 4) + { + unsigned int oldval = *reinterpret_cast(output); + unsigned int newval = oldval; + reducer.reduce(accum, reinterpret_cast(&newval)); + if (newval == oldval) { + return; + } + unsigned int readback; + while ((readback = atomicCAS((unsigned int*)output, oldval, newval)) != oldval) { + oldval = readback; + newval = oldval; + reducer.reduce(accum, reinterpret_cast(&newval)); + if (newval == oldval) { + return; + } + } + } + else if (sizeof(T) == 8) { + unsigned long long oldval = *reinterpret_cast(output); + unsigned long long newval = oldval; + reducer.reduce(accum, reinterpret_cast(&newval)); + if (newval == oldval) { + return; + } + unsigned long long readback; + while ((readback = atomicCAS((unsigned long long*)output, oldval, newval)) != oldval) { + oldval = readback; + newval = oldval; + reducer.reduce(accum, reinterpret_cast(&newval)); + if (newval == oldval) { + return; + } + } + } + else { + assert(0 && "Wordsize not supported"); + } +#else + assert(0 && "Shouldn't be called on unsupported device"); +#endif +} + +// We extend atomicExch to support extra data types +template +__device__ inline Type atomicExchCustom(Type* address, Type val) { + return atomicExch(address, val); +} + +template <> +__device__ inline double atomicExchCustom(double* address, double val) { + unsigned long long int* address_as_ull = reinterpret_cast(address); + return __longlong_as_double(atomicExch(address_as_ull, __double_as_longlong(val))); +} + +#ifdef EIGEN_HAS_CUDA_FP16 +template